diff --git a/CMakeLists.txt b/CMakeLists.txt index 64fa26e..9714755 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -12,6 +12,7 @@ install( DIRECTORY launch config + rviz DESTINATION share/${PROJECT_NAME} ) diff --git a/config/smb261.cam0.yaml b/config/smb261_cam0.yaml similarity index 96% rename from config/smb261.cam0.yaml rename to config/smb261_cam0.yaml index f10cd83..c8d505b 100644 --- a/config/smb261.cam0.yaml +++ b/config/smb261_cam0.yaml @@ -1,6 +1,6 @@ image_width: 1440 image_height: 1080 -camera_name: 0 +camera_name: rgb_camera camera_matrix: rows: 3 cols: 3 diff --git a/config/smb264_cam0.yaml b/config/smb264_cam0.yaml index 8f90433..69f7c0d 100644 --- a/config/smb264_cam0.yaml +++ b/config/smb264_cam0.yaml @@ -1,6 +1,6 @@ image_width: 1440 image_height: 1080 -camera_name: 0 +camera_name: rgb_camera camera_matrix: rows: 3 cols: 3 diff --git a/config/twist_mux_topics.yaml b/config/twist_mux_topics.yaml new file mode 100644 index 0000000..09946b7 --- /dev/null +++ b/config/twist_mux_topics.yaml @@ -0,0 +1,19 @@ +# Input topics handled/muxed. +# For each topic: +# - name : name identifier to select the topic (*sub-namespace, see below) +# - topic : input topic of geometry_msgs::Twist type +# - timeout : timeout in seconds to start discarding old messages, and use 0.0 speed instead +# - priority: priority in the range [0, 255]; the higher the more priority over other topics + +twist_mux: + ros__parameters: + use_stamped: true # Use stamped messages (geometry_msgs::TwistStamped) if available + topics: + navigation: + topic : nav_vel + timeout : 0.5 + priority: 10 + joystick: + topic : rc_vel + timeout : 0.5 + priority: 100 \ No newline at end of file diff --git a/launch/flir_camera.launch.py b/launch/flir_camera.launch.py deleted file mode 100644 index 6e73fe0..0000000 --- a/launch/flir_camera.launch.py +++ /dev/null @@ -1,205 +0,0 @@ -# ----------------------------------------------------------------------------- -# Copyright 2022 Bernd Pfrommer -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. -# -# - -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument as LaunchArg -from launch.actions import OpaqueFunction -from launch.substitutions import LaunchConfiguration as LaunchConfig -from launch.substitutions import PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -example_parameters = { - 'blackfly_s': { - 'debug': False, - 'compute_brightness': False, - 'adjust_timestamp': True, - 'dump_node_map': False, - # set parameters defined in blackfly_s.yaml - 'gain_auto': 'Continuous', - 'pixel_format': 'BGR8', - 'exposure_auto': 'Continuous', - # to use a user set, do this: - # 'user_set_selector': 'UserSet0', - # 'user_set_load': 'Yes', - # These are useful for GigE cameras - # 'device_link_throughput_limit': 380000000, - # 'gev_scps_packet_size': 9000, - # ---- to reduce the sensor width and shift the crop - # 'image_width': 1408, - # 'image_height': 1080, - # 'offset_x': 16, - # 'offset_y': 0, - # 'binning_x': 1, - # 'binning_y': 1, - # 'connect_while_subscribed': True, - 'frame_rate_auto': 'Off', - 'frame_rate': 40.0, - 'frame_rate_enable': True, - 'buffer_queue_size': 10, - 'trigger_mode': 'Off', - 'chunk_mode_active': True, - 'chunk_selector_frame_id': 'FrameID', - 'chunk_enable_frame_id': True, - 'chunk_selector_exposure_time': 'ExposureTime', - 'chunk_enable_exposure_time': True, - 'chunk_selector_gain': 'Gain', - 'chunk_enable_gain': True, - 'chunk_selector_timestamp': 'Timestamp', - 'chunk_enable_timestamp': True, - }, - 'blackfly': { - 'debug': False, - 'dump_node_map': False, - 'gain_auto': 'Continuous', - 'pixel_format': 'BayerRG8', - 'exposure_auto': 'Continuous', - 'frame_rate_auto': 'Off', - 'frame_rate': 40.0, - 'frame_rate_enable': True, - 'buffer_queue_size': 10, - 'trigger_mode': 'Off', - # 'stream_buffer_handling_mode': 'NewestFirst', - # 'multicast_monitor_mode': False - }, - 'chameleon': { - 'debug': False, - 'compute_brightness': False, - 'dump_node_map': False, - # set parameters defined in chameleon.yaml - 'gain_auto': 'Continuous', - 'exposure_auto': 'Continuous', - 'offset_x': 0, - 'offset_y': 0, - 'image_width': 2048, - 'image_height': 1536, - 'pixel_format': 'RGB8', # 'BayerRG8, 'RGB8' or 'Mono8' - 'frame_rate_continous': True, - 'frame_rate': 100.0, - 'trigger_mode': 'Off', - 'chunk_mode_active': True, - 'chunk_selector_frame_id': 'FrameID', - 'chunk_enable_frame_id': True, - 'chunk_selector_exposure_time': 'ExposureTime', - 'chunk_enable_exposure_time': True, - 'chunk_selector_gain': 'Gain', - 'chunk_enable_gain': True, - 'chunk_selector_timestamp': 'Timestamp', - 'chunk_enable_timestamp': True, - }, - 'grasshopper': { - 'debug': False, - 'compute_brightness': False, - 'dump_node_map': False, - # set parameters defined in grasshopper.yaml - 'gain_auto': 'Continuous', - 'exposure_auto': 'Continuous', - 'frame_rate_auto': 'Off', - 'frame_rate': 100.0, - 'trigger_mode': 'Off', - 'chunk_mode_active': True, - 'chunk_selector_frame_id': 'FrameID', - 'chunk_enable_frame_id': True, - 'chunk_selector_exposure_time': 'ExposureTime', - 'chunk_enable_exposure_time': True, - 'chunk_selector_gain': 'Gain', - 'chunk_enable_gain': True, - 'chunk_selector_timestamp': 'Timestamp', - 'chunk_enable_timestamp': True, - }, - 'flir_ax5': { - 'debug': False, - 'compute_brightness': False, - 'adjust_timestamp': False, - 'dump_node_map': False, - # --- Set parameters defined in flir_ax5.yaml - 'pixel_format': 'Mono8', - 'gev_scps_packet_size': 576, - 'image_width': 640, - 'image_height': 512, - 'offset_x': 0, - 'offset_y': 0, - 'sensor_gain_mode': 'HighGainMode', # "HighGainMode" "LowGainMode" - 'nuc_mode': 'Automatic', # "Automatic" "External" "Manual" - 'sensor_dde_mode': 'Automatic', # "Automatic" "Manual" - 'sensor_video_standard': 'NTSC30HZ', # "NTSC30HZ" "PAL25Hz" "NTSC60HZ" "PAL50HZ" - # valid values: "PlateauHistogram" "OnceBright" "AutoBright" "Manual" "Linear" - 'image_adjust_method': 'PlateauHistogram', - 'video_orientation': 'Normal', # "Normal" "Invert" "Revert" "InvertRevert" - }, -} - - -def launch_setup(context, *args, **kwargs): - """Launch camera driver node.""" - parameter_file = LaunchConfig('parameter_file').perform(context) - camera_type = LaunchConfig('camera_type').perform(context) - if not parameter_file: - parameter_file = PathJoinSubstitution( - [FindPackageShare('spinnaker_camera_driver'), 'config', camera_type + '.yaml'] - ) - if camera_type not in example_parameters: - raise Exception('no example parameters available for type ' + camera_type) - - node = Node( - package='spinnaker_camera_driver', - executable='camera_driver_node', - output='screen', - name=[LaunchConfig('camera_name')], - parameters=[ - example_parameters[camera_type], - { - 'ffmpeg_image_transport.encoding': 'hevc_nvenc', - 'parameter_file': parameter_file, - 'serial_number': [LaunchConfig('serial')], - }, - ], - remappings=[ - ('~/control', '/exposure_control/control'), - ], - ) - - return [node] - - -def generate_launch_description(): - """Create composable node by calling opaque function.""" - return LaunchDescription( - [ - LaunchArg( - 'camera_name', - default_value=['flir_camera'], - description='camera name (ros node name)', - ), - LaunchArg( - 'camera_type', - default_value='blackfly_s', - description='type of camera (blackfly_s, chameleon...)', - ), - LaunchArg( - 'serial', - default_value="'20435008'", - description='FLIR serial number of camera (in quotes!!)', - ), - LaunchArg( - 'parameter_file', - default_value='', - description='path to ros parameter definition file (override camera type)', - ), - OpaqueFunction(function=launch_setup), - ] - ) diff --git a/launch/jetson_rgb_camera.launch.py b/launch/jetson_rgb_camera.launch.py deleted file mode 100644 index 30cb021..0000000 --- a/launch/jetson_rgb_camera.launch.py +++ /dev/null @@ -1,41 +0,0 @@ -from launch import LaunchDescription -from launch_ros.actions import ComposableNodeContainer -from launch_ros.descriptions import ComposableNode - -def generate_launch_description(): - return LaunchDescription([ - ComposableNodeContainer( - name='rgb_camera_container', - namespace='rgb_camera', - package='rclcpp_components', - executable='component_container', - composable_node_descriptions=[ - ComposableNode( - package='spinnaker_camera_driver', - plugin='spinnaker_camera_driver::CameraDriver', - name='rgb_camera_node', - parameters=[{ - 'serial_number': '20435008', - 'frame_id': 'rgb_camera_optical_link', - 'parameter_file': '/path/to/camera.yaml', - 'acquisition_frame_rate': 30.0, - 'image_format_color_coding': 'BayerRG8', - 'color_processing_algorithm': 'HQ_LINEAR', - 'exposure_auto': 'Continuous', - 'gain_auto': 'Continuous', - }] - ), - ComposableNode( - package='image_proc', - plugin='image_proc::DebayerNode', - name='debayer_node', - ), - ComposableNode( - package='image_proc', - plugin='image_proc::RectifyNode', - name='rectify_node', - ) - ], - output='screen', - ) - ]) diff --git a/launch/motor_controller.launch.py b/launch/motor_controller.launch.py deleted file mode 100644 index 9281ea3..0000000 --- a/launch/motor_controller.launch.py +++ /dev/null @@ -1,38 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -def generate_launch_description(): - - # Run speed_control_node from package roboteq_controller - speed_control_group = GroupAction([ - Node(namespace='roboteq_controller', - package='roboteq_controller', - executable='speed_control_node' - ) - ]) - - - # Differential drive launch group - diff_drive_group = GroupAction([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("smb_kinematics_ros2"), "launch", "smb_differential_drive.launch.py" - ]) - ), - ), - ]) - - - - return LaunchDescription( - [ - speed_control_group, - diff_drive_group, - ] - ) diff --git a/launch/rgb_camera_driver.launch.py b/launch/rgb_camera_driver.launch.py index ee77d37..243f5e6 100644 --- a/launch/rgb_camera_driver.launch.py +++ b/launch/rgb_camera_driver.launch.py @@ -19,10 +19,11 @@ from launch.actions import DeclareLaunchArgument as LaunchArg from launch.actions import OpaqueFunction from launch.substitutions import LaunchConfiguration as LaunchConfig -from launch.substitutions import PathJoinSubstitution +from launch.substitutions import PathJoinSubstitution, PythonExpression from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +import os blackfly_s_param = { 'debug': False, @@ -31,7 +32,8 @@ 'dump_node_map': False, # set parameters defined in blackfly_s.yaml 'gain_auto': 'Continuous', - 'pixel_format': 'BGR8', + # 'pixel_format': 'BayerRG8', + 'pixel_format': 'RGB8', 'exposure_auto': 'Continuous', # to use a user set, do this: # 'user_set_selector': 'UserSet0', @@ -48,7 +50,7 @@ # 'binning_y': 1, # 'connect_while_subscribed': True, 'frame_rate_auto': 'Off', - 'frame_rate': 40.0, + 'frame_rate': 5.0, 'frame_rate_enable': True, 'buffer_queue_size': 10, 'trigger_mode': 'Off', @@ -67,6 +69,13 @@ def launch_setup(context, *args, **kwargs): """Launch camera driver node.""" parameter_file = LaunchConfig('parameter_file').perform(context) camera_type = LaunchConfig('camera_type').perform(context) + robot_id = os.environ.get('ROBOT_ID', '000') + calibration_file = PathJoinSubstitution( + [FindPackageShare('smb_bringup'), 'config', 'smb' + robot_id + '_cam0.yaml'] + ) + + calibration_file_url = 'file://' + calibration_file.perform(context) + if not parameter_file: parameter_file = PathJoinSubstitution( [FindPackageShare('spinnaker_camera_driver'), 'config', camera_type + '.yaml'] @@ -82,6 +91,7 @@ def launch_setup(context, *args, **kwargs): 'ffmpeg_image_transport.encoding': 'hevc_nvenc', 'parameter_file': parameter_file, 'serial_number': [LaunchConfig('serial')], + 'camerainfo_url': calibration_file_url, }, ], remappings=[ @@ -98,7 +108,7 @@ def generate_launch_description(): [ LaunchArg( 'camera_name', - default_value=['flir_camera'], + default_value=['rgb_camera'], description='camera name (ros node name)', ), LaunchArg( @@ -108,7 +118,7 @@ def generate_launch_description(): ), LaunchArg( 'serial', - default_value="'20435008'", + default_value="'20010195'", description='FLIR serial number of camera (in quotes!!)', ), LaunchArg( diff --git a/launch/sensors.launch.py b/launch/sensors.launch.py index 5169056..f28f01b 100644 --- a/launch/sensors.launch.py +++ b/launch/sensors.launch.py @@ -10,135 +10,6 @@ def generate_launch_description(): - # Declare all launch arguments - declared_arguments = [ - DeclareLaunchArgument("launch_lidar", default_value="true", description="Launch LiDAR"), - DeclareLaunchArgument("launch_tracking_cam", default_value="false", description="Launch Tracking Camera"), - DeclareLaunchArgument("launch_depth_cam", default_value="false", description="Launch Depth Camera"), - # FIXME: launch_imu_interface needs rosserial_python, but it does not exist in ros2, need to find another option - DeclareLaunchArgument("launch_imu_interface", default_value="false", description="Launch IMU Interface"), - DeclareLaunchArgument("launch_rgb_cam", default_value="false", description="Launch RGB Camera"), - DeclareLaunchArgument("launch_powerstatus", default_value="false", description="Launch Power Status"), - DeclareLaunchArgument("tracking_cam_calib_odom_file", default_value="$(find smb)/config/tracking_camera_config.json", description="Path to config for odometry input to tracking camera"), - DeclareLaunchArgument("smb_name", default_value="$(env SMB_NAME)", description="Name of the SMB in the format smb26x (relevant for calibrations)"), - DeclareLaunchArgument("GPU_user", default_value="$(env USER)", description="Username to use on the Jetson Xavier GPU"), - ] - - # Robosense LiDAR - rslidar_config_file = get_package_share_directory('smb_bringup') + '/config/rslidar_config.yaml' - lidar_group = GroupAction([ - Node( - package="rslidar_sdk", - executable="rslidar_sdk_node", - name="rslidar_sdk_node", - output="screen", - parameters=[{'config_path': rslidar_config_file}] - ) - ], condition=IfCondition(LaunchConfiguration("launch_lidar"))) - - # # Intel RealSense Tracking Camera T265 - # tracking_camera_group = GroupAction([ - # Node( - # package="realsense2_camera", - # executable="realsense2_camera_node", - # namespace="tracking_camera", - # parameters=[{ - # "tracking_module.enable_mapping": False, - # "tracking_module.enable_pose_jumping": False, - # "tracking_module.enable_relocalization": False, - # "device_type": "t265", - # "enable_pose": True, - # "enable_accel": True, - # "enable_gyro": True, - # "publish_tf": False, - # "publish_odom_tf": False, - # "topic_odom_in": "/control/smb_diff_drive/odom", - # "unite_imu_method": "linear_interpolation", - # "calib_odom_file": LaunchConfiguration("tracking_cam_calib_odom_file"), - # }], - # output="screen", - # ) - # ], condition=IfCondition(LaunchConfiguration("launch_tracking_cam"))) - - # # Intel RealSense Depth Camera D435 - # depth_camera_group = GroupAction([ - # IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # PathJoinSubstitution([ - # FindPackageShare("realsense2_camera"), "launch", "rs_launch.py" - # ]) - # ), - # launch_arguments={"camera": "depth_camera", "device_type": "d435"}.items(), - # ) - # ], condition=IfCondition(LaunchConfiguration("launch_depth_cam"))) - - # # IMU Interface - # imu_interface_group = GroupAction([ - # Node( - # # FIXME:rosserial does not exist in ros2, need to find another option - # package="rosserial_python", - # executable="serial_node", - # name="rosserial_python", - # parameters=[{"_port": "/dev/versavis", "_baud": 250000}], - # output="screen", - # respawn=True, - # ), - # Node( - # package="versavis_adis16448_receiver", - # executable="versavis_imu_receiver", - # name="versavis_imu_receiver", - # parameters=[{ - # "imu_accelerator_sensitivity": 0.000833, - # "imu_gyro_sensitivity": 0.04, - # "imu_acceleration_covariance": 0.043864908, - # "imu_gyro_covariance": 6e-9, - # "imu_sub_topic": "/versavis/imu_micro", - # "imu_pub_topic": "/imu", - # }], - # output="screen", - # ), - # ], condition=IfCondition(LaunchConfiguration("launch_imu_interface"))) - - # RGB Camera (FLIR Driver) - # rgb_camera_group = GroupAction([ - # ComposableNodeContainer( - # name="camera_container", - # namespace="rgb_camera", - # package="rclcpp_components", - # executable="component_container", - # composable_node_descriptions=[ - # ComposableNode( - # package="spinnaker_camera_driver", - # plugin="spinnaker_camera_driver::CameraDriver", - # name="rgb_camera_node", - # parameters=[{ - # "frame_id": "rgb_camera_optical_link", - # "serial_number": "0", - # "parameter_file": "/smb_ros2_workspace/src/core/smb_bringup/config/smb261.cam0.yaml", - # "acquisition_frame_rate": 4, - # "acquisition_frame_rate_enable": True, - # "image_format_color_coding": "BayerRG8", - # "color_processing_algorithm": "HQ_LINEAR", - # # "camerainfo_url": "file:///smb_ros2_workspace/src/core/smb_bringup/config/smb261.cam0.yaml", - # "acquisition_mode": "Continuous", - # "enable_trigger": "Off", - # "exposure_mode": "Timed", - # "exposure_auto": "Continuous", - # "auto_exposure_time_upper_limit": 5000, - # "auto_exposure_time_lower_limit": 300, - # "auto_gain": "Continuous", - # "auto_white_balance": "Continuous", - # }], - # ), - # ComposableNode( - # package="image_proc", - # plugin="image_proc::DebayerNode", - # name="image_proc_debayer", - # ), - # ], - # output="screen", - # ) - # ], condition=IfCondition(LaunchConfiguration("launch_rgb_cam"))) rgb_camera_group = GroupAction([ IncludeLaunchDescription( PythonLaunchDescriptionSource( @@ -147,44 +18,37 @@ def generate_launch_description(): ]) ), launch_arguments={ - 'serial': "'20010195'" # for Jetson only + 'serial': "'20023237'" # for Jetson only }.items() ) - ], condition=IfCondition(LaunchConfiguration("launch_rgb_cam"))) - - # # Power Status - # power_status_group = GroupAction([ - # IncludeLaunchDescription( - # PythonLaunchDescriptionSource( - # PathJoinSubstitution([ - # FindPackageShare("smb_powerstatus"), "launch", "smb_powerstatus.launch.py" - # ]) - # ) - # ) - # ], condition=IfCondition(LaunchConfiguration("launch_powerstatus"))) - - - # Visualize in Rviz - # rviz_config = get_package_share_directory('smb_bringup')+'/config/smb_vis.rviz' - - # rviz_group = GroupAction([ - # Node(namespace='rviz2', - # package='rviz2', - # executable='rviz2', - # arguments=['-d',rviz_config] - # ) - # ]) + ]) + + container = ComposableNodeContainer( + name='image_proc_container', + namespace='rgb_camera', + package='rclcpp_components', + executable='component_container', + composable_node_descriptions=[ + ComposableNode( + package='image_proc', + plugin='image_proc::DebayerNode', + name='debayer_node', + remappings=[ + ('image_raw', 'rgb_camera/image_raw'), + ('image_color', 'rgb_camera/image_debayered') + ] + ) + ], + output='screen', + parameters=[ + {"use_sim_time": False}, + ], + ) return LaunchDescription( - declared_arguments + [ - lidar_group, - # tracking_camera_group, - # depth_camera_group, - # imu_interface_group, rgb_camera_group, - # power_status_group, - # rviz_group, + container ] - ) \ No newline at end of file + ) diff --git a/launch/smb.launch.py b/launch/smb.launch.py deleted file mode 100644 index 7f4d7e4..0000000 --- a/launch/smb.launch.py +++ /dev/null @@ -1,128 +0,0 @@ -from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, IncludeLaunchDescription, GroupAction -from launch.conditions import IfCondition -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import LaunchConfiguration, PathJoinSubstitution -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackageShare - -def generate_launch_description(): - # Declare all launch arguments - declared_arguments = [ - DeclareLaunchArgument("simulation", default_value="false", description="Set simulation mode"), - DeclareLaunchArgument("launch_sensors", default_value="false", description="Launch sensors"), - DeclareLaunchArgument("use_lidar_odometry", default_value="false", description="Use LiDAR odometry"), - DeclareLaunchArgument("launch_tracking_cam", default_value="false", description="Launch tracking camera odometry"), - DeclareLaunchArgument("control_namespace", default_value="control", description="Namespace for the control node"), - DeclareLaunchArgument("description_name", default_value="smb_description", description="Name for the robot description"), - DeclareLaunchArgument("command_smb", default_value="true", description="Send twist commands from the software stack to the robot"), - DeclareLaunchArgument("mpc", default_value="false", description="Launch the MPC node"), - DeclareLaunchArgument("mpc_track_local_plan", default_value="false", description="Track the plan published from the mpc_path_publisher.py script in smb_mpc package"), - DeclareLaunchArgument("keyboard_teleop", default_value="false", description="Launch node to send control commands using the keyboard"), - DeclareLaunchArgument("joystick", default_value="false", description="Launch node to send control commands using the joystick"), - DeclareLaunchArgument("launch_rviz", default_value="false", description="Launch RViz"), - ] - - # Sensors launch group - sensors_group = GroupAction([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("smb"), "launch", "sensors.launch.py" - ]) - ), - condition=IfCondition(LaunchConfiguration("launch_sensors")), - launch_arguments={ - "smb_name": LaunchConfiguration("smb_name"), - "launch_imu_interface": LaunchConfiguration("launch_imu_interface"), - "launch_rgb_cam": LaunchConfiguration("launch_rgb_cam"), - "launch_depth_cam": LaunchConfiguration("launch_depth_cam"), - "launch_tracking_cam": LaunchConfiguration("launch_tracking_cam"), - }.items(), - ), - ]) - - # Lidar odometry launch group - lidar_odometry_group = GroupAction([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("smb_msf_graph"), "launch", "smb_msf_graph.launch.py" - ]) - ), - condition=IfCondition(LaunchConfiguration("use_lidar_odometry")), - ), - ]) - - # Tracking camera odometry node - tracking_camera_node = GroupAction([ - Node( - package="odometry_conversion", - executable="odometry_conversion_node", - name="tracking_camera_odometry_conversion", - parameters=[{ - "in_odom_frame": "tracking_camera_pose_frame", - "in_sensor_frame": "tracking_camera_pose_frame", - "out_odom_frame": "tracking_camera_odom", - "out_base_frame": "base_link", - "in_odom_topic": "/tracking_camera/odom/sample", - "out_odom_topic": "/base_odom", - "is_odom_child": True, - }], - condition=IfCondition(LaunchConfiguration("launch_tracking_cam")), - ), - ]) - - # Low-level controller - lowlevel_controller = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("smb_lowlevel_controller"), "launch", "smb_lowlevel_controller.launch.py" - ]) - ), - launch_arguments={ - "control_namespace": LaunchConfiguration("control_namespace"), - "description_name": LaunchConfiguration("description_name"), - "command_smb": LaunchConfiguration("command_smb"), - }.items(), - ) - - # Control node - control_node = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("smb_control"), "launch", "smb_control.launch.py" - ]) - ), - launch_arguments={ - "description_name": LaunchConfiguration("description_name"), - "control_namespace": LaunchConfiguration("control_namespace"), - "mpc": LaunchConfiguration("mpc"), - "mpc_track_local_plan": LaunchConfiguration("mpc_track_local_plan"), - "keyboard_teleop": LaunchConfiguration("keyboard_teleop"), - "joystick": LaunchConfiguration("joystick"), - "tracking_camera": LaunchConfiguration("launch_tracking_cam"), - }.items(), - ) - - # RViz launch - rviz_launch = IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare("smb_opc"), "launch", "opc.launch.py" - ]) - ), - condition=IfCondition(LaunchConfiguration("launch_rviz")), - ) - - return LaunchDescription( - declared_arguments + - [ - sensors_group, - lidar_odometry_group, - tracking_camera_node, - lowlevel_controller, - control_node, - rviz_launch, - ] - ) diff --git a/launch/smb_nuc_system.launch.py b/launch/smb_nuc_system.launch.py deleted file mode 100644 index da0ddd8..0000000 --- a/launch/smb_nuc_system.launch.py +++ /dev/null @@ -1,31 +0,0 @@ -from launch import LaunchDescription -from launch.actions import IncludeLaunchDescription -from launch.launch_description_sources import PythonLaunchDescriptionSource -from launch.substitutions import PathJoinSubstitution -from launch_ros.substitutions import FindPackageShare - - -def generate_launch_description(): - return LaunchDescription([ - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('smb_bringup'), 'launch', 'motor_controller.launch.py' - ]) - ) - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('smb_bringup'), 'launch', 'sensors.launch.py' - ]) - ) - ), - IncludeLaunchDescription( - PythonLaunchDescriptionSource( - PathJoinSubstitution([ - FindPackageShare('smb_bringup'), 'launch', 'smb.launch.py' - ]) - ) - ) - ]) diff --git a/rviz/debug.rviz b/rviz/debug.rviz new file mode 100644 index 0000000..d6f440f --- /dev/null +++ b/rviz/debug.rviz @@ -0,0 +1,652 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /MarkerArray1 + - /MarkerArray1/Namespaces1 + - /MarkerArray3 + Splitter Ratio: 0.4324618875980377 + Tree Height: 696 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Pose Estimate1 + - /Publish Point1 + - /Waypoint1 + - /Goalpoint1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + - /Current View1/Focal Point1 + Name: Views + Splitter Ratio: 0.5157480239868164 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: Deskewed Scan + - Class: joy_panel_cpp/AutonomyPanel + Name: AutonomyPanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Filter (blacklist): "" + Filter (whitelist): "" + Frame Timeout: 9999 + Frames: + All Enabled: true + Marker Scale: 5 + Name: tf tree + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: odom + Radius: 0.25 + Reference Frame: odom + Value: true + - Class: rviz_default_plugins/Axes + Enabled: true + Length: 1 + Name: base_link + Radius: 0.25 + Reference Frame: base_link + Value: true + Enabled: true + Name: Frames + - Class: rviz_common/Group + Displays: + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1024 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Deskewed Scan + Position Transformer: XYZ + Selectable: true + Size (Pixels): 2 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dlio/odom_node/pointcloud/deskewed + Use Fixed Frame: true + Use rainbow: true + Value: true + - Alpha: 0.10000000149011612 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 0 + Color Transformer: Intensity + Decay Time: 9999 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1024 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Dense Map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.009999999776482582 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dlio/odom_node/pointcloud/deskewed + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Autocompute Intensity Bounds: false + Autocompute Value Bounds: + Max Value: 10 + Min Value: 0 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1024 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Sparse Map + Position Transformer: XYZ + Selectable: true + Size (Pixels): 1 + Size (m): 0.05000000074505806 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dlio/map_node/map + Use Fixed Frame: true + Use rainbow: true + Value: false + Enabled: false + Name: Point Clouds + - Class: rviz_common/Group + Displays: + - Angle Tolerance: 0.009999999776482582 + Class: rviz_default_plugins/Odometry + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: false + Enabled: false + Keep: 1 + Name: Pose + Position Tolerance: 0.009999999776482582 + Shape: + Alpha: 1 + Axes Length: 1 + Axes Radius: 0.25 + Color: 255; 25; 0 + Head Length: 0.30000001192092896 + Head Radius: 0.10000000149011612 + Shaft Length: 1 + Shaft Radius: 0.05000000074505806 + Value: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dlio/odom_node/odom + Value: false + - Alpha: 1 + Arrow Length: 0.30000001192092896 + Axes Length: 0.5 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseArray + Color: 255; 25; 0 + Enabled: false + Head Length: 0.07000000029802322 + Head Radius: 0.029999999329447746 + Name: Keyframes + Shaft Length: 0.23000000417232513 + Shaft Radius: 0.009999999776482582 + Shape: Axes + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dlio/odom_node/keyframes + Value: false + - Alpha: 1 + Buffer Length: 1 + Class: rviz_default_plugins/Path + Color: 255; 209; 0 + Enabled: true + Head Diameter: 0.30000001192092896 + Head Length: 0.20000000298023224 + Length: 0.30000001192092896 + Line Style: Billboards + Line Width: 0.10000000149011612 + Name: Trajectory + Offset: + X: 0 + Y: 0 + Z: 0 + Pose Color: 255; 85; 255 + Pose Style: None + Radius: 0.029999999329447746 + Shaft Diameter: 0.10000000149011612 + Shaft Length: 0.10000000149011612 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /dlio/odom_node/path + Value: true + Enabled: true + Name: Odometry + - Alpha: 1 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 1 + Min Color: 0; 0; 0 + Min Intensity: 1 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.10000000149011612 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /free_paths + Use Fixed Frame: true + Use rainbow: true + 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_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 0.5398077964782715 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.05000000074505806 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /terrain_map + Use Fixed Frame: true + Use rainbow: true + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + angle_direct: false + boundary_edge: false + boundary_vertex: false + freespace_vertex: false + freespace_vgraph: true + frontier_vertex: false + global_vertex: false + global_vgraph: false + localrange_vertex: false + odom_edge: false + polygon_edge: true + to_goal_edge: false + trajectory_edge: false + trajectory_vertex: false + updating_vertex: false + vertex_angle: false + vertices_matches: true + visibility_edge: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /viz_graph_topic + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MarkerArray + Namespaces: + corner_direct: true + extend_viewpoint: true + origin_viewpoint: true + raytracing_line: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /viz_viewpoint_extend_topic + Value: true + - Alpha: 1 + Class: rviz_default_plugins/PointStamped + Color: 204; 41; 204 + Enabled: true + History Length: 1 + Name: PointStamped + Radius: 0.20000000298023224 + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /way_point + Value: true + - Class: rviz_default_plugins/Marker + Enabled: true + Name: Marker + Namespaces: + global_path: true + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /viz_path_topic + 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_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: Intensity + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 204 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: PointCloud2 + Position Transformer: XYZ + Selectable: true + Size (Pixels): 3 + Size (m): 0.009999999776482582 + Style: Flat Squares + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 1 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + LF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + LH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Link Tree Style: Links in Alphabetic Order + RF_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + RH_WHEEL: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_footprint: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + base_inertia: + Alpha: 1 + Show Axes: false + Show Trail: false + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + imu_sensor: + Alpha: 1 + Show Axes: false + Show Trail: false + lidar_mount_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rgb_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rgb_camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rslidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tracking_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + tracking_camera_pose_frame: + Alpha: 1 + Show Axes: false + Show Trail: false + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /viz_grid_map_topic + Value: false + Enabled: true + Global Options: + Background Color: 0; 0; 0 + Fixed Frame: odom + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /move_base_simple/goal + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + - Class: waypoint_rviz_plugin/WaypointTool + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: waypoint + - Class: goalpoint_rviz_plugin/GoalpointTool + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: goalpoint + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 19.277053833007812 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 1.1553990840911865 + Y: 0.07634935528039932 + Z: 0.7678568959236145 + Focal Shape Fixed Size: false + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 1.3250004053115845 + Target Frame: base_link + Value: Orbit (rviz_default_plugins) + Yaw: 3.1249778270721436 + Saved: ~ +Window Geometry: + AutonomyPanel: + collapsed: false + Displays: + collapsed: false + Height: 962 + Hide Left Dock: false + Hide Right Dock: true + QMainWindow State: 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 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: true + Width: 1918 + X: 0 + Y: 28