From 40abdfb0918bf6f41fa22a4a2287c2425dd7e36e Mon Sep 17 00:00:00 2001 From: SimonR99 Date: Tue, 18 Jun 2024 22:14:10 -0400 Subject: [PATCH] modification for real hardware --- src/rove_bringup/launch/autonomy.launch.py | 4 +- src/rove_bringup/launch/real.launch.py | 2 +- src/rove_description/config/basic.rviz | 130 ++++++++------------ src/rove_description/urdf/sensor.urdf.xacro | 24 ++++ src/rove_slam/config/ekf.yaml | 6 +- src/rove_slam/launch/slam3d_full.launch.py | 6 +- src/rove_zed/config/zed_mapping.yaml | 49 ++++---- src/rove_zed/launch/zed_mapping.launch.py | 7 +- 8 files changed, 113 insertions(+), 115 deletions(-) diff --git a/src/rove_bringup/launch/autonomy.launch.py b/src/rove_bringup/launch/autonomy.launch.py index 076b9d1..650994f 100644 --- a/src/rove_bringup/launch/autonomy.launch.py +++ b/src/rove_bringup/launch/autonomy.launch.py @@ -24,7 +24,7 @@ def generate_launch_description(): os.path.join(slam_pkg_path, "launch", "online_async_launch.py"), ), launch_arguments={ - "use_sim_time": "true", + "use_sim_time": use_sim_time, "slam_params_file": os.path.join( pkg_rove_slam, "config", "slam_config.yaml" ) @@ -34,7 +34,7 @@ def generate_launch_description(): slam3d = IncludeLaunchDescription( PythonLaunchDescriptionSource( - os.path.join(pkg_rove_slam, "launch", "slam3d_full.launch.py"), + os.path.join(pkg_rove_slam, "launch", "velodyne_3d.launch.py"), ), launch_arguments={ "use_sim_time": use_sim_time, diff --git a/src/rove_bringup/launch/real.launch.py b/src/rove_bringup/launch/real.launch.py index 5b5eff6..5d6a83e 100644 --- a/src/rove_bringup/launch/real.launch.py +++ b/src/rove_bringup/launch/real.launch.py @@ -124,7 +124,7 @@ def create_controller_node(node_name:str): zed = IncludeLaunchDescription( PythonLaunchDescriptionSource( os.path.join(pkg_rove_zed, 'launch', 'zed_mapping.launch.py'), - ) + ), ) return LaunchDescription([ diff --git a/src/rove_description/config/basic.rviz b/src/rove_description/config/basic.rviz index 60dbd12..9c7fd79 100644 --- a/src/rove_description/config/basic.rviz +++ b/src/rove_description/config/basic.rviz @@ -7,7 +7,7 @@ Panels: - /Global Options1 - /DepthCloud1/Auto Size1 Splitter Ratio: 0.5 - Tree Height: 517 + Tree Height: 325 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -52,11 +52,6 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true - camera_link: - Alpha: 1 - Show Axes: false - Show Trail: false - Value: true flipper_fl: Alpha: 1 Show Axes: false @@ -118,6 +113,11 @@ Visualization Manager: Show Axes: false Show Trail: false Value: true + zed_camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true Mass Properties: Inertia: false Mass: false @@ -159,9 +159,9 @@ Visualization Manager: Enabled: true Invert Rainbow: false Max Color: 255; 255; 255 - Max Intensity: 0 + Max Intensity: 208 Min Color: 0; 0; 0 - Min Intensity: 0 + Min Intensity: 1 Name: LaserScan Position Transformer: XYZ Selectable: true @@ -179,79 +179,19 @@ Visualization Manager: Use rainbow: true Value: true - Class: rviz_default_plugins/TF - Enabled: true + Enabled: false Frame Timeout: 15 Frames: All Enabled: false - base_link: - Value: true - camera_link: - Value: true - flipper_fl: - Value: true - flipper_fr: - Value: true - flipper_rl: - Value: true - flipper_rr: - Value: true - map: - Value: true - odom: - Value: true - sensor_link: - Value: true - track_fl: - Value: true - track_fr: - Value: true - track_l: - Value: true - track_r: - Value: true - track_rl: - Value: true - track_rr: - Value: true - velodyne_laser: - Value: true - velodyne_link: - Value: true Marker Scale: 1 Name: TF Show Arrows: true Show Axes: true Show Names: false Tree: - map: - odom: - base_link: - flipper_fl: - {} - flipper_fr: - {} - flipper_rl: - {} - flipper_rr: - {} - sensor_link: - camera_link: - {} - track_l: - track_fl: - {} - track_rl: - {} - track_r: - track_fr: - {} - track_rr: - {} - velodyne_link: - velodyne_laser: - {} + {} Update Interval: 0 - Value: true + Value: false - Alpha: 0.699999988079071 Class: rviz_default_plugins/Map Color Scheme: map @@ -357,7 +297,7 @@ Visualization Manager: y_scale: 1 z_scale: 1 Class: rviz_imu_plugin/Imu - Enabled: true + Enabled: false Name: Imu Topic: Depth: 5 @@ -366,7 +306,7 @@ Visualization Manager: History Policy: Keep Last Reliability Policy: Reliable Value: /imu - Value: true + Value: false fixed_frame_orientation: true - Alpha: 1 Autocompute Intensity Bounds: true @@ -400,7 +340,7 @@ Visualization Manager: Node filtering radius (m): 0 Position Transformer: XYZ Size (Pixels): 3 - Size (m): 0.10000000149011612 + Size (m): 0.05000000074505806 Style: Flat Squares Topic: Depth: 5 @@ -451,6 +391,32 @@ Visualization Manager: Use Fixed Frame: true Use rainbow: true Value: false + - Class: rviz_default_plugins/Camera + Enabled: true + Far Plane Distance: 100 + Image Rendering: background + Name: Camera + Overlay Alpha: 0.5 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /zed/zed_node/left/image_rect_color + Value: true + Visibility: + DepthCloud: true + Grid: true + Imu: true + LaserScan: true + Map: true + MapCloud: true + Path: true + RobotModel: true + TF: true + Value: true + filtered position: true + Zoom Factor: 1 Enabled: true Global Options: Background Color: 48; 48; 48 @@ -497,33 +463,35 @@ Visualization Manager: Views: Current: Class: rviz_default_plugins/Orbit - Distance: 11.600849151611328 + Distance: 6.484375 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: -1.209733486175537 - Y: -1.5692439079284668 - Z: 2.5361857414245605 + X: -0.19487914443016052 + Y: -0.5717933773994446 + Z: 2.147127151489258 Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 0.9553995728492737 + Pitch: 0.6753999590873718 Target Frame: Value: Orbit (rviz) - Yaw: 3.6536216735839844 + Yaw: 0.813623309135437 Saved: ~ Window Geometry: + Camera: + collapsed: false Displays: collapsed: false Height: 814 Hide Left Dock: false Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000015600000290fc020000000afb000000100044006900730070006c006100790073010000003d00000290000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + QMainWindow State: 000000ff00000000fd00000004000000000000015600000290fc020000000bfb000000100044006900730070006c006100790073010000003d000001d0000000c900fffffffb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000a0049006d00610067006500000002770000011b0000000000000000fb0000000a0049006d006100670065000000003b0000035e0000000000000000fb0000000c00430061006d0065007200610100000213000000ba0000002800ffffff000000010000010f0000035efc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b0000035e000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000044f0000003efc0100000002fb0000000800540069006d006501000000000000044f000002fb00fffffffb0000000800540069006d00650100000000000004500000000000000000000002f30000029000000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: diff --git a/src/rove_description/urdf/sensor.urdf.xacro b/src/rove_description/urdf/sensor.urdf.xacro index 3b01c59..b250f45 100644 --- a/src/rove_description/urdf/sensor.urdf.xacro +++ b/src/rove_description/urdf/sensor.urdf.xacro @@ -8,6 +8,12 @@ + + + + + + @@ -32,6 +38,24 @@ + + + + + + + + + + + + + + + + + + diff --git a/src/rove_slam/config/ekf.yaml b/src/rove_slam/config/ekf.yaml index 56c3cc8..c0c8684 100644 --- a/src/rove_slam/config/ekf.yaml +++ b/src/rove_slam/config/ekf.yaml @@ -2,7 +2,7 @@ ekf_filter_node_local: ros__parameters: frequency: 30.0 - two_d_mode: false + two_d_mode: true publish_tf: true map_frame: map odom_frame: odom @@ -18,9 +18,9 @@ ekf_filter_node_local: imu0: /imu imu0_config: [false, false, false, # x y z - false, false, true, # roll pitch yaw + true, true, true, # roll pitch yaw false, false, false, # vx vy vz - false, false, true, # vroll vpitch vyaw + true, true, true, # vroll vpitch vyaw false, false, false] # ax ay az imu0_queue_size: 20 diff --git a/src/rove_slam/launch/slam3d_full.launch.py b/src/rove_slam/launch/slam3d_full.launch.py index 4492c1a..70ba8ef 100644 --- a/src/rove_slam/launch/slam3d_full.launch.py +++ b/src/rove_slam/launch/slam3d_full.launch.py @@ -23,7 +23,7 @@ def generate_launch_description(): 'subscribe_depth':True, 'subscribe_odom_info':False, 'subscribe_scan_cloud': True, - 'subscribe_rgbd':True, + 'subscribe_rgb':True, 'approx_sync': True, 'publish_tf':True, 'Odom/Strategy':'0', @@ -81,7 +81,7 @@ def generate_launch_description(): output='screen', parameters=[{ 'max_clouds': 10, - 'fixed_frame_id':'', + 'fixed_frame_id':'sensor_link', 'use_sim_time': use_sim_time, }], remappings=[ @@ -121,5 +121,5 @@ def generate_launch_description(): camera_sync, point_cloud_assembler_node, rtabmap_node, - #rtabmap_viz_node + rtabmap_viz_node ]) diff --git a/src/rove_zed/config/zed_mapping.yaml b/src/rove_zed/config/zed_mapping.yaml index 58144ed..4391f4f 100644 --- a/src/rove_zed/config/zed_mapping.yaml +++ b/src/rove_zed/config/zed_mapping.yaml @@ -1,26 +1,27 @@ -# Parameters to override zed_wrapper parameters +# Parameters to override zed_wrapper parameters /**: - ros__parameters: - general: - grab_resolution: 'HD720' # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' - pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images - grab_frame_rate: 30 # ZED SDK internal grabbing rate - mapping: - mapping_enabled: false # True to enable mapping and fused point cloud pubblication - resolution: 0.2 # maps resolution in meters [min: 0.01f - max: 0.2f] - max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] - fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud - clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection - pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference. - pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference - depth: - depth_mode: "PERFORMANCE" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) - depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] - openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] - point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) - depth_confidence: 100 # [DYNAMIC] - depth_texture_conf: 100 # [DYNAMIC] - remove_saturated_areas: true # [DYNAMIC] + ros__parameters: + general: + grab_resolution: "VGA" # The native camera grab resolution. 'HD2K', 'HD1080', 'HD720', 'VGA', 'AUTO' + pub_frame_rate: 30.0 # frequency of publishing of visual images and depth images + grab_frame_rate: 30 # ZED SDK internal grabbing rate + camera_name: "zed" + mapping: + mapping_enabled: false # True to enable mapping and fused point cloud pubblication + resolution: 0.2 # maps resolution in meters [min: 0.01f - max: 0.2f] + max_mapping_range: 5.0 # maximum depth range while mapping in meters (-1 for automatic calculation) [2.0, 20.0] + fused_pointcloud_freq: 1.0 # frequency of the publishing of the fused colored point cloud + clicked_point_topic: "/clicked_point" # Topic published by Rviz when a point of the cloud is clicked. Used for plane detection + pd_max_distance_threshold: 0.15 # Plane detection: controls the spread of plane by checking the position difference. + pd_normal_similarity_threshold: 15.0 # Plane detection: controls the spread of plane by checking the angle difference + depth: + depth_mode: "PERFORMANCE" # Matches the ZED SDK setting: 'NONE', 'PERFORMANCE', 'QUALITY', 'ULTRA', 'NEURAL', 'NEURAL_PLUS' - Note: if 'NONE' all the modules that requires depth extraction are disabled by default (Pos. Tracking, Obj. Detection, Mapping, ...) + depth_stabilization: 1 # Forces positional tracking to start if major than 0 - Range: [0,100] + openni_depth_mode: false # 'false': 32bit float [meters], 'true': 16bit unsigned int [millimeters] + point_cloud_freq: 30.0 # [DYNAMIC] - frequency of the pointcloud publishing (equal or less to `grab_frame_rate` value) + depth_confidence: 100 # [DYNAMIC] + depth_texture_conf: 100 # [DYNAMIC] + remove_saturated_areas: true # [DYNAMIC] - min_depth: 0.2 # Min: 0.2, Max: 3.0 - max_depth: 10.0 # Max: 40.0 \ No newline at end of file + min_depth: 0.2 # Min: 0.2, Max: 3.0 + max_depth: 10.0 # Max: 40.0 diff --git a/src/rove_zed/launch/zed_mapping.launch.py b/src/rove_zed/launch/zed_mapping.launch.py index cb78318..9e1c9bf 100644 --- a/src/rove_zed/launch/zed_mapping.launch.py +++ b/src/rove_zed/launch/zed_mapping.launch.py @@ -21,7 +21,12 @@ def generate_launch_description(): launch_description_source=launch_file_path, launch_arguments=[ # `ros2 launch rove_zed zed_body_trck.launch.py --show-arguments` ['camera_model', camera_model], - ['ros_params_override_path', config_override_path] + ['ros_params_override_path', config_override_path], + ['publish_tf', 'false'], + ['publish_map_tf', 'false'], + ['map_frame', 'map'], + ['odometry_frame', 'odom'], + ['use_sim_time', 'false'], ] ) ]) \ No newline at end of file