diff --git a/autonomous_robotics_ros/src/kobuki_msgs b/autonomous_robotics_ros/src/kobuki_msgs deleted file mode 160000 index ec50979..0000000 --- a/autonomous_robotics_ros/src/kobuki_msgs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit ec509794beaf2f7b5e8dea971054386bfee817f8 diff --git a/autonomous_robotics_ros/src/serial b/autonomous_robotics_ros/src/serial deleted file mode 160000 index cbcca7c..0000000 --- a/autonomous_robotics_ros/src/serial +++ /dev/null @@ -1 +0,0 @@ -Subproject commit cbcca7c83745fedd75afb7a0a27ee5c4112435c2 diff --git a/autonomous_robotics_ros/src/turtlebot_msgs b/autonomous_robotics_ros/src/turtlebot_msgs deleted file mode 160000 index 834c448..0000000 --- a/autonomous_robotics_ros/src/turtlebot_msgs +++ /dev/null @@ -1 +0,0 @@ -Subproject commit 834c448fb313ea7edb834d5552211daf53823395 diff --git a/autonomous_robotics_ros/src/turtlebot_simulator b/autonomous_robotics_ros/src/turtlebot_simulator deleted file mode 160000 index beae4b3..0000000 --- a/autonomous_robotics_ros/src/turtlebot_simulator +++ /dev/null @@ -1 +0,0 @@ -Subproject commit beae4b3cdf806fe82ab75fd933189c67e89ed8b0 diff --git a/ros_driver/README.txt b/ros_driver/README.txt new file mode 100644 index 0000000..db25f26 --- /dev/null +++ b/ros_driver/README.txt @@ -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 + + -------------------------------------------------------------------------------------------------------------------------------------------------- diff --git a/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt b/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt index f8b609a..52cf578 100644 --- a/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt +++ b/ros_driver/src/ti_mmwave_rospkg/CMakeLists.txt @@ -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 ## ############# diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/README.txt b/ros_driver/src/ti_mmwave_rospkg/bin/README.txt new file mode 100644 index 0000000..2398120 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/bin/README.txt @@ -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\\packages\ti\demo\\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 + +------------------------------------------------------------------------------- + + diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/small_obstacle_detection_68xx.bin b/ros_driver/src/ti_mmwave_rospkg/bin/small_obstacle_detection_68xx.bin new file mode 100644 index 0000000..496f174 Binary files /dev/null and b/ros_driver/src/ti_mmwave_rospkg/bin/small_obstacle_detection_68xx.bin differ diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/xwr14xx_mmw_demo.bin b/ros_driver/src/ti_mmwave_rospkg/bin/xwr14xx_mmw_demo.bin new file mode 100644 index 0000000..018d62e Binary files /dev/null and b/ros_driver/src/ti_mmwave_rospkg/bin/xwr14xx_mmw_demo.bin differ diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/xwr16xx_mmw_demo.bin b/ros_driver/src/ti_mmwave_rospkg/bin/xwr16xx_mmw_demo.bin new file mode 100644 index 0000000..9c6f2d6 Binary files /dev/null and b/ros_driver/src/ti_mmwave_rospkg/bin/xwr16xx_mmw_demo.bin differ diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/xwr18xx_mmw_demo.bin b/ros_driver/src/ti_mmwave_rospkg/bin/xwr18xx_mmw_demo.bin new file mode 100644 index 0000000..6e3d75c Binary files /dev/null and b/ros_driver/src/ti_mmwave_rospkg/bin/xwr18xx_mmw_demo.bin differ diff --git a/ros_driver/src/ti_mmwave_rospkg/bin/xwr64xxAOP_mmw_demo.bin b/ros_driver/src/ti_mmwave_rospkg/bin/xwr64xxAOP_mmw_demo.bin new file mode 100644 index 0000000..cafa69d Binary files /dev/null and b/ros_driver/src/ti_mmwave_rospkg/bin/xwr64xxAOP_mmw_demo.bin differ diff --git a/ros_driver/src/ti_mmwave_rospkg/cfg/6843ISK_Occupancy.cfg b/ros_driver/src/ti_mmwave_rospkg/cfg/6843ISK_Occupancy.cfg new file mode 100644 index 0000000..3e8a769 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/cfg/6843ISK_Occupancy.cfg @@ -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 diff --git a/ros_driver/src/ti_mmwave_rospkg/cfg/ISK_3d_Tracking.cfg b/ros_driver/src/ti_mmwave_rospkg/cfg/ISK_3d_Tracking.cfg new file mode 100644 index 0000000..1b3d05d --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/cfg/ISK_3d_Tracking.cfg @@ -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 \ No newline at end of file diff --git a/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h b/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h index ee83232..45c5d14 100644 --- a/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h +++ b/ros_driver/src/ti_mmwave_rospkg/include/DataHandlerClass.h @@ -2,6 +2,7 @@ #define _DATA_HANDLER_CLASS_ #include +#include #include "mmWave.h" #include #include @@ -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 diff --git a/ros_driver/src/ti_mmwave_rospkg/include/mmWave.h b/ros_driver/src/ti_mmwave_rospkg/include/mmWave.h index acc8e91..3926258 100644 --- a/ros_driver/src/ti_mmwave_rospkg/include/mmWave.h +++ b/ros_driver/src/ti_mmwave_rospkg/include/mmWave.h @@ -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}; diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.launch b/ros_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.launch new file mode 100644 index 0000000..3cb7bf9 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.launch @@ -0,0 +1,34 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_driver/src/ti_mmwave_rospkg/launch/ISK_3d_Tracking.launch b/ros_driver/src/ti_mmwave_rospkg/launch/ISK_3d_Tracking.launch new file mode 100644 index 0000000..458623e --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/launch/ISK_3d_Tracking.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_driver/src/ti_mmwave_rospkg/msg/RadarOccupancy.msg b/ros_driver/src/ti_mmwave_rospkg/msg/RadarOccupancy.msg new file mode 100644 index 0000000..f933bb2 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/msg/RadarOccupancy.msg @@ -0,0 +1,2 @@ +Header header +uint32 state \ No newline at end of file diff --git a/ros_driver/src/ti_mmwave_rospkg/msg/RadarPointTrackID.msg b/ros_driver/src/ti_mmwave_rospkg/msg/RadarPointTrackID.msg new file mode 100644 index 0000000..3ca48b0 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/msg/RadarPointTrackID.msg @@ -0,0 +1,2 @@ +Header header +uint8 tid diff --git a/ros_driver/src/ti_mmwave_rospkg/msg/RadarTrackArray.msg b/ros_driver/src/ti_mmwave_rospkg/msg/RadarTrackArray.msg new file mode 100644 index 0000000..a991e42 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/msg/RadarTrackArray.msg @@ -0,0 +1,3 @@ +Header header +uint32 num_tracks +RadarTrackContents[] track diff --git a/ros_driver/src/ti_mmwave_rospkg/msg/RadarTrackContents.msg b/ros_driver/src/ti_mmwave_rospkg/msg/RadarTrackContents.msg new file mode 100644 index 0000000..acf4df3 --- /dev/null +++ b/ros_driver/src/ti_mmwave_rospkg/msg/RadarTrackContents.msg @@ -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 diff --git a/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp b/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp index f89fac4..4420d0d 100644 --- a/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp +++ b/ros_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp @@ -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("/ti_mmwave/radar_scan_pcl", 100); radar_scan_pub = nh->advertise("/ti_mmwave/radar_scan", 100); + radar_occupancy_pub = nh->advertise("/ti_mmwave/radar_occupancy", 100); marker_pub = nh->advertise("/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> RScan(new pcl::PointCloud); boost::shared_ptr> RScan(new pcl::PointCloud); 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(¤tBufp_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, ¤tBufp->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, ¤tBufp->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, ¤tBufp->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); -} \ No newline at end of file +} diff --git a/ros_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp b/ros_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp index 6ff657a..5f4bfc5 100644 --- a/ros_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp +++ b/ros_driver/src/ti_mmwave_rospkg/src/ParameterParser.cpp @@ -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); -} -} \ No newline at end of file +} +} diff --git a/ros_driver/src/ti_mmwave_rospkg/src/mmWaveSync.cpp b/ros_driver/src/ti_mmwave_rospkg/src/mmWaveSync.cpp index c5e1193..8d352da 100644 --- a/ros_driver/src/ti_mmwave_rospkg/src/mmWaveSync.cpp +++ b/ros_driver/src/ti_mmwave_rospkg/src/mmWaveSync.cpp @@ -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; -} \ No newline at end of file +} diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/CMakeLists.txt b/ros_driver/src/ti_mmwave_tracker_rospkg/CMakeLists.txt new file mode 100644 index 0000000..2d2264d --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/CMakeLists.txt @@ -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) diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/CMakeLists.txt~ b/ros_driver/src/ti_mmwave_tracker_rospkg/CMakeLists.txt~ new file mode 100644 index 0000000..b276fd2 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/CMakeLists.txt~ @@ -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) diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/README.md b/ros_driver/src/ti_mmwave_tracker_rospkg/README.md new file mode 100644 index 0000000..10dc927 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/README.md @@ -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 `/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 ``: + +``` +catkin_make && source devel/setup.bash +echo "source /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 `` (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 `/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 +``` diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/bin/3D_people_count_68xx_demo.bin b/ros_driver/src/ti_mmwave_tracker_rospkg/bin/3D_people_count_68xx_demo.bin new file mode 100644 index 0000000..2dc1298 Binary files /dev/null and b/ros_driver/src/ti_mmwave_tracker_rospkg/bin/3D_people_count_68xx_demo.bin differ diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/bin/README.txt b/ros_driver/src/ti_mmwave_tracker_rospkg/bin/README.txt new file mode 100644 index 0000000..2398120 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/bin/README.txt @@ -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\\packages\ti\demo\\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 + +------------------------------------------------------------------------------- + + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/cfg/AOP_3d_Tracking.cfg b/ros_driver/src/ti_mmwave_tracker_rospkg/cfg/AOP_3d_Tracking.cfg new file mode 100644 index 0000000..8f04020 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/cfg/AOP_3d_Tracking.cfg @@ -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 \ No newline at end of file diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/include/DataHandlerClass.h b/ros_driver/src/ti_mmwave_tracker_rospkg/include/DataHandlerClass.h new file mode 100644 index 0000000..c54bc5e --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/include/DataHandlerClass.h @@ -0,0 +1,157 @@ + #ifndef _DATA_HANDLER_CLASS_ +#define _DATA_HANDLER_CLASS_ + +#include +#include +#include +#include +#include "mmWave.h" +#include +#include +#include +#include +#include "ros/ros.h" +#include "sensor_msgs/PointCloud2.h" +#include +#include +#include "pcl_ros/point_cloud.h" +#include "sensor_msgs/PointField.h" +#include "sensor_msgs/PointCloud2.h" +#include "sensor_msgs/point_cloud2_iterator.h" +#include +#include +#include +#include +#include +#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 pingPongBuffers[2]; + + /*Pointer to current data (sort)*/ + std::vector* currentBufp; + + /*Pointer to new data (read)*/ + std::vector* 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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/include/ParameterParser.h b/ros_driver/src/ti_mmwave_tracker_rospkg/include/ParameterParser.h new file mode 100644 index 0000000..3e1e9d5 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/include/ParameterParser.h @@ -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 +#include +#include +#include +#include +#include +#include +#include +#include + +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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWave.h b/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWave.h new file mode 100644 index 0000000..2f43f9c --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWave.h @@ -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 +#include +#include +#include "serial/serial.h" +#include "ros/ros.h" +#include +#include + +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 + + + + + + + + + + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWaveCommSrv.hpp b/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWaveCommSrv.hpp new file mode 100644 index 0000000..34e050d --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWaveCommSrv.hpp @@ -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 +#include + +/*Include standard C/C++ headers*/ +#include +#include +#include + +/*mmWave Driver Headers*/ +#include + +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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWaveDataHdl.hpp b/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWaveDataHdl.hpp new file mode 100644 index 0000000..0325854 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/include/mmWaveDataHdl.hpp @@ -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 +#include + +/*Include standard C/C++ headers*/ +#include +#include +#include + +/*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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/AOP_3d_Tracking.launch b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/AOP_3d_Tracking.launch new file mode 100644 index 0000000..fcc7aae --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/AOP_3d_Tracking.launch @@ -0,0 +1,35 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/camera_overlay.launch b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/camera_overlay.launch new file mode 100644 index 0000000..accf60c --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/camera_overlay.launch @@ -0,0 +1,43 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rosbag_show.launch b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rosbag_show.launch new file mode 100644 index 0000000..72561dc --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rosbag_show.launch @@ -0,0 +1,15 @@ + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave.rviz b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave.rviz new file mode 100644 index 0000000..a9f5de9 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave.rviz @@ -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: + 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: + 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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_3d.rviz b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_3d.rviz new file mode 100644 index 0000000..7627540 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_3d.rviz @@ -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: + 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: + 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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_camera_overlay.rviz b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_camera_overlay.rviz new file mode 100644 index 0000000..b090d04 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_camera_overlay.rviz @@ -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: + 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: + 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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_multi.rviz b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_multi.rviz new file mode 100644 index 0000000..bdea2ac --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_multi.rviz @@ -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: + 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: + 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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_quad.rviz b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_quad.rviz new file mode 100644 index 0000000..2206c0b --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/rviz/ti_mmwave_quad.rviz @@ -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: + 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: + 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: + 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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/viz_remote_pc.launch b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/viz_remote_pc.launch new file mode 100644 index 0000000..b33de40 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/viz_remote_pc.launch @@ -0,0 +1,10 @@ + + + + + + + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/launch/viz_remote_pc_quad.launch b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/viz_remote_pc_quad.launch new file mode 100644 index 0000000..0ebaa69 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/launch/viz_remote_pc_quad.launch @@ -0,0 +1,10 @@ + + + + + + + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/mmWave_nodelets.xml b/ros_driver/src/ti_mmwave_tracker_rospkg/mmWave_nodelets.xml new file mode 100644 index 0000000..2fe7370 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/mmWave_nodelets.xml @@ -0,0 +1,15 @@ + + + + Command Service Nodelet + + + + + + Data Handler Nodelet + + + + serial + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarPointTrackID.msg b/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarPointTrackID.msg new file mode 100644 index 0000000..3ca48b0 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarPointTrackID.msg @@ -0,0 +1,2 @@ +Header header +uint8 tid diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarScan.msg b/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarScan.msg new file mode 100644 index 0000000..e8506d0 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarScan.msg @@ -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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarTrackArray.msg b/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarTrackArray.msg new file mode 100644 index 0000000..a991e42 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarTrackArray.msg @@ -0,0 +1,3 @@ +Header header +uint32 num_tracks +RadarTrackContents[] track diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarTrackContents.msg b/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarTrackContents.msg new file mode 100644 index 0000000..acf4df3 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/msg/RadarTrackContents.msg @@ -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 diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/package.xml b/ros_driver/src/ti_mmwave_tracker_rospkg/package.xml new file mode 100644 index 0000000..8399a75 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/package.xml @@ -0,0 +1,60 @@ + + + ti_mmwave_tracker_rospkg + 3.3.0 + The ti_mmwave_tracker_rospkg package. Publish messages with doppler information. Multi-sensor and camera overlay support. Modified by Leo Zhang + + + + + Leo Zhang + Allison Wendell + + + + + BSD + + + + + + https://github.com/radar-lab/ti_mmwave_rospkg + https://bitbucket.itg.ti.com/scm/mmwave_apps/ros_multisensor_demo/ti_mmwave_rospkg + + + + + Leo Zhang + Allison Wendell + + + + + + + + + + + + + + catkin + nodelet + roscpp + serial + std_msgs + message_generation + nodelet + roscpp + serial + std_msgs + message_runtime + + + + + + + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/src/DataHandlerClass.cpp b/ros_driver/src/ti_mmwave_tracker_rospkg/src/DataHandlerClass.cpp new file mode 100644 index 0000000..02a0b40 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/src/DataHandlerClass.cpp @@ -0,0 +1,895 @@ +#include +#define PCL_NO_PRECOMPILE + +//#include +#include +#include +#include +#include + +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("/ti_mmwave/radar_scan_pcl", 100); + radar_scan_pub = nh->advertise("/ti_mmwave/radar_scan", 100); + radar_trackid_pub = nh->advertise("/ti_mmwave/radar_trackid", 100); + radar_trackarray_pub = nh->advertise("/ti_mmwave/radar_trackarray", 100); + marker_pub = nh->advertise("/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(¤tBufp_mutex); + pthread_mutex_lock(&nextBufp_mutex); + + std::vector* tempBufp = currentBufp; + + this->currentBufp = this->nextBufp; + + this->nextBufp = tempBufp; + + pthread_mutex_unlock(¤tBufp_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> RScan(new pcl::PointCloud); + boost::shared_ptr> RScan(new pcl::PointCloud); + 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(¤tBufp_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, ¤tBufp->at(currentDatap), sizeof(mmwData.header.version)); + currentDatap += ( sizeof(mmwData.header.version) ); + + //get totalPacketLen (4 bytes) + memcpy( &mmwData.header.totalPacketLen, ¤tBufp->at(currentDatap), sizeof(mmwData.header.totalPacketLen)); + currentDatap += ( sizeof(mmwData.header.totalPacketLen) ); + + //get platform (4 bytes) + memcpy( &mmwData.header.platform, ¤tBufp->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, ¤tBufp->at(currentDatap), sizeof(mmwData.header.frameNumber)); + currentDatap += ( sizeof(mmwData.header.frameNumber) ); + + //get timeCpuCycles (4 bytes) + memcpy( &mmwData.header.timeCpuCycles, ¤tBufp->at(currentDatap), sizeof(mmwData.header.timeCpuCycles)); + currentDatap += ( sizeof(mmwData.header.timeCpuCycles) ); + + //get numDetectedObj (4 bytes) + memcpy( &mmwData.header.numDetectedObj, ¤tBufp->at(currentDatap), sizeof(mmwData.header.numDetectedObj)); + currentDatap += ( sizeof(mmwData.header.numDetectedObj) ); + + //get numTLVs (4 bytes) + memcpy( &mmwData.header.numTLVs, ¤tBufp->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, ¤tBufp->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, ¤tBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.elevationUnit)); + currentDatap += ( sizeof(mmwData.newSphereCloudOut.elevationUnit) ); + + //get object Azimuth unit (rad) + memcpy( &mmwData.newSphereCloudOut.azimuthUnit, ¤tBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.azimuthUnit)); + currentDatap += ( sizeof(mmwData.newSphereCloudOut.azimuthUnit) ); + + //get object Doppler unit (m/s) + memcpy( &mmwData.newSphereCloudOut.dopplerUnit, ¤tBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.dopplerUnit)); + currentDatap += ( sizeof(mmwData.newSphereCloudOut.dopplerUnit) ); + + //get object Range unit (meters) + memcpy( &mmwData.newSphereCloudOut.rangeUnit, ¤tBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.rangeUnit)); + currentDatap += ( sizeof(mmwData.newSphereCloudOut.rangeUnit) ); + + //get object SNR unit (ratio) + memcpy( &mmwData.newSphereCloudOut.snrUnit, ¤tBufp->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, ¤tBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.elevation)); + currentDatap += ( sizeof(mmwData.newSphereCloudOut.elevation) ); + + //get Azimuth value to multiply by + memcpy( &mmwData.newSphereCloudOut.azimuth, ¤tBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.azimuth)); + currentDatap += ( sizeof(mmwData.newSphereCloudOut.azimuth) ); + + //get Doppler value to multiply by + memcpy( &mmwData.newSphereCloudOut.doppler, ¤tBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.doppler)); + currentDatap += ( sizeof(mmwData.newSphereCloudOut.doppler) ); + + //get Range value to multiply by + memcpy( &mmwData.newSphereCloudOut.range, ¤tBufp->at(currentDatap), sizeof(mmwData.newSphereCloudOut.range)); + currentDatap += ( sizeof(mmwData.newSphereCloudOut.range) ); + + //get SNR value to multiply by + memcpy( &mmwData.newSphereCloudOut.snr, ¤tBufp->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, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.tid)); + currentDatap += ( sizeof(mmwData.newListOut.tid) ); + + //get Track position in X dimension (m) + memcpy( &mmwData.newListOut.posX, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.posX)); + currentDatap += ( sizeof(mmwData.newListOut.posX) ); + + //get Track position in Y dimension (m) + memcpy( &mmwData.newListOut.posY, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.posY)); + currentDatap += ( sizeof(mmwData.newListOut.posY) ); + + //get Track position in Z dimension (m) + memcpy( &mmwData.newListOut.posZ, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.posZ)); + currentDatap += ( sizeof(mmwData.newListOut.posZ) ); + + //get Track velocity in X dimension (m) + memcpy( &mmwData.newListOut.velX, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.velX)); + currentDatap += ( sizeof(mmwData.newListOut.velX) ); + + //get Track velocity in Y dimension (m) + memcpy( &mmwData.newListOut.velY, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.velY)); + currentDatap += ( sizeof(mmwData.newListOut.velY) ); + + //get Track velocity in Z dimension (m) + memcpy( &mmwData.newListOut.velZ, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.velZ)); + currentDatap += ( sizeof(mmwData.newListOut.velZ) ); + + //get Track acceleration in X dimension (m) + memcpy( &mmwData.newListOut.accX, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.accX)); + currentDatap += ( sizeof(mmwData.newListOut.accX) ); + + //get Track acceleration in Y dimension (m) + memcpy( &mmwData.newListOut.accY, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.accY)); + currentDatap += ( sizeof(mmwData.newListOut.accY) ); + + //get Track acceleration in Z dimension (m) + memcpy( &mmwData.newListOut.accZ, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.accZ)); + currentDatap += ( sizeof(mmwData.newListOut.accZ) ); + + //Throw Away + memcpy( &mmwData.newListOut.ec, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.ec)); + currentDatap += ( sizeof(mmwData.newListOut.ec) ); + + //Throw Away + memcpy( &mmwData.newListOut.g, ¤tBufp->at(currentDatap), sizeof(mmwData.newListOut.g)); + currentDatap += ( sizeof(mmwData.newListOut.g) ); + + //Throw Away + memcpy( &mmwData.newListOut.confidenceLevel, ¤tBufp->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, ¤tBufp->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, ¤tBufp->at(currentDatap), sizeof(tlvType)); + currentDatap += ( sizeof(tlvType) ); + + + //get tlvLen (32 bits) & remove from queue + memcpy( &tlvLen, ¤tBufp->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(¤tBufp_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(¤tBufp_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(¤tBufp_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(¤tBufp_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(context)->readIncomingData()); +} + +void* DataUARTHandler::sortIncomingData_helper(void *context) +{ + return (static_cast(context)->sortIncomingData()); +} + +void* DataUARTHandler::syncedBufferSwap_helper(void *context) +{ + return (static_cast(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); +} diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/src/ParameterParser.cpp b/ros_driver/src/ti_mmwave_tracker_rospkg/src/ParameterParser.cpp new file mode 100644 index 0000000..8ba2f19 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/src/ParameterParser.cpp @@ -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 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); +} + +} \ No newline at end of file diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveCommSrv.cpp b/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveCommSrv.cpp new file mode 100644 index 0000000..e18b543 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveCommSrv.cpp @@ -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; +} + +} + + + diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveDataHdl.cpp b/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveDataHdl.cpp new file mode 100644 index 0000000..e2e8b8a --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveDataHdl.cpp @@ -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"); +} + +} \ No newline at end of file diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveLoader.cpp b/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveLoader.cpp new file mode 100644 index 0000000..6bd98b6 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveLoader.cpp @@ -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; +} diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveQuickConfig.cpp b/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveQuickConfig.cpp new file mode 100644 index 0000000..5403352 --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/src/mmWaveQuickConfig.cpp @@ -0,0 +1,66 @@ +#include "ros/ros.h" +#include "ti_mmwave_tracker_rospkg/mmWaveCLI.h" +#include +#include +#include +#include +#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(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; +} diff --git a/ros_driver/src/ti_mmwave_tracker_rospkg/srv/mmWaveCLI.srv b/ros_driver/src/ti_mmwave_tracker_rospkg/srv/mmWaveCLI.srv new file mode 100644 index 0000000..2aa1e1d --- /dev/null +++ b/ros_driver/src/ti_mmwave_tracker_rospkg/srv/mmWaveCLI.srv @@ -0,0 +1,3 @@ +string comm +--- +string resp