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

Merge pull request #4 in MITL/mmwave_ti_ros from JD-J7-robo-sdk to master

Browse Source 
* commit '5f4426fe55bc746a09f584fcb92a99b1a6e9a76c':
  Parametrized CLI service name
  updated pcl message header to include timestamp
  Added launch files for visualization on remote PC
  Renamed launch file. Fixed default values
  Improved escalating to sigterm fix
  Fixed terminating with ctrl+c causing escalating to SIGTERM.
  Modified launch file for radar driver on J7. Added launch file for multiple radar case
  Added support for running node on ROS Noetic on TDA4VM
This commit is contained in:
Josh Dye
2022-03-24 14:09:53 -05:00
parent dd5be20aeb 5f4426fe55
commit 980c8697ff
30 changed files with 506 additions and 37 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -1,6 +1,9 @@
cmake_minimum_required(VERSION 2.8.3)
project(ti_mmwave_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)
@@ -207,7 +210,6 @@ add_dependencies(mmwave ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TA
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
set (CMAKE_CXX_STANDARD 11)
add_executable(${PROJECT_NAME} src/mmWaveLoader.cpp)
target_link_libraries(mmwave ${serial_LIBRARIES} ${PCL_LIBRARIES})

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

@@ -19,6 +19,7 @@
#include <pcl/point_types.h>
#include <visualization_msgs/Marker.h>
#include <cmath>
#include <signal.h>
#define COUNT_SYNC_MAX 2
class DataUARTHandler{
@@ -48,13 +49,19 @@ public:
/*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;
@@ -90,6 +97,9 @@ private:
/*Mutex protected variable which synchronizes threads*/
int countSync;
/*Boolean used to notify threads to exit*/
bool stop_threads;
/*Read/Write Buffers*/
std::vector<uint8_t> pingPongBuffers[2];

2
ros_driver/src/ti_mmwave_rospkg/include/mmWaveCommSrv.hpp
View File

@@ -76,6 +76,8 @@ class mmWaveCommSrv : public nodelet::Nodelet
std::string mySerialPort;
int myBaudRate;
std::string mmWaveCLIName;
}; //Class mmWaveCommSrv
} //namespace ti_mmwave_rospkg

5
ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_0.launch
View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_2d_1.launch
View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_0.launch
View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/1443_multi_3d_1.launch
View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1443_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

7
ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_0.launch
View File

@@ -19,12 +19,15 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>
<node pkg="rviz" type="rviz" name="rviz" args="-d $(find ti_mmwave_rospkg)/launch/rviz/ti_mmwave_multi.rviz"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/1642_multi_2d_1.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1642_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 -1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_0.launch
View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_2d_1.launch
View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_0.launch
View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/1843_multi_3d_1.launch
View File

@@ -15,11 +15,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/1843_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 1 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_0.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_2d_1.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_0.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/6843AOP_multi_3d_1.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843AOP_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

40
ros_driver/src/ti_mmwave_rospkg/launch/6843_3d_TDA4VM.launch Normal file
View File

@@ -0,0 +1,40 @@
<!--
This file will launch the mmWave sensor node and configure a TI mmWave 6843 sensor using a 3D config. This launch file is intended
to be used to launch the mmWave sensor node on TI's TDA4VM Jacinto Processors, as such a separate command must be used to launch rViz
for visualization on a PC.
-->
<launch>
<!-- Input arguments -->
<arg name="device_type" value="6843" doc="TI mmWave sensor device type [1443, 1642, 6843]"/>
<arg name="config" value="3d" doc="TI mmWave sensor device configuration [3d_best_range_res (not supported by 1642 EVM), 2d_best_range_res]"/>
<arg name="max_allowed_elevation_angle_deg" default="90" doc="Maximum allowed elevation angle in degrees for detected object data [0 > value >= 90]}"/>
<arg name="max_allowed_azimuth_angle_deg" default="90" doc="Maximum allowed azimuth angle in degrees for detected object data [0 > value >= 90]}"/>
<arg name="device_num" default="0"/>
<arg name="com_user" default="/dev/ttyUSB0"/>
<arg name="com_data" default="/dev/ttyUSB1"/>
<arg name="node_start_delay" default="0"/>
<arg name="tf_args" default="0 0 0 0 0 0" doc="Values to be used for tf transformation"/>
<!-- mmWave_Manager node -->
<node pkg="ti_mmwave_rospkg" type="ti_mmwave_rospkg" name="ti_mmwave" ns="radar_$(arg device_num)" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'">
<param name="command_port" value="$(arg com_user)" />
<param name="command_rate" value="115200" />
<param name="data_port" value="$(arg com_data)" />
<param name="data_rate" value="921600" />
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="ti_mmwave_$(arg device_num)"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI_$(arg device_num)" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_$(arg device_num)"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_$(arg device_num)" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'">
<param name="mmWaveCLI_name" value="/mmWaveCLI_$(arg device_num)" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_$(arg device_num)" args="$(arg tf_args) ti_mmwave_pcl ti_mmwave_$(arg device_num) 100" launch-prefix="bash -c 'sleep $(arg node_start_delay); $0 $@'"/>
</launch>

5
ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_0.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_2d_1.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_2d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_0.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_0" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

5
ros_driver/src/ti_mmwave_rospkg/launch/6843_multi_3d_1.launch
View File

@@ -19,11 +19,14 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_1"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_1"/>
</node>
<!-- mmWaveQuickConfig node (terminates after configuring mmWave sensor) -->
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="ti_mmwave_config" ns="radar_1" args="$(find ti_mmwave_rospkg)/cfg/6843_3d.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink_1" args="5 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_1 100"/>

40
ros_driver/src/ti_mmwave_rospkg/launch/6843_quad_3d_TDA4VM.launch Normal file
View File

@@ -0,0 +1,40 @@
<!--
This file will launch four instances of the mmWave sensor node and configure each TI mmWave 6843 sensor using a 3D config. This launch file is intended
to be used to launch the mmWave sensor node on TI's TDA4VM Jacinto Processors, as such a separate command must be used to launch rViz
for visualization on a PC.
-->
<launch>
<include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
<arg name="device_num" value="0" />
<arg name="tf_args" value="0 0 0 0 0 0" />
<arg name="com_user" value="/dev/ttyUSB0"/>
<arg name="com_data" value="/dev/ttyUSB1"/>
</include>
<include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
<arg name="device_num" value="1" />
<arg name="tf_args" value="0 0 0 0 0 0" />
<arg name="com_user" value="/dev/ttyUSB2"/>
<arg name="com_data" value="/dev/ttyUSB3"/>
<arg name="node_start_delay" value="2"/>
</include>
<include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
<arg name="device_num" value="2" />
<arg name="tf_args" value="0 0 0 0 0 0" />
<arg name="com_user" value="/dev/ttyUSB4"/>
<arg name="com_data" value="/dev/ttyUSB5"/>
<arg name="node_start_delay" value="4"/>
</include>
<include file="$(find ti_mmwave_rospkg)/launch/6843_3d_TDA4VM.launch">
<arg name="device_num" value="3" />
<arg name="tf_args" value="0 0 0 0 0 0" />
<arg name="com_user" value="/dev/ttyUSB6"/>
<arg name="com_data" value="/dev/ttyUSB7"/>
<arg name="node_start_delay" value="6"/>
</include>
</launch>

5
ros_driver/src/ti_mmwave_rospkg/launch/camera_overlay.launch
View File

@@ -25,10 +25,13 @@
<param name="max_allowed_elevation_angle_deg" value="$(arg max_allowed_elevation_angle_deg)" />
<param name="max_allowed_azimuth_angle_deg" value="$(arg max_allowed_azimuth_angle_deg)" />
<param name="frame_id" value="/ti_mmwave_0"/>
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
<remap from="/ti_mmwave/radar_scan_pcl" to="/ti_mmwave/radar_scan_pcl_0"/>
</node>
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="mmWaveQuickConfig" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642es2_short_range.cfg" output="screen" />
<node pkg="ti_mmwave_rospkg" type="mmWaveQuickConfig" name="mmWaveQuickConfig" ns="radar_0" args="$(find ti_mmwave_rospkg)/cfg/1642es2_short_range.cfg" output="screen" >
<param name="mmWaveCLI_name" value="/mmWaveCLI" />
</node>
<node pkg="tf" type="static_transform_publisher" name="radar_baselink" args="0 0 0 0 0 0 ti_mmwave_pcl ti_mmwave_0 100"/>

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

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

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

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

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

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

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

@@ -7,6 +7,19 @@
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
DataUARTHandler* gDataHandlerPtr;
void DataUARTHandler::sigHandler(int32_t sig)
{
switch(sig)
{
case SIGINT:
gDataHandlerPtr->stop();
}
}
struct mmWaveCloudType
{
PCL_ADD_POINT4D;
@@ -54,6 +67,10 @@ DataUARTHandler::DataUARTHandler(ros::NodeHandle* nh) : currentBufp(&pingPongBuf
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)
@@ -228,8 +245,16 @@ void *DataUARTHandler::syncedBufferSwap(void)
while(countSync < COUNT_SYNC_MAX)
{
if(stop_threads)
{
pthread_mutex_unlock(&countSync_mutex);
pthread_cond_signal(&sort_go_cv);
pthread_cond_signal(&read_go_cv);
pthread_exit(NULL);
}
pthread_cond_wait(&countSync_max_cv, &countSync_mutex);
pthread_mutex_lock(&currentBufp_mutex);
pthread_mutex_lock(&nextBufp_mutex);
@@ -282,7 +307,7 @@ void *DataUARTHandler::sortIncomingData( void )
while(ros::ok())
{
switch(sorterState)
{
@@ -381,6 +406,7 @@ void *DataUARTHandler::sortIncomingData( void )
// RScan->header.seq = 0;
// RScan->header.stamp = (uint64_t)(ros::Time::now());
// RScan->header.stamp = (uint32_t) mmwData.header.timeCpuCycles;
pcl_conversions::toPCL(ros::Time::now(), RScan->header.stamp);
RScan->header.frame_id = frameID;
RScan->height = 1;
RScan->width = mmwData.numObjOut;
@@ -671,9 +697,8 @@ void *DataUARTHandler::sortIncomingData( void )
//ROS_INFO("mmwData.numObjOut after = %d", mmwData.numObjOut);
//ROS_INFO("DataUARTHandler Sort Thread: number of obj = %d", mmwData.numObjOut );
DataUARTHandler_pub.publish(RScan);
}
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;
@@ -782,10 +807,18 @@ void *DataUARTHandler::sortIncomingData( void )
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);
@@ -817,24 +850,29 @@ void DataUARTHandler::start(void)
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(iret1, NULL);
pthread_join(uartThread, NULL);
ROS_INFO("DataUARTHandler Read Thread joined");
pthread_join(iret2, NULL);
pthread_join(sorterThread, NULL);
ROS_INFO("DataUARTHandler Sort Thread joined");
pthread_join(iret3, NULL);
pthread_join(swapThread, NULL);
ROS_INFO("DataUARTHandler Swap Thread joined");
pthread_mutex_destroy(&countSync_mutex);
pthread_mutex_destroy(&nextBufp_mutex);
pthread_mutex_destroy(&currentBufp_mutex);
pthread_cond_destroy(&countSync_max_cv);
pthread_cond_destroy(&read_go_cv);
pthread_cond_destroy(&sort_go_cv);
}
void* DataUARTHandler::readIncomingData_helper(void *context)
@@ -881,4 +919,16 @@ void DataUARTHandler::visualize(const ti_mmwave_rospkg::RadarScan &msg){
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);
}

3
ros_driver/src/ti_mmwave_rospkg/src/mmWaveCommSrv.cpp
View File

@@ -15,11 +15,12 @@ void mmWaveCommSrv::onInit()
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("/mmWaveCLI", &mmWaveCommSrv::commSrv_cb, this);
commSrv = private_nh.advertiseService(mmWaveCLIName, &mmWaveCommSrv::commSrv_cb, this);
NODELET_DEBUG("mmWaveCommsrv: Finished onInit function");
}

7
ros_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp
View File

@@ -9,6 +9,7 @@
int main(int argc, char **argv) {
ros::init(argc, argv, "mmWaveQuickConfig");
ros::NodeHandle nh;
ros::NodeHandle private_nh("~");
ti_mmwave_rospkg::mmWaveCLI srv;
if (argc != 2) {
ROS_INFO("mmWaveQuickConfig: usage: mmWaveQuickConfig /file_directory/params.cfg");
@@ -16,11 +17,13 @@ int main(int argc, char **argv) {
} else
ROS_INFO("mmWaveQuickConfig: Configuring mmWave device using config file: %s", argv[1]);
ros::ServiceClient client = nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>("/mmWaveCLI");
std::string mmWaveCLIName;
private_nh.getParam("mmWaveCLI_name", mmWaveCLIName);
ros::ServiceClient client = nh.serviceClient<ti_mmwave_rospkg::mmWaveCLI>(mmWaveCLIName);
std::ifstream myParams;
ti_mmwave_rospkg::ParameterParser parser;
//wait for service to become available
ros::service::waitForService("/mmWaveCLI", 10000);
ros::service::waitForService(mmWaveCLIName, 10000);
//wait 0.5 secs to avoid multi-sensor conflicts
ros::Duration(0.5).sleep();