From e2311daf1afe0d040bd039f019a376ef2baa9f14 Mon Sep 17 00:00:00 2001 From: JuneChul Roh Date: Thu, 16 Nov 2023 14:26:42 -0600 Subject: [PATCH 1/2] [ros2_driver] Following updates made to ros2_driver/src/ti_mmwave_rospkg: 1. Updated CMakeLists.txt to install the "cfg" folder. 2. Path to .cfg file is now set using get_package_share_directory(). 3. Updated 6843*.py launch files to add a feature optionally disable rViz with an launch argument "rviz". To disable rViz, pass "rviz:=false" in the launch command. --- .../src/ti_mmwave_rospkg/CMakeLists.txt | 4 ++ .../ti_mmwave_rospkg/launch/1443_Standard.py | 13 ++-- .../ti_mmwave_rospkg/launch/1642_Standard.py | 13 ++-- .../ti_mmwave_rospkg/launch/1843_Standard.py | 13 ++-- .../ti_mmwave_rospkg/launch/6432_Standard.py | 12 ++-- .../launch/6843AOP_Standard.py | 66 +++++++++++-------- .../launch/6843AOP_Tracking.py | 66 +++++++++++-------- .../launch/6843ISK_Occupancy.py | 66 +++++++++++-------- .../launch/6843ISK_Standard.py | 66 +++++++++++-------- .../launch/6843ISK_Tracking.py | 66 +++++++++++-------- .../ti_mmwave_rospkg/launch/rviz_launch.py | 21 ++++++ 11 files changed, 244 insertions(+), 162 deletions(-) create mode 100644 ros2_driver/src/ti_mmwave_rospkg/launch/rviz_launch.py diff --git a/ros2_driver/src/ti_mmwave_rospkg/CMakeLists.txt b/ros2_driver/src/ti_mmwave_rospkg/CMakeLists.txt index 7e87dc9..4a3d906 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/CMakeLists.txt +++ b/ros2_driver/src/ti_mmwave_rospkg/CMakeLists.txt @@ -224,6 +224,10 @@ install(DIRECTORY config DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY + cfg + DESTINATION share/${PROJECT_NAME} +) install(TARGETS mmWaveCommSrv DESTINATION lib/${PROJECT_NAME} diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/1443_Standard.py b/ros2_driver/src/ti_mmwave_rospkg/launch/1443_Standard.py index 0568084..fd74409 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/1443_Standard.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/1443_Standard.py @@ -8,14 +8,14 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): # Enter Path and Name Here - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/1443_Standard.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','1443_Standard.cfg') device = "1443" name = "/mmWaveCLI" command_port = "/dev/ttyUSB0" command_rate = "115200" data_port = "/dev/ttyUSB1" data_rate = "921600" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') ld = LaunchDescription() ConfigParameters = os.path.join( @@ -49,7 +49,7 @@ def generate_launch_description(): {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -90,10 +90,9 @@ def generate_launch_description(): ) Rviz2 = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] ) ld.add_action(global_param_node) diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/1642_Standard.py b/ros2_driver/src/ti_mmwave_rospkg/launch/1642_Standard.py index ddaf314..3e6cf98 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/1642_Standard.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/1642_Standard.py @@ -8,14 +8,14 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): # Enter Path and Name Here - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/1642_Standard.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','1642_Standard.cfg') name = "/mmWaveCLI" device = "1642" command_port = "/dev/ttyUSB0" command_rate = "115200" data_port = "/dev/ttyUSB1" data_rate = "921600" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') ld = LaunchDescription() ConfigParameters = os.path.join( @@ -49,7 +49,7 @@ def generate_launch_description(): {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -90,10 +90,9 @@ def generate_launch_description(): ) Rviz2 = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] ) ld.add_action(global_param_node) diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/1843_Standard.py b/ros2_driver/src/ti_mmwave_rospkg/launch/1843_Standard.py index 3e21f4c..1732e38 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/1843_Standard.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/1843_Standard.py @@ -8,14 +8,14 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): # Enter Path and Name Here - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/1843_Standard.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','1843_Standard.cfg') device = "1843" name = "/mmWaveCLI" command_port = "/dev/ttyUSB0" command_rate = "115200" data_port = "/dev/ttyUSB1" data_rate = "921600" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') ld = LaunchDescription() ConfigParameters = os.path.join( @@ -49,7 +49,7 @@ def generate_launch_description(): {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -90,10 +90,9 @@ def generate_launch_description(): ) Rviz2 = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] ) ld.add_action(global_param_node) diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6432_Standard.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6432_Standard.py index 3c1f4dd..0586bdd 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6432_Standard.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6432_Standard.py @@ -11,14 +11,14 @@ from ament_index_python.packages import get_package_share_directory def generate_launch_description(): - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6432_Standard_Uncompressed.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','6432_Standard_Uncompressed.cfg') device = "6432" name = "/mmWaveCLI" command_port = "/dev/ttyACM0" command_rate = "115200" data_port = "/dev/ttyACM0" data_rate = "115200" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') ld = LaunchDescription() ConfigParameters = os.path.join( @@ -52,7 +52,7 @@ def generate_launch_description(): {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -79,7 +79,7 @@ def generate_launch_description(): ) delay = TimerAction( - period=5.0, + period=5.0, actions=[Node( package="ti_mmwave_rospkg", executable="DataHandlerClass", @@ -97,7 +97,7 @@ def generate_launch_description(): package='rviz2', executable='rviz2', arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + )] ) @@ -108,4 +108,4 @@ def generate_launch_description(): ld.add_action(delay) return ld - + diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Standard.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Standard.py index 99a9174..aac0af1 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Standard.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Standard.py @@ -1,23 +1,32 @@ import os -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch import LaunchDescription, conditions +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + # Declare the launch argument + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable RViz' + ) + + # Use the launch argument in the condition for adding the RViz node + rviz_enabled = LaunchConfiguration('rviz') + # Enter Path and Name Here - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Standard.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','6843AOP_Standard.cfg') device = "6843" name = "/mmWaveCLI" command_port = "/dev/ttyUSB0" command_rate = "115200" data_port = "/dev/ttyUSB1" data_rate = "921600" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') - ld = LaunchDescription() ConfigParameters = os.path.join( my_package_dir, 'config', @@ -32,24 +41,24 @@ def generate_launch_description(): ) mmWaveCommSrv = Node( - package="ti_mmwave_rospkg", - executable="mmWaveCommSrv", - name="mmWaveCommSrv", - output="screen", - emulate_tty=True, - parameters=[ - {"command_port": command_port}, - {"command_rate": command_rate}, - {"data_port": data_port}, - {"data_rate": data_rate}, - {"max_allowed_elevation_angle_deg": "90"}, - {"max_allowed_azimuth_angle_deg": "90"}, - {"frame_id": "/ti_mmwave_0"}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} + package="ti_mmwave_rospkg", + executable="mmWaveCommSrv", + name="mmWaveCommSrv", + output="screen", + emulate_tty=True, + parameters=[ + {"command_port": command_port}, + {"command_rate": command_rate}, + {"data_port": data_port}, + {"data_rate": data_rate}, + {"max_allowed_elevation_angle_deg": "90"}, + {"max_allowed_azimuth_angle_deg": "90"}, + {"frame_id": "/ti_mmwave_0"}, + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -89,13 +98,16 @@ def generate_launch_description(): ] ) + Rviz2 = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')], + condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true ) + ld = LaunchDescription() + ld.add_action(rviz_arg) ld.add_action(global_param_node) ld.add_action(mmWaveCommSrv) ld.add_action(mmWaveQuickConfig) @@ -103,4 +115,4 @@ def generate_launch_description(): ld.add_action(DataHandlerClass) ld.add_action(Rviz2) - return ld \ No newline at end of file + return ld diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Tracking.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Tracking.py index c194669..85feaf9 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Tracking.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Tracking.py @@ -1,23 +1,32 @@ import os -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch import LaunchDescription, conditions +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + # Declare the launch argument + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable RViz' + ) + + # Use the launch argument in the condition for adding the RViz node + rviz_enabled = LaunchConfiguration('rviz') + # Enter Path and Name Here - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Tracking.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','6843AOP_Tracking.cfg') device = "6843" name = "/mmWaveCLI" command_port = "/dev/ttyUSB0" command_rate = "115200" data_port = "/dev/ttyUSB1" data_rate = "921600" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') - ld = LaunchDescription() ConfigParameters = os.path.join( my_package_dir, 'config', @@ -32,24 +41,24 @@ def generate_launch_description(): ) mmWaveCommSrv = Node( - package="ti_mmwave_rospkg", - executable="mmWaveCommSrv", - name="mmWaveCommSrv", - output="screen", - emulate_tty=True, - parameters=[ - {"command_port": command_port}, - {"command_rate": command_rate}, - {"data_port": data_port}, - {"data_rate": data_rate}, - {"max_allowed_elevation_angle_deg": "90"}, - {"max_allowed_azimuth_angle_deg": "90"}, - {"frame_id": "/ti_mmwave_0"}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} + package="ti_mmwave_rospkg", + executable="mmWaveCommSrv", + name="mmWaveCommSrv", + output="screen", + emulate_tty=True, + parameters=[ + {"command_port": command_port}, + {"command_rate": command_rate}, + {"data_port": data_port}, + {"data_rate": data_rate}, + {"max_allowed_elevation_angle_deg": "90"}, + {"max_allowed_azimuth_angle_deg": "90"}, + {"frame_id": "/ti_mmwave_0"}, + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -89,13 +98,16 @@ def generate_launch_description(): ] ) + Rviz2 = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')], + condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true ) + ld = LaunchDescription() + ld.add_action(rviz_arg) ld.add_action(global_param_node) ld.add_action(mmWaveCommSrv) ld.add_action(mmWaveQuickConfig) @@ -103,4 +115,4 @@ def generate_launch_description(): ld.add_action(DataHandlerClass) ld.add_action(Rviz2) - return ld \ No newline at end of file + return ld diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.py index 889a940..8213389 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.py @@ -1,23 +1,32 @@ import os -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch import LaunchDescription, conditions +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + # Declare the launch argument + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable RViz' + ) + + # Use the launch argument in the condition for adding the RViz node + rviz_enabled = LaunchConfiguration('rviz') + # Enter Path and Name Here - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843ISK_Occupancy.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','6843ISK_Occupancy.cfg') device = "6843" name = "/mmWaveCLI" command_port = "/dev/ttyUSB0" command_rate = "115200" data_port = "/dev/ttyUSB1" data_rate = "921600" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') - ld = LaunchDescription() ConfigParameters = os.path.join( my_package_dir, 'config', @@ -32,24 +41,24 @@ def generate_launch_description(): ) mmWaveCommSrv = Node( - package="ti_mmwave_rospkg", - executable="mmWaveCommSrv", - name="mmWaveCommSrv", - output="screen", - emulate_tty=True, - parameters=[ - {"command_port": command_port}, - {"command_rate": command_rate}, - {"data_port": data_port}, - {"data_rate": data_rate}, - {"max_allowed_elevation_angle_deg": "90"}, - {"max_allowed_azimuth_angle_deg": "90"}, - {"frame_id": "/ti_mmwave_0"}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} + package="ti_mmwave_rospkg", + executable="mmWaveCommSrv", + name="mmWaveCommSrv", + output="screen", + emulate_tty=True, + parameters=[ + {"command_port": command_port}, + {"command_rate": command_rate}, + {"data_port": data_port}, + {"data_rate": data_rate}, + {"max_allowed_elevation_angle_deg": "90"}, + {"max_allowed_azimuth_angle_deg": "90"}, + {"frame_id": "/ti_mmwave_0"}, + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -89,13 +98,16 @@ def generate_launch_description(): ] ) + Rviz2 = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')], + condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true ) + ld = LaunchDescription() + ld.add_action(rviz_arg) ld.add_action(global_param_node) ld.add_action(mmWaveCommSrv) ld.add_action(mmWaveQuickConfig) @@ -103,4 +115,4 @@ def generate_launch_description(): ld.add_action(DataHandlerClass) ld.add_action(Rviz2) - return ld \ No newline at end of file + return ld diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Standard.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Standard.py index 71dd161..ee0d1cd 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Standard.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Standard.py @@ -1,23 +1,32 @@ import os -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch import LaunchDescription, conditions +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + # Declare the launch argument + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable RViz' + ) + + # Use the launch argument in the condition for adding the RViz node + rviz_enabled = LaunchConfiguration('rviz') + # Enter Path and Name Here - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843ISK_Standard.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','6843ISK_Standard.cfg') device = "6843" name = "/mmWaveCLI" command_port = "/dev/ttyUSB0" command_rate = "115200" data_port = "/dev/ttyUSB1" data_rate = "921600" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') - ld = LaunchDescription() ConfigParameters = os.path.join( my_package_dir, 'config', @@ -32,24 +41,24 @@ def generate_launch_description(): ) mmWaveCommSrv = Node( - package="ti_mmwave_rospkg", - executable="mmWaveCommSrv", - name="mmWaveCommSrv", - output="screen", - emulate_tty=True, - parameters=[ - {"command_port": command_port}, - {"command_rate": command_rate}, - {"data_port": data_port}, - {"data_rate": data_rate}, - {"max_allowed_elevation_angle_deg": "90"}, - {"max_allowed_azimuth_angle_deg": "90"}, - {"frame_id": "/ti_mmwave_0"}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} + package="ti_mmwave_rospkg", + executable="mmWaveCommSrv", + name="mmWaveCommSrv", + output="screen", + emulate_tty=True, + parameters=[ + {"command_port": command_port}, + {"command_rate": command_rate}, + {"data_port": data_port}, + {"data_rate": data_rate}, + {"max_allowed_elevation_angle_deg": "90"}, + {"max_allowed_azimuth_angle_deg": "90"}, + {"frame_id": "/ti_mmwave_0"}, + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -89,13 +98,16 @@ def generate_launch_description(): ] ) + Rviz2 = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')], + condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true ) + ld = LaunchDescription() + ld.add_action(rviz_arg) ld.add_action(global_param_node) ld.add_action(mmWaveCommSrv) ld.add_action(mmWaveQuickConfig) @@ -103,4 +115,4 @@ def generate_launch_description(): ld.add_action(DataHandlerClass) ld.add_action(Rviz2) - return ld \ No newline at end of file + return ld diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Tracking.py b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Tracking.py index c194669..85feaf9 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Tracking.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Tracking.py @@ -1,23 +1,32 @@ import os -from launch import LaunchDescription -from launch.substitutions import PathJoinSubstitution +from launch import LaunchDescription, conditions +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare from ament_index_python.packages import get_package_share_directory def generate_launch_description(): + # Declare the launch argument + rviz_arg = DeclareLaunchArgument( + 'rviz', + default_value='true', + description='Enable RViz' + ) + + # Use the launch argument in the condition for adding the RViz node + rviz_enabled = LaunchConfiguration('rviz') + # Enter Path and Name Here - path = "/home/rosi2/ros2_driver/src/ti_mmwave_rospkg/cfg/6843AOP_Tracking.cfg"; + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + path = os.path.join(my_package_dir,'cfg','6843AOP_Tracking.cfg') device = "6843" name = "/mmWaveCLI" command_port = "/dev/ttyUSB0" command_rate = "115200" data_port = "/dev/ttyUSB1" data_rate = "921600" - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') - ld = LaunchDescription() ConfigParameters = os.path.join( my_package_dir, 'config', @@ -32,24 +41,24 @@ def generate_launch_description(): ) mmWaveCommSrv = Node( - package="ti_mmwave_rospkg", - executable="mmWaveCommSrv", - name="mmWaveCommSrv", - output="screen", - emulate_tty=True, - parameters=[ - {"command_port": command_port}, - {"command_rate": command_rate}, - {"data_port": data_port}, - {"data_rate": data_rate}, - {"max_allowed_elevation_angle_deg": "90"}, - {"max_allowed_azimuth_angle_deg": "90"}, - {"frame_id": "/ti_mmwave_0"}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} + package="ti_mmwave_rospkg", + executable="mmWaveCommSrv", + name="mmWaveCommSrv", + output="screen", + emulate_tty=True, + parameters=[ + {"command_port": command_port}, + {"command_rate": command_rate}, + {"data_port": data_port}, + {"data_rate": data_rate}, + {"max_allowed_elevation_angle_deg": "90"}, + {"max_allowed_azimuth_angle_deg": "90"}, + {"frame_id": "/ti_mmwave_0"}, + {"mmwavecli_name": name}, + {"mmwavecli_cfg": path} ] ) - + mmWaveQuickConfig = Node( package="ti_mmwave_rospkg", executable="mmWaveQuickConfig", @@ -89,13 +98,16 @@ def generate_launch_description(): ] ) + Rviz2 = Node( - package='rviz2', - executable='rviz2', - arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] - + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')], + condition=conditions.IfCondition(rviz_enabled) # only launch RViz if the argument is true ) + ld = LaunchDescription() + ld.add_action(rviz_arg) ld.add_action(global_param_node) ld.add_action(mmWaveCommSrv) ld.add_action(mmWaveQuickConfig) @@ -103,4 +115,4 @@ def generate_launch_description(): ld.add_action(DataHandlerClass) ld.add_action(Rviz2) - return ld \ No newline at end of file + return ld diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/rviz_launch.py b/ros2_driver/src/ti_mmwave_rospkg/launch/rviz_launch.py new file mode 100644 index 0000000..138131c --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/rviz_launch.py @@ -0,0 +1,21 @@ +import os +from launch import LaunchDescription +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory + +def generate_launch_description(): + + # Enter Path and Name Here + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + + Rviz2 = Node( + package='rviz2', + executable='rviz2', + arguments=['-d', os.path.join(my_package_dir, 'launch', 'rviz.rviz')] + ) + + ld = LaunchDescription([ + Rviz2 + ]) + + return ld \ No newline at end of file From 1ffa862e94e72aad43a1701e7f0b7c30384d3237 Mon Sep 17 00:00:00 2001 From: JuneChul Roh Date: Fri, 17 Nov 2023 15:04:41 -0600 Subject: [PATCH 2/2] [ros2_driver] Updated to get rid of error message at Ctrl+C termination --- .../ti_mmwave_rospkg/src/DataHandlerClass.cpp | 141 +++++++++--------- .../src/mmWaveQuickConfig.cpp | 23 ++- 2 files changed, 82 insertions(+), 82 deletions(-) diff --git a/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp b/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp index 5d80736..fab762c 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp +++ b/ros2_driver/src/ti_mmwave_rospkg/src/DataHandlerClass.cpp @@ -10,7 +10,6 @@ #include using namespace std::chrono_literals; -std::shared_ptr node = nullptr; rclcpp::Publisher::SharedPtr DataUARTHandler_pub; rclcpp::Publisher::SharedPtr radar_scan_pub; rclcpp::Publisher::SharedPtr radar_occupancy_pub; @@ -38,7 +37,9 @@ void DataUARTHandler::sigHandler(int32_t sig) gDataHandlerPtr->stop(); } - + + rclcpp::shutdown(); + exit(0); } struct mmWaveCloudType @@ -63,7 +64,7 @@ POINT_CLOUD_REGISTER_POINT_STRUCT (mmWaveCloudType, (float, intensity, intensity) (float, velocity, velocity)) -DataUARTHandler::DataUARTHandler(std::shared_ptr nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) +DataUARTHandler::DataUARTHandler(std::shared_ptr nh) : currentBufp(&pingPongBuffers[0]) , nextBufp(&pingPongBuffers[1]) { DataUARTHandler_pub = nh->create_publisher("/ti_mmwave/radar_scan_pcl", 100); radar_scan_pub = nh->create_publisher("/ti_mmwave/radar_scan", 100); @@ -111,7 +112,7 @@ 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(10000)); mySerialObject.setPort(dataSerialPort); @@ -120,7 +121,7 @@ void *DataUARTHandler::readIncomingData(void) try { mySerialObject.open(); - } + } catch (std::exception &e1) { RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Read Thread: Failed to open Data serial port with error: %s", e1.what()); RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Read Thread: Waiting 20 seconds before trying again..."); @@ -129,20 +130,20 @@ void *DataUARTHandler::readIncomingData(void) // Wait 10 seconds and try to open serial port again rclcpp::Rate rate(10000); mySerialObject.open(); - } + } catch (std::exception &e2) { RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Read Thread: Failed second time to open Data serial port, error: %s", e1.what()); RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Read Thread: Port could not be opened. Port is \"%s\" and baud rate is %d", dataSerialPort, dataBaudRate); pthread_exit(NULL); } } - + if(mySerialObject.isOpen()) RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Read Thread: Port is open"); else RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Read Thread: Port could not be opened"); } - + /*Quick magicWord check to synchronize program with data Stream*/ while(!isMagicWord(last8Bytes)) { @@ -156,10 +157,10 @@ void *DataUARTHandler::readIncomingData(void) last8Bytes[6] = last8Bytes[7]; mySerialObject.read(&last8Bytes[7], 1); } - + /*Lock nextBufp before entering main loop*/ pthread_mutex_lock(&nextBufp_mutex); - + while(rclcpp::ok()) { /*Start reading UART data and writing to buffer while also checking for magicWord*/ @@ -176,43 +177,43 @@ void *DataUARTHandler::readIncomingData(void) //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"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]); nextBufp->push_back( last8Bytes[7] ); //push byte onto buffer - + /*If a magicWord is found wait for sorting to finish and switch buffers*/ if( isMagicWord(last8Bytes) ) { /*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); } @@ -221,7 +222,7 @@ void *DataUARTHandler::readIncomingData(void) 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]) @@ -235,7 +236,7 @@ int DataUARTHandler::isMagicWord(uint8_t last8Bytes[8]) val = 1; } - return val; + return val; } void *DataUARTHandler::syncedBufferSwap(void) @@ -258,21 +259,21 @@ void *DataUARTHandler::syncedBufferSwap(void) 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); @@ -280,7 +281,7 @@ void *DataUARTHandler::syncedBufferSwap(void) } pthread_exit(NULL); - + } void *DataUARTHandler::sortIncomingData(void) @@ -316,9 +317,9 @@ void *DataUARTHandler::sortIncomingData(void) pthread_cond_wait(&sort_go_cv, &countSync_mutex); pthread_mutex_unlock(&countSync_mutex); pthread_mutex_lock(¤tBufp_mutex); - + while(rclcpp::ok()) - { + { switch(sorterState) { case READ_HEADER: @@ -343,7 +344,7 @@ void *DataUARTHandler::sortIncomingData(void) //get platform (4 bytes) memcpy( &mmwData.header.platform, ¤tBufp->at(currentDatap), sizeof(mmwData.header.platform)); - 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) @@ -356,7 +357,7 @@ void *DataUARTHandler::sortIncomingData(void) headerSize = 8 * 4; // header includes subFrameNumber field } - if(currentBufp->size() < headerSize) + if(currentBufp->size() < headerSize) { sorterState = SWAP_BUFFERS; break; @@ -379,7 +380,7 @@ void *DataUARTHandler::sortIncomingData(void) currentDatap += ( sizeof(mmwData.header.numTLVs) ); //get subFrameNumber (4 bytes) (not used for XWR1443) - if((mmwData.header.platform & 0xFFFF) != 0x1443) + if((mmwData.header.platform & 0xFFFF) != 0x1443) { memcpy( &mmwData.header.subFrameNumber, ¤tBufp->at(currentDatap), sizeof(mmwData.header.subFrameNumber)); currentDatap += ( sizeof(mmwData.header.subFrameNumber) ); @@ -416,14 +417,14 @@ void *DataUARTHandler::sortIncomingData(void) { mmwData.numObjOut = mmwData.header.numDetectedObj; } - + RScan->header.stamp = static_cast(clocker.now().seconds()); 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); @@ -435,36 +436,36 @@ void *DataUARTHandler::sortIncomingData(void) //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"maxElevationAngleRatioSquared = %f", maxElevationAngleRatioSquared); //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"maxAzimuthAngleRatio = %f", maxAzimuthAngleRatio); //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"mmwData.numObjOut before = %d", mmwData.numObjOut); - + // Populate pointcloud while( i < mmwData.numObjOut ) { if (((mmwData.header.version >> 24) & 0xFF) < 3) { // SDK version is older than 3.x //get object range index memcpy( &mmwData.objOut.rangeIdx, ¤tBufp->at(currentDatap), sizeof(mmwData.objOut.rangeIdx)); currentDatap += ( sizeof(mmwData.objOut.rangeIdx) ); - + //get object doppler index memcpy( &mmwData.objOut.dopplerIdx, ¤tBufp->at(currentDatap), sizeof(mmwData.objOut.dopplerIdx)); currentDatap += ( sizeof(mmwData.objOut.dopplerIdx) ); - + //get object peak intensity value memcpy( &mmwData.objOut.peakVal, ¤tBufp->at(currentDatap), sizeof(mmwData.objOut.peakVal)); currentDatap += ( sizeof(mmwData.objOut.peakVal) ); - + //get object x-coordinate memcpy( &mmwData.objOut.x, ¤tBufp->at(currentDatap), sizeof(mmwData.objOut.x)); currentDatap += ( sizeof(mmwData.objOut.x) ); - + //get object y-coordinate memcpy( &mmwData.objOut.y, ¤tBufp->at(currentDatap), sizeof(mmwData.objOut.y)); currentDatap += ( sizeof(mmwData.objOut.y) ); - + //get object z-coordinate memcpy( &mmwData.objOut.z, ¤tBufp->at(currentDatap), sizeof(mmwData.objOut.z)); currentDatap += ( sizeof(mmwData.objOut.z) ); - + float temp[7]; - + temp[0] = (float) mmwData.objOut.x; temp[1] = (float) mmwData.objOut.y; temp[2] = (float) mmwData.objOut.z; @@ -473,14 +474,14 @@ void *DataUARTHandler::sortIncomingData(void) for (int j = 0; j < 4; j++) { if (temp[j] > 32767) temp[j] -= 65536; if (j < 3) temp[j] = temp[j] / pow(2 , mmwData.xyzQFormat); - } - + } + temp[7] = temp[3] * vvel; temp[4] = (float) mmwData.objOut.rangeIdx * vrange; temp[5] = 10 * log10(mmwData.objOut.peakVal + 1); // intensity temp[6] = std::atan2(-temp[0], temp[1]) / M_PI * 180; - + uint16_t tmp = (uint16_t)(temp[3] + nd / 2); // Map mmWave sensor coordinates to ROS coordinate system @@ -488,7 +489,7 @@ void *DataUARTHandler::sortIncomingData(void) RScan->points[i].y = -temp[0]; // ROS standard coordinate system Y-axis is left which is the mmWave sensor -(X-axis) RScan->points[i].z = temp[2]; // ROS standard coordinate system Z-axis is up which is the same as mmWave sensor Z-axis //RScan->points[i].intensity = temp[5]; - + radarscan.frame_id = frameID; radarscan.stamp = static_cast(clocker.now().seconds()); radarscan.point_id = i; @@ -499,8 +500,8 @@ void *DataUARTHandler::sortIncomingData(void) radarscan.velocity = temp[7]; radarscan.doppler_bin = tmp; radarscan.bearing = temp[6]; - radarscan.intensity = temp[5]; - } + radarscan.intensity = temp[5]; + } else { // SDK version is 3.x+ //get object x-coordinate (meters) @@ -532,7 +533,7 @@ void *DataUARTHandler::sortIncomingData(void) radarscan.y = -mmwData.newObjOut.x; radarscan.z = mmwData.newObjOut.z; radarscan.velocity = mmwData.newObjOut.velocity; - radar_scan_pub->publish(radarscan); + radar_scan_pub->publish(radarscan); // For SDK 3.x, intensity is replaced by snr in sideInfo and is parsed in the READ_SIDE_INFO code } @@ -552,7 +553,7 @@ void *DataUARTHandler::sortIncomingData(void) } sorterState = CHECK_TLV_TYPE; - + break; } @@ -565,7 +566,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) ); @@ -602,7 +603,7 @@ void *DataUARTHandler::sortIncomingData(void) radaroccupancy.state = mmwData.occupancy.state; if (radaroccupancy.state == 0){ - // RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Area is clear!"); + // RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Area is clear!"); } else{ // RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"Area is OCCUPIED!"); @@ -621,7 +622,7 @@ void *DataUARTHandler::sortIncomingData(void) i = 0; offset = 0; - mmwData.numObjOut = mmwData.header.numDetectedObj; + mmwData.numObjOut = mmwData.header.numDetectedObj; RScan->header.frame_id = frameID; RScan->height = 1; RScan->width = mmwData.numObjOut; @@ -854,7 +855,7 @@ void *DataUARTHandler::sortIncomingData(void) memcpy( &mmwData.newPointCloudCompOut.z, ¤tBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.z)); currentDatap += ( sizeof(mmwData.newPointCloudCompOut.z) ); - //get Doppler value + //get Doppler value memcpy( &mmwData.newPointCloudCompOut.doppler, ¤tBufp->at(currentDatap), sizeof(mmwData.newPointCloudCompOut.doppler)); currentDatap += ( sizeof(mmwData.newPointCloudCompOut.doppler) ); @@ -944,13 +945,13 @@ void *DataUARTHandler::sortIncomingData(void) } sorterState = CHECK_TLV_TYPE; - break; + break; case READ_LOG_MAG_RANGE: { sorterState = CHECK_TLV_TYPE; break; - } + } case READ_NOISE: { @@ -1048,14 +1049,14 @@ void *DataUARTHandler::sortIncomingData(void) //get tlvType (32 bits) & remove from queue memcpy( &tlvType, ¤tBufp->at(currentDatap), sizeof(tlvType)); currentDatap += ( sizeof(tlvType) ); - + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : sizeof(tlvType) = %d", sizeof(tlvType)); //get tlvLen (32 bits) & remove from queue memcpy( &tlvLen, ¤tBufp->at(currentDatap), sizeof(tlvLen)); currentDatap += ( sizeof(tlvLen) ); //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : sizeof(tlvLen) = %d", sizeof(tlvLen)); - + //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : tlvType = %d, tlvLen = %d", (int) tlvType, tlvLen); switch(tlvType) @@ -1123,12 +1124,12 @@ void *DataUARTHandler::sortIncomingData(void) sorterState = READ_TARGET_INDEX; break; - case MMWDEMO_OUTPUT_EXT_MSG_DETECTED_POINTS: + case MMWDEMO_OUTPUT_EXT_MSG_DETECTED_POINTS: //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"DataUARTHandler Sort Thread : Compressed Points TLV MMWAVE-L SDK 5.x"); sorterState = READ_COMPRESSED_POINT_CLOUD; break; - default: + default: break; } } @@ -1159,7 +1160,7 @@ void *DataUARTHandler::sortIncomingData(void) break; } - default: + default: break; } } @@ -1184,9 +1185,9 @@ void DataUARTHandler::start(void) 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) @@ -1213,7 +1214,7 @@ void DataUARTHandler::start(void) s = pthread_sigmask(SIG_UNBLOCK, &set, NULL); signal(SIGINT, sigHandler); - + pthread_join(uartThread, NULL); pthread_join(sorterThread, NULL); pthread_join(swapThread, NULL); @@ -1227,17 +1228,17 @@ void DataUARTHandler::start(void) } 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()); } @@ -1247,16 +1248,16 @@ void DataUARTHandler::stop() stop_threads = true; rclcpp::shutdown(); - + pthread_cond_signal(&read_go_cv); pthread_cond_signal(&sort_go_cv); pthread_cond_signal(&countSync_max_cv); } -int main(int argc, char **argv) +int main(int argc, char **argv) { rclcpp::init(argc, argv); - node = std::make_shared(); + auto node = std::make_shared(); std::string mySerialPort = node->get_parameter("data_port").as_string(); std::string myBaudRate = node->get_parameter("data_rate").as_string(); std::string myFrameID = "/ti_mmwave_0"; diff --git a/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp b/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp index a9d0e70..2f76fad 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp +++ b/ros2_driver/src/ti_mmwave_rospkg/src/mmWaveQuickConfig.cpp @@ -17,9 +17,8 @@ #include using namespace std::chrono_literals; -std::shared_ptr nodeptr = nullptr; -class mmWaveQuickConfig : public rclcpp::Node +class mmWaveQuickConfig : public rclcpp::Node { public: @@ -28,14 +27,14 @@ public: this->declare_parameter("mmwavecli_name", rclcpp::PARAMETER_STRING); this->declare_parameter("mmwavecli_cfg", rclcpp::PARAMETER_STRING); } - + private: }; int main(int argc, char **argv) { rclcpp::init(argc, argv); - nodeptr = std::make_shared(); + auto nodeptr = std::make_shared(); rclcpp::Client::SharedPtr client = nodeptr->create_client("/ti_mmwave_rospkg_msgs/mmwave_cli"); auto request = std::make_shared(); auto response = std::make_shared(); @@ -48,27 +47,27 @@ int main(int argc, char **argv) { std::ifstream myParams; //wait for service to become available - client->wait_for_service(std::chrono::seconds(5)); + client->wait_for_service(std::chrono::seconds(5)); //wait 0.5 secs to avoid multi-sensor conflicts rclcpp::Rate rate(500); myParams.open(mmWaveCLIcfg); - if (myParams.is_open()) + if (myParams.is_open()) { RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"File was opened"); - while( std::getline(myParams, request->comm)) + while( std::getline(myParams, request->comm)) { // Remove Windows carriage-return if present request->comm.erase(std::remove(request->comm.begin(), request->comm.end(), '\r'), request->comm.end()); // Ignore comment lines (first non-space char is '%') or blank lines - if (!(std::regex_match(request->comm, std::regex("^\\s*%.*")) || std::regex_match(request->comm, std::regex("^\\s*")))) + if (!(std::regex_match(request->comm, std::regex("^\\s*%.*")) || std::regex_match(request->comm, std::regex("^\\s*")))) { //RCLCPP_INFO(rclcpp::get_logger("rclcpp"),"mmWaveQuickConfig: Sending command: '%s'", request->comm.c_str() ); //parser.ParamsParser(request, param_node); auto result = client->async_send_request(request); - if (!(rclcpp::spin_until_future_complete(nodeptr, result) == rclcpp::FutureReturnCode::SUCCESS)) - { + if (!(rclcpp::spin_until_future_complete(nodeptr, result) == rclcpp::FutureReturnCode::SUCCESS)) + { RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"\n\n==================================\nmmWave ROS Driver is shutting down\n==================================\n"); // return 1; } @@ -78,8 +77,8 @@ int main(int argc, char **argv) { myParams.close(); rclcpp::shutdown(); exit(0); - } - else + } + else { RCLCPP_WARN(rclcpp::get_logger("rclcpp"),"mmWaveQuickConfig: Terminated %s", mmWaveCLIcfg.c_str()); return 0;