From be917505ac457309c8b85002cf3384fabc6cb1b2 Mon Sep 17 00:00:00 2001 From: JuneChul Roh Date: Wed, 6 Mar 2024 18:27:52 -0600 Subject: [PATCH] [ros2_driver/ti_mmwave_rospkg] Updated the launch files and the rviz file. - Added a common launch file IWR6843.py for all the IWR6843x launch files. - CLI config file, command port, and data port can passed at the command line with launch arguments. For example, below is equivalent to '6843ISK_Tracking.py': ros2 launch ti_mmwave_rospkg IWR6843.py cfg_file:=6843ISK_Tracking.cfg - 6843{ISK,AOP}_{Standard,Tracking,Occupancy}.py now call the common launch file. - Updated the rviz file to display the XYZ axes. --- .../launch/6843AOP_Standard.py | 119 ++-------------- .../launch/6843AOP_Tracking.py | 119 ++-------------- .../launch/6843ISK_Occupancy.py | 119 ++-------------- .../launch/6843ISK_Standard.py | 119 ++-------------- .../launch/6843ISK_Tracking.py | 119 ++-------------- .../src/ti_mmwave_rospkg/launch/IWR6843.py | 131 ++++++++++++++++++ .../src/ti_mmwave_rospkg/launch/rviz.rviz | 18 ++- 7 files changed, 219 insertions(+), 525 deletions(-) create mode 100644 ros2_driver/src/ti_mmwave_rospkg/launch/IWR6843.py 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 aac0af1..1d1e920 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Standard.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Standard.py @@ -1,118 +1,29 @@ import os -from launch import LaunchDescription, conditions +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource 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' - ) + 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 - 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" - - ConfigParameters = os.path.join( - my_package_dir, - 'config', - 'global_params.yaml', - 'launch/*.rviz' - ) - global_param_node = Node( - package='ti_mmwave_rospkg', - executable='ConfigParameterServer', - name='ConfigParameterServer', - parameters=[ConfigParameters] - ) - - 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} - ] - ) - - mmWaveQuickConfig = Node( - package="ti_mmwave_rospkg", - executable="mmWaveQuickConfig", - name="mmWaveQuickConfig", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - ParameterParser = Node( - package="ti_mmwave_rospkg", - executable="ParameterParser", - name="ParameterParser", - output="screen", - emulate_tty=True, - parameters=[ - {"device_name": device}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - DataHandlerClass = Node( - package="ti_mmwave_rospkg", - executable="DataHandlerClass", - name="DataHandlerClass", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path}, - {"data_port": data_port}, - {"data_rate": data_rate} - ] - - ) - - Rviz2 = Node( - 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 + # include IWR6843.py + package_dir = get_package_share_directory('ti_mmwave_rospkg') + iwr6843_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(package_dir,'launch','IWR6843.py')), + launch_arguments={ + "cfg_file": '6843AOP_Standard.cfg', + "command_port": "/dev/ttyUSB0", + "data_port": "/dev/ttyUSB1", + "rviz": LaunchConfiguration('rviz'), + }.items() ) ld = LaunchDescription() ld.add_action(rviz_arg) - ld.add_action(global_param_node) - ld.add_action(mmWaveCommSrv) - ld.add_action(mmWaveQuickConfig) - ld.add_action(ParameterParser) - ld.add_action(DataHandlerClass) - ld.add_action(Rviz2) + ld.add_action(iwr6843_include) 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 85feaf9..1040b48 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Tracking.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843AOP_Tracking.py @@ -1,118 +1,29 @@ import os -from launch import LaunchDescription, conditions +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource 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' - ) + 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 - 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" - - ConfigParameters = os.path.join( - my_package_dir, - 'config', - 'global_params.yaml', - 'launch/*.rviz' - ) - global_param_node = Node( - package='ti_mmwave_rospkg', - executable='ConfigParameterServer', - name='ConfigParameterServer', - parameters=[ConfigParameters] - ) - - 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} - ] - ) - - mmWaveQuickConfig = Node( - package="ti_mmwave_rospkg", - executable="mmWaveQuickConfig", - name="mmWaveQuickConfig", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - ParameterParser = Node( - package="ti_mmwave_rospkg", - executable="ParameterParser", - name="ParameterParser", - output="screen", - emulate_tty=True, - parameters=[ - {"device_name": device}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - DataHandlerClass = Node( - package="ti_mmwave_rospkg", - executable="DataHandlerClass", - name="DataHandlerClass", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path}, - {"data_port": data_port}, - {"data_rate": data_rate} - ] - - ) - - Rviz2 = Node( - 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 + # include IWR6843.py + package_dir = get_package_share_directory('ti_mmwave_rospkg') + iwr6843_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(package_dir,'launch','IWR6843.py')), + launch_arguments={ + "cfg_file": '6843AOP_Tracking.cfg', + "command_port": "/dev/ttyUSB0", + "data_port": "/dev/ttyUSB1", + "rviz": LaunchConfiguration('rviz'), + }.items() ) ld = LaunchDescription() ld.add_action(rviz_arg) - ld.add_action(global_param_node) - ld.add_action(mmWaveCommSrv) - ld.add_action(mmWaveQuickConfig) - ld.add_action(ParameterParser) - ld.add_action(DataHandlerClass) - ld.add_action(Rviz2) + ld.add_action(iwr6843_include) 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 8213389..07f89de 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Occupancy.py @@ -1,118 +1,29 @@ import os -from launch import LaunchDescription, conditions +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource 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' - ) + 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 - 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" - - ConfigParameters = os.path.join( - my_package_dir, - 'config', - 'global_params.yaml', - 'launch/*.rviz' - ) - global_param_node = Node( - package='ti_mmwave_rospkg', - executable='ConfigParameterServer', - name='ConfigParameterServer', - parameters=[ConfigParameters] - ) - - 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} - ] - ) - - mmWaveQuickConfig = Node( - package="ti_mmwave_rospkg", - executable="mmWaveQuickConfig", - name="mmWaveQuickConfig", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - ParameterParser = Node( - package="ti_mmwave_rospkg", - executable="ParameterParser", - name="ParameterParser", - output="screen", - emulate_tty=True, - parameters=[ - {"device_name": device}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - DataHandlerClass = Node( - package="ti_mmwave_rospkg", - executable="DataHandlerClass", - name="DataHandlerClass", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path}, - {"data_port": data_port}, - {"data_rate": data_rate} - ] - - ) - - Rviz2 = Node( - 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 + # include IWR6843.py + package_dir = get_package_share_directory('ti_mmwave_rospkg') + iwr6843_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(package_dir,'launch','IWR6843.py')), + launch_arguments={ + "cfg_file": '6843ISK_Occupancy.cfg', + "command_port": "/dev/ttyUSB0", + "data_port": "/dev/ttyUSB1", + "rviz": LaunchConfiguration('rviz'), + }.items() ) ld = LaunchDescription() ld.add_action(rviz_arg) - ld.add_action(global_param_node) - ld.add_action(mmWaveCommSrv) - ld.add_action(mmWaveQuickConfig) - ld.add_action(ParameterParser) - ld.add_action(DataHandlerClass) - ld.add_action(Rviz2) + ld.add_action(iwr6843_include) 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 ee0d1cd..6901dad 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Standard.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Standard.py @@ -1,118 +1,29 @@ import os -from launch import LaunchDescription, conditions +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource 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' - ) + 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 - 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" - - ConfigParameters = os.path.join( - my_package_dir, - 'config', - 'global_params.yaml', - 'launch/*.rviz' - ) - global_param_node = Node( - package='ti_mmwave_rospkg', - executable='ConfigParameterServer', - name='ConfigParameterServer', - parameters=[ConfigParameters] - ) - - 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} - ] - ) - - mmWaveQuickConfig = Node( - package="ti_mmwave_rospkg", - executable="mmWaveQuickConfig", - name="mmWaveQuickConfig", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - ParameterParser = Node( - package="ti_mmwave_rospkg", - executable="ParameterParser", - name="ParameterParser", - output="screen", - emulate_tty=True, - parameters=[ - {"device_name": device}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - DataHandlerClass = Node( - package="ti_mmwave_rospkg", - executable="DataHandlerClass", - name="DataHandlerClass", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path}, - {"data_port": data_port}, - {"data_rate": data_rate} - ] - - ) - - Rviz2 = Node( - 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 + # include IWR6843.py + package_dir = get_package_share_directory('ti_mmwave_rospkg') + iwr6843_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(package_dir,'launch','IWR6843.py')), + launch_arguments={ + "cfg_file": '6843ISK_Standard.cfg', + "command_port": "/dev/ttyUSB0", + "data_port": "/dev/ttyUSB1", + "rviz": LaunchConfiguration('rviz'), + }.items() ) ld = LaunchDescription() ld.add_action(rviz_arg) - ld.add_action(global_param_node) - ld.add_action(mmWaveCommSrv) - ld.add_action(mmWaveQuickConfig) - ld.add_action(ParameterParser) - ld.add_action(DataHandlerClass) - ld.add_action(Rviz2) + ld.add_action(iwr6843_include) 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 10d76cb..4528922 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Tracking.py +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/6843ISK_Tracking.py @@ -1,118 +1,29 @@ import os -from launch import LaunchDescription, conditions +from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import IncludeLaunchDescription from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node +from launch.launch_description_sources import PythonLaunchDescriptionSource 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' - ) + 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 - my_package_dir = get_package_share_directory('ti_mmwave_rospkg') - path = os.path.join(my_package_dir,'cfg','6843ISK_Tracking.cfg') - device = "6843" - name = "/mmWaveCLI" - command_port = "/dev/ttyUSB0" - command_rate = "115200" - data_port = "/dev/ttyUSB1" - data_rate = "921600" - - ConfigParameters = os.path.join( - my_package_dir, - 'config', - 'global_params.yaml', - 'launch/*.rviz' - ) - global_param_node = Node( - package='ti_mmwave_rospkg', - executable='ConfigParameterServer', - name='ConfigParameterServer', - parameters=[ConfigParameters] - ) - - 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} - ] - ) - - mmWaveQuickConfig = Node( - package="ti_mmwave_rospkg", - executable="mmWaveQuickConfig", - name="mmWaveQuickConfig", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - ParameterParser = Node( - package="ti_mmwave_rospkg", - executable="ParameterParser", - name="ParameterParser", - output="screen", - emulate_tty=True, - parameters=[ - {"device_name": device}, - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path} - ] - ) - - DataHandlerClass = Node( - package="ti_mmwave_rospkg", - executable="DataHandlerClass", - name="DataHandlerClass", - output="screen", - emulate_tty=True, - parameters=[ - {"mmwavecli_name": name}, - {"mmwavecli_cfg": path}, - {"data_port": data_port}, - {"data_rate": data_rate} - ] - - ) - - Rviz2 = Node( - 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 + # include IWR6843.py + package_dir = get_package_share_directory('ti_mmwave_rospkg') + iwr6843_include = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join(package_dir,'launch','IWR6843.py')), + launch_arguments={ + "cfg_file": '6843ISK_Tracking.cfg', + "command_port": "/dev/ttyUSB0", + "data_port": "/dev/ttyUSB1", + "rviz": LaunchConfiguration('rviz'), + }.items() ) ld = LaunchDescription() ld.add_action(rviz_arg) - ld.add_action(global_param_node) - ld.add_action(mmWaveCommSrv) - ld.add_action(mmWaveQuickConfig) - ld.add_action(ParameterParser) - ld.add_action(DataHandlerClass) - ld.add_action(Rviz2) + ld.add_action(iwr6843_include) return ld diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/IWR6843.py b/ros2_driver/src/ti_mmwave_rospkg/launch/IWR6843.py new file mode 100644 index 0000000..565d55e --- /dev/null +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/IWR6843.py @@ -0,0 +1,131 @@ +import os +from launch import LaunchDescription, conditions +from launch.actions import DeclareLaunchArgument +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from ament_index_python.packages import get_package_share_directory +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution + +def generate_launch_description(): + + # Declare the launch argument + cfg_file_arg = DeclareLaunchArgument( + 'cfg_file', + default_value='6843ISK_Tracking.cfg' + ) + + command_port_arg = DeclareLaunchArgument( + 'command_port', + default_value='/dev/ttyUSB0' + ) + + data_port_arg = DeclareLaunchArgument( + 'data_port', + default_value='/dev/ttyUSB1' + ) + + 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 + my_package_dir = get_package_share_directory('ti_mmwave_rospkg') + cfg_file = LaunchConfiguration("cfg_file") + mmwavecli_cfg_path = PathJoinSubstitution([my_package_dir, 'cfg', cfg_file]) + + device = "6843" + name = "/mmWaveCLI" + command_port = LaunchConfiguration("command_port") + command_rate = "115200" + data_port = LaunchConfiguration("data_port") + data_rate = "921600" + + ConfigParameters = os.path.join( + my_package_dir, + 'config', + 'global_params.yaml', + 'launch/*.rviz' + ) + global_param_node = Node( + package='ti_mmwave_rospkg', + executable='ConfigParameterServer', + name='ConfigParameterServer', + parameters=[ConfigParameters] + ) + + mmWaveCommSrv = Node( + package="ti_mmwave_rospkg", + executable="mmWaveCommSrv", + name="mmWaveCommSrv", + output="screen", + emulate_tty=True, + parameters=[ + {"command_port": command_port}, + {"command_rate": command_rate}, + ] + ) + + mmWaveQuickConfig = Node( + package="ti_mmwave_rospkg", + executable="mmWaveQuickConfig", + name="mmWaveQuickConfig", + output="screen", + emulate_tty=True, + parameters=[ + {"mmwavecli_cfg": mmwavecli_cfg_path} + ] + ) + + ParameterParser = Node( + package="ti_mmwave_rospkg", + executable="ParameterParser", + name="ParameterParser", + output="screen", + emulate_tty=True, + parameters=[ + {"device_name": device}, + {"mmwavecli_cfg": mmwavecli_cfg_path} + ] + ) + + DataHandlerClass = Node( + package="ti_mmwave_rospkg", + executable="DataHandlerClass", + name="DataHandlerClass", + output="screen", + emulate_tty=True, + parameters=[ + {"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"} + ] + + ) + + Rviz2 = Node( + 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(cfg_file_arg) + ld.add_action(command_port_arg) + ld.add_action(data_port_arg) + ld.add_action(rviz_arg) + ld.add_action(global_param_node) + ld.add_action(mmWaveCommSrv) + ld.add_action(mmWaveQuickConfig) + ld.add_action(ParameterParser) + ld.add_action(DataHandlerClass) + ld.add_action(Rviz2) + + return ld diff --git a/ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz b/ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz index 1c2497a..6849f29 100644 --- a/ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz +++ b/ros2_driver/src/ti_mmwave_rospkg/launch/rviz.rviz @@ -7,8 +7,9 @@ Panels: - /Global Options1 - /Status1 - /PointCloud21 + - /Axes1 Splitter Ratio: 0.5 - Tree Height: 549 + Tree Height: 555 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -63,9 +64,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 0; 255; 0 - Max Intensity: 20.0 + Max Intensity: 20 Min Color: 255; 0; 0 - Min Intensity: 15.0 + Min Intensity: 15 Name: PointCloud2 Position Transformer: XYZ Selectable: true @@ -82,10 +83,17 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: false Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 0.5 + Name: Axes + Radius: 0.05000000074505806 + Reference Frame: ti_mmwave_0 + Value: true Enabled: true Global Options: Background Color: 48; 48; 48 - Fixed Frame: map + Fixed Frame: ti_mmwave_0 Frame Rate: 30 Name: root Tools: @@ -154,7 +162,7 @@ Window Geometry: Height: 846 Hide Left Dock: false Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000156000002b0fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002b0000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b0000002fb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd000000040000000000000156000002b4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000002b4000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002b0fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d000002b0000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000354000002b400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: