yemai/mmwave_ti_ros
1
0
Fork 0
mirror of https://git.ti.com/git/mmwave_radar/mmwave_ti_ros.git synced 2026-05-21 09:22:15 +00:00
Code Issues Packages Projects Releases Wiki Activity

Pull request #8: Moved from personal repo ROS_Driver_With_Mobile_Tracker to this repo

Browse Source 
Merge in MITL/mmwave_ti_ros from MMWAVE_APPS-1018-ros-driver-demo-changes-to-public-repo to master

* commit '2f756a2707f9c23686537a578ba37dd5654e0a17':
  Moved from personal repo ROS_Driver_With_Mobile_Tracker to this repo
This commit is contained in:
Pedrhom Nafisi
2022-11-07 16:24:22 -06:00
parent 0338025e79 2f756a2707
commit 6323d2aed9
59 changed files with 4115 additions and 34 deletions
Download Patch File Download Diff File Expand all files Collapse all files

1
autonomous_robotics_ros/src/kobuki_msgs

Submodule autonomous_robotics_ros/src/kobuki_msgs deleted from ec509794be

1
autonomous_robotics_ros/src/serial

Submodule autonomous_robotics_ros/src/serial deleted from cbcca7c837

1
autonomous_robotics_ros/src/turtlebot_msgs

Submodule autonomous_robotics_ros/src/turtlebot_msgs deleted from 834c448fb3

1
autonomous_robotics_ros/src/turtlebot_simulator

Submodule autonomous_robotics_ros/src/turtlebot_simulator deleted from beae4b3cdf

89
ros_driver/README.txt Normal file
View File

@@ -0,0 +1,89 @@
--------------------------------------------------------------------------------------------------------------------------------------------------
v1.0
ROS Driver + Tracking added
Detection layer for ti_mmwave_tracker_rospkg will now be based off of Capon algorithm and processing chain with tracking, as opposed to the Bartlett algorithm which is also used in Out of Box Demo and no tracking. Capon algorithm allows for better point cloud density and accuracy, as well as tracking information.
Tested with Industrial Toolbox for mmWave Sensors 4.12.0
--------------------------------------------------------------------------------------------------------------------------------------------------
Setup:
1. Follow the ROS Driver user guide up until the step that requires you to use "roslaunch"
https://dev.ti.com/tirex/explore/node?a=VLyFKFf__4.12.0&node=AAFPzcnzNHunUphPQZyTyg__VLyFKFf__4.12.0
**NOTE** For anywhere the guide mentions Out of Box demo make sure you are using the People Counting binary file.
2. Use the console command "roslaunch ti_mmwave_tracker_rospkg AOP_3d_Tracking.launch" to start the sensor which will start the sensor and Rviz to show the point cloud. The tracking information is not visualized but number of tracks detected is currently being printed to console via a "ROS_INFO" command within DataHandlerClass.cpp.
If you want to avoid having Rviz start at the same time as the sensor, delete the entire line near the bottom of AOP_3d_Tracking.launch that says "rviz" in it
--------------------------------------------------------------------------------------------------------------------------------------------------
Developer's Guide:
All tracker information is handled within the "DataHandlerClass.cpp" file in the src folder.
Each track has 112 bytes of information.
Track List 3D 112 Bytes * Number of Tracks
'tid', {'uint32', 4}, ... % Track ID
'posX', {'float', 4}, ... % Track position in X dimension, m
'posY', {'float', 4}, ... % Track position in Y dimension, m
'posZ', {'float', 4}, ... % Track position in Z dimension, m
'velX', {'float', 4}, ... % Track velocity in X dimension, m/s
'velY', {'float', 4}, ... % Track velocity in Y dimension, m/s
'velZ', {'float', 4}, ... % Track velocity in Z dimension, m/s
'accX', {'float', 4}, ... % Track acceleration in X dimension, m/s2
'accY', {'float', 4}, ... % Track acceleration in Y dimension, m/s
'accZ', {'float', 4}, ... % Track acceleration in Z dimension, m/s
--------------------------------------------------------------------------------------------------------------------------------------------------
Every point is given a track index value, assigning that point to a track ID it falls into. If a point does not fall into a track for whatever reason, a special value is given for why.
Track Index 1 Byte * Number of Points
'targetID', {'uint8', 1}); % Track ID
Special Values:
253 - Point not associated, SNR too weak
254 - Point not associated, located outside boundary of interest
255 - Point not associated, considered as noise
--------------------------------------------------------------------------------------------------------------------------------------------------
3D Spherical Compressed Point Cloud: 20 bytes + (7 bytes * Number of Points)
For data compression, the 20 bytes worth of unit values are sent first and only once. These unit values need to be multiplied by every point's 7 bytes of values to get the point's real values.
‘elevationUnit', {'float', 4}, ... % unit resolution of elevation report, in rad
'azimuthUnit', {'float', 4}, ... % unit resolution of azimuth report, in rad
'dopplerUint', {'float', 4}, ... % unit resolution of Doppler report, in m/s
'rangeUint', {'float', 4}, ... % unit resolution of Range report, in m
'snrUint', {'float', 4}); % unit resolution of SNR report, ratio
Each point has the following:
pointStruct = struct(...
‘elevation', {'int8', 1}, ... % Elevation report, in number of elevationUnit
'azimuth', {'int8', 1}, ... % Azimuth report, in number of azimuthUnit
'doppler', {'int16', 1}, ... % Doppler, in number of dopplerUint
'range', {‘uint16', 2}, ... % Range, in number of rangeUint
'snr', {‘uint16', 2}); % SNR, in number of snrUint
--------------------------------------------------------------------------------------------------------------------------------------------------

7
ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt
View File

@@ -63,6 +63,7 @@ find_package(PCL 1.7.2 REQUIRED)
add_message_files(
FILES
RadarScan.msg
RadarOccupancy.msg
)
## Generate services in the 'srv' folder
@@ -117,7 +118,7 @@ generate_messages(
catkin_package(
INCLUDE_DIRS include
LIBRARIES mmwave
#CATKIN_DEPENDS nodelet roscpp serial std_msgs
CATKIN_DEPENDS nodelet roscpp serial std_msgs
DEPENDS system_lib
)
@@ -220,10 +221,6 @@ add_executable(mmWaveQuickConfig src/mmWaveQuickConfig.cpp)
target_link_libraries(mmWaveQuickConfig ${catkin_LIBRARIES} mmwave ${PCL_LIBRARIES})
add_dependencies(mmWaveQuickConfig ${catkin_EXPORTED_TARGETS} mmwave)
add_executable(mmWaveSync src/mmWaveSync.cpp)
target_link_libraries(mmWaveSync ${catkin_LIBRARIES} mmwave ti_gpio)
add_dependencies(mmWaveSync ${catkin_EXPORTED_TARGETS} mmwave)
#############
## Testing ##
#############

39
ros_driver/src/ti_mmwave_rospkg/bin/README.txt Normal file
View File

@@ -0,0 +1,39 @@
-------------------------------------------------------------------------------
The binaries/firmware (.bin) that need to be flashed can be found on our
Industrial Toolbox (ITB) for mmWave Sensors located in the TI Resource Explorer.
You can download the ITB by clicking the three vertical buttons that appear
when hovering your mouse over the Industrial Toolbox folder on the page.
ITB 4.12 Newest Version as of 11/3/22
https://dev.ti.com/tirex/explore/node?a=VLyFKFf__4.12.0&node=A__AL83WtzTg4DL4DGRimSv.g__com.ti.mmwave_industrial_toolbox__VLyFKFf__4.12.0
The Out of Box demo prebuilt binaries come with the mmWave SDK which after
downloaded and installed can be found at:
C:\ti\<mmwave_sdk_version>\packages\ti\demo\<device>\mmw
-------------------------------------------------------------------------------
Correct binary to be flashed for ROS Driver:
xWR6843AOP/ISK + Tracking (Capon Chain):
3D People Counting binary
xWR6843ISK + Occupancy/Zone Detection:
Small Obstacle Detection binary
xWR6843AOP:
Out of Box Demo binary (Use 64xxAOP)
xWR1443:
Out of Box Demo binary
xWR1642:
Out of Box Demo binary
xWR1843:
Out of Box Demo binary
-------------------------------------------------------------------------------

BIN
ros_driver/src/ti_mmwave_rospkg/bin/small_obstacle_detection_68xx.bin Normal file
View File

Binary file not shown.

BIN
ros_driver/src/ti_mmwave_rospkg/bin/xwr14xx_mmw_demo.bin Normal file
View File

Binary file not shown.

BIN
ros_driver/src/ti_mmwave_rospkg/bin/xwr16xx_mmw_demo.bin Normal file
View File

Binary file not shown.

BIN
ros_driver/src/ti_mmwave_rospkg/bin/xwr18xx_mmw_demo.bin Normal file
View File

Binary file not shown.

BIN
ros_driver/src/ti_mmwave_rospkg/bin/xwr64xxAOP_mmw_demo.bin Normal file
View File

Binary file not shown.

57
ros_driver/src/ti_mmwave_rospkg/cfg/6843ISK_Occupancy.cfg Normal file
View File

@@ -0,0 +1,57 @@
% ***************************************************************
% Created for SDK ver:03.02
% Created using Visualizer ver:3.2.0.1
% Frequency:60
% Platform:xWR68xx
% Scene Classifier:best_range_res
% Azimuth Resolution(deg):15 + Elevation
% Range Resolution(m):0.047
% Maximum unambiguous Range(m):9.04
% Maximum Radial Velocity(m/s):5.09
% Radial velocity resolution(m/s):0.64
% Frame Duration(msec):33.333
% Range Detection Threshold (dB):15
% Doppler Detection Threshold (dB):15
% Range Peak Grouping:enabled
% Doppler Peak Grouping:enabled
% Static clutter removal:disabled
% Angle of Arrival FoV: Full FoV
% Range FoV: Full FoV
% Doppler FoV: Full FoV
% ***************************************************************
sensorStop
flushCfg
dfeDataOutputMode 1
channelCfg 15 7 0
adcCfg 2 1
adcbufCfg -1 0 1 1 1
profileCfg 0 60 7 3 24 0 0 166 1 256 12500 0 0 158
chirpCfg 0 0 0 0 0 0 0 1
chirpCfg 1 1 0 0 0 0 0 4
chirpCfg 2 2 0 0 0 0 0 2
frameCfg 0 2 32 0 50 1 0
lowPower 0 0
guiMonitor -1 1 0 0 0 0 0
cfarCfg -1 0 2 8 4 3 0 20 0
cfarCfg -1 1 0 4 2 3 1 20 0
multiObjBeamForming -1 1 0.5
clutterRemoval -1 0
calibDcRangeSig -1 0 -5 8 256
extendedMaxVelocity -1 0
bpmCfg -1 0 0 0
lvdsStreamCfg -1 0 0 0
compRangeBiasAndRxChanPhase 0.0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
measureRangeBiasAndRxChanPhase 0 1.0 0.2
CQRxSatMonitor 0 3 4 63 0
CQSigImgMonitor 0 127 4
analogMonitor 0 0
aoaFovCfg -1 -90 90 -90 90
cfarFovCfg -1 0 0.25 9.00
cfarFovCfg -1 1 -20.16 20.16
calibData 0 0 0
occStateMach 1 3 5 3 2 3 0 10
zoneDef 0 -0.8 0.8 0.2 1.50 -0.5 1.5
sensorStart

43
ros_driver/src/ti_mmwave_rospkg/cfg/ISK_3d_Tracking.cfg Normal file
View File

@@ -0,0 +1,43 @@
% SDK Parameters
% See the SDK user's guide for more information
% "C:\ti\mmwave_sdk_[VER]\docs\mmwave_sdk_user_guide.pdf"
sensorStop
flushCfg
dfeDataOutputMode 1
channelCfg 15 7 0
adcCfg 2 1
adcbufCfg -1 0 1 1 1
lowPower 0 0
% Radar and Point Cloud Detection Layer Parameters
% See the Detection Layer Tuning Guide for more information
% "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_detection_layer_tuning_guide.pdf"
profileCfg 0 60.75 10.00 10.00 59.10 657930 0 54.71 1 94 3000.00 2 1 36
chirpCfg 0 0 0 0 0 0 0 1
chirpCfg 1 1 0 0 0 0 0 2
chirpCfg 2 2 0 0 0 0 0 4
frameCfg 0 2 64 0 50 1 0
dynamicRACfarCfg -1 4 1 2 2 8 12 4 8 15.00 18.00 0.40 1 1
staticRACfarCfg -1 4 1 2 2 8 8 6 4 12.00 19.00 0.30 0 0
dynamicRangeAngleCfg -1 0.75 0.0010 1 0
dynamic2DAngleCfg -1 1.5 0.0300 1 0 1 0.30 0.85 8.00
staticRangeAngleCfg -1 0 8 8
antGeometry0 0 -1 -2 -3 -2 -3 -4 -5 -4 -5 -6 -7
antGeometry1 -1 -1 -1 -1 0 0 0 0 -1 -1 -1 -1
antPhaseRot 1 1 1 1 1 1 1 1 1 1 1 1
fovCfg -1 70.0 30.0
compRangeBiasAndRxChanPhase 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
% Tracker Layer Parameters
% See the Tracking Layer Tuning Guide for more information
% "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_tracker_layer_tuning_guide.pdf"
staticBoundaryBox -1.8 1.8 0.4 7.8 -0.2 1.8
boundaryBox -2 2 0.5 8 -0.3 2
sensorPosition 0.5 0 0
gatingParam 4 2 2 2 10
stateParam 10 5 5 50 1 600
allocationParam 20 20 0.05 4 2 2
maxAcceleration 0.1 0.1 0.1
trackingCfg 1 2 250 10 20 260 94
presenceBoundaryBox -3 3 2 6 0.5 2.5
sensorStart

8
ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h
View File

@@ -2,6 +2,7 @@
#define _DATA_HANDLER_CLASS_
#include <ti_mmwave_rospkg/RadarScan.h>
#include <ti_mmwave_rospkg/RadarOccupancy.h>
#include "mmWave.h"
#include <iostream>
#include <cstdio>
@@ -79,6 +80,12 @@ private:
float vrange;
float max_vel;
float vvel;
float zminx;
float zmaxx;
float zminy;
float zmaxy;
float zminz;
float zmaxz;
char* frameID;
/*Contains the name of the serial port*/
@@ -147,6 +154,7 @@ private:
ros::Publisher DataUARTHandler_pub;
ros::Publisher radar_scan_pub;
ros::Publisher marker_pub;
ros::Publisher radar_occupancy_pub;
};
#endif

30
ros_driver/src/ti_mmwave_rospkg/include/mmWave.h
View File

@@ -54,25 +54,29 @@ enum MmwDemo_Output_TLV_Types
{
MMWDEMO_OUTPUT_MSG_NULL = 0,
/*! @brief List of detected points */
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS,
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS = 1,
/*! @brief Range profile */
MMWDEMO_OUTPUT_MSG_RANGE_PROFILE,
MMWDEMO_OUTPUT_MSG_RANGE_PROFILE = 2,
/*! @brief Noise floor profile */
MMWDEMO_OUTPUT_MSG_NOISE_PROFILE,
MMWDEMO_OUTPUT_MSG_NOISE_PROFILE = 3,
/*! @brief Samples to calculate static azimuth heatmap */
MMWDEMO_OUTPUT_MSG_AZIMUTH_STATIC_HEAT_MAP,
MMWDEMO_OUTPUT_MSG_AZIMUTH_STATIC_HEAT_MAP = 4,
/*! @brief Range/Doppler detection matrix */
MMWDEMO_OUTPUT_MSG_RANGE_DOPPLER_HEAT_MAP,
MMWDEMO_OUTPUT_MSG_RANGE_DOPPLER_HEAT_MAP = 5,
/*! @brief Stats information */
MMWDEMO_OUTPUT_MSG_STATS,
MMWDEMO_OUTPUT_MSG_STATS = 6,
/*! @brief List of detected points side information */
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS_SIDE_INFO,
MMWDEMO_OUTPUT_MSG_DETECTED_POINTS_SIDE_INFO = 7,
/*! @brief Occupancy State Machine information TLV */
MMWDEMO_OUTPUT_MSG_OCCUPANCY_STATE_MACHINE = 1030,
MMWDEMO_OUTPUT_MSG_MAX
};
@@ -86,7 +90,8 @@ enum SorterState{ READ_HEADER,
READ_DOPPLER,
READ_STATS,
SWAP_BUFFERS,
READ_SIDE_INFO};
READ_SIDE_INFO,
READ_OCCUPANCY};
struct MmwDemo_output_message_header_t
{
@@ -165,6 +170,14 @@ int16_t noise;
}DPIF_PointCloudSideInfo;
typedef struct DPIF_PointCloudOccupancy_t
{
/*! @brief state - CFAR cell to side noise ratio in dB expressed in 0.1 steps of dB */
uint32_t state;
}DPIF_PointCloudOccupancy;
struct mmwDataPacket{
MmwDemo_output_message_header_t header;
uint16_t numObjOut;
@@ -173,6 +186,7 @@ MmwDemo_DetectedObj objOut; // only used for SDK 1.x and 2.x
DPIF_PointCloudCartesian_t newObjOut; // used for SDK 3.x
DPIF_PointCloudSideInfo_t sideInfo; // used for SDK 3.x
DPIF_PointCloudOccupancy occupancy; // added for Occupancy Zones
};
const uint8_t magicWord[8] = {2, 1, 4, 3, 6, 5, 8, 7};

34
ros_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.launch Normal file
View File

@@ -0,0 +1,34 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642, 6843]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
<arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyUSB0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyUSB1" />
<param name="data_rate" value="921600" />
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843ISK_Occupancy.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>
</launch>

35
ros_driver/src/ti_mmwave_rospkg/launch/ISK_3d_Tracking.launch Normal file
View File

@@ -0,0 +1,35 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="command_port" default="/dev/ttyUSB0"/>
<arg name="data_port" default="/dev/ttyUSB1"/>
<arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
<arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="$(arg command_port)" />
<param name="command_rate" value="115200" />
<param name="data_port" value="$(arg data_port)" />
<param name="data_rate" value="921600" />
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/ISK_3d_Tracking.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="5 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d$(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>
</launch>

2
ros_driver/src/ti_mmwave_rospkg/msg/RadarOccupancy.msg Normal file
View File

@@ -0,0 +1,2 @@
Header header
uint32 state

2
ros_driver/src/ti_mmwave_rospkg/msg/RadarPointTrackID.msg Normal file
View File

@@ -0,0 +1,2 @@
Header header
uint8 tid

3
ros_driver/src/ti_mmwave_rospkg/msg/RadarTrackArray.msg Normal file
View File

@@ -0,0 +1,3 @@
Header header
uint32 num_tracks
RadarTrackContents[] track

11
ros_driver/src/ti_mmwave_rospkg/msg/RadarTrackContents.msg Normal file
View File

@@ -0,0 +1,11 @@
Header header
uint32 tid
float32 posx
float32 posy
float32 posz
float32 velx
float32 vely
float32 velz
float32 accx
float32 accy
float32 accz

54
ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp
View File

@@ -45,6 +45,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (mmWaveCloudType,
DataUARTHandler::DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) {
DataUARTHandler_pub = nh->advertise<sensor_msgs::PointCloud2>("/ti_mmwave/radar_scan_pcl", 100);
radar_scan_pub = nh->advertise<ti_mmwave_rospkg::RadarScan>("/ti_mmwave/radar_scan", 100);
radar_occupancy_pub = nh->advertise<ti_mmwave_rospkg::RadarOccupancy>("/ti_mmwave/radar_occupancy", 100);
marker_pub = nh->advertise<visualization_msgs::Marker>("/ti_mmwave/radar_scan_markers", 100);
maxAllowedElevationAngleDeg = 90; // Use max angle if none specified
maxAllowedAzimuthAngleDeg = 90; // Use max angle if none specified
@@ -64,7 +65,13 @@ DataUARTHandler::DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuf
nh->getParam("/ti_mmwave/range_resolution", vrange);
nh->getParam("/ti_mmwave/max_doppler_vel", max_vel);
nh->getParam("/ti_mmwave/doppler_vel_resolution", vvel);
nh->getParam("/ti_mmwave/zoneMinX", zminx);
nh->getParam("/ti_mmwave/zoneMaxX", zmaxx);
nh->getParam("/ti_mmwave/zoneMinY", zminy);
nh->getParam("/ti_mmwave/zoneMaxY", zmaxy);
nh->getParam("/ti_mmwave/zoneMinZ", zminz);
nh->getParam("/ti_mmwave/zoneMaxZ", zmaxz);
ROS_INFO("\n\n==============================\nList of parameters\n==============================\nNumber of range samples: %d\nNumber of chirps: %d\nf_s: %.3f MHz\nf_c: %.3f GHz\nBandwidth: %.3f MHz\nPRI: %.3f us\nFrame time: %.3f ms\nMax range: %.3f m\nRange resolution: %.3f m\nMax Doppler: +-%.3f m/s\nDoppler resolution: %.3f m/s\n==============================\n", \
nr, nd, fs/1e6, fc/1e9, BW/1e6, PRI*1e6, tfr*1e3, max_range, vrange, max_vel/2, vvel);
@@ -297,12 +304,12 @@ void *DataUARTHandler::sortIncomingData( void )
//boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>> RScan(new pcl::PointCloud<pcl::PointXYZI>);
boost::shared_ptr<pcl::PointCloud<mmWaveCloudType>> RScan(new pcl::PointCloud<mmWaveCloudType>);
ti_mmwave_rospkg::RadarScan radarscan;
ti_mmwave_rospkg::RadarOccupancy radaroccupancy;
//wait for first packet to arrive
pthread_mutex_lock(&countSync_mutex);
pthread_cond_wait(&sort_go_cv, &countSync_mutex);
pthread_mutex_unlock(&countSync_mutex);
pthread_mutex_lock(&currentBufp_mutex);
while(ros::ok())
@@ -558,8 +565,7 @@ void *DataUARTHandler::sortIncomingData( void )
{
//get snr (unit is 0.1 steps of dB)
memcpy( &mmwData.sideInfo.snr, &currentBufp->at(currentDatap), sizeof(mmwData.sideInfo.snr));
currentDatap += ( sizeof(mmwData.sideInfo.snr) );
currentDatap += ( sizeof(mmwData.sideInfo.snr) );
//get noise (unit is 0.1 steps of dB)
memcpy( &mmwData.sideInfo.noise, &currentBufp->at(currentDatap), sizeof(mmwData.sideInfo.noise));
currentDatap += ( sizeof(mmwData.sideInfo.noise) );
@@ -583,10 +589,37 @@ void *DataUARTHandler::sortIncomingData( void )
break;
case READ_OCCUPANCY:
RScan->header.frame_id = frameID;
RScan->height = 1;
RScan->width = mmwData.numObjOut;
RScan->is_dense = 1;
RScan->points.resize(RScan->width * RScan->height);
//get Occupancy State which is a uint32, 0 means that zone is unoccupied. Anything else means the stop zone is occupied
memcpy( &mmwData.occupancy.state, &currentBufp->at(currentDatap), sizeof(mmwData.occupancy.state));
currentDatap += ( sizeof(mmwData.occupancy.state) );
radaroccupancy.state = mmwData.occupancy.state;
if (radaroccupancy.state == 0){
// ROS_INFO("Area is clear!");
}
else{
// ROS_INFO("Area is OCCUPIED!");
}
radar_occupancy_pub.publish(radaroccupancy);
sorterState = CHECK_TLV_TYPE;
break;
case READ_LOG_MAG_RANGE:
{
sorterState = CHECK_TLV_TYPE;
}
@@ -765,6 +798,11 @@ void *DataUARTHandler::sortIncomingData( void )
//ROS_INFO("DataUARTHandler Sort Thread : Header TLV");
sorterState = READ_HEADER;
break;
case MMWDEMO_OUTPUT_MSG_OCCUPANCY_STATE_MACHINE:
//ROS_INFO("DataUARTHandler Sort Thread : Occupancy State Machine TLV");
sorterState = READ_OCCUPANCY;
break;
default: break;
}
@@ -891,7 +929,8 @@ void* DataUARTHandler::syncedBufferSwap_helper(void *context)
}
void DataUARTHandler::visualize(const ti_mmwave_rospkg::RadarScan &msg){
visualization_msgs::Marker marker;
visualization_msgs::Marker marker;
marker.header.frame_id = frameID;
marker.header.stamp = ros::Time::now();
@@ -921,6 +960,7 @@ void DataUARTHandler::visualize(const ti_mmwave_rospkg::RadarScan &msg){
marker_pub.publish(marker);
}
void DataUARTHandler::stop()
{
ROS_DEBUG("Stopping Threads");
@@ -931,4 +971,4 @@ void DataUARTHandler::stop()
pthread_cond_signal(&read_go_cv);
pthread_cond_signal(&sort_go_cv);
pthread_cond_signal(&countSync_max_cv);
}
}

33
ros_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp
View File

@@ -53,11 +53,28 @@ void ParameterParser::ParamsParser(ti_mmwave_rospkg::mmWaveCLI &srv, ros::NodeHa
case 5:
nh.setParam("/ti_mmwave/framePeriodicity", std::stof(token)); break;
}
}
} else req = token;
} else if (!req.compare("zoneDef")) {
switch (i) {
case 2:
nh.setParam("/ti_mmwave/zoneMinX", std::stoi(token)); break;
case 3:
nh.setParam("/ti_mmwave/zoneMaxX", std::stoi(token)); break;
case 4:
nh.setParam("/ti_mmwave/zoneMinY", std::stoi(token)); break;
case 5:
nh.setParam("/ti_mmwave/zoneMaxY", std::stoi(token)); break;
case 6:
nh.setParam("/ti_mmwave/zoneMinZ", std::stoi(token)); break;
case 7:
nh.setParam("/ti_mmwave/zoneMaxZ", std::stoi(token)); break;
}
} else req = token;
i++;
}
}
}
}
void ParameterParser::CalParams(ros::NodeHandle &nh) {
float c0 = 299792458;
@@ -72,6 +89,12 @@ void ParameterParser::CalParams(ros::NodeHandle &nh) {
float digOutSampleRate;
float freqSlopeConst;
float numAdcSamples;
float zoneMinX;
float zoneMaxX;
float zoneMinY;
float zoneMaxY;
float zoneMinZ;
float zoneMaxZ;
nh.getParam("/ti_mmwave/startFreq", startFreq);
nh.getParam("/ti_mmwave/idleTime", idleTime);
@@ -112,6 +135,6 @@ void ParameterParser::CalParams(ros::NodeHandle &nh) {
nh.setParam("/ti_mmwave/range_resolution", vrange);
nh.setParam("/ti_mmwave/max_doppler_vel", max_vel);
nh.setParam("/ti_mmwave/doppler_vel_resolution", vvel);
}
}
}
}

10
ros_driver/src/ti_mmwave_rospkg/src/mmWaveSync.cpp
View File

@@ -82,10 +82,10 @@ void Sync::configureSensors()
}
else
{
ROS_ERROR("mmWaveSync: Command failed (mmWave sensor did not respond with 'Done')");
ROS_ERROR("mmWaveSync: Response: '%s'", srv.response.resp.c_str() );
m_done = true;
return;
// ROS_ERROR("mmWaveSync: Command failed (mmWave sensor did not respond with 'Done')");
// ROS_ERROR("mmWaveSync: Response: '%s'", srv.response.resp.c_str() );
// m_done = true;
// return;
}
}
else
@@ -267,4 +267,4 @@ int main(int argc, char **argv)
g_syncObjPtr->start();
return 0;
}
}

240
ros_driver/src/ti_mmwave_tracker_rospkg/CMakeLists.txt Normal file
View File

@@ -0,0 +1,240 @@
cmake_minimum_required(VERSION 2.8.3)
project(ti_mmwave_tracker_rospkg)
set (CMAKE_CXX_STANDARD 14)
set (CMAKE_CXX_STANDARD_REQUIRED ON)
## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
nodelet
roscpp
serial
std_msgs
sensor_msgs
message_generation
pluginlib
)
find_package(Boost REQUIRED)
find_package(Threads REQUIRED)
find_package(PCL 1.7.2 REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
RadarScan.msg
RadarPointTrackID.msg
RadarTrackArray.msg
RadarTrackContents.msg
)
## Generate services in the 'srv' folder
add_service_files(
FILES
mmWaveCLI.srv
)
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
sensor_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES tracker_mmwave
CATKIN_DEPENDS nodelet roscpp serial std_msgs message_runtime
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories( include
${catkin_INCLUDE_DIRS}
${ti_mmwave_tracker_rospkg_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${pthread_INCLUDE_DIRS}
${rt_INCLUDE_DIRS}
)
link_directories( ${Boost_LIBRARIES_DIRS} )
link_directories( ${PCL_LIBRARY_DIRS} )
add_definitions(${PCL_DEFINITIONS})
## Declare a C++ library
add_library(tracker_mmwave
src/mmWaveDataHdl.cpp
src/mmWaveCommSrv.cpp
src/DataHandlerClass.cpp
src/mmWaveQuickConfig.cpp
src/ParameterParser.cpp)
# target_link_libraries(tracker_mmwave mmWaveDataHdl mmWaveCommSrv DataHandlerClass mmWaveQuickConfig ParameterParser)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(tracker_mmwave ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/ti_mmwave_tracker_rospkg_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
add_executable(${PROJECT_NAME} src/mmWaveLoader.cpp)
target_link_libraries(tracker_mmwave ${serial_LIBRARIES} ${PCL_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} tracker_mmwave ${serial_LIBRARIES} ${PCL_LIBRARIES})
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} tracker_mmwave ${serial_EXPORTED_TARGETS})
add_executable(mmWaveQuickConfigTracker src/mmWaveQuickConfig.cpp)
target_link_libraries(mmWaveQuickConfigTracker ${catkin_LIBRARIES} tracker_mmwave ${PCL_LIBRARIES})
add_dependencies(mmWaveQuickConfigTracker ${catkin_EXPORTED_TARGETS} tracker_mmwave)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ti_mmwave_tracker_rospkg.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

239
ros_driver/src/ti_mmwave_tracker_rospkg/CMakeLists.txt~ Normal file
View File

@@ -0,0 +1,239 @@
cmake_minimum_required(VERSION 2.8.3)
project(ti_mmwave_tracker_rospkg)
set (CMAKE_CXX_STANDARD 14)
set (CMAKE_CXX_STANDARD_REQUIRED ON)
## Add support for C++11, supported in ROS Kinetic and newer
add_definitions(-std=c++11)
## Find catkin macros and libraries
## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
## is used, also find other catkin packages
find_package(catkin REQUIRED COMPONENTS
nodelet
roscpp
serial
std_msgs
sensor_msgs
message_generation
pluginlib
)
find_package(Boost REQUIRED)
find_package(Threads REQUIRED)
find_package(PCL 1.7.2 REQUIRED)
## System dependencies are found with CMake's conventions
# find_package(Boost REQUIRED COMPONENTS system)
## Uncomment this if the package has a setup.py. This macro ensures
## modules and global scripts declared therein get installed
## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
# catkin_python_setup()
################################################
## Declare ROS messages, services and actions ##
################################################
## To declare and build messages, services or actions from within this
## package, follow these steps:
## * Let MSG_DEP_SET be the set of packages whose message types you use in
## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
## * In the file package.xml:
## * add a build_depend tag for "message_generation"
## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET
## * If MSG_DEP_SET isn't empty the following dependency has been pulled in
## but can be declared for certainty nonetheless:
## * add a run_depend tag for "message_runtime"
## * In this file (CMakeLists.txt):
## * add "message_generation" and every package in MSG_DEP_SET to
## find_package(catkin REQUIRED COMPONENTS ...)
## * add "message_runtime" and every package in MSG_DEP_SET to
## catkin_package(CATKIN_DEPENDS ...)
## * uncomment the add_*_files sections below as needed
## and list every .msg/.srv/.action file to be processed
## * uncomment the generate_messages entry below
## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
## Generate messages in the 'msg' folder
add_message_files(
FILES
RadarScan.msg
RadarPointTrackID.msg
RadarTrackArray.msg
RadarTrackContents.msg
)
## Generate services in the 'srv' folder
add_service_files(
FILES
mmWaveCLI.srv
)
## Generate actions in the 'action' folder
# add_action_files(
# FILES
# Action1.action
# Action2.action
# )
## Generate added messages and services with any dependencies listed here
generate_messages(
DEPENDENCIES
std_msgs
sensor_msgs
)
################################################
## Declare ROS dynamic reconfigure parameters ##
################################################
## To declare and build dynamic reconfigure parameters within this
## package, follow these steps:
## * In the file package.xml:
## * add a build_depend and a run_depend tag for "dynamic_reconfigure"
## * In this file (CMakeLists.txt):
## * add "dynamic_reconfigure" to
## find_package(catkin REQUIRED COMPONENTS ...)
## * uncomment the "generate_dynamic_reconfigure_options" section below
## and list every .cfg file to be processed
## Generate dynamic reconfigure parameters in the 'cfg' folder
# generate_dynamic_reconfigure_options(
# cfg/DynReconf1.cfg
# cfg/DynReconf2.cfg
# )
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## INCLUDE_DIRS: uncomment this if you package contains header files
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES mmwave
CATKIN_DEPENDS nodelet roscpp serial std_msgs message_runtime
DEPENDS system_lib
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
# include_directories(include)
include_directories( include
${catkin_INCLUDE_DIRS}
${ti_mmwave_tracker_rospkg_INCLUDE_DIRS}
${PCL_INCLUDE_DIRS}
${pthread_INCLUDE_DIRS}
${rt_INCLUDE_DIRS}
)
link_directories( ${Boost_LIBRARIES_DIRS} )
link_directories( ${PCL_LIBRARY_DIRS} )
add_definitions(${PCL_DEFINITIONS})
## Declare a C++ library
add_library(mmwave
src/mmWaveDataHdl.cpp
src/mmWaveCommSrv.cpp
src/DataHandlerClass.cpp
src/mmWaveQuickConfig.cpp
src/ParameterParser.cpp
)
## Add cmake target dependencies of the library
## as an example, code may need to be generated before libraries
## either from message generation or dynamic reconfigure
add_dependencies(mmwave ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Declare a C++ executable
## With catkin_make all packages are built within a single CMake context
## The recommended prefix ensures that target names across packages don't collide
# add_executable(${PROJECT_NAME}_node src/ti_mmwave_tracker_rospkg_node.cpp)
## Rename C++ executable without prefix
## The above recommended prefix causes long target names, the following renames the
## target back to the shorter version for ease of user use
## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
## Add cmake target dependencies of the executable
## same as for the library above
# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
## Specify libraries to link a library or executable target against
# target_link_libraries(${PROJECT_NAME}_node
# ${catkin_LIBRARIES}
# )
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS ${PROJECT_NAME} ${PROJECT_NAME}_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
# install(DIRECTORY include/${PROJECT_NAME}/
# DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
# FILES_MATCHING PATTERN "*.h"
# PATTERN ".svn" EXCLUDE
# )
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
add_executable(${PROJECT_NAME} src/mmWaveLoader.cpp)
target_link_libraries(mmwave ${serial_LIBRARIES} ${PCL_LIBRARIES})
target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} mmwave ${serial_LIBRARIES} ${PCL_LIBRARIES})
add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS} mmwave ${serial_EXPORTED_TARGETS})
add_executable(mmWaveQuickConfig src/mmWaveQuickConfig.cpp)
target_link_libraries(mmWaveQuickConfig ${catkin_LIBRARIES} mmwave ${PCL_LIBRARIES})
add_dependencies(mmWaveQuickConfig ${catkin_EXPORTED_TARGETS} mmwave)
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
# catkin_add_gtest(${PROJECT_NAME}-test test/test_ti_mmwave_tracker_rospkg.cpp)
# if(TARGET ${PROJECT_NAME}-test)
# target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
# endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

144
ros_driver/src/ti_mmwave_tracker_rospkg/README.md Normal file
View File

@@ -0,0 +1,144 @@
# TI mmWave ROS Package (Customized)
#### Based on updates from Dr. Leo Zhang (University of Arizona)
---
### Most recent change from Dr. Zhang:
Add support for XWR18XX devices. SDK version: 3.2.0.4.
### Most recent change from Allison Wendell:
Add support for XWR68XX devices. SDK version: 3.2.0.4
---
Initially derived from TI's origin ROS package in Industrial Toolbox 2.3.0 (new version available [Industrial Toolbox 2.5.2](http://dev.ti.com/tirex/#/?link=Software%2FmmWave%20Sensors%2FIndustrial%20Toolbox)).
### Differences from origin TI's version:
1. Added all radar parameters from calculations and can be read from `rosparam get`.
2. Added Doppler data from detecting targets and form a customized ROS message `/ti_mmwave/radar_scan`.
3. Added support for multiple radars working together.
4. Added support for camera overlay (for sensor fusion).
5. Working with xWR1443 and xWR1642 ES1.0 and ES2.0 (ES1.0 is deprecated from TI)
---
### Available devices:
```
TI mmWave xWR1443BOOST
TI mmWave xWR1642BOOST
TI mmWave xWR1642BOOST ES2.0/3.0 EVM (not tested)
TI mmWave xWR1642BOOST ES2.0 EVM
TI mmWave AWR1843BOOST ES1.0 EVM
TI mmWave IWR6843ISK ES1.0 EVM
```
---
### Quick start guide (AWR1642BOOST ES2.0 EVM):
1. Mount AWR1642BOOST ES2.0 EVM (as below), connect 5V/2.5A power supply and connect a micro-USB cable to host Ubuntu 16.04 LTS with [ROS Kinetic](http://wiki.ros.org/kinetic).
Note: Tested with Ubuntu 16.04 LTS with ROS Kinectic and Ubuntu 18.04 LTS with [ROS Melodic](http://wiki.ros.org/melodic)
2. Download SDK 2.0 or above (suggested SDK 2.1) from [here](http://www.ti.com/tool/MMWAVE-SDK) and use [UNIFLASH](http://www.ti.com/tool/UNIFLASH) to flash xwr16xx_mmw_demo.bin to your device. **Do not forget SOP2 jumper when flashing.**
3. Clone this repo and ROS serial onto your `<workspace dir>/src`:
```
git clone https://github.com/wjwwood/serial.git
git clone https://bitbucket.itg.ti.com/scm/mmwave_apps/ros_multisensor_demo.git
```
4. Go back to `<workspace dir>`:
```
catkin_make && source devel/setup.bash
echo "source <workspace_dir>/devel/setup.bash" >> ~/.bashrc
```
5. Enable command and data ports on Linux:
```
sudo chmod 666 /dev/ttyACM0
sudo chmod 666 /dev/ttyACM1
```
Note: If multiple sensors are used, enable additional ports `/dev/ttyACM2` and `/dev/ttyACM3`, etc. the same as this step.
6. Launch AWR1642 short range config:
```
roslaunch ti_mmwave_tracker_rospkg 1642es2_short_range.launch
```
Note: If you want to build your own config, use [mmWave Demo Visualizer](https://dev.ti.com/mmwavedemovisualizer) and link the launch file to the config.
7. ROS topics can be accessed as follows:
```
rostopic list
rostopic echo /ti_mmwave/radar_scan
```
8. ROS parameters can be accessed as follows:
```
rosparam list
rosparam get /ti_mmwave/max_doppler_vel
```
---
### Message format:
```
header:
seq: 6264
stamp:
secs: 1538888235
nsecs: 712113897
frame_id: "ti_mmwave" # Frame ID used for multi-sensor scenarios
point_id: 17 # Point ID of the detecting frame (Every frame starts with 0)
x: 8.650390625 # Point x coordinates in m (front from antenna)
y: 6.92578125 # Point y coordinates in m (left/right from antenna, right positive)
z: 0.0 # Point z coordinates in m (up/down from antenna, up positive)
range: 11.067276001 # Radar measured range in m
velocity: 0.0 # Radar measured range rate in m/s
doppler_bin: 8 # Doppler bin location of the point (total bins = num of chirps)
bearing: 38.6818885803 # Radar measured angle in degrees (right positive)
intensity: 13.6172780991 # Radar measured intensity in dB
```
---
### Troubleshooting
1.
```
mmWaveCommSrv: Failed to open User serial port with error: IO Exception (13): Permission denied
mmWaveCommSrv: Waiting 20 seconds before trying again...
```
This happens when serial port is called without superuser permission, do the following steps:
```
sudo chmod 666 /dev/ttyACM0
sudo chmod 666 /dev/ttyACM1
```
2.
```
mmWaveQuickConfig: Command failed (mmWave sensor did not respond with 'Done')
mmWaveQuickConfig: Response: 'sensorStop
'?`????`????`???~' is not recognized as a CLI command
mmwDemo:/>'
```
When this happens, re-run the command you send to sensor. If it continues, shut down and restart the sensor.
---
### Multiple devices support (dual AWR1642 ES2.0 EVM):
1. Connect two devices and try `ll /dev/serial/by-id` or `ls /dev`. In this case, `/dev/ttyACM0` to `/dev/ttyACM3` should shown.
2. To avoid serial port confliction, you need to launch devices separately. So for first device (it will open rviz):
```
roslaunch ti_mmwave_tracker_rospkg multi_1642_0.launch
```
3. Change radars' location in first six arguments `<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>` (stands for x,y,z for positions in meters and yaw, pitch, roll for angles in radians) in launch file `multi_1642_1.launch`. And launch second device:
```
roslaunch ti_mmwave_tracker_rospkg multi_1642_1.launch
```
Note: As serial connection and the original code, you need to launch devices separately using different launch files.
---
### Camera overlay support (working with USB camera or CV camera):
1. Download and build USB camera repo [here](https://github.com/radar-lab/usb_webcam`). And set parameters of camera in `<usb_webcam dir>/launch/usb_webcam.launch`.
2. To test the device image working, try:
```
roslaunch usb_webcam usb_webcam.launch
rosrun rqt_image_view rqt_image_view
```
3. Make sure you have done [ROS camera calibration](http://wiki.ros.org/camera_calibration) and create a `*.yaml` configuration file accordingly.
4. Launch radar-camera system using:
```
roslaunch ti_mmwave_tracker_rospkg camera_overlay.launch
```

BIN
ros_driver/src/ti_mmwave_tracker_rospkg/bin/3D_people_count_68xx_demo.bin Normal file
View File

Binary file not shown.

39
ros_driver/src/ti_mmwave_tracker_rospkg/bin/README.txt Normal file
View File

@@ -0,0 +1,39 @@
-------------------------------------------------------------------------------
The binaries/firmware (.bin) that need to be flashed can be found on our
Industrial Toolbox (ITB) for mmWave Sensors located in the TI Resource Explorer.
You can download the ITB by clicking the three vertical buttons that appear
when hovering your mouse over the Industrial Toolbox folder on the page.
ITB 4.12 Newest Version as of 11/3/22
https://dev.ti.com/tirex/explore/node?a=VLyFKFf__4.12.0&node=A__AL83WtzTg4DL4DGRimSv.g__com.ti.mmwave_industrial_toolbox__VLyFKFf__4.12.0
The Out of Box demo prebuilt binaries come with the mmWave SDK which after
downloaded and installed can be found at:
C:\ti\<mmwave_sdk_version>\packages\ti\demo\<device>\mmw
-------------------------------------------------------------------------------
Correct binary to be flashed for ROS Driver:
xWR6843AOP/ISK + Tracking (Capon Chain):
3D People Counting binary
xWR6843ISK + Occupancy/Zone Detection:
Small Obstacle Detection binary
xWR6843AOP:
Out of Box Demo binary (Use 64xxAOP)
xWR1443:
Out of Box Demo binary
xWR1642:
Out of Box Demo binary
xWR1843:
Out of Box Demo binary
-------------------------------------------------------------------------------

44
ros_driver/src/ti_mmwave_tracker_rospkg/cfg/AOP_3d_Tracking.cfg Normal file
View File

@@ -0,0 +1,44 @@
% SDK Parameters
% See the SDK user's guide for more information
% "C:\ti\mmwave_sdk_[VER]\docs\mmwave_sdk_user_guide.pdf"
resetDevice
sensorStop
flushCfg
dfeDataOutputMode 1
channelCfg 15 7 0
adcCfg 2 1
adcbufCfg -1 0 1 1 1
lowPower 0 0
% Radar and Point Cloud Detection Layer Parameters
% See the Detection Layer Tuning Guide for more information
% "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_detection_layer_tuning_guide.pdf"
profileCfg 0 60.75 10.00 10.00 59.10 657930 0 54.71 1 94 3000.00 2 1 36
chirpCfg 0 0 0 0 0 0 0 1
chirpCfg 1 1 0 0 0 0 0 2
chirpCfg 2 2 0 0 0 0 0 4
frameCfg 0 2 64 0 50 1 0
dynamicRACfarCfg -1 4 1 2 2 8 12 4 8 15.00 18.00 0.40 1 1
staticRACfarCfg -1 4 1 2 2 8 8 6 4 12.00 19.00 0.30 0 0
dynamicRangeAngleCfg -1 0.75 0.0010 1 0
dynamic2DAngleCfg -1 1.5 0.0300 1 0 1 0.30 0.85 8.00
staticRangeAngleCfg -1 0 8 8
antGeometry0 -1 -1 0 0 -3 -3 -2 -2 -1 -1 0 0
antGeometry1 -1 0 -1 0 -3 -2 -3 -2 -3 -2 -3 -2
antPhaseRot 1 -1 1 -1 1 -1 1 -1 1 -1 1 -1
fovCfg -1 70.0 30.0
compRangeBiasAndRxChanPhase 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0 1 0
% Tracker Layer Parameters
% See the Tracking Layer Tuning Guide for more information
% "C:\ti\mmwave_industrial_toolbox_[VER]\labs\people_counting\docs\3D_people_counting_tracker_layer_tuning_guide.pdf"
staticBoundaryBox -1.8 1.8 0.4 7.8 -0.2 1.8
boundaryBox -2 2 0.5 8 -0.3 2
sensorPosition 0.5 0 0
gatingParam 4 2 2 2 10
stateParam 10 5 5 50 1 600
allocationParam 20 20 0.05 4 2 2
maxAcceleration 0.1 0.1 0.1
trackingCfg 1 2 250 10 20 260 94
presenceBoundaryBox -3 3 2 6 0.5 2.5
sensorStart

157
ros_driver/src/ti_mmwave_tracker_rospkg/include/DataHandlerClass.h Normal file
View File

@@ -0,0 +1,157 @@
#ifndef _DATA_HANDLER_CLASS_
#define _DATA_HANDLER_CLASS_
#include <ti_mmwave_tracker_rospkg/RadarScan.h>
#include <ti_mmwave_tracker_rospkg/RadarPointTrackID.h>
#include <ti_mmwave_tracker_rospkg/RadarTrackArray.h>
#include <ti_mmwave_tracker_rospkg/RadarTrackContents.h>
#include "mmWave.h"
#include <iostream>
#include <cstdio>
#include <cstdlib>
#include <boost/shared_ptr.hpp>
#include "ros/ros.h"
#include "sensor_msgs/PointCloud2.h"
#include <pthread.h>
#include <algorithm>
#include "pcl_ros/point_cloud.h"
#include "sensor_msgs/PointField.h"
#include "sensor_msgs/PointCloud2.h"
#include "sensor_msgs/point_cloud2_iterator.h"
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
#include <signal.h>
#define COUNT_SYNC_MAX 2
class DataUARTHandler{
public:
/*Constructor*/
//void DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) {}
DataUARTHandler(ros::NodeHandle* nh);
void setFrameID(char* myFrameID);
/*User callable function to set the UARTPort*/
void setUARTPort(char* mySerialPort);
/*User callable function to set the BaudRate*/
void setBaudRate(int myBaudRate);
/*User callable function to set maxAllowedElevationAngleDeg*/
void setMaxAllowedElevationAngleDeg(int myMaxAllowedElevationAngleDeg);
/*User callable function to set maxAllowedElevationAngleDeg*/
void setMaxAllowedAzimuthAngleDeg(int myMaxAllowedAzimuthAngleDeg);
void setNodeHandle(ros::NodeHandle* nh);
/*User callable function to start the handler's internal threads*/
void start(void);
/*User callable function to stop the handler's internal threads*/
void stop();
/*Helper functions to allow pthread compatability*/
static void* readIncomingData_helper(void *context);
static void* sortIncomingData_helper(void *context);
static void* syncedBufferSwap_helper(void *context);
/* Function to handle signals such as SIGINT */
static void sigHandler(int32_t sig);
/*Sorted mmwDemo Data structure*/
mmwDataPacket mmwData;
private:
int nr;
int nd;
int ntx;
float fs;
float fc;
float BW;
float PRI;
float tfr;
float max_range;
float vrange;
float max_vel;
float vvel;
char* frameID;
/*Contains the name of the serial port*/
char* dataSerialPort;
/*Contains the baud Rate*/
int dataBaudRate;
/*Contains the max_allowed_elevation_angle_deg (points with elevation angles
outside +/- max_allowed_elevation_angle_deg will be removed)*/
int maxAllowedElevationAngleDeg;
/*Contains the max_allowed_azimuth_angle_deg (points with azimuth angles
outside +/- max_allowed_azimuth_angle_deg will be removed)*/
int maxAllowedAzimuthAngleDeg;
/*Mutex protected variable which synchronizes threads*/
int countSync;
/*Boolean used to notify threads to exit*/
bool stop_threads;
/*Read/Write Buffers*/
std::vector<uint8_t> pingPongBuffers[2];
/*Pointer to current data (sort)*/
std::vector<uint8_t>* currentBufp;
/*Pointer to new data (read)*/
std::vector<uint8_t>* nextBufp;
/*Mutex protecting the countSync variable */
pthread_mutex_t countSync_mutex;
/*Mutex protecting the nextBufp pointer*/
pthread_mutex_t nextBufp_mutex;
/*Mutex protecting the currentBufp pointer*/
pthread_mutex_t currentBufp_mutex;
/*Condition variable which blocks the Swap Thread until signaled*/
pthread_cond_t countSync_max_cv;
/*Condition variable which blocks the Read Thread until signaled*/
pthread_cond_t read_go_cv;
/*Condition variable which blocks the Sort Thread until signaled*/
pthread_cond_t sort_go_cv;
/*Swap Buffer Pointers Thread*/
void *syncedBufferSwap(void);
/*Checks if the magic word was found*/
int isMagicWord(uint8_t last8Bytes[8]);
/*Read incoming UART Data Thread*/
void *readIncomingData(void);
/*Sort incoming UART Data Thread*/
void *sortIncomingData(void);
void visualize(const ti_mmwave_tracker_rospkg::RadarScan &msg);
ros::NodeHandle* nodeHandle;
ros::Publisher DataUARTHandler_pub;
ros::Publisher radar_scan_pub;
ros::Publisher radar_trackid_pub;
ros::Publisher radar_trackarray_pub;
ros::Publisher marker_pub;
};
#endif

35
ros_driver/src/ti_mmwave_tracker_rospkg/include/ParameterParser.h Normal file
View File

@@ -0,0 +1,35 @@
#ifndef _PARAM_PARSER_CLASS_
#define _PARAM_PARSER_CLASS_
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "ti_mmwave_tracker_rospkg/mmWaveCLI.h"
#include <nodelet/nodelet.h>
#include <pluginlib/class_list_macros.h>
#include <cstdlib>
#include <fstream>
#include <iostream>
#include <cstdio>
#include <sstream>
#include <string>
#include <vector>
namespace ti_mmwave_tracker_rospkg {
class ParameterParser : public nodelet::Nodelet{
public:
ParameterParser();
void ParamsParser(ti_mmwave_tracker_rospkg::mmWaveCLI &srv, ros::NodeHandle &n);
void CalParams(ros::NodeHandle &nh);
private:
virtual void onInit();
ti_mmwave_tracker_rospkg::mmWaveCLI srv;
};
}
#endif

273
ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWave.h Normal file
View File

@@ -0,0 +1,273 @@
/*
* mmWave.h
*
* This file contains various defines used within this package.
*
*
* Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the
* distribution.
*
* Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef _TI_IWR14XX_
#define _TI_IWR14XX_
#include <iostream>
#include <iostream>
#include <cstdio>
#include "serial/serial.h"
#include "ros/ros.h"
#include <boost/thread.hpp>
#include <cstdint>
enum MmwDemo_Output_TLV_Types
{
MMWDEMO_OUTPUT_MSG_NULL = 0,
/*! @brief Tracker TLV's */
MMWDEMO_OUTPUT_MSG_TRACKERPROC_3D_TARGET_LIST = 1010,
/*! @brief Tracker TLV's*/
MMWDEMO_OUTPUT_MSG_TRACKERPROC_TARGET_INDEX = 1011,
/*! @brief 3D Spherical Compressed Point Cloud */
MMWDEMO_OUTPUT_MSG_COMPRESSED_POINTS = 1020,
MMWDEMO_OUTPUT_MSG_MAX = 3
};
enum SorterState{ READ_HEADER,
CHECK_TLV_TYPE,
READ_SPHERE_POINT_CLOUD,
READ_3D_TARGET_LIST,
READ_TARGET_INDEX,
SWAP_BUFFERS,
READ_SIDE_INFO};
struct MmwDemo_output_message_header_t
{
/*! brief Version: : MajorNum * 2^24 + MinorNum * 2^16 + BugfixNum * 2^8 + BuildNum */
uint32_t version;
/*! @brief Total packet length including header in Bytes */
uint32_t totalPacketLen;
/*! @brief platform type */
uint32_t platform;
/*! @brief Frame number */
uint32_t frameNumber;
/*! @brief Time in CPU cycles when the message was created. For XWR16xx: DSP CPU cycles, for XWR14xx: R4F CPU cycles */
uint32_t timeCpuCycles;
/*! @brief Number of detected objects */
uint32_t numDetectedObj;
/*! @brief Number of TLVs */
uint32_t numTLVs;
/*! @brief Sub-frame Number (not used with XWR14xx) */
uint32_t subFrameNumber;
};
// Detected object structure for mmWave SDK 1.x and 2.x
struct MmwDemo_DetectedObj
{
uint16_t rangeIdx; /*!< @brief Range index */
uint16_t dopplerIdx; /*!< @brief Dopler index */
uint16_t peakVal; /*!< @brief Peak value */
int16_t x; /*!< @brief x - coordinate in meters. Q format depends on the range resolution */
int16_t y; /*!< @brief y - coordinate in meters. Q format depends on the range resolution */
int16_t z; /*!< @brief z - coordinate in meters. Q format depends on the range resolution */
};
// Detected object structures for mmWave SDK 3.x (DPIF_PointCloudCartesian_t and DPIF_PointCloudSideInfo_t)
/**
* @brief
* Point cloud definition in Cartesian coordinate system - floating point format
*
*/
typedef struct DPIF_PointCloudCartesian_t
{
/*! @brief x - coordinate in meters */
float x;
/*! @brief y - coordinate in meters */
float y;
/*! @brief z - coordinate in meters */
float z;
/*! @brief Doppler velocity estimate in m/s. Positive velocity means target
* is moving away from the sensor and negative velocity means target
* is moving towards the sensor. */
float velocity;
}DPIF_PointCloudCartesian;
/**
* @brief
* Point cloud side information such as SNR and noise level
*
*/
typedef struct DPIF_PointCloudSideInfo_t
{
/*! @brief snr - CFAR cell to side noise ratio in dB expressed in 0.1 steps of dB */
int16_t snr;
/*! @brief y - CFAR noise level of the side of the detected cell in dB expressed in 0.1 steps of dB */
int16_t noise;
}DPIF_PointCloudSideInfo;
typedef struct DPIF_TargetList3D_t
{
/*! Track ID */
int32_t tid;
// Target position in X dimension, m
float posX;
// Target position in Y dimension, m
float posY;
// Target position in Z dimension, m
float posZ;
// Target velocity in X dimension, m/s
float velX;
// Target velocity in Y dimension, m/s
float velY;
// Target velocity in Z dimension, m/s
float velZ;
// Target acceleration in X dimension, m/s2
float accX;
// Target acceleration in Y dimension, m/s2
float accY;
// Target acceleration in Z dimension, m/s2
float accZ;
// Throw Away
float ec[4][4];
// Throw Away
float g;
// Throw Away
float confidenceLevel;
}DPIF_TargetList3D_t;
typedef struct DPIF_TargetIndex_t
{
/*! TrackID 253, 254, and 255 are special values where points are not associated due to SNR being too weak, located outside of boundary, or considered noise respectively */
int8_t targetID;
}DPIF_TargetIndex_t;
typedef struct DPIF_SphericalPointCloud_t
{
/*
As the Spherical Point Cloud is compressed, we decompress after pulling all these values by multipling the parameter by its 'unit' counterpart
*/
float elevationUnit;
float azimuthUnit;
float dopplerUnit;
float rangeUnit;
float snrUnit;
int8_t elevation;
int8_t azimuth;
int16_t doppler;
uint16_t range;
uint16_t snr;
}DPIF_SphericalPointCloud_t;
struct mmwDataPacket{
MmwDemo_output_message_header_t header;
uint16_t numObjOut;
/*
uint16_t xyzQFormat; // only used for SDK 1.x and 2.x
MmwDemo_DetectedObj objOut; // only used for SDK 1.x and 2.x
DPIF_PointCloudCartesian_t newObjOut; // used for SDK 3.x
DPIF_PointCloudSideInfo_t sideInfo; // used for SDK 3.x
*/
DPIF_TargetList3D_t newListOut;
DPIF_TargetIndex_t newIndexOut;
DPIF_SphericalPointCloud_t newSphereCloudOut;
};
const uint8_t magicWord[8] = {2, 1, 4, 3, 6, 5, 8, 7};
#endif

85
ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWaveCommSrv.hpp Normal file
View File

@@ -0,0 +1,85 @@
/*
* mmWaveCommSrv.hpp
*
* This file defines a ROS nodelet which will open up a serial port provided by the user
* at a certain baud rate (also provided by user) that will interface with the 1443EVM mmwDemo
* User UART to be used for board configuration.
*
*
* Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the
* distribution.
*
* Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef MMWAVE_COMM_SRV_H
#define MMWAVE_COMM_SRV_H
/*Include ROS specific headers*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
/*Include standard C/C++ headers*/
#include <iostream>
#include <cstdio>
#include <sstream>
/*mmWave Driver Headers*/
#include <ti_mmwave_tracker_rospkg/mmWaveCLI.h>
namespace ti_mmwave_tracker_rospkg
{
class mmWaveCommSrv : public nodelet::Nodelet
{
public:
mmWaveCommSrv();
private:
virtual void onInit();
bool commSrv_cb(ti_mmwave_tracker_rospkg::mmWaveCLI::Request &req, ti_mmwave_tracker_rospkg::mmWaveCLI::Response &res);
ros::ServiceServer commSrv;
std::string mySerialPort;
int myBaudRate;
std::string mmWaveCLIName;
}; //Class mmWaveCommSrv
} //namespace ti_mmwave_tracker_rospkg
#endif

80
ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWaveDataHdl.hpp Normal file
View File

@@ -0,0 +1,80 @@
/*
* mmWaveDataHdl.hpp
*
* This file defines a ROS nodelet which will open up a serial port provided by the user
* at a certain baud rate (also provided by user) that will interface with the 1443EVM mmwDemo
* Data UART to be used for board configuration.
*
*
* Copyright (C) 2017 Texas Instruments Incorporated - http://www.ti.com/
*
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
*
* Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in the
* documentation and/or other materials provided with the
* distribution.
*
* Neither the name of Texas Instruments Incorporated nor the names of
* its contributors may be used to endorse or promote products derived
* from this software without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
* A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
* OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
* SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
* LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
* DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
* THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
* OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*
*/
#ifndef MMWAVE_DATA_HDL_H
#define MMWAVE_DATA_HDL_H
/*Include ROS specific headers*/
#include "ros/ros.h"
#include "std_msgs/String.h"
#include "serial/serial.h"
#include <pluginlib/class_list_macros.h>
#include <nodelet/nodelet.h>
/*Include standard C/C++ headers*/
#include <iostream>
#include <cstdio>
#include <sstream>
/*mmWave Driver Headers*/
#include "DataHandlerClass.h"
namespace ti_mmwave_tracker_rospkg
{
class mmWaveDataHdl : public nodelet::Nodelet
{
public:
mmWaveDataHdl();
private:
virtual void onInit();
//char* mySerialPort;
//int myBaudRate;
}; //Class mmWaveDataHdl
} //namespace ti_mmwave_tracker_rospkg
#endif

35
ros_driver/src/ti_mmwave_tracker_rospkg/launch/AOP_3d_Tracking.launch Normal file
View File

@@ -0,0 +1,35 @@
<!--
This file will launch rViz along with the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config
-->
<launch>
<!-- Input arguments -->
<arg name="device" value="6843" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="3d_Tracking" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
<arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_tracker_rospkg" type="ti_mmwave_tracker_rospkg" name="ti_mmwave" ns="radar_1" output="screen">
<param name="command_port" value="/dev/ttyUSB0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyUSB1" />
<param name="data_rate" value="921600" />
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_tracker_rospkg" type="mmWaveQuickConfigTracker" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_tracker_rospkg)/cfg/AOP_3d_Tracking.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="5 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d$(find ti_mmwave_tracker_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>
</launch>

43
ros_driver/src/ti_mmwave_tracker_rospkg/launch/camera_overlay.launch Normal file
View File

@@ -0,0 +1,43 @@
<launch>
<arg name="device" value="1642" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="2d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
<arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
<node name="usb_webcam" pkg="usb_webcam" type="usb_webcam_node" output="screen" >
<param name="video_device" value="/dev/video1" />
<param name="camera_info_url" value="file:///home/leo/workspace/usb_webcam/src/cfg/elp-usb8mp02g-l36.yaml" />
<param name="image_width" value="640" />
<param name="image_height" value="480" />
<param name="pixel_format" value="yuyv" />
<param name="camera_frame_id" value="usb_webcam" />
<param name="io_method" value="mmap"/>
<param name="framerate" value="30" />
<param name="autoexposure" value="true" />
<param name="autofocus" value="true" />
</node>
<node pkg="ti_mmwave_tracker_rospkg" type="ti_mmwave_tracker_rospkg" name="ti_mmwave" ns="radar_0" output="screen">
<param name="command_port" value="/dev/ttyACM0" />
<param name="command_rate" value="115200" />
<param name="data_port" value="/dev/ttyACM1" />
<param name="data_rate" value="921600" />
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<node pkg="ti_mmwave_tracker_rospkg" type="mmWaveQuickConfig" name="mmWaveQuickConfig" ns="radar_0" args="$(find ti_mmwave_tracker_rospkg)/cfg/1642es2_short_range.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>
<node pkg="tf" type="static_transform_publisher" name="camera_baselink" args="-2 0 .95 -1.5707963267948966 0 -1.5707963267948966 ti_mmwave_pcl usb_webcam 100"/>
<!-- Launch Rviz with pre-defined configuration to view mmWave sensor detected object data (color by intensity) -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_tracker_rospkg)/launch/rviz/ti_mmwave_camera_overlay.rviz"/>
</launch>

15
ros_driver/src/ti_mmwave_tracker_rospkg/launch/rosbag_show.launch Normal file
View File

@@ -0,0 +1,15 @@
<launch>
<!-- Input arguments -->
<arg name="device" value="1642" doc="TI mmWave sensor device type [1443, 1642]"/>
<arg name="config" value="2d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
<arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
<!-- Static transform from map to base_radar_link for visualization of stand-alone mmWave sensor using Rviz -->
<node pkg="tf" type="static_transform_publisher" name="static_tf_map_to_base_radar_link" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave 30"/>
<!-- Launch Rviz with pre-defined configuration to view mmWave sensor detected object data (color by intensity) -->
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_tracker_rospkg)/launch/rviz/ti_mmwave.rviz"/>
</launch>

174
ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave.rviz Normal file
View File

@@ -0,0 +1,174 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Grid1
- /Grid1/Offset1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 569
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0.0399999991
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 35.2955856
Min Color: 255; 255; 0
Min Intensity: 9.54242516
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Class: rviz/Image
Enabled: false
Image Topic: /usb_webcam/image_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
- Class: rviz/Image
Enabled: false
Image Topic: /usb_webcam/image_raw
Max Value: 1
Median window: 5
Min Value: 0
Name: Image
Normalize Range: true
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: false
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 12.8599997
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.56979632
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.14159274
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 876
Hide Left Dock: false
Hide Right Dock: true
Image:
collapsed: false
QMainWindow State: 000000ff00000000fd000000040000000000000208000002c8fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afc00000028000002c8000000dd00fffffffa000000010100000002fb0000000a0049006d0061006700650000000000ffffffff0000005c00fffffffb000000100044006900730070006c0061007900730100000000000002080000016a00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d0061006700650200000156000000000000042b00000384000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff00000058fc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003f1000002c800000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1535
X: 65
Y: 24

148
ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_3d.rviz Normal file
View File

@@ -0,0 +1,148 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Grid1
- /Grid1/Offset1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 595
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: AxisColor
Decay Time: 0.100000001
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 37.8426056
Min Color: 0; 0; 255
Min Intensity: 3.01029992
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 0.0181882642
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.0697968528
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.20659256
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 876
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd00000004000000000000020b000002e2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002e2000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000003efc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d00650100000000000004500000000000000000000003ee000002e200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1535
X: 65
Y: 24

164
ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_camera_overlay.rviz Normal file
View File

@@ -0,0 +1,164 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Grid1
- /Grid1/Offset1
- /PointCloud21
- /PointCloud21/Autocompute Value Bounds1
- /Camera1
Splitter Ratio: 0.5
Tree Height: 1430
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0.0399999991
Enabled: true
Invert Rainbow: false
Max Color: 255; 255; 255
Max Intensity: 31.970047
Min Color: 255; 0; 0
Min Intensity: 16.3346844
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_0
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Class: rviz/Camera
Enabled: true
Image Rendering: background and overlay
Image Topic: /usb_webcam/image_raw
Name: Camera
Overlay Alpha: 0.5
Queue Size: 2
Transport Hint: raw
Unreliable: false
Value: true
Visibility:
Grid: true
PointCloud2: true
Value: true
Zoom Factor: 1
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 12.8599997
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 1.56979632
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.14159274
Saved: ~
Window Geometry:
Camera:
collapsed: false
Displays:
collapsed: false
Height: 1764
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001ad00000637fc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006800fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003400000637000000e900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d006500720061030000024f0000009a000009ef00000610000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000ca00fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000c1e0000004afc0100000002fb0000000800540069006d0065010000000000000c1e0000043900fffffffb0000000800540069006d0065010000000000000450000000000000000000000a680000063700000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 3102
X: 98
Y: 36

178
ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_multi.rviz Normal file
View File

@@ -0,0 +1,178 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1/Offset1
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 583
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.588679016
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.0299999993
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: Intensity
Decay Time: 0.400000006
Enabled: true
Invert Rainbow: false
Max Color: 102; 0; 0
Max Intensity: 21.2000008
Min Color: 255; 102; 102
Min Intensity: 11.3999996
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_0
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 204; 204; 0
Color Transformer: Intensity
Decay Time: 0.400000006
Enabled: true
Invert Rainbow: false
Max Color: 102; 102; 0
Max Intensity: 27.7999992
Min Color: 255; 255; 51
Min Intensity: 12.8999996
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.100000001
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_1
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Topic: /initialpose
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 13.5551214
Enable Stereo Rendering:
Stereo Eye Separation: 0.0599999987
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.0500000007
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.00999999978
Pitch: 0.234797031
Target Frame: <Fixed Frame>
Value: Orbit (rviz)
Yaw: 3.07158875
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 876
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001ad000002d6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002d6000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000004afc0100000002fb0000000800540069006d00650100000000000005ff0000030000fffffffb0000000800540069006d006501000000000000045000000000000000000000044c000002d600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1535
X: 65
Y: 24

244
ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_quad.rviz Normal file
View File

@@ -0,0 +1,244 @@
Panels:
- Class: rviz/Displays
Help Height: 78
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Grid1/Offset1
- /PointCloud21/Autocompute Value Bounds1
Splitter Ratio: 0.5
Tree Height: 573
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
- /Current View1/Focal Point1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Experimental: false
Name: Time
SyncMode: 0
SyncSource: PointCloud2
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 5
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 0; 0
Color Transformer: Intensity
Decay Time: 0.4000000059604645
Enabled: true
Invert Rainbow: false
Max Color: 102; 0; 0
Min Color: 255; 102; 102
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_0
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 0.5
Min Value: -0.5
Value: false
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 204; 204; 0
Color Transformer: Intensity
Decay Time: 0.4000000059604645
Enabled: true
Invert Rainbow: false
Max Color: 102; 102; 0
Min Color: 255; 255; 51
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_1
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0.4000000059604645
Enabled: true
Invert Rainbow: false
Max Color: 74; 112; 0
Min Color: 170; 255; 0
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_2
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 1
Autocompute Intensity Bounds: true
Autocompute Value Bounds:
Max Value: 10
Min Value: -10
Value: true
Axis: Z
Channel Name: intensity
Class: rviz/PointCloud2
Color: 255; 255; 255
Color Transformer: Intensity
Decay Time: 0.4000000059604645
Enabled: true
Invert Rainbow: false
Max Color: 0; 118; 118
Min Color: 0; 255; 255
Name: PointCloud2
Position Transformer: XYZ
Queue Size: 10
Selectable: true
Size (Pixels): 3
Size (m): 0.10000000149011612
Style: Spheres
Topic: /ti_mmwave/radar_scan_pcl_3
Unreliable: false
Use Fixed Frame: true
Use rainbow: false
Value: true
- Alpha: 1
Class: rviz/Axes
Enabled: true
Length: 1
Name: Axes
Radius: 0.10000000149011612
Reference Frame: <Fixed Frame>
Show Trail: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: ti_mmwave_pcl
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 14.119680404663086
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 5
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: 1.5597963333129883
Target Frame: <Fixed Frame>
Yaw: 3.144761800765991
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 876
Hide Left Dock: false
Hide Right Dock: true
QMainWindow State: 000000ff00000000fd0000000400000000000001ad000002c6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002c6000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002abfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000041000002ab000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005ff0000004afc0100000002fb0000000800540069006d00650100000000000005ff0000024400fffffffb0000000800540069006d006501000000000000045000000000000000000000044c000002c600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: true
Width: 1535
X: 331
Y: 27

10
ros_driver/src/ti_mmwave_tracker_rospkg/launch/viz_remote_pc.launch Normal file
View File

@@ -0,0 +1,10 @@
<!--
This file will launch rViz for visualization of point cloud data collected by mmWave radar.
This file is intended to be used for visualization on a remote PC while radar driver runs on TDA4 device.
-->
<launch>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_tracker_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>
</launch>

10
ros_driver/src/ti_mmwave_tracker_rospkg/launch/viz_remote_pc_quad.launch Normal file
View File

@@ -0,0 +1,10 @@
<!--
This file will launch rViz for visualization of point cloud data collected by four mmWave radar sensors.
This file is intended to be used for visualization on a remote PC while the radar driver runs on TDA4 device.
-->
<launch>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_tracker_rospkg)/launch/rviz/ti_mmwave_quad.rviz"/>
</launch>

15
ros_driver/src/ti_mmwave_tracker_rospkg/mmWave_nodelets.xml Normal file
View File

@@ -0,0 +1,15 @@
<library path="lib/libtracker_mmwave">
<class name="ti_mmwave_tracker_rospkg/mmWaveCommSrv" type="ti_mmwave_tracker_rospkg::mmWaveCommSrv" base_class_type="nodelet::Nodelet">
<description>
Command Service Nodelet
</description>
</class>
<class name="ti_mmwave_tracker_rospkg/mmWaveDataHdl" type="ti_mmwave_tracker_rospkg::mmWaveDataHdl" base_class_type="nodelet::Nodelet">
<description>
Data Handler Nodelet
</description>
</class>
<depend>serial</depend>
</library>

2
ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarPointTrackID.msg Normal file
View File

@@ -0,0 +1,2 @@
Header header
uint8 tid

10
ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarScan.msg Normal file
View File

@@ -0,0 +1,10 @@
Header header
uint16 point_id
float32 x
float32 y
float32 z
float32 range
float32 velocity
uint16 doppler_bin
float32 bearing
float32 intensity

3
ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarTrackArray.msg Normal file
View File

@@ -0,0 +1,3 @@
Header header
uint32 num_tracks
RadarTrackContents[] track

11
ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarTrackContents.msg Normal file
View File

@@ -0,0 +1,11 @@
Header header
uint32 tid
float32 posx
float32 posy
float32 posz
float32 velx
float32 vely
float32 velz
float32 accx
float32 accy
float32 accz

60
ros_driver/src/ti_mmwave_tracker_rospkg/package.xml Normal file
View File

@@ -0,0 +1,60 @@
<?xml version="1.0"?>
<package>
<name>ti_mmwave_tracker_rospkg</name>
<version>3.3.0</version>
<description>The ti_mmwave_tracker_rospkg package. Publish messages with doppler information. Multi-sensor and camera overlay support. Modified by Leo Zhang</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="dr.leo.zhang@outlook.com">Leo Zhang</maintainer>
<maintainer email="a-wendell@ti.com">Allison Wendell</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>BSD</license>
<!-- Url tags are optional, but multiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<url type="website">https://github.com/radar-lab/ti_mmwave_rospkg</url>
<url type="website">https://bitbucket.itg.ti.com/scm/mmwave_apps/ros_multisensor_demo/ti_mmwave_rospkg</url>
<!-- Author tags are optional, multiple are allowed, one per tag -->
<!-- Authors do not have to be maintainers, but could be -->
<!-- Example: -->
<author email="dr.leo.zhang@outlook.com">Leo Zhang</author>
<author email="a-wendell@ti.com">Allison Wendell</author>
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>nodelet</build_depend>
<build_depend>roscpp</build_depend>
<build_depend>serial</build_depend>
<build_depend>std_msgs</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>nodelet</run_depend>
<run_depend>roscpp</run_depend>
<run_depend>serial</run_depend>
<run_depend>std_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<nodelet plugin="${prefix}/mmWave_nodelets.xml" />
</export>
</package>

895
ros_driver/src/ti_mmwave_tracker_rospkg/src/DataHandlerClass.cpp Normal file
View File

@@ -0,0 +1,895 @@
#include <DataHandlerClass.h>
#define PCL_NO_PRECOMPILE
//#include <pcl/memory.h>
#include <pcl/pcl_macros.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
DataUARTHandler* gDataHandlerPtr;
void DataUARTHandler::sigHandler(int32_t sig)
{
switch(sig)
{
case SIGINT:
gDataHandlerPtr->stop();
}
}
struct mmWaveCloudType
{
PCL_ADD_POINT4D;
union
{
struct
{
float intensity;
float velocity;
};
float data_c[4];
};
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
} EIGN_ALIGN16;
POINT_CLOUD_REGISTER_POINT_STRUCT (mmWaveCloudType,
(float, x, x)
(float, y, y)
(float, z, z)
(float, intensity, intensity)
(float, velocity, velocity))
DataUARTHandler::DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) {
// Make sure to add ALL publishers below to DataHandlerClass.h as an include at the top
// File will be generated during catkin_make, use existing publishers to understand syntax
DataUARTHandler_pub = nh->advertise<sensor_msgs::PointCloud2>("/ti_mmwave/radar_scan_pcl", 100);
radar_scan_pub = nh->advertise<ti_mmwave_tracker_rospkg::RadarScan>("/ti_mmwave/radar_scan", 100);
radar_trackid_pub = nh->advertise<ti_mmwave_tracker_rospkg::RadarPointTrackID>("/ti_mmwave/radar_trackid", 100);
radar_trackarray_pub = nh->advertise<ti_mmwave_tracker_rospkg::RadarTrackArray>("/ti_mmwave/radar_trackarray", 100);
marker_pub = nh->advertise<visualization_msgs::Marker>("/ti_mmwave/radar_scan_markers", 100);
maxAllowedElevationAngleDeg = 90; // Use max angle if none specified
maxAllowedAzimuthAngleDeg = 90; // Use max angle if none specified
// Wait for parameters
while(!nh->hasParam("/ti_mmwave/doppler_vel_resolution")){}
nh->getParam("/ti_mmwave/numAdcSamples", nr);
nh->getParam("/ti_mmwave/numLoops", nd);
nh->getParam("/ti_mmwave/num_TX", ntx);
nh->getParam("/ti_mmwave/f_s", fs);
nh->getParam("/ti_mmwave/f_c", fc);
nh->getParam("/ti_mmwave/BW", BW);
nh->getParam("/ti_mmwave/PRI", PRI);
nh->getParam("/ti_mmwave/t_fr", tfr);
nh->getParam("/ti_mmwave/max_range", max_range);
nh->getParam("/ti_mmwave/range_resolution", vrange);
nh->getParam("/ti_mmwave/max_doppler_vel", max_vel);
nh->getParam("/ti_mmwave/doppler_vel_resolution", vvel);
ROS_INFO("\n\n==============================\nList of parameters\n==============================\nNumber of range samples: %d\nNumber of chirps: %d\nf_s: %.3f MHz\nf_c: %.3f GHz\nBandwidth: %.3f MHz\nPRI: %.3f us\nFrame time: %.3f ms\nMax range: %.3f m\nRange resolution: %.3f m\nMax Doppler: +-%.3f m/s\nDoppler resolution: %.3f m/s\n==============================\n", \
nr, nd, fs/1e6, fc/1e9, BW/1e6, PRI*1e6, tfr*1e3, max_range, vrange, max_vel/2, vvel);
gDataHandlerPtr = this;
stop_threads = false;
}
void DataUARTHandler::setFrameID(char* myFrameID)
{
frameID = myFrameID;
}
/*Implementation of setUARTPort*/
void DataUARTHandler::setUARTPort(char* mySerialPort)
{
dataSerialPort = mySerialPort;
}
/*Implementation of setBaudRate*/
void DataUARTHandler::setBaudRate(int myBaudRate)
{
dataBaudRate = myBaudRate;
}
/*Implementation of setMaxAllowedElevationAngleDeg*/
void DataUARTHandler::setMaxAllowedElevationAngleDeg(int myMaxAllowedElevationAngleDeg)
{
maxAllowedElevationAngleDeg = myMaxAllowedElevationAngleDeg;
}
/*Implementation of setMaxAllowedAzimuthAngleDeg*/
void DataUARTHandler::setMaxAllowedAzimuthAngleDeg(int myMaxAllowedAzimuthAngleDeg)
{
maxAllowedAzimuthAngleDeg = myMaxAllowedAzimuthAngleDeg;
}
/*Implementation of readIncomingData*/
void *DataUARTHandler::readIncomingData(void)
{
int firstPacketReady = 0;
uint8_t last8Bytes[8] = {0};
/*Open UART Port and error checking*/
serial::Serial mySerialObject("", dataBaudRate, serial::Timeout::simpleTimeout(100));
mySerialObject.setPort(dataSerialPort);
try
{
mySerialObject.open();
} catch (std::exception &e1) {
ROS_INFO("DataUARTHandler Read Thread: Failed to open Data serial port with error: %s", e1.what());
ROS_INFO("DataUARTHandler Read Thread: Waiting 20 seconds before trying again...");
try
{
// Wait 20 seconds and try to open serial port again
ros::Duration(20).sleep();
mySerialObject.open();
} catch (std::exception &e2) {
ROS_ERROR("DataUARTHandler Read Thread: Failed second time to open Data serial port, error: %s", e1.what());
ROS_ERROR("DataUARTHandler Read Thread: Port could not be opened. Port is \"%s\" and baud rate is %d", dataSerialPort, dataBaudRate);
pthread_exit(NULL);
}
}
if(mySerialObject.isOpen())
ROS_INFO("DataUARTHandler Read Thread: Port is open");
else
ROS_ERROR("DataUARTHandler Read Thread: Port could not be opened");
/*Quick magicWord check to synchronize program with data Stream*/
while(!isMagicWord(last8Bytes))
{
last8Bytes[0] = last8Bytes[1];
last8Bytes[1] = last8Bytes[2];
last8Bytes[2] = last8Bytes[3];
last8Bytes[3] = last8Bytes[4];
last8Bytes[4] = last8Bytes[5];
last8Bytes[5] = last8Bytes[6];
last8Bytes[6] = last8Bytes[7];
mySerialObject.read(&last8Bytes[7], 1);
}
/*Lock nextBufp before entering main loop*/
pthread_mutex_lock(&nextBufp_mutex);
while(ros::ok())
{
/*Start reading UART data and writing to buffer while also checking for magicWord*/
last8Bytes[0] = last8Bytes[1];
last8Bytes[1] = last8Bytes[2];
last8Bytes[2] = last8Bytes[3];
last8Bytes[3] = last8Bytes[4];
last8Bytes[4] = last8Bytes[5];
last8Bytes[5] = last8Bytes[6];
last8Bytes[6] = last8Bytes[7];
mySerialObject.read(&last8Bytes[7], 1);
nextBufp->push_back( last8Bytes[7] ); //push byte onto buffer
//ROS_INFO("DataUARTHandler Read Thread: last8bytes = %02x%02x %02x%02x %02x%02x %02x%02x", last8Bytes[7], last8Bytes[6], last8Bytes[5], last8Bytes[4], last8Bytes[3], last8Bytes[2], last8Bytes[1], last8Bytes[0]);
/*If a magicWord is found wait for sorting to finish and switch buffers*/
if( isMagicWord(last8Bytes) )
{
//ROS_INFO("Found magic word");
/*Lock countSync Mutex while unlocking nextBufp so that the swap thread can use it*/
pthread_mutex_lock(&countSync_mutex);
pthread_mutex_unlock(&nextBufp_mutex);
/*increment countSync*/
countSync++;
/*If this is the first packet to be found, increment countSync again since Sort thread is not reading data yet*/
if(firstPacketReady == 0)
{
countSync++;
firstPacketReady = 1;
}
/*Signal Swap Thread to run if countSync has reached its max value*/
if(countSync == COUNT_SYNC_MAX)
{
pthread_cond_signal(&countSync_max_cv);
}
/*Wait for the Swap thread to finish swapping pointers and signal us to continue*/
pthread_cond_wait(&read_go_cv, &countSync_mutex);
/*Unlock countSync so that Swap Thread can use it*/
pthread_mutex_unlock(&countSync_mutex);
pthread_mutex_lock(&nextBufp_mutex);
nextBufp->clear();
memset(last8Bytes, 0, sizeof(last8Bytes));
}
}
mySerialObject.close();
pthread_exit(NULL);
}
int DataUARTHandler::isMagicWord(uint8_t last8Bytes[8])
{
int val = 0, i = 0, j = 0;
for(i = 0; i < 8 ; i++)
{
if( last8Bytes[i] == magicWord[i])
{
j++;
}
}
if( j == 8)
{
val = 1;
}
return val;
}
void *DataUARTHandler::syncedBufferSwap(void)
{
while(ros::ok())
{
pthread_mutex_lock(&countSync_mutex);
while(countSync < COUNT_SYNC_MAX)
{
if(stop_threads)
{
pthread_mutex_unlock(&countSync_mutex);
pthread_cond_signal(&sort_go_cv);
pthread_cond_signal(&read_go_cv);
pthread_exit(NULL);
}
pthread_cond_wait(&countSync_max_cv, &countSync_mutex);
pthread_mutex_lock(&currentBufp_mutex);
pthread_mutex_lock(&nextBufp_mutex);
std::vector<uint8_t>* tempBufp = currentBufp;
this->currentBufp = this->nextBufp;
this->nextBufp = tempBufp;
pthread_mutex_unlock(&currentBufp_mutex);
pthread_mutex_unlock(&nextBufp_mutex);
countSync = 0;
pthread_cond_signal(&sort_go_cv);
pthread_cond_signal(&read_go_cv);
}
pthread_mutex_unlock(&countSync_mutex);
}
pthread_exit(NULL);
}
void *DataUARTHandler::sortIncomingData( void )
{
MmwDemo_Output_TLV_Types tlvType = MMWDEMO_OUTPUT_MSG_NULL;
uint32_t tlvLen = 0;
uint32_t headerSize;
unsigned int currentDatap = 0;
SorterState sorterState = READ_HEADER;
int i = 0, tlvCount = 0, offset = 0;
int j = 0;
float maxElevationAngleRatioSquared;
float maxAzimuthAngleRatio;
float realElevation;
float realAzimuth;
float realDoppler;
float realRange;
float realSNR;
//boost::shared_ptr<pcl::PointCloud<pcl::PointXYZI>> RScan(new pcl::PointCloud<pcl::PointXYZI>);
boost::shared_ptr<pcl::PointCloud<mmWaveCloudType>> RScan(new pcl::PointCloud<mmWaveCloudType>);
ti_mmwave_tracker_rospkg::RadarScan radarscan;
ti_mmwave_tracker_rospkg::RadarPointTrackID radartrackid;
ti_mmwave_tracker_rospkg::RadarTrackArray radartrackarray;
ti_mmwave_tracker_rospkg::RadarTrackContents radartrackcontents;
//wait for first packet to arrive
pthread_mutex_lock(&countSync_mutex);
pthread_cond_wait(&sort_go_cv, &countSync_mutex);
pthread_mutex_unlock(&countSync_mutex);
pthread_mutex_lock(&currentBufp_mutex);
while(ros::ok())
{
switch(sorterState)
{
case READ_HEADER:
//init variables
mmwData.numObjOut = 0;
//make sure packet has at least first three fields (12 bytes) before we read them (does not include magicWord since it was already removed)
if(currentBufp->size() < 12)
{
sorterState = SWAP_BUFFERS;
break;
}
//get version (4 bytes)
memcpy( &mmwData.header.version, &currentBufp->at(currentDatap), sizeof(mmwData.header.version));
currentDatap += ( sizeof(mmwData.header.version) );
//get totalPacketLen (4 bytes)
memcpy( &mmwData.header.totalPacketLen, &currentBufp->at(currentDatap), sizeof(mmwData.header.totalPacketLen));
currentDatap += ( sizeof(mmwData.header.totalPacketLen) );
//get platform (4 bytes)
memcpy( &mmwData.header.platform, &currentBufp->at(currentDatap), sizeof(mmwData.header.platform));
currentDatap += ( sizeof(mmwData.header.platform) );
//if packet doesn't have correct header size (which is based on platform), throw it away
// (does not include magicWord since it was already removed)
if ((mmwData.header.platform & 0xFFFF) == 0x1443) // platform is xWR1443)
{
headerSize = 7 * 4; // xWR1443 SDK demo header does not have subFrameNumber field
}
else
{
headerSize = 8 * 4; // header includes subFrameNumber field
}
if(currentBufp->size() < headerSize) {
sorterState = SWAP_BUFFERS;
break;
}
//get frameNumber (4 bytes)
memcpy( &mmwData.header.frameNumber, &currentBufp->at(currentDatap), sizeof(mmwData.header.frameNumber));
currentDatap += ( sizeof(mmwData.header.frameNumber) );
//get timeCpuCycles (4 bytes)
memcpy( &mmwData.header.timeCpuCycles, &currentBufp->at(currentDatap), sizeof(mmwData.header.timeCpuCycles));
currentDatap += ( sizeof(mmwData.header.timeCpuCycles) );
//get numDetectedObj (4 bytes)
memcpy( &mmwData.header.numDetectedObj, &currentBufp->at(currentDatap), sizeof(mmwData.header.numDetectedObj));
currentDatap += ( sizeof(mmwData.header.numDetectedObj) );
//get numTLVs (4 bytes)
memcpy( &mmwData.header.numTLVs, &currentBufp->at(currentDatap), sizeof(mmwData.header.numTLVs));
currentDatap += ( sizeof(mmwData.header.numTLVs) );
//get subFrameNumber (4 bytes) (not used for XWR1443)
if((mmwData.header.platform & 0xFFFF) != 0x1443) {
memcpy( &mmwData.header.subFrameNumber, &currentBufp->at(currentDatap), sizeof(mmwData.header.subFrameNumber));
currentDatap += ( sizeof(mmwData.header.subFrameNumber) );
}
//if packet lengths do not match, throw it away
if(mmwData.header.totalPacketLen == currentBufp->size() )
{
sorterState = CHECK_TLV_TYPE;
}
else sorterState = SWAP_BUFFERS;
break;
case READ_SPHERE_POINT_CLOUD:
// CHECK_TLV_TYPE code has already read tlvType and tlvLen
i = 0;
offset = 0;
mmwData.numObjOut = mmwData.header.numDetectedObj;
pcl_conversions::toPCL(ros::Time::now(), RScan->header.stamp);
RScan->header.frame_id = frameID;
RScan->height = 1;
RScan->width = mmwData.numObjOut;
RScan->is_dense = 1;
RScan->points.resize(RScan->width * RScan->height);
// Calculate ratios for max desired elevation and azimuth angles
if ((maxAllowedElevationAngleDeg >= 0) && (maxAllowedElevationAngleDeg < 90)) {
maxElevationAngleRatioSquared = tan(maxAllowedElevationAngleDeg * M_PI / 180.0);
maxElevationAngleRatioSquared = maxElevationAngleRatioSquared * maxElevationAngleRatioSquared;
} else maxElevationAngleRatioSquared = -1;
if ((maxAllowedAzimuthAngleDeg >= 0) && (maxAllowedAzimuthAngleDeg < 90)) maxAzimuthAngleRatio = tan(maxAllowedAzimuthAngleDeg * M_PI / 180.0);
else maxAzimuthAngleRatio = -1;
//get object Elevation unit (rad)
memcpy( &mmwData.newSphereCloudOut.elevationUnit, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.elevationUnit));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.elevationUnit) );
//get object Azimuth unit (rad)
memcpy( &mmwData.newSphereCloudOut.azimuthUnit, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.azimuthUnit));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.azimuthUnit) );
//get object Doppler unit (m/s)
memcpy( &mmwData.newSphereCloudOut.dopplerUnit, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.dopplerUnit));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.dopplerUnit) );
//get object Range unit (meters)
memcpy( &mmwData.newSphereCloudOut.rangeUnit, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.rangeUnit));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.rangeUnit) );
//get object SNR unit (ratio)
memcpy( &mmwData.newSphereCloudOut.snrUnit, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.snrUnit));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.snrUnit) );
while( i < mmwData.numObjOut ) {
//get Elevation value to multiply by
memcpy( &mmwData.newSphereCloudOut.elevation, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.elevation));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.elevation) );
//get Azimuth value to multiply by
memcpy( &mmwData.newSphereCloudOut.azimuth, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.azimuth));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.azimuth) );
//get Doppler value to multiply by
memcpy( &mmwData.newSphereCloudOut.doppler, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.doppler));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.doppler) );
//get Range value to multiply by
memcpy( &mmwData.newSphereCloudOut.range, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.range));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.range) );
//get SNR value to multiply by
memcpy( &mmwData.newSphereCloudOut.snr, &currentBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.snr));
currentDatap += ( sizeof(mmwData.newSphereCloudOut.snr) );
//multiple sensor output value by unit value to decompress data
realElevation = mmwData.newSphereCloudOut.elevationUnit * mmwData.newSphereCloudOut.elevation;
realAzimuth = mmwData.newSphereCloudOut.azimuthUnit * mmwData.newSphereCloudOut.azimuth;
realDoppler = mmwData.newSphereCloudOut.dopplerUnit * mmwData.newSphereCloudOut.doppler;
realRange = mmwData.newSphereCloudOut.rangeUnit * mmwData.newSphereCloudOut.range;
realSNR = mmwData.newSphereCloudOut.snrUnit * mmwData.newSphereCloudOut.snr;
// Map mmWave sensor coordinates to ROS coordinate system while also converting from spherical to cartesian
RScan->points[i].x = realRange * cos(realAzimuth) * cos(realElevation); // ROS standard coordinate system X-axis is forward which is the mmWave sensor Y-axis
RScan->points[i].y = -1 * realRange * sin(realAzimuth) * cos(realElevation); // ROS standard coordinate system Y-axis is left which is the mmWave sensor -(X-axis)
RScan->points[i].z = realRange * sin(realElevation); // ROS standard coordinate system Z-axis is up which is the same as mmWave sensor Z-axis
RScan->points[i].velocity = realDoppler;
RScan->points[i].intensity = realSNR;
radarscan.header.frame_id = frameID;
radarscan.header.stamp = ros::Time::now();
radarscan.point_id = i;
radarscan.x = realRange * cos(realAzimuth) * cos(realElevation);
radarscan.y = -1 * realRange * sin(realAzimuth) * cos(realElevation);
radarscan.z = realRange * sin(realElevation);
radarscan.range = realRange;
radarscan.velocity = realDoppler;
radarscan.intensity = realSNR;
if (((maxElevationAngleRatioSquared == -1) ||
(((RScan->points[i].z * RScan->points[i].z) / (RScan->points[i].x * RScan->points[i].x +
RScan->points[i].y * RScan->points[i].y)
) < maxElevationAngleRatioSquared)
) &&
((maxAzimuthAngleRatio == -1) || (fabs(RScan->points[i].y / RScan->points[i].x) < maxAzimuthAngleRatio)) &&
(RScan->points[i].x != 0)
)
{
radar_scan_pub.publish(radarscan);
}
i++;
}
sorterState = CHECK_TLV_TYPE;
break;
case READ_3D_TARGET_LIST:
i = 0;
offset = 0;
pcl_conversions::toPCL(ros::Time::now(), RScan->header.stamp);
mmwData.numObjOut = mmwData.header.numDetectedObj;
radartrackarray.header.frame_id = frameID;
radartrackarray.header.stamp = ros::Time::now();
radartrackarray.num_tracks = (int) tlvLen / 112;
//ROS_INFO("Number of Tracks is: %d",(tlvLen / 112));
while( i < radartrackarray.num_tracks ) {
//get Track ID
memcpy( &mmwData.newListOut.tid, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.tid));
currentDatap += ( sizeof(mmwData.newListOut.tid) );
//get Track position in X dimension (m)
memcpy( &mmwData.newListOut.posX, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.posX));
currentDatap += ( sizeof(mmwData.newListOut.posX) );
//get Track position in Y dimension (m)
memcpy( &mmwData.newListOut.posY, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.posY));
currentDatap += ( sizeof(mmwData.newListOut.posY) );
//get Track position in Z dimension (m)
memcpy( &mmwData.newListOut.posZ, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.posZ));
currentDatap += ( sizeof(mmwData.newListOut.posZ) );
//get Track velocity in X dimension (m)
memcpy( &mmwData.newListOut.velX, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.velX));
currentDatap += ( sizeof(mmwData.newListOut.velX) );
//get Track velocity in Y dimension (m)
memcpy( &mmwData.newListOut.velY, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.velY));
currentDatap += ( sizeof(mmwData.newListOut.velY) );
//get Track velocity in Z dimension (m)
memcpy( &mmwData.newListOut.velZ, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.velZ));
currentDatap += ( sizeof(mmwData.newListOut.velZ) );
//get Track acceleration in X dimension (m)
memcpy( &mmwData.newListOut.accX, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.accX));
currentDatap += ( sizeof(mmwData.newListOut.accX) );
//get Track acceleration in Y dimension (m)
memcpy( &mmwData.newListOut.accY, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.accY));
currentDatap += ( sizeof(mmwData.newListOut.accY) );
//get Track acceleration in Z dimension (m)
memcpy( &mmwData.newListOut.accZ, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.accZ));
currentDatap += ( sizeof(mmwData.newListOut.accZ) );
//Throw Away
memcpy( &mmwData.newListOut.ec, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.ec));
currentDatap += ( sizeof(mmwData.newListOut.ec) );
//Throw Away
memcpy( &mmwData.newListOut.g, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.g));
currentDatap += ( sizeof(mmwData.newListOut.g) );
//Throw Away
memcpy( &mmwData.newListOut.confidenceLevel, &currentBufp->at(currentDatap), sizeof(mmwData.newListOut.confidenceLevel));
currentDatap += ( sizeof(mmwData.newListOut.confidenceLevel) );
//throw away first track in order to allign with PointCloud
/*
if (frameID == 0){
radartrack.header.frame_id = frameID;
}
else{
radartrack.header.frame_id = frameID - 1;
}
*/
radartrackcontents.header.frame_id = frameID;
radartrackcontents.header.stamp = ros::Time::now();
radartrackcontents.tid = mmwData.newListOut.tid;
radartrackcontents.posx = mmwData.newListOut.posY; // ROS standard coordinate system X-axis is forward which is the mmWave sensor Y-axis
radartrackcontents.posy = -mmwData.newListOut.posX; // ROS standard coordinate system Y-axis is left which is the mmWave sensor -(X-axis)
radartrackcontents.posz = mmwData.newListOut.posZ;
radartrackcontents.velx = mmwData.newListOut.velY;
radartrackcontents.vely = -mmwData.newListOut.velX;
radartrackcontents.velz = mmwData.newListOut.velZ;
radartrackcontents.accx = mmwData.newListOut.accY;
radartrackcontents.accy = -mmwData.newListOut.accX;
radartrackcontents.accz = mmwData.newListOut.accZ;
radartrackarray.track.push_back(radartrackcontents);
i++;
}
radar_trackarray_pub.publish(radartrackarray);
radartrackarray.track.clear();
sorterState = CHECK_TLV_TYPE;
break;
case READ_TARGET_INDEX:
i = 0;
offset = 0;
mmwData.numObjOut = mmwData.header.numDetectedObj;
pcl_conversions::toPCL(ros::Time::now(), RScan->header.stamp);
RScan->header.frame_id = frameID;
RScan->height = 1;
RScan->width = mmwData.numObjOut;
RScan->is_dense = 1;
RScan->points.resize(RScan->width * RScan->height);
while( i < tlvLen ) {
//get point's associated Track (int)
memcpy( &mmwData.newIndexOut.targetID, &currentBufp->at(currentDatap), sizeof(mmwData.newIndexOut.targetID));
currentDatap += ( sizeof(mmwData.newIndexOut.targetID) );
// throw away first track in order to allign with PointCloud
// if (frameID == 0){
radartrackid.header.frame_id = frameID;
// }
// else{
// radartrackid.header.frame_id = frameID - 1;
// }
radartrackid.header.stamp = ros::Time::now();
radartrackid.tid = mmwData.newIndexOut.targetID;
radar_trackid_pub.publish(radartrackid);
i++;
}
sorterState = CHECK_TLV_TYPE;
break;
case CHECK_TLV_TYPE:
//ROS_INFO("DataUARTHandler Sort Thread : tlvCount = %d, numTLV = %d", tlvCount, mmwData.header.numTLVs);
if(tlvCount++ >= mmwData.header.numTLVs) // Done parsing all received TLV sections
{
// Publish detected object pointcloud
if (mmwData.numObjOut > 0)
{
j = 0;
for (i = 0; i < mmwData.numObjOut; i++)
{
// Keep point if elevation and azimuth angles are less than specified max values
// (NOTE: The following calculations are done using ROS standard coordinate system axis definitions where X is forward and Y is left)
if (((maxElevationAngleRatioSquared == -1) ||
(((RScan->points[i].z * RScan->points[i].z) / (RScan->points[i].x * RScan->points[i].x +
RScan->points[i].y * RScan->points[i].y)
) < maxElevationAngleRatioSquared)
) &&
((maxAzimuthAngleRatio == -1) || (fabs(RScan->points[i].y / RScan->points[i].x) < maxAzimuthAngleRatio)) &&
(RScan->points[i].x != 0)
)
{
//ROS_INFO("Kept point");
// copy: points[i] => points[j]
memcpy( &RScan->points[j], &RScan->points[i], sizeof(RScan->points[i]));
j++;
}
}
mmwData.numObjOut = j; // update number of objects as some points may have been removed
// Resize point cloud since some points may have been removed
RScan->width = mmwData.numObjOut;
RScan->points.resize(RScan->width * RScan->height);
//ROS_INFO("mmwData.numObjOut after = %d", mmwData.numObjOut);
//ROS_INFO("DataUARTHandler Sort Thread: number of obj = %d", mmwData.numObjOut );
}
DataUARTHandler_pub.publish(RScan);
//ROS_INFO("DataUARTHandler Sort Thread : CHECK_TLV_TYPE state says tlvCount max was reached, going to switch buffer state");
sorterState = SWAP_BUFFERS;
}
else // More TLV sections to parse
{
//get tlvType (32 bits) & remove from queue
memcpy( &tlvType, &currentBufp->at(currentDatap), sizeof(tlvType));
currentDatap += ( sizeof(tlvType) );
//get tlvLen (32 bits) & remove from queue
memcpy( &tlvLen, &currentBufp->at(currentDatap), sizeof(tlvLen));
currentDatap += ( sizeof(tlvLen) );
;
switch(tlvType)
{
case MMWDEMO_OUTPUT_MSG_NULL:
break;
case MMWDEMO_OUTPUT_MSG_COMPRESSED_POINTS:
//ROS_INFO("DataUARTHandler Sort Thread : Compressed Points TLV");
sorterState = READ_SPHERE_POINT_CLOUD;
break;
case MMWDEMO_OUTPUT_MSG_TRACKERPROC_3D_TARGET_LIST:
//ROS_INFO("DataUARTHandler Sort Thread : 3D Target List TLV");
sorterState = READ_3D_TARGET_LIST;
break;
case MMWDEMO_OUTPUT_MSG_TRACKERPROC_TARGET_INDEX:
//ROS_INFO("DataUARTHandler Sort Thread : Target Index TLV");
sorterState = READ_TARGET_INDEX;
break;
case MMWDEMO_OUTPUT_MSG_MAX:
//ROS_INFO("DataUARTHandler Sort Thread : Header TLV");
sorterState = READ_HEADER;
break;
default: break;
}
}
break;
case SWAP_BUFFERS:
pthread_mutex_lock(&countSync_mutex);
pthread_mutex_unlock(&currentBufp_mutex);
countSync++;
if(countSync == COUNT_SYNC_MAX)
{
pthread_cond_signal(&countSync_max_cv);
}
pthread_cond_wait(&sort_go_cv, &countSync_mutex);
pthread_mutex_unlock(&countSync_mutex);
pthread_mutex_lock(&currentBufp_mutex);
currentDatap = 0;
tlvCount = 0;
sorterState = READ_HEADER;
break;
default: break;
}
}
pthread_exit(NULL);
}
void DataUARTHandler::start(void)
{
pthread_t uartThread, sorterThread, swapThread;
int iret1, iret2, iret3;
sigset_t set;
int s;
/* Block SIGINT on main thread and subsequently created threads */
sigemptyset(&set);
sigaddset(&set, SIGINT);
s = pthread_sigmask(SIG_BLOCK, &set, NULL);
pthread_mutex_init(&countSync_mutex, NULL);
pthread_mutex_init(&nextBufp_mutex, NULL);
pthread_mutex_init(&currentBufp_mutex, NULL);
pthread_cond_init(&countSync_max_cv, NULL);
pthread_cond_init(&read_go_cv, NULL);
pthread_cond_init(&sort_go_cv, NULL);
countSync = 0;
/* Create independent threads each of which will execute function */
iret1 = pthread_create( &uartThread, NULL, this->readIncomingData_helper, this);
if(iret1)
{
ROS_INFO("Error - pthread_create() return code: %d\n",iret1);
ros::shutdown();
}
iret2 = pthread_create( &sorterThread, NULL, this->sortIncomingData_helper, this);
if(iret2)
{
ROS_INFO("Error - pthread_create() return code: %d\n",iret1);
ros::shutdown();
}
iret3 = pthread_create( &swapThread, NULL, this->syncedBufferSwap_helper, this);
if(iret3)
{
ROS_INFO("Error - pthread_create() return code: %d\n",iret1);
ros::shutdown();
}
/* Unlock SIGINT on main thread */
s = pthread_sigmask(SIG_UNBLOCK, &set, NULL);
signal(SIGINT, sigHandler);
ros::spin();
pthread_join(uartThread, NULL);
ROS_INFO("DataUARTHandler Read Thread joined");
pthread_join(sorterThread, NULL);
ROS_INFO("DataUARTHandler Sort Thread joined");
pthread_join(swapThread, NULL);
ROS_INFO("DataUARTHandler Swap Thread joined");
pthread_mutex_destroy(&countSync_mutex);
pthread_mutex_destroy(&nextBufp_mutex);
pthread_mutex_destroy(&currentBufp_mutex);
pthread_cond_destroy(&countSync_max_cv);
pthread_cond_destroy(&read_go_cv);
pthread_cond_destroy(&sort_go_cv);
}
void* DataUARTHandler::readIncomingData_helper(void *context)
{
return (static_cast<DataUARTHandler*>(context)->readIncomingData());
}
void* DataUARTHandler::sortIncomingData_helper(void *context)
{
return (static_cast<DataUARTHandler*>(context)->sortIncomingData());
}
void* DataUARTHandler::syncedBufferSwap_helper(void *context)
{
return (static_cast<DataUARTHandler*>(context)->syncedBufferSwap());
}
void DataUARTHandler::visualize(const ti_mmwave_tracker_rospkg::RadarScan &msg){
visualization_msgs::Marker marker;
marker.header.frame_id = frameID;
marker.header.stamp = ros::Time::now();
marker.id = msg.point_id;
marker.type = visualization_msgs::Marker::SPHERE;
marker.lifetime = ros::Duration(tfr);
marker.action = marker.ADD;
marker.pose.position.x = msg.x;
marker.pose.position.y = msg.y;
marker.pose.position.z = 0;
marker.pose.orientation.x = 0;
marker.pose.orientation.y = 0;
marker.pose.orientation.z = 0;
marker.pose.orientation.w = 0;
marker.scale.x = .03;
marker.scale.y = .03;
marker.scale.z = .03;
marker.color.a = 1;
marker.color.r = (int) 255 * msg.intensity;
marker.color.g = (int) 255 * msg.intensity;
marker.color.b = 1;
marker_pub.publish(marker);
}
void DataUARTHandler::stop()
{
ROS_DEBUG("Stopping Threads");
stop_threads = true;
ros::shutdown();
pthread_cond_signal(&read_go_cv);
pthread_cond_signal(&sort_go_cv);
pthread_cond_signal(&countSync_max_cv);
}

117
ros_driver/src/ti_mmwave_tracker_rospkg/src/ParameterParser.cpp Normal file
View File

@@ -0,0 +1,117 @@
#include "ParameterParser.h"
namespace ti_mmwave_tracker_rospkg {
PLUGINLIB_EXPORT_CLASS(ti_mmwave_tracker_rospkg::ParameterParser, nodelet::Nodelet);
ParameterParser::ParameterParser() {}
void ParameterParser::onInit() {}
void ParameterParser::ParamsParser(ti_mmwave_tracker_rospkg::mmWaveCLI &srv, ros::NodeHandle &nh) {
// ROS_ERROR("%s",srv.request.comm.c_str());
// ROS_ERROR("%s",srv.response.resp.c_str());
std::vector <std::string> v;
std::string s = srv.request.comm.c_str();
std::istringstream ss(s);
std::string token;
std::string req;
int i = 0;
while (std::getline(ss, token, ' ')) {
v.push_back(token);
if (i > 0) {
if (!req.compare("profileCfg")) {
switch (i) {
case 2:
nh.setParam("/ti_mmwave/startFreq", std::stof(token)); break;
case 3:
nh.setParam("/ti_mmwave/idleTime", std::stof(token)); break;
case 4:
nh.setParam("/ti_mmwave/adcStartTime", std::stof(token)); break;
case 5:
nh.setParam("/ti_mmwave/rampEndTime", std::stof(token)); break;
case 8:
nh.setParam("/ti_mmwave/freqSlopeConst", std::stof(token)); break;
case 10:
nh.setParam("/ti_mmwave/numAdcSamples", std::stoi(token)); break;
case 11:
nh.setParam("/ti_mmwave/digOutSampleRate", std::stof(token)); break;
case 14:
nh.setParam("/ti_mmwave/rxGain", std::stof(token)); break;
}
} else if (!req.compare("frameCfg")) {
switch (i) {
case 1:
nh.setParam("/ti_mmwave/chirpStartIdx", std::stoi(token)); break;
case 2:
nh.setParam("/ti_mmwave/chirpEndIdx", std::stoi(token)); break;
case 3:
nh.setParam("/ti_mmwave/numLoops", std::stoi(token)); break;
case 4:
nh.setParam("/ti_mmwave/numFrames", std::stoi(token)); break;
case 5:
nh.setParam("/ti_mmwave/framePeriodicity", std::stof(token)); break;
}
}
} else req = token;
i++;
}
}
void ParameterParser::CalParams(ros::NodeHandle &nh) {
float c0 = 299792458;
int chirpStartIdx;
int chirpEndIdx;
int numLoops;
float framePeriodicity;
float startFreq;
float idleTime;
float adcStartTime;
float rampEndTime;
float digOutSampleRate;
float freqSlopeConst;
float numAdcSamples;
nh.getParam("/ti_mmwave/startFreq", startFreq);
nh.getParam("/ti_mmwave/idleTime", idleTime);
nh.getParam("/ti_mmwave/adcStartTime", adcStartTime);
nh.getParam("/ti_mmwave/rampEndTime", rampEndTime);
nh.getParam("/ti_mmwave/digOutSampleRate", digOutSampleRate);
nh.getParam("/ti_mmwave/freqSlopeConst", freqSlopeConst);
nh.getParam("/ti_mmwave/numAdcSamples", numAdcSamples);
nh.getParam("/ti_mmwave/chirpStartIdx", chirpStartIdx);
nh.getParam("/ti_mmwave/chirpEndIdx", chirpEndIdx);
nh.getParam("/ti_mmwave/numLoops", numLoops);
nh.getParam("/ti_mmwave/framePeriodicity", framePeriodicity);
int ntx = chirpEndIdx - chirpStartIdx + 1;
int nd = numLoops;
int nr = numAdcSamples;
float tfr = framePeriodicity * 1e-3;
float fs = digOutSampleRate * 1e3;
float kf = freqSlopeConst * 1e12;
float adc_duration = nr / fs;
float BW = adc_duration * kf;
float PRI = (idleTime + rampEndTime) * 1e-6;
float fc = startFreq * 1e9 + kf * (adcStartTime * 1e-6 + adc_duration / 2);
float vrange = c0 / (2 * BW);
float max_range = nr * vrange;
float max_vel = c0 / (2 * fc * PRI) / ntx;
float vvel = max_vel / nd;
nh.setParam("/ti_mmwave/num_TX", ntx);
nh.setParam("/ti_mmwave/f_s", fs);
nh.setParam("/ti_mmwave/f_c", fc);
nh.setParam("/ti_mmwave/BW", BW);
nh.setParam("/ti_mmwave/PRI", PRI);
nh.setParam("/ti_mmwave/t_fr", tfr);
nh.setParam("/ti_mmwave/max_range", max_range);
nh.setParam("/ti_mmwave/range_resolution", vrange);
nh.setParam("/ti_mmwave/max_doppler_vel", max_vel);
nh.setParam("/ti_mmwave/doppler_vel_resolution", vvel);
}
}

76
ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveCommSrv.cpp Normal file
View File

@@ -0,0 +1,76 @@
#include "mmWaveCommSrv.hpp"
namespace ti_mmwave_tracker_rospkg
{
PLUGINLIB_EXPORT_CLASS(ti_mmwave_tracker_rospkg::mmWaveCommSrv, nodelet::Nodelet);
mmWaveCommSrv::mmWaveCommSrv() {}
void mmWaveCommSrv::onInit()
{
ros::NodeHandle private_nh = getPrivateNodeHandle();
ros::NodeHandle private_nh2("~"); // follow namespace for multiple sensors
private_nh2.getParam("command_port", mySerialPort);
private_nh2.getParam("command_rate", myBaudRate);
private_nh2.getParam("mmWaveCLI_name", mmWaveCLIName);
ROS_INFO("mmWaveCommSrv: command_port = %s", mySerialPort.c_str());
ROS_INFO("mmWaveCommSrv: command_rate = %d", myBaudRate);
commSrv = private_nh.advertiseService(mmWaveCLIName, &mmWaveCommSrv::commSrv_cb, this);
NODELET_DEBUG("mmWaveCommsrv: Finished onInit function");
}
bool mmWaveCommSrv::commSrv_cb(mmWaveCLI::Request &req , mmWaveCLI::Response &res) {
NODELET_DEBUG("mmWaveCommSrv: Port is \"%s\" and baud rate is %d", mySerialPort.c_str(), myBaudRate);
/*Open Serial port and error check*/
serial::Serial mySerialObject("", myBaudRate, serial::Timeout::simpleTimeout(1000));
mySerialObject.setPort(mySerialPort.c_str());
try {
mySerialObject.open();
} catch (std::exception &e1) {
ROS_INFO("mmWaveCommSrv: Failed to open User serial port with error: %s", e1.what());
ROS_INFO("mmWaveCommSrv: Waiting 20 seconds before trying again...");
try {
// Wait 20 seconds and try to open serial port again
ros::Duration(20).sleep();
mySerialObject.open();
} catch (std::exception &e2) {
ROS_ERROR("mmWaveCommSrv: Failed second time to open User serial port, error: %s", e1.what());
NODELET_ERROR("mmWaveCommSrv: Port could not be opened. Port is \"%s\" and baud rate is %d", mySerialPort.c_str(), myBaudRate);
return false;
}
}
/*Read any previous pending response(s)*/
while (mySerialObject.available() > 0)
{
mySerialObject.readline(res.resp, 1024, ":/>");
ROS_INFO("mmWaveCommSrv: Received (previous) response from sensor: '%s'", res.resp.c_str());
res.resp = "";
}
/*Send out command received from the client*/
ROS_INFO("mmWaveCommSrv: Sending command to sensor: '%s'", req.comm.c_str());
req.comm.append("\n");
int bytesSent = mySerialObject.write(req.comm.c_str());
/*Read output from mmwDemo*/
mySerialObject.readline(res.resp, 1024, ":/>");
ROS_INFO("mmWaveCommSrv: Received response from sensor: '%s'", res.resp.c_str());
mySerialObject.close();
return true;
}
}

51
ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveDataHdl.cpp Normal file
View File

@@ -0,0 +1,51 @@
#include "mmWaveDataHdl.hpp"
#include "DataHandlerClass.h"
namespace ti_mmwave_tracker_rospkg
{
PLUGINLIB_EXPORT_CLASS(ti_mmwave_tracker_rospkg::mmWaveDataHdl, nodelet::Nodelet);
mmWaveDataHdl::mmWaveDataHdl() {}
void mmWaveDataHdl::onInit()
{
ros::NodeHandle private_nh("~");
std::string mySerialPort;
std::string myFrameID;
int myBaudRate;
int myMaxAllowedElevationAngleDeg;
int myMaxAllowedAzimuthAngleDeg;
private_nh.getParam("data_port", mySerialPort);
private_nh.getParam("data_rate", myBaudRate);
private_nh.getParam("frame_id", myFrameID);
if (!(private_nh.getParam("max_allowed_elevation_angle_deg", myMaxAllowedElevationAngleDeg))) {
myMaxAllowedElevationAngleDeg = 90; // Use max angle if none specified
}
if (!(private_nh.getParam("max_allowed_azimuth_angle_deg", myMaxAllowedAzimuthAngleDeg))) {
myMaxAllowedAzimuthAngleDeg = 90; // Use max angle if none specified
}
ROS_INFO("mmWaveDataHdl: data_port = %s", mySerialPort.c_str());
ROS_INFO("mmWaveDataHdl: data_rate = %d", myBaudRate);
ROS_INFO("mmWaveDataHdl: max_allowed_elevation_angle_deg = %d", myMaxAllowedElevationAngleDeg);
ROS_INFO("mmWaveDataHdl: max_allowed_azimuth_angle_deg = %d", myMaxAllowedAzimuthAngleDeg);
DataUARTHandler DataHandler(&private_nh);
DataHandler.setFrameID( (char*) myFrameID.c_str() );
DataHandler.setUARTPort( (char*) mySerialPort.c_str() );
DataHandler.setBaudRate( myBaudRate );
DataHandler.setMaxAllowedElevationAngleDeg( myMaxAllowedElevationAngleDeg );
DataHandler.setMaxAllowedAzimuthAngleDeg( myMaxAllowedAzimuthAngleDeg );
DataHandler.start();
NODELET_DEBUG("mmWaveDataHdl: Finished onInit function");
}
}

22
ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveLoader.cpp Normal file
View File

@@ -0,0 +1,22 @@
#include "ros/ros.h"
#include "nodelet/loader.h"
int main(int argc, char **argv)
{
ros::init(argc, argv, "mmWave_Manager");
nodelet::Loader manager(true);
nodelet::M_string remap(ros::names::getRemappings());
nodelet::V_string nargv;
manager.load("mmWaveCommSrv", "ti_mmwave_tracker_rospkg/mmWaveCommSrv", remap, nargv);
manager.load("mmWaveDataHdl", "ti_mmwave_tracker_rospkg/mmWaveDataHdl", remap, nargv);
ros::spin();
return 0;
}

66
ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveQuickConfig.cpp Normal file
View File

@@ -0,0 +1,66 @@
#include "ros/ros.h"
#include "ti_mmwave_tracker_rospkg/mmWaveCLI.h"
#include <cstdlib>
#include <fstream>
#include <stdio.h>
#include <regex>
#include "ParameterParser.h"
int main(int argc, char **argv) {
ros::init(argc, argv, "mmWaveQuickConfig");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
ti_mmwave_tracker_rospkg::mmWaveCLI srv;
if (argc != 2) {
ROS_INFO("mmWaveQuickConfig: usage: mmWaveQuickConfig /file_directory/params.cfg");
return 1;
} else
ROS_INFO("mmWaveQuickConfig: Configuring mmWave device using config file: %s", argv[1]);
std::string mmWaveCLIName;
private_nh.getParam("mmWaveCLI_name", mmWaveCLIName);
ros::ServiceClient client = nh.serviceClient<ti_mmwave_tracker_rospkg::mmWaveCLI>(mmWaveCLIName);
std::ifstream myParams;
ti_mmwave_tracker_rospkg::ParameterParser parser;
//wait for service to become available
ros::service::waitForService(mmWaveCLIName, 10000);
//wait 0.5 secs to avoid multi-sensor conflicts
ros::Duration(0.5).sleep();
myParams.open(argv[1]);
if (myParams.is_open()) {
while( std::getline(myParams, srv.request.comm)) {
// Remove Windows carriage-return if present
srv.request.comm.erase(std::remove(srv.request.comm.begin(), srv.request.comm.end(), '\r'), srv.request.comm.end());
// Ignore comment lines (first non-space char is '%') or blank lines
if (!(std::regex_match(srv.request.comm, std::regex("^\\s*%.*")) || std::regex_match(srv.request.comm, std::regex("^\\s*")))) {
// ROS_INFO("mmWaveQuickConfig: Sending command: '%s'", srv.request.comm.c_str() );
if (client.call(srv)) {
/*
if (std::regex_search(srv.response.resp, std::regex("Done"))) {
// ROS_INFO("mmWaveQuickConfig: Command successful (mmWave sensor responded with 'Done')");
parser.ParamsParser(srv, nh);
} else {
ROS_ERROR("mmWaveQuickConfig: Command failed (mmWave sensor did not respond with 'Done')");
ROS_ERROR("mmWaveQuickConfig: Response: '%s'", srv.response.resp.c_str() );
return 1;
}
*/
} else {
ROS_ERROR("mmWaveQuickConfig: Failed to call service mmWaveCLI");
ROS_ERROR("%s", srv.request.comm.c_str() );
return 1;
}
}
}
parser.CalParams(nh);
myParams.close();
} else {
ROS_ERROR("mmWaveQuickConfig: Failed to open File %s", argv[1]);
return 1;
}
ROS_INFO("mmWaveQuickConfig: mmWaveQuickConfig will now terminate. Done configuring mmWave device using config file: %s", argv[1]);
return 0;
}

3
ros_driver/src/ti_mmwave_tracker_rospkg/srv/mmWaveCLI.srv Normal file
View File

@@ -0,0 +1,3 @@
string comm
---
string resp