From 62503a8a59fe70bf9e02e73cefda0493bacd1611 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Wed, 19 Feb 2025 19:06:37 +0000 Subject: [PATCH 01/15] Add Velodyne in simulation. Signed-off-by: Jelmer de Wolde --- .../{realsense.sh => sensors.sh} | 8 +- dockerfiles/rcdt_robotics.Dockerfile | 4 +- ros2_ws/src/rcdt_panther/CMakeLists.txt | 2 +- .../src/rcdt_panther/launch/panther.launch.py | 15 ++- .../src/rcdt_panther/urdf/panther.urdf.xacro | 29 ++++ .../urdf/rcdt_velodyne.urdf.xacro | 124 ++++++++++++++++++ .../launch/gazebo_robot.launch.py | 9 ++ 7 files changed, 183 insertions(+), 8 deletions(-) rename dockerfiles/install_scripts/{realsense.sh => sensors.sh} (75%) create mode 100644 ros2_ws/src/rcdt_panther/urdf/panther.urdf.xacro create mode 100644 ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro diff --git a/dockerfiles/install_scripts/realsense.sh b/dockerfiles/install_scripts/sensors.sh similarity index 75% rename from dockerfiles/install_scripts/realsense.sh rename to dockerfiles/install_scripts/sensors.sh index 45a54887..2255dbe1 100755 --- a/dockerfiles/install_scripts/realsense.sh +++ b/dockerfiles/install_scripts/sensors.sh @@ -3,9 +3,13 @@ # SPDX-License-Identifier: Apache-2.0 set -e - -pip install pyrealsense2 apt update + +# Realsense: apt install -y \ ros-humble-realsense2-camera \ ros-humble-realsense2-description +pip install pyrealsense2 + +# Velodyne: +apt install -y ros-humble-velodyne-description \ No newline at end of file diff --git a/dockerfiles/rcdt_robotics.Dockerfile b/dockerfiles/rcdt_robotics.Dockerfile index 3fa5f5b5..ae6a915b 100644 --- a/dockerfiles/rcdt_robotics.Dockerfile +++ b/dockerfiles/rcdt_robotics.Dockerfile @@ -32,8 +32,8 @@ RUN ./rosboard.sh COPY ./install_scripts/pyflow.sh . RUN ./pyflow.sh -COPY ./install_scripts/realsense.sh . -RUN ./realsense.sh +COPY ./install_scripts/sensors.sh . +RUN ./sensors.sh COPY ./install_scripts/sphinx.sh . RUN ./sphinx.sh diff --git a/ros2_ws/src/rcdt_panther/CMakeLists.txt b/ros2_ws/src/rcdt_panther/CMakeLists.txt index 8169cd7d..5194c756 100644 --- a/ros2_ws/src/rcdt_panther/CMakeLists.txt +++ b/ros2_ws/src/rcdt_panther/CMakeLists.txt @@ -17,7 +17,7 @@ install( # Shared folders: install( - DIRECTORY launch config + DIRECTORY launch config urdf DESTINATION share/${PROJECT_NAME} ) diff --git a/ros2_ws/src/rcdt_panther/launch/panther.launch.py b/ros2_ws/src/rcdt_panther/launch/panther.launch.py index facb73e6..66adc233 100644 --- a/ros2_ws/src/rcdt_panther/launch/panther.launch.py +++ b/ros2_ws/src/rcdt_panther/launch/panther.launch.py @@ -12,13 +12,17 @@ ) use_rviz_arg = LaunchArgument("rviz", True, [True, False]) +use_velodyne_arg = LaunchArgument("velodyne", False, [True, False]) def launch_setup(context: LaunchContext) -> None: use_rviz = use_rviz_arg.value(context) + use_velodyne = use_velodyne_arg.value(context) - xacro_path = get_file_path("panther_description", ["urdf"], "panther.urdf.xacro") - xacro_arguments = {"use_sim": "true"} + xacro_path = get_file_path("rcdt_panther", ["urdf"], "panther.urdf.xacro") + components_path = get_file_path("rcdt_panther", ["config"], "components.yaml") + xacro_arguments = {"use_sim": "true", "components_config_path": components_path} + xacro_arguments["load_velodyne"] = "true" if use_velodyne else "false" robot_description = get_robot_description(xacro_path, xacro_arguments) robot_state_publisher = Node( @@ -41,7 +45,11 @@ def launch_setup(context: LaunchContext) -> None: joint_state_broadcaster = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster"], + arguments=[ + "joint_state_broadcaster", + "-t", + "joint_state_broadcaster/JointStateBroadcaster", + ], ) controllers = IncludeLaunchDescription( @@ -87,6 +95,7 @@ def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ use_rviz_arg.declaration, + use_velodyne_arg.declaration, OpaqueFunction(function=launch_setup), ] ) diff --git a/ros2_ws/src/rcdt_panther/urdf/panther.urdf.xacro b/ros2_ws/src/rcdt_panther/urdf/panther.urdf.xacro new file mode 100644 index 00000000..49679107 --- /dev/null +++ b/ros2_ws/src/rcdt_panther/urdf/panther.urdf.xacro @@ -0,0 +1,29 @@ + + + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro b/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro new file mode 100644 index 00000000..163d6d5d --- /dev/null +++ b/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro @@ -0,0 +1,124 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + true + ${update_rate} + + ${topic} + false + + ${name}_lidar + ${name}_lidar + + + + + ${samples} + 1 + ${min_angle} + ${max_angle} + + + ${lasers} + 1 + -${15.0*M_PI/180.0} + ${15.0*M_PI/180.0} + + + + ${min_range} + ${max_range} + 0.001 + + + gaussian + 0.0 + 0.0 + + + + + + + \ No newline at end of file diff --git a/ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py b/ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py index e438d639..a38453f4 100644 --- a/ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py +++ b/ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py @@ -12,12 +12,14 @@ load_gazebo_ui_arg = LaunchArgument("load_gazebo_ui", False, [True, False]) world_arg = LaunchArgument("world", "empty_camera.sdf") use_realsense_arg = LaunchArgument("realsense", False, [True, False]) +use_velodyne_arg = LaunchArgument("velodyne", False, [True, False]) def launch_setup(context: LaunchContext) -> List: load_gazebo_ui = load_gazebo_ui_arg.value(context) world = world_arg.value(context) use_realsense = use_realsense_arg.value(context) + use_velodyne = use_velodyne_arg.value(context) sdf_file = get_file_path("rcdt_gz_worlds", ["worlds"], world) gz_args = f" -r {sdf_file}" @@ -45,6 +47,12 @@ def launch_setup(context: LaunchContext) -> List: "/camera/camera/depth/image_rect_raw_float@sensor_msgs/msg/Image@gz.msgs.Image", ] ) + if use_velodyne: + bridge_topics.extend( + [ + "/velodyne_points/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked", + ] + ) bridge = Node( package="ros_gz_bridge", @@ -65,6 +73,7 @@ def generate_launch_description() -> LaunchDescription: load_gazebo_ui_arg.declaration, world_arg.declaration, use_realsense_arg.declaration, + use_velodyne_arg.declaration, OpaqueFunction(function=launch_setup), ] ) From 08203c9ce6044e288907abe0b56358dc6e0dcbea Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 08:36:48 +0000 Subject: [PATCH 02/15] Change lidar topic to /scan to integrate with slam. Signed-off-by: Jelmer de Wolde --- ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro | 2 +- ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro b/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro index 163d6d5d..c0ab4479 100644 --- a/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro +++ b/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro @@ -18,7 +18,7 @@ Based on: *origin parent:=cover_link name:=velodyne - topic:=/velodyne_points + topic:=/scan update_rate:=10 lasers:=16 samples:=1875 diff --git a/ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py b/ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py index a38453f4..25892355 100644 --- a/ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py +++ b/ros2_ws/src/rcdt_utilities/launch/gazebo_robot.launch.py @@ -50,7 +50,8 @@ def launch_setup(context: LaunchContext) -> List: if use_velodyne: bridge_topics.extend( [ - "/velodyne_points/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked", + "/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan", + "/scan/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked", ] ) From 5e9e1b41187989e0c0951e479bc0e84a8de3afa6 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 08:41:39 +0000 Subject: [PATCH 03/15] Copy default slam parameter file. Signed-off-by: Jelmer de Wolde --- .../config/mapper_params_online_async.yaml | 74 +++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100644 ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml diff --git a/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml b/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml new file mode 100644 index 00000000..c8739f20 --- /dev/null +++ b/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml @@ -0,0 +1,74 @@ +slam_toolbox: + ros__parameters: + # Plugin params + solver_plugin: solver_plugins::CeresSolver + ceres_linear_solver: SPARSE_NORMAL_CHOLESKY + ceres_preconditioner: SCHUR_JACOBI + ceres_trust_strategy: LEVENBERG_MARQUARDT + ceres_dogleg_type: TRADITIONAL_DOGLEG + ceres_loss_function: None + + # ROS Parameters + odom_frame: odom + map_frame: map + base_frame: base_footprint + scan_topic: /scan + use_map_saver: true + mode: mapping #localization + + # if you'd like to immediately start continuing a map at a given pose + # or at the dock, but they are mutually exclusive, if pose is given + # will use pose + #map_file_name: test_steve + # map_start_pose: [0.0, 0.0, 0.0] + #map_start_at_dock: true + + debug_logging: false + throttle_scans: 1 + transform_publish_period: 0.02 #if 0 never publishes odometry + map_update_interval: 5.0 + resolution: 0.05 + min_laser_range: 0.0 #for rastering images + max_laser_range: 20.0 #for rastering images + minimum_time_interval: 0.5 + transform_timeout: 0.2 + tf_buffer_duration: 30. + stack_size_to_use: 40000000 #// program needs a larger stack size to serialize large maps + enable_interactive_mode: true + + # General Parameters + use_scan_matching: true + use_scan_barycenter: true + minimum_travel_distance: 0.5 + minimum_travel_heading: 0.5 + scan_buffer_size: 10 + scan_buffer_maximum_scan_distance: 10.0 + link_match_minimum_response_fine: 0.1 + link_scan_maximum_distance: 1.5 + loop_search_maximum_distance: 3.0 + do_loop_closing: true + loop_match_minimum_chain_size: 10 + loop_match_maximum_variance_coarse: 3.0 + loop_match_minimum_response_coarse: 0.35 + loop_match_minimum_response_fine: 0.45 + + # Correlation Parameters - Correlation Parameters + correlation_search_space_dimension: 0.5 + correlation_search_space_resolution: 0.01 + correlation_search_space_smear_deviation: 0.1 + + # Correlation Parameters - Loop Closure Parameters + loop_search_space_dimension: 8.0 + loop_search_space_resolution: 0.05 + loop_search_space_smear_deviation: 0.03 + + # Scan Matcher Parameters + distance_variance_penalty: 0.5 + angle_variance_penalty: 1.0 + + fine_search_angle_offset: 0.00349 + coarse_search_angle_offset: 0.349 + coarse_angle_resolution: 0.0349 + minimum_angle_penalty: 0.9 + minimum_distance_penalty: 0.5 + use_response_expansion: true From 4e8b9f52e36f90f2bd8345ea2729481520631147 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 09:08:02 +0000 Subject: [PATCH 04/15] Update parameters. Signed-off-by: Jelmer de Wolde --- .../src/rcdt_sensors/config/mapper_params_online_async.yaml | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml b/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml index c8739f20..72831267 100644 --- a/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml +++ b/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml @@ -11,7 +11,7 @@ slam_toolbox: # ROS Parameters odom_frame: odom map_frame: map - base_frame: base_footprint + base_frame: base_link scan_topic: /scan use_map_saver: true mode: mapping #localization @@ -28,7 +28,7 @@ slam_toolbox: transform_publish_period: 0.02 #if 0 never publishes odometry map_update_interval: 5.0 resolution: 0.05 - min_laser_range: 0.0 #for rastering images + min_laser_range: 1.0 #for rastering images max_laser_range: 20.0 #for rastering images minimum_time_interval: 0.5 transform_timeout: 0.2 From eda485e75cd2ac405cdd423119e5531ac07e37a7 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 13:23:59 +0000 Subject: [PATCH 05/15] Add rviz configuration for lidar. Signed-off-by: Jelmer de Wolde --- .../src/rcdt_panther/launch/panther.launch.py | 14 +- ros2_ws/src/rcdt_utilities/rviz/lidar.rviz | 256 ++++++++++++++++++ 2 files changed, 268 insertions(+), 2 deletions(-) create mode 100644 ros2_ws/src/rcdt_utilities/rviz/lidar.rviz diff --git a/ros2_ws/src/rcdt_panther/launch/panther.launch.py b/ros2_ws/src/rcdt_panther/launch/panther.launch.py index 66adc233..72aac173 100644 --- a/ros2_ws/src/rcdt_panther/launch/panther.launch.py +++ b/ros2_ws/src/rcdt_panther/launch/panther.launch.py @@ -11,11 +11,13 @@ get_robot_description, ) +load_gazebo_ui_arg = LaunchArgument("load_gazebo_ui", False, [True, False]) use_rviz_arg = LaunchArgument("rviz", True, [True, False]) use_velodyne_arg = LaunchArgument("velodyne", False, [True, False]) def launch_setup(context: LaunchContext) -> None: + load_gazebo_ui = load_gazebo_ui_arg.value(context) use_rviz = use_rviz_arg.value(context) use_velodyne = use_velodyne_arg.value(context) @@ -32,7 +34,11 @@ def launch_setup(context: LaunchContext) -> None: ) robot = IncludeLaunchDescription( - get_file_path("rcdt_utilities", ["launch"], "gazebo_robot.launch.py") + get_file_path("rcdt_utilities", ["launch"], "gazebo_robot.launch.py"), + launch_arguments={ + "velodyne": str(use_velodyne), + "load_gazebo_ui": str(load_gazebo_ui), + }.items(), ) static_transform_publisher = Node( @@ -57,7 +63,10 @@ def launch_setup(context: LaunchContext) -> None: ) rviz = IncludeLaunchDescription( - get_file_path("rcdt_utilities", ["launch"], "rviz.launch.py") + get_file_path("rcdt_utilities", ["launch"], "rviz.launch.py"), + launch_arguments={ + "rviz_display_config": "lidar.rviz" if use_velodyne else "general.rviz", + }.items(), ) joy = Node( @@ -94,6 +103,7 @@ def launch_setup(context: LaunchContext) -> None: def generate_launch_description() -> LaunchDescription: return LaunchDescription( [ + load_gazebo_ui_arg.declaration, use_rviz_arg.declaration, use_velodyne_arg.declaration, OpaqueFunction(function=launch_setup), diff --git a/ros2_ws/src/rcdt_utilities/rviz/lidar.rviz b/ros2_ws/src/rcdt_utilities/rviz/lidar.rviz new file mode 100644 index 00000000..54e98421 --- /dev/null +++ b/ros2_ws/src/rcdt_utilities/rviz/lidar.rviz @@ -0,0 +1,256 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 78 + Name: Displays + Property Tree Widget: + Expanded: + - /Global Options1 + - /Status1 + Splitter Ratio: 0.5 + Tree Height: 752 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: + - /2D Goal Pose1 + - /Publish Point1 + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: rviz_common/Time + Experimental: false + Name: Time + SyncMode: 0 + SyncSource: PointCloud2 +Visualization Manager: + Class: "" + Displays: + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: true + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: true + - 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 + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + body_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + cover_link: + Alpha: 1 + Show Axes: false + Show Trail: false + fl_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + fr_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + front_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + lights_channel_1_link: + Alpha: 1 + Show Axes: false + Show Trail: false + lights_channel_2_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rear_bumper_link: + Alpha: 1 + Show Axes: false + Show Trail: false + rl_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + rr_wheel_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_lidar: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: RobotModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: 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 + 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 + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /scan/points + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Global Options: + Background Color: 48; 48; 48 + Fixed Frame: world + 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: /goal_pose + - Class: rviz_default_plugins/PublishPoint + Single click: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /clicked_point + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Class: rviz_default_plugins/Orbit + Distance: 2.8377578258514404 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: -0.15223704278469086 + Y: -0.2675058841705322 + Z: 0.3384692668914795 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Pitch: 0.32539844512939453 + Target Frame: + Value: Orbit (rviz) + Yaw: 0.965397834777832 + Saved: ~ +Window Geometry: + Displays: + collapsed: false + Height: 1043 + Hide Left Dock: false + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd00000004000000000000015600000379fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b00000379000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000379fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b00000379000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007800000003efc0100000002fb0000000800540069006d00650100000000000007800000025300fffffffb0000000800540069006d006501000000000000045000000000000000000000050f0000037900000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Selection: + collapsed: false + Time: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 1920 + X: 294 + Y: 0 From 8ee1a8d854b63443114355d5379deca9c25213e3 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 13:24:43 +0000 Subject: [PATCH 06/15] Add panther documentation. Signed-off-by: Jelmer de Wolde --- docs/content/panther.md | 143 ++++++++++++++++++++++++ docs/img/panther/simulation.png | 3 + docs/img/panther/teltonika_settings.png | 3 + docs/img/panther/velodyne_settings.png | 3 + docs/index.rst | 1 + 5 files changed, 153 insertions(+) create mode 100644 docs/content/panther.md create mode 100644 docs/img/panther/simulation.png create mode 100644 docs/img/panther/teltonika_settings.png create mode 100644 docs/img/panther/velodyne_settings.png diff --git a/docs/content/panther.md b/docs/content/panther.md new file mode 100644 index 00000000..a4ac347a --- /dev/null +++ b/docs/content/panther.md @@ -0,0 +1,143 @@ + + +# Panther + +This page gives information about usage of the Husarion Panther. + +## Physical robot + +### Quick start + +The physical robot can be started by: + +- Enable the battery (switch at front of robot). +- Start the robot (press red power button). +- Wait, till the [E_STOP animation](https://husarion.com/manuals/panther/software/ros2/robot-management/#led-animations) is played +- Release hardware stop (rotate red emergency button if it was pressed). +- Start Logitech gamepad: + - press Logitech button. + - press *mode* button if mode light is on (should be off). + - have the switch at the back on *D*. +- Remove E_STOP by *Left Trigger + A* on the gamepad. +- You can drive by pressing *Left Button* and use the two joysticks. +- You can enable the E_STOp by pressing *B*. +- See [this](https://husarion.com/manuals/panther/software/ros2/robot-management/#gamepad) for more information about gamepad control. + +The robot can be shut down by: + +- Shut down the robot (hold red button next to battery switch till it starts blinking). +- Wait till all lights are off. +- Disable the battery (switch at front of robot) + +### Configuration + +Our Husarion Panther is configured with 3 computers with the following default settings: + +| computer | ip-address | ssh username | ssh password | +|--------------------------|------------|--------------|--------------| +| Teltonika RUTX11 | 10.15.20.1 | root | Husarion1 | +| Raspberry Pi 4 | 10.15.20.2 | husarion | husarion | +| Lenovo ThinkStation P360 | 10.15.20.3 | husarion | husarion | + +When the Panther is started, two WiFi networks (*Panther_* and *Panther_5G_*) should be available, both with default password `husarion`. One can connect with one of the WiFi networks or connect using a ethernet cable directly to the Teltonika (this may require to remove on of the other ethernet cables, like the one of the lidar). After connecting, it should be possible to ssh into the three computers. + +**Teltonika RUTX11:** +\ +This is an industrial router. The *Raspberry Pi 4* and *Lenovo ThinkStation P360* are connected to the *Teltonika* by Ethernet. Also the *Velodyne Lidar* and is connected to the *Teltonika* by Ethernet. A [combo anetnna](https://teltonika-networks.com/products/accessories/antenna-options/combo-mimo-mobilegnsswi-fi-roof-sma-antenna) (black dome) is also connected, which enables the *Teltonika* to obtain GPS location. + +**Raspberry Pi 4 :** +\ +The *Raspberry Pi 4* is built in the front of the Panther and not directly accessible. Two Docker images are pre-installed: a [docker image](https://hub.docker.com/r/husarion/panther) of the [panther_ros](https://github.com/husarion/panther_ros) repository and a [docker image](https://hub.docker.com/r/husarion/joy2twist) of the [joy_to_wist](https://github.com/husarion/joy2twist) repository. Both images are started automatically when the robot starts. The first image runs all the required software to use the robot, like motor control and led control. The second image enables gamepad control with the Logitech gamepad shipped with the robot, when the USB receiver is connected to the USB port at the front of the robot. + +We have also cloned the [nmea-gps-docker](https://github.com/husarion/nmea-gps-docker/tree/ros2) repository with a docker that enables use of GPS in ROS. This docker images get's started automatically as well when the the Panther starts. For more information about the use of this docker, see the Sensors section. + +**Lenovo ThinkStation P360** +\ +The *Lenovo ThinkStation P360* is a powerful computer, used to handle the camera stream. The robot is shipped with a ZED 2i depth camera, which can be connected to the *ThinkStation* by USB-C. We have installed the required Nvidia software, based on our own [Docker installation](docker.md) to run dockers with use of the Nvidia CPU. Next, we cloned Husarion's [zed-docker](https://github.com/husarion/zed-docker), [realsense-docker](https://github.com/husarion/realsense-docker/tree/ros2) and [velodyne-docker](https://github.com/husarion/velodyne-docker/tree/ros2) to the *ThinkStation*. These docker images enable easy use of the ZED 2i and Realsense depth camera's or *Velodyne* lidar. For more information about the use of these dockers, see the Sensors section + +### Sensors + +The Husarion Panther is shipped with different sensors: + +- ZED 2i depth camera +- Velodyne VLP 16 lidar +- GPS (using Teltonika RUTX 11) + +For all sensors, Husarion provides docker images to easily start them connected with ROS. + +**ZED 2i:** +\ +To use the ZED 2i camera, ssh into the *ThinkStation*. Here you can find the cloned `zed-docker` repository folder. One can start the docker with these commands: + +```bash +cd ~/zed-docker/demo; +xhost local:root; +export ZED_IMAGE=husarion/zed-desktop:humble; +export CAMERA_MODEL=zed2; +docker compose up zed; +``` + +We also created the script `~/zed` which executes these commands. Running the docker with the camera connected should start the ROS nodes that publish the camera stream on ROS topics. + +**Realsense:** +\ +Use of a Realsense is similar to the use of ZED camera. You can find the `realsense-docker` repository folder on the *ThinkStation* and start the docker: + +```bash +cd ~/realsense-docker/demo; +xhost local:root; +docker compose up realsense; +``` + +We also created the scrip `~/realsense` which executes these commands. Running the docker with the camera connected should start the ROS nodes that publish the camera stream on ROS topics. + +**Velodyne:** +\ +To use the Velodyne lidar, we have to make sure that the *Velodyne* uses the *ThinkStation* as host. The settings of the **Velodyne* can be reached by it's ip-adress, which can be found by a network scan: + +```bash +nmap -sn 10.15.20.0/24 +``` + +One of the devices in the network should be `che.lan` (which is the *Velodyne*), which in our case has the ip-adress `10.15.20.154`. Now we can open the settings by going to this ip-adress in a browser: + +![velodyne-settings](../img/panther/velodyne_settings.png) + +Here, we can set the host ip-adress to `10.15.20.3` (the ip-adress of the *ThinkStation*). Next, you can find the `velodyne-docker` repository folder on the *ThinkStation* and start the docker: + +```bash +cd ~/velodyne-docker/demo; +docker compose up; +``` + +We also created the script `~/velodyne` which executes these commands. Note that the settings of this docker can be changed in the two files in the `~velodyne-docker/demo/config/` folder. Especially the *device_ip* parameter in the `panther_velodyne_driver.yaml` is important and needs to be the ip-adress of the *Velodyne* (`10.15.20.154` in our case). + +**GPS:** +\ +To use the GPS in ROS, the `nmea-gps-docker` image is started automatically on the *Raspberry Pi*. This docker only works if the *Teltonika* forwards the GPS data to the *Raspberry Pi*. The corresponding settings can be adapted by going to the ip-adress of the *Teltonika* (`10.15.20.1`). Default username is `admin` and default password is `Husarion1` and logging in should give you the settings: + +![teltonika-settings](../img/panther/teltonika_settings.png) + +Make sure that Hostname is set to the ip-adress of the *Raspberry Pi* (`10.15.20.2`). The docker image should now publish the GPS location on a ROS topic, if the location is available. Note that GPS might not work indoor. The GPS can also be tested in the *Teltonika* [map page](https://10.15.20.1/services/gps/map). + +## Simulation + +You can start a simulation of the Panther using the `rcdt_panther/rcdt_panther.launch.py` file. It can be started including a simulated Velodyne lidar by passing the `velodyne:=True` flag: + +```bash +ros2 launch rcdt_franka franka.launch.py velodyne:=True +``` + +This should start the simulation with the following Rviz visualization: + +![simulattion](../img/panther/simulation.png) + +You can control the simulated Panther if a gamepad is connected. By default, a simulated hardware stop is triggered and you need to reset this before you can control the Panther: + +```bash +ros2 service call /hardware/e_stop_reset std_srvs/srv/Trigger {} +``` diff --git a/docs/img/panther/simulation.png b/docs/img/panther/simulation.png new file mode 100644 index 00000000..ae23edb3 --- /dev/null +++ b/docs/img/panther/simulation.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:8496a962b63147ae4cbfcd05f520ac0481ce55652d46e68b52b7a221143126d4 +size 194122 diff --git a/docs/img/panther/teltonika_settings.png b/docs/img/panther/teltonika_settings.png new file mode 100644 index 00000000..44dd6a20 --- /dev/null +++ b/docs/img/panther/teltonika_settings.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:050b955e3a6db886fc5a52a65ad3d63c34bc9a9c43be409a19993efda9c17be9 +size 109998 diff --git a/docs/img/panther/velodyne_settings.png b/docs/img/panther/velodyne_settings.png new file mode 100644 index 00000000..dd2dd8ca --- /dev/null +++ b/docs/img/panther/velodyne_settings.png @@ -0,0 +1,3 @@ +version https://git-lfs.github.com/spec/v1 +oid sha256:b0b6c165edf646da9e67cc209e686058c3642a933043878669de4e7f13f999ff +size 118989 diff --git a/docs/index.rst b/docs/index.rst index 63de6464..26650c34 100644 --- a/docs/index.rst +++ b/docs/index.rst @@ -21,6 +21,7 @@ content/conventions/ros_package_msgs.md content/franka.md + content/panther.md content/moveit.md content/realsense.md content/pyflow.md From 78805686468fd535dab7f6a13d7f0a3278e57e82 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 13:28:07 +0000 Subject: [PATCH 07/15] Fix typos. Signed-off-by: Jelmer de Wolde --- docs/content/panther.md | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/docs/content/panther.md b/docs/content/panther.md index a4ac347a..40ed52fe 100644 --- a/docs/content/panther.md +++ b/docs/content/panther.md @@ -24,7 +24,7 @@ The physical robot can be started by: - have the switch at the back on *D*. - Remove E_STOP by *Left Trigger + A* on the gamepad. - You can drive by pressing *Left Button* and use the two joysticks. -- You can enable the E_STOp by pressing *B*. +- You can enable the E_STOP by pressing *B*. - See [this](https://husarion.com/manuals/panther/software/ros2/robot-management/#gamepad) for more information about gamepad control. The robot can be shut down by: @@ -47,7 +47,7 @@ When the Panther is started, two WiFi networks (*Panther_* and *P **Teltonika RUTX11:** \ -This is an industrial router. The *Raspberry Pi 4* and *Lenovo ThinkStation P360* are connected to the *Teltonika* by Ethernet. Also the *Velodyne Lidar* and is connected to the *Teltonika* by Ethernet. A [combo anetnna](https://teltonika-networks.com/products/accessories/antenna-options/combo-mimo-mobilegnsswi-fi-roof-sma-antenna) (black dome) is also connected, which enables the *Teltonika* to obtain GPS location. +This is an industrial router. The *Raspberry Pi 4* and *Lenovo ThinkStation P360* are connected to the *Teltonika* by Ethernet. Also the *Velodyne Lidar* and is connected to the *Teltonika* by Ethernet. A [combo antenna](https://teltonika-networks.com/products/accessories/antenna-options/combo-mimo-mobilegnsswi-fi-roof-sma-antenna) (black dome) is also connected, which enables the *Teltonika* to obtain GPS location. **Raspberry Pi 4 :** \ @@ -57,7 +57,7 @@ We have also cloned the [nmea-gps-docker](https://github.com/husarion/nmea-gps-d **Lenovo ThinkStation P360** \ -The *Lenovo ThinkStation P360* is a powerful computer, used to handle the camera stream. The robot is shipped with a ZED 2i depth camera, which can be connected to the *ThinkStation* by USB-C. We have installed the required Nvidia software, based on our own [Docker installation](docker.md) to run dockers with use of the Nvidia CPU. Next, we cloned Husarion's [zed-docker](https://github.com/husarion/zed-docker), [realsense-docker](https://github.com/husarion/realsense-docker/tree/ros2) and [velodyne-docker](https://github.com/husarion/velodyne-docker/tree/ros2) to the *ThinkStation*. These docker images enable easy use of the ZED 2i and Realsense depth camera's or *Velodyne* lidar. For more information about the use of these dockers, see the Sensors section +The *Lenovo ThinkStation P360* is a powerful computer, used to handle the camera stream. The robot is shipped with a ZED 2i depth camera, which can be connected to the *ThinkStation* by USB-C. We have installed the required Nvidia software, based on our own [Docker installation](docker.md) to run dockers with use of the Nvidia CPU. Next, we cloned Husarion's [zed-docker](https://github.com/husarion/zed-docker), [realsense-docker](https://github.com/husarion/realsense-docker/tree/ros2) and [velodyne-docker](https://github.com/husarion/velodyne-docker/tree/ros2) to the *ThinkStation*. These docker images enable easy use of the *ZED* and *Realsense* camera's or *Velodyne* lidar. For more information about the use of these dockers, see the Sensors section ### Sensors @@ -97,17 +97,17 @@ We also created the scrip `~/realsense` which executes these commands. Running t **Velodyne:** \ -To use the Velodyne lidar, we have to make sure that the *Velodyne* uses the *ThinkStation* as host. The settings of the **Velodyne* can be reached by it's ip-adress, which can be found by a network scan: +To use the Velodyne lidar, we have to make sure that the *Velodyne* uses the *ThinkStation* as host. The settings of the **Velodyne* can be reached by it's ip-address, which can be found by a network scan: ```bash nmap -sn 10.15.20.0/24 ``` -One of the devices in the network should be `che.lan` (which is the *Velodyne*), which in our case has the ip-adress `10.15.20.154`. Now we can open the settings by going to this ip-adress in a browser: +One of the devices in the network should be `che.lan` (which is the *Velodyne*), which in our case has the ip-address `10.15.20.154`. Now we can open the settings by going to this ip-address in a browser: ![velodyne-settings](../img/panther/velodyne_settings.png) -Here, we can set the host ip-adress to `10.15.20.3` (the ip-adress of the *ThinkStation*). Next, you can find the `velodyne-docker` repository folder on the *ThinkStation* and start the docker: +Here, we can set the host ip-address to `10.15.20.3` (the ip-address of the *ThinkStation*). Next, you can find the `velodyne-docker` repository folder on the *ThinkStation* and start the docker: ```bash cd ~/velodyne-docker/demo; @@ -118,11 +118,11 @@ We also created the script `~/velodyne` which executes these commands. Note that **GPS:** \ -To use the GPS in ROS, the `nmea-gps-docker` image is started automatically on the *Raspberry Pi*. This docker only works if the *Teltonika* forwards the GPS data to the *Raspberry Pi*. The corresponding settings can be adapted by going to the ip-adress of the *Teltonika* (`10.15.20.1`). Default username is `admin` and default password is `Husarion1` and logging in should give you the settings: +To use the GPS in ROS, the `nmea-gps-docker` image is started automatically on the *Raspberry Pi*. This docker only works if the *Teltonika* forwards the GPS data to the *Raspberry Pi*. The corresponding settings can be adapted by going to the ip-address of the *Teltonika* (`10.15.20.1`). Default username is `admin` and default password is `Husarion1` and logging in should give you the settings: ![teltonika-settings](../img/panther/teltonika_settings.png) -Make sure that Hostname is set to the ip-adress of the *Raspberry Pi* (`10.15.20.2`). The docker image should now publish the GPS location on a ROS topic, if the location is available. Note that GPS might not work indoor. The GPS can also be tested in the *Teltonika* [map page](https://10.15.20.1/services/gps/map). +Make sure that Hostname is set to the ip-address of the *Raspberry Pi* (`10.15.20.2`). The docker image should now publish the GPS location on a ROS topic, if the location is available. Note that GPS might not work indoor. The GPS can also be tested in the *Teltonika* [map page](https://10.15.20.1/services/gps/map). ## Simulation @@ -134,7 +134,7 @@ ros2 launch rcdt_franka franka.launch.py velodyne:=True This should start the simulation with the following Rviz visualization: -![simulattion](../img/panther/simulation.png) +![simulation](../img/panther/simulation.png) You can control the simulated Panther if a gamepad is connected. By default, a simulated hardware stop is triggered and you need to reset this before you can control the Panther: From b71d3050bc119fd614992d503f1bf7dba4671e99 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 13:31:48 +0000 Subject: [PATCH 08/15] Fix reuse. Signed-off-by: Jelmer de Wolde --- docs/img/panther/simulation.png.license | 3 +++ docs/img/panther/teltonika_settings.png.license | 3 +++ docs/img/panther/velodyne_settings.png.license | 3 +++ .../src/rcdt_sensors/config/mapper_params_online_async.yaml | 4 ++++ ros2_ws/src/rcdt_utilities/rviz/lidar.rviz | 4 ++++ 5 files changed, 17 insertions(+) create mode 100644 docs/img/panther/simulation.png.license create mode 100644 docs/img/panther/teltonika_settings.png.license create mode 100644 docs/img/panther/velodyne_settings.png.license diff --git a/docs/img/panther/simulation.png.license b/docs/img/panther/simulation.png.license new file mode 100644 index 00000000..14a6ee96 --- /dev/null +++ b/docs/img/panther/simulation.png.license @@ -0,0 +1,3 @@ +SPDX-FileCopyrightText: Alliander N. V. + +SPDX-License-Identifier: Apache-2.0 diff --git a/docs/img/panther/teltonika_settings.png.license b/docs/img/panther/teltonika_settings.png.license new file mode 100644 index 00000000..14a6ee96 --- /dev/null +++ b/docs/img/panther/teltonika_settings.png.license @@ -0,0 +1,3 @@ +SPDX-FileCopyrightText: Alliander N. V. + +SPDX-License-Identifier: Apache-2.0 diff --git a/docs/img/panther/velodyne_settings.png.license b/docs/img/panther/velodyne_settings.png.license new file mode 100644 index 00000000..14a6ee96 --- /dev/null +++ b/docs/img/panther/velodyne_settings.png.license @@ -0,0 +1,3 @@ +SPDX-FileCopyrightText: Alliander N. V. + +SPDX-License-Identifier: Apache-2.0 diff --git a/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml b/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml index 72831267..48d5d306 100644 --- a/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml +++ b/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml @@ -1,3 +1,7 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + slam_toolbox: ros__parameters: # Plugin params diff --git a/ros2_ws/src/rcdt_utilities/rviz/lidar.rviz b/ros2_ws/src/rcdt_utilities/rviz/lidar.rviz index 54e98421..f958bb45 100644 --- a/ros2_ws/src/rcdt_utilities/rviz/lidar.rviz +++ b/ros2_ws/src/rcdt_utilities/rviz/lidar.rviz @@ -1,3 +1,7 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + Panels: - Class: rviz_common/Displays Help Height: 78 From a883c7be59beb21d5e9a6417de5ad1eafbbdf678 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 13:50:59 +0000 Subject: [PATCH 09/15] Add nav2 and slam to docker. Signed-off-by: Jelmer de Wolde --- dockerfiles/install_scripts/dev_packages.sh | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/dockerfiles/install_scripts/dev_packages.sh b/dockerfiles/install_scripts/dev_packages.sh index 1a2b0f5b..c887d95e 100755 --- a/dockerfiles/install_scripts/dev_packages.sh +++ b/dockerfiles/install_scripts/dev_packages.sh @@ -8,4 +8,9 @@ set -e pip install "numpy>=1.23.0,<2.0" # Specifying currently newest version of transforms3d to avoid conflict with imported numpy.float in older version. -pip install "transforms3d>=0.4.2" \ No newline at end of file +pip install "transforms3d>=0.4.2" + +sudo apt update +sudo apt install -y \ + ros-humble-navigation2 \ + ros-humble-slam-toolbox \ No newline at end of file From 15bf4cdbfcf5402c164502f0cc90a4b4d6dd26cc Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Thu, 20 Feb 2025 15:11:42 +0000 Subject: [PATCH 10/15] Add world for panther. Signed-off-by: Jelmer de Wolde --- ros2_ws/src/rcdt_gz_worlds/worlds/walls.sdf | 229 ++++++++++++++++++ .../src/rcdt_panther/launch/panther.launch.py | 1 + 2 files changed, 230 insertions(+) create mode 100644 ros2_ws/src/rcdt_gz_worlds/worlds/walls.sdf diff --git a/ros2_ws/src/rcdt_gz_worlds/worlds/walls.sdf b/ros2_ws/src/rcdt_gz_worlds/worlds/walls.sdf new file mode 100644 index 00000000..7e306ca0 --- /dev/null +++ b/ros2_ws/src/rcdt_gz_worlds/worlds/walls.sdf @@ -0,0 +1,229 @@ + + + + +]> + + + + 0.001 + 1.0 + + + + + + + + + + + + + + true + 0 0 10 0 0 0 + 0.8 0.8 0.8 1 + 0.2 0.2 0.2 1 + + 1000 + 0.9 + 0.01 + 0.001 + + -0.5 0.1 -0.9 + + + + true + + + + + 0 0 1 + 100 100 + + + + + + + 0 0 1 + 100 100 + + + + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + 0.8 0.8 0.8 1 + + + + + + + true + 5 0 0.5 0 0 0 + + + + + 0.05 10.0 1 + + + + + + + 0.05 10.0 1 + + + + + + + + true + -5 0 0.5 0 0 0 + + + + + 0.05 10.0 1 + + + + + + + 0.05 10.0 1 + + + + + + + + true + 0 5 0.5 0 0 0 + + + + + 10.0 0.05 1 + + + + + + + 10.0 0.05 1 + + + + + + + + true + 0 -5 0.5 0 0 0 + + + + + 10.0 0.05 1 + + + + + + + 10.0 0.05 1 + + + + + + + + true + 2 2 0.5 0 0 0 + + + + + 1 1 1 + + + + + + + 1 1 1 + + + + + + + + true + 2 -2 0.5 0 0 0 + + + + + 1 + 2 + + + + + + + 1 + 2 + + + + + + + + true + -2.5 0 0.5 0 0 0 + + + + + 0.5 5 1 + + + + + + + 0.5 5 1 + + + + + + + + \ No newline at end of file diff --git a/ros2_ws/src/rcdt_panther/launch/panther.launch.py b/ros2_ws/src/rcdt_panther/launch/panther.launch.py index 72aac173..88a869e6 100644 --- a/ros2_ws/src/rcdt_panther/launch/panther.launch.py +++ b/ros2_ws/src/rcdt_panther/launch/panther.launch.py @@ -36,6 +36,7 @@ def launch_setup(context: LaunchContext) -> None: robot = IncludeLaunchDescription( get_file_path("rcdt_utilities", ["launch"], "gazebo_robot.launch.py"), launch_arguments={ + "world": "walls.sdf", "velodyne": str(use_velodyne), "load_gazebo_ui": str(load_gazebo_ui), }.items(), From 6468be3370df9607d18ef10321e0f740fc9a8553 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Fri, 21 Feb 2025 10:26:06 +0000 Subject: [PATCH 11/15] Add node to pass cmd_vel to controller. Signed-off-by: Jelmer de Wolde --- .../src_py/twist_to_twist_stamped.py | 42 +++++++++++++++++++ 1 file changed, 42 insertions(+) create mode 100755 ros2_ws/src/rcdt_utilities/src_py/twist_to_twist_stamped.py diff --git a/ros2_ws/src/rcdt_utilities/src_py/twist_to_twist_stamped.py b/ros2_ws/src/rcdt_utilities/src_py/twist_to_twist_stamped.py new file mode 100755 index 00000000..0ec8a444 --- /dev/null +++ b/ros2_ws/src/rcdt_utilities/src_py/twist_to_twist_stamped.py @@ -0,0 +1,42 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + +import rclpy +from geometry_msgs.msg import Twist, TwistStamped +from rcdt_utilities.launch_utils import spin_node +from rclpy import logging +from rclpy.node import Node + +ros_logger = logging.get_logger(__name__) + + +class TwistToTwistStamped(Node): + def __init__(self) -> bool: + super().__init__("twist_to_twist_stamped_node") + self.declare_parameter("sub_topic", "/cmd_vel") + self.declare_parameter("pub_topic", "/diff_drive_controller/cmd_vel") + + sub_topic = self.get_parameter("sub_topic").get_parameter_value().string_value + pub_topic = self.get_parameter("pub_topic").get_parameter_value().string_value + + self.create_subscription(Twist, sub_topic, self.handle_input, 10) + self.pub = self.create_publisher(TwistStamped, pub_topic, 10) + self.pub_msg = TwistStamped() + + def handle_input(self, sub_msg: Twist) -> None: + self.pub_msg.twist = sub_msg + self.pub_msg.header.stamp = self.get_clock().now().to_msg() + self.pub.publish(self.pub_msg) + + +def main(args: str = None) -> None: + rclpy.init(args=args) + node = TwistToTwistStamped() + spin_node(node) + + +if __name__ == "__main__": + main() From 8d7e5886539f4ca54ecb5bb562759eea18c01caf Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Fri, 21 Feb 2025 13:00:47 +0000 Subject: [PATCH 12/15] Add nav2_bringup package to docker. Signed-off-by: Jelmer de Wolde --- dockerfiles/install_scripts/dev_packages.sh | 1 + 1 file changed, 1 insertion(+) diff --git a/dockerfiles/install_scripts/dev_packages.sh b/dockerfiles/install_scripts/dev_packages.sh index c887d95e..d585f567 100755 --- a/dockerfiles/install_scripts/dev_packages.sh +++ b/dockerfiles/install_scripts/dev_packages.sh @@ -13,4 +13,5 @@ pip install "transforms3d>=0.4.2" sudo apt update sudo apt install -y \ ros-humble-navigation2 \ + ros-humble-nav2-bringup \ ros-humble-slam-toolbox \ No newline at end of file From 45d8e2bdcf2d8977358b11d80ee83db695ff268b Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Fri, 21 Feb 2025 13:02:38 +0000 Subject: [PATCH 13/15] Add params to rcdt_panther. Signed-off-by: Jelmer de Wolde --- .../src/rcdt_panther/config/nav2_params.yaml | 360 ++++++++++++++++++ .../config/slam_params.yaml} | 0 2 files changed, 360 insertions(+) create mode 100644 ros2_ws/src/rcdt_panther/config/nav2_params.yaml rename ros2_ws/src/{rcdt_sensors/config/mapper_params_online_async.yaml => rcdt_panther/config/slam_params.yaml} (100%) diff --git a/ros2_ws/src/rcdt_panther/config/nav2_params.yaml b/ros2_ws/src/rcdt_panther/config/nav2_params.yaml new file mode 100644 index 00000000..a2e050d4 --- /dev/null +++ b/ros2_ws/src/rcdt_panther/config/nav2_params.yaml @@ -0,0 +1,360 @@ +amcl: + ros__parameters: + use_sim_time: True + alpha1: 0.2 + alpha2: 0.2 + alpha3: 0.2 + alpha4: 0.2 + alpha5: 0.2 + base_frame_id: "base_footprint" + beam_skip_distance: 0.5 + beam_skip_error_threshold: 0.9 + beam_skip_threshold: 0.3 + do_beamskip: false + global_frame_id: "map" + lambda_short: 0.1 + laser_likelihood_max_dist: 2.0 + laser_max_range: 100.0 + laser_min_range: -1.0 + laser_model_type: "likelihood_field" + max_beams: 60 + max_particles: 2000 + min_particles: 500 + odom_frame_id: "odom" + pf_err: 0.05 + pf_z: 0.99 + recovery_alpha_fast: 0.0 + recovery_alpha_slow: 0.0 + resample_interval: 1 + robot_model_type: "nav2_amcl::DifferentialMotionModel" + save_pose_rate: 0.5 + sigma_hit: 0.2 + tf_broadcast: true + transform_tolerance: 1.0 + update_min_a: 0.2 + update_min_d: 0.25 + z_hit: 0.5 + z_max: 0.05 + z_rand: 0.5 + z_short: 0.05 + scan_topic: scan + +bt_navigator: + ros__parameters: + use_sim_time: True + global_frame: map + robot_base_frame: base_link + odom_topic: /odom + bt_loop_duration: 10 + default_server_timeout: 20 + wait_for_service_timeout: 1000 + # 'default_nav_through_poses_bt_xml' and 'default_nav_to_pose_bt_xml' are use defaults: + # nav2_bt_navigator/navigate_to_pose_w_replanning_and_recovery.xml + # nav2_bt_navigator/navigate_through_poses_w_replanning_and_recovery.xml + # They can be set here or via a RewrittenYaml remap from a parent launch file to Nav2. + plugin_lib_names: + - nav2_compute_path_to_pose_action_bt_node + - nav2_compute_path_through_poses_action_bt_node + - nav2_smooth_path_action_bt_node + - nav2_follow_path_action_bt_node + - nav2_spin_action_bt_node + - nav2_wait_action_bt_node + - nav2_assisted_teleop_action_bt_node + - nav2_back_up_action_bt_node + - nav2_drive_on_heading_bt_node + - nav2_clear_costmap_service_bt_node + - nav2_is_stuck_condition_bt_node + - nav2_goal_reached_condition_bt_node + - nav2_goal_updated_condition_bt_node + - nav2_globally_updated_goal_condition_bt_node + - nav2_is_path_valid_condition_bt_node + - nav2_initial_pose_received_condition_bt_node + - nav2_reinitialize_global_localization_service_bt_node + - nav2_rate_controller_bt_node + - nav2_distance_controller_bt_node + - nav2_speed_controller_bt_node + - nav2_truncate_path_action_bt_node + - nav2_truncate_path_local_action_bt_node + - nav2_goal_updater_node_bt_node + - nav2_recovery_node_bt_node + - nav2_pipeline_sequence_bt_node + - nav2_round_robin_node_bt_node + - nav2_transform_available_condition_bt_node + - nav2_time_expired_condition_bt_node + - nav2_path_expiring_timer_condition + - nav2_distance_traveled_condition_bt_node + - nav2_single_trigger_bt_node + - nav2_goal_updated_controller_bt_node + - nav2_is_battery_low_condition_bt_node + - nav2_navigate_through_poses_action_bt_node + - nav2_navigate_to_pose_action_bt_node + - nav2_remove_passed_goals_action_bt_node + - nav2_planner_selector_bt_node + - nav2_controller_selector_bt_node + - nav2_goal_checker_selector_bt_node + - nav2_controller_cancel_bt_node + - nav2_path_longer_on_approach_bt_node + - nav2_wait_cancel_bt_node + - nav2_spin_cancel_bt_node + - nav2_back_up_cancel_bt_node + - nav2_assisted_teleop_cancel_bt_node + - nav2_drive_on_heading_cancel_bt_node + - nav2_is_battery_charging_condition_bt_node + +bt_navigator_navigate_through_poses_rclcpp_node: + ros__parameters: + use_sim_time: True + +bt_navigator_navigate_to_pose_rclcpp_node: + ros__parameters: + use_sim_time: True + +controller_server: + ros__parameters: + use_sim_time: True + controller_frequency: 20.0 + min_x_velocity_threshold: 0.001 + min_y_velocity_threshold: 0.5 + min_theta_velocity_threshold: 0.001 + failure_tolerance: 0.3 + progress_checker_plugin: "progress_checker" + goal_checker_plugins: ["general_goal_checker"] # "precise_goal_checker" + controller_plugins: ["FollowPath"] + + # Progress checker parameters + progress_checker: + plugin: "nav2_controller::SimpleProgressChecker" + required_movement_radius: 0.5 + movement_time_allowance: 10.0 + # Goal checker parameters + #precise_goal_checker: + # plugin: "nav2_controller::SimpleGoalChecker" + # xy_goal_tolerance: 0.25 + # yaw_goal_tolerance: 0.25 + # stateful: True + general_goal_checker: + stateful: True + plugin: "nav2_controller::SimpleGoalChecker" + xy_goal_tolerance: 0.25 + yaw_goal_tolerance: 0.25 + # DWB parameters + FollowPath: + plugin: "dwb_core::DWBLocalPlanner" + debug_trajectory_details: True + min_vel_x: 0.0 + min_vel_y: 0.0 + max_vel_x: 0.26 + max_vel_y: 0.0 + max_vel_theta: 1.0 + min_speed_xy: 0.0 + max_speed_xy: 0.26 + min_speed_theta: 0.0 + # Add high threshold velocity for turtlebot 3 issue. + # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75 + acc_lim_x: 2.5 + acc_lim_y: 0.0 + acc_lim_theta: 3.2 + decel_lim_x: -2.5 + decel_lim_y: 0.0 + decel_lim_theta: -3.2 + vx_samples: 20 + vy_samples: 5 + vtheta_samples: 20 + sim_time: 1.7 + linear_granularity: 0.05 + angular_granularity: 0.025 + transform_tolerance: 0.2 + xy_goal_tolerance: 0.25 + trans_stopped_velocity: 0.25 + short_circuit_trajectory_evaluation: True + stateful: True + critics: + [ + "RotateToGoal", + "Oscillation", + "BaseObstacle", + "GoalAlign", + "PathAlign", + "PathDist", + "GoalDist", + ] + BaseObstacle.scale: 0.02 + PathAlign.scale: 32.0 + PathAlign.forward_point_distance: 0.1 + GoalAlign.scale: 24.0 + GoalAlign.forward_point_distance: 0.1 + PathDist.scale: 32.0 + GoalDist.scale: 24.0 + RotateToGoal.scale: 32.0 + RotateToGoal.slowing_factor: 5.0 + RotateToGoal.lookahead_time: -1.0 + +local_costmap: + local_costmap: + ros__parameters: + update_frequency: 5.0 + publish_frequency: 2.0 + global_frame: odom + robot_base_frame: base_link + use_sim_time: True + rolling_window: true + width: 3 + height: 3 + resolution: 0.05 + robot_radius: 0.22 + plugins: ["voxel_layer", "inflation_layer"] + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + voxel_layer: + plugin: "nav2_costmap_2d::VoxelLayer" + enabled: True + publish_voxel_map: True + origin_z: 0.0 + z_resolution: 0.05 + z_voxels: 16 + max_obstacle_height: 2.0 + mark_threshold: 0 + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + always_send_full_costmap: True + +global_costmap: + global_costmap: + ros__parameters: + update_frequency: 1.0 + publish_frequency: 1.0 + global_frame: map + robot_base_frame: base_link + use_sim_time: True + robot_radius: 0.22 + resolution: 0.05 + track_unknown_space: true + plugins: ["static_layer", "obstacle_layer", "inflation_layer"] + obstacle_layer: + plugin: "nav2_costmap_2d::ObstacleLayer" + enabled: True + observation_sources: scan + scan: + topic: /scan + max_obstacle_height: 2.0 + clearing: True + marking: True + data_type: "LaserScan" + raytrace_max_range: 3.0 + raytrace_min_range: 0.0 + obstacle_max_range: 2.5 + obstacle_min_range: 0.0 + static_layer: + plugin: "nav2_costmap_2d::StaticLayer" + map_subscribe_transient_local: True + inflation_layer: + plugin: "nav2_costmap_2d::InflationLayer" + cost_scaling_factor: 3.0 + inflation_radius: 0.55 + always_send_full_costmap: True + +map_server: + ros__parameters: + use_sim_time: True + # Overridden in launch by the "map" launch configuration or provided default value. + # To use in yaml, remove the default "map" value in the tb3_simulation_launch.py file & provide full path to map below. + yaml_filename: "" + +map_saver: + ros__parameters: + use_sim_time: True + save_map_timeout: 5.0 + free_thresh_default: 0.25 + occupied_thresh_default: 0.65 + map_subscribe_transient_local: True + +planner_server: + ros__parameters: + expected_planner_frequency: 20.0 + use_sim_time: True + planner_plugins: ["GridBased"] + GridBased: + plugin: "nav2_navfn_planner/NavfnPlanner" + tolerance: 0.5 + use_astar: false + allow_unknown: true + +smoother_server: + ros__parameters: + use_sim_time: True + smoother_plugins: ["simple_smoother"] + simple_smoother: + plugin: "nav2_smoother::SimpleSmoother" + tolerance: 1.0e-10 + max_its: 1000 + do_refinement: True + +behavior_server: + ros__parameters: + costmap_topic: local_costmap/costmap_raw + footprint_topic: local_costmap/published_footprint + cycle_frequency: 10.0 + behavior_plugins: + ["spin", "backup", "drive_on_heading", "assisted_teleop", "wait"] + spin: + plugin: "nav2_behaviors/Spin" + backup: + plugin: "nav2_behaviors/BackUp" + drive_on_heading: + plugin: "nav2_behaviors/DriveOnHeading" + wait: + plugin: "nav2_behaviors/Wait" + assisted_teleop: + plugin: "nav2_behaviors/AssistedTeleop" + global_frame: odom + robot_base_frame: base_link + transform_tolerance: 0.1 + use_sim_time: true + simulate_ahead_time: 2.0 + max_rotational_vel: 1.0 + min_rotational_vel: 0.4 + rotational_acc_lim: 3.2 + +robot_state_publisher: + ros__parameters: + use_sim_time: True + +waypoint_follower: + ros__parameters: + use_sim_time: True + loop_rate: 20 + stop_on_failure: false + waypoint_task_executor_plugin: "wait_at_waypoint" + wait_at_waypoint: + plugin: "nav2_waypoint_follower::WaitAtWaypoint" + enabled: True + waypoint_pause_duration: 200 + +velocity_smoother: + ros__parameters: + use_sim_time: True + smoothing_frequency: 20.0 + scale_velocities: False + feedback: "OPEN_LOOP" + max_velocity: [0.26, 0.0, 1.0] + min_velocity: [-0.26, 0.0, -1.0] + max_accel: [2.5, 0.0, 3.2] + max_decel: [-2.5, 0.0, -3.2] + odom_topic: "odom" + odom_duration: 0.1 + deadband_velocity: [0.0, 0.0, 0.0] + velocity_timeout: 1.0 diff --git a/ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml b/ros2_ws/src/rcdt_panther/config/slam_params.yaml similarity index 100% rename from ros2_ws/src/rcdt_sensors/config/mapper_params_online_async.yaml rename to ros2_ws/src/rcdt_panther/config/slam_params.yaml From 5db17aaa2b20ee9f1e5e5fbe64c3834095c52df2 Mon Sep 17 00:00:00 2001 From: Jelmer de Wolde Date: Fri, 21 Feb 2025 13:12:22 +0000 Subject: [PATCH 14/15] Increase robot size. Signed-off-by: Jelmer de Wolde --- ros2_ws/src/rcdt_panther/config/nav2_params.yaml | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/ros2_ws/src/rcdt_panther/config/nav2_params.yaml b/ros2_ws/src/rcdt_panther/config/nav2_params.yaml index a2e050d4..f5da3ad6 100644 --- a/ros2_ws/src/rcdt_panther/config/nav2_params.yaml +++ b/ros2_ws/src/rcdt_panther/config/nav2_params.yaml @@ -1,3 +1,7 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + amcl: ros__parameters: use_sim_time: True @@ -240,7 +244,7 @@ global_costmap: global_frame: map robot_base_frame: base_link use_sim_time: True - robot_radius: 0.22 + robot_radius: 0.6 resolution: 0.05 track_unknown_space: true plugins: ["static_layer", "obstacle_layer", "inflation_layer"] From 35a3a937d5e2b4ce092c8948f3fe6c910b4a00c2 Mon Sep 17 00:00:00 2001 From: Eva Dienske Date: Mon, 24 Feb 2025 15:10:54 +0000 Subject: [PATCH 15/15] Collision monitoring added --- .../src/rcdt_panther/launch/panther.launch.py | 6 +- .../launch/panther_coll_mntr.launch.py | 130 ++++++++++++++++++ .../config/collision_monitor_params.yaml | 63 +++++++++ .../urdf/rcdt_velodyne.urdf.xacro | 2 +- .../src/rcdt_utilities/src_py/joy_to_twist.py | 14 +- .../src_py/joy_to_twist_stamped.py | 88 ++++++++++++ .../src_py/twist_to_twist_stamped.py | 2 +- 7 files changed, 291 insertions(+), 14 deletions(-) create mode 100644 ros2_ws/src/rcdt_panther/launch/panther_coll_mntr.launch.py create mode 100644 ros2_ws/src/rcdt_sensors/config/collision_monitor_params.yaml create mode 100755 ros2_ws/src/rcdt_utilities/src_py/joy_to_twist_stamped.py diff --git a/ros2_ws/src/rcdt_panther/launch/panther.launch.py b/ros2_ws/src/rcdt_panther/launch/panther.launch.py index 88a869e6..1b9782e4 100644 --- a/ros2_ws/src/rcdt_panther/launch/panther.launch.py +++ b/ros2_ws/src/rcdt_panther/launch/panther.launch.py @@ -78,9 +78,9 @@ def launch_setup(context: LaunchContext) -> None: ], ) - joy_to_twist_node = Node( + joy_to_twist_stamped_node = Node( package="rcdt_utilities", - executable="joy_to_twist.py", + executable="joy_to_twist_stamped.py", parameters=[ {"pub_topic": "/diff_drive_controller/cmd_vel"}, {"config_pkg": "rcdt_panther"}, @@ -97,7 +97,7 @@ def launch_setup(context: LaunchContext) -> None: controllers, rviz if use_rviz else skip, joy, - joy_to_twist_node, + joy_to_twist_stamped_node, ] diff --git a/ros2_ws/src/rcdt_panther/launch/panther_coll_mntr.launch.py b/ros2_ws/src/rcdt_panther/launch/panther_coll_mntr.launch.py new file mode 100644 index 00000000..ce95139e --- /dev/null +++ b/ros2_ws/src/rcdt_panther/launch/panther_coll_mntr.launch.py @@ -0,0 +1,130 @@ +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + +from launch import LaunchContext, LaunchDescription, LaunchDescriptionEntity +from launch.actions import IncludeLaunchDescription, OpaqueFunction +from launch_ros.actions import Node, SetParameter +from rcdt_utilities.launch_utils import ( + LaunchArgument, + get_file_path, + get_robot_description, +) + +load_gazebo_ui_arg = LaunchArgument("load_gazebo_ui", False, [True, False]) +use_rviz_arg = LaunchArgument("rviz", True, [True, False]) +use_velodyne_arg = LaunchArgument("velodyne", False, [True, False]) + + +def launch_setup(context: LaunchContext) -> None: + load_gazebo_ui = load_gazebo_ui_arg.value(context) + use_rviz = use_rviz_arg.value(context) + use_velodyne = use_velodyne_arg.value(context) + + xacro_path = get_file_path("rcdt_panther", ["urdf"], "panther.urdf.xacro") + components_path = get_file_path("rcdt_panther", ["config"], "components.yaml") + xacro_arguments = {"use_sim": "true", "components_config_path": components_path} + xacro_arguments["load_velodyne"] = "true" if use_velodyne else "false" + robot_description = get_robot_description(xacro_path, xacro_arguments) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + parameters=[robot_description], + ) + + robot = IncludeLaunchDescription( + get_file_path("rcdt_utilities", ["launch"], "gazebo_robot.launch.py"), + launch_arguments={ + "world": "walls.sdf", + "velodyne": str(use_velodyne), + "load_gazebo_ui": str(load_gazebo_ui), + }.items(), + ) + + static_transform_publisher = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_tf_world", + arguments=["--frame-id", "world", "--child-frame-id", "odom"], + ) + + joint_state_broadcaster = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "-t", + "joint_state_broadcaster/JointStateBroadcaster", + ], + ) + + controllers = IncludeLaunchDescription( + get_file_path("rcdt_panther", ["launch"], "controllers.launch.py"), + ) + + rviz = IncludeLaunchDescription( + get_file_path("rcdt_utilities", ["launch"], "rviz.launch.py"), + launch_arguments={ + "rviz_display_config": "lidar.rviz" if use_velodyne else "general.rviz", + }.items(), + ) + + collision_monitoring = IncludeLaunchDescription( + get_file_path("nav2_collision_monitor", ["launch"], "collision_monitor_node.launch.py"), + launch_arguments={ + "params_file": "/home/rcdt/rcdt_robotics/ros2_ws/src/rcdt_sensors/config/collision_monitor_params.yaml", + }.items(), + ) + + twist_to_twist_stamped = Node( + package="rcdt_utilities", + executable="twist_to_twist_stamped.py", + parameters=[ + {"sub_topic": "/coll_mntr_cmd_vel"}, + {"pub_topic": "/diff_drive_controller/cmd_vel"} + ], + ) + + joy = Node( + package="joy", + executable="game_controller_node", + parameters=[ + {"sticky_buttons": True}, + ], + ) + + joy_to_twist = Node( + package="rcdt_utilities", + executable="joy_to_twist.py", + parameters=[ + {"pub_topic": "/joy_cmd_vel"}, + {"config_pkg": "rcdt_panther"}, + ], + ) + + skip = LaunchDescriptionEntity() + return [ + SetParameter(name="use_sim_time", value=True), + robot_state_publisher, + robot, + static_transform_publisher, + joint_state_broadcaster, + controllers, + rviz if use_rviz else skip, + collision_monitoring, + twist_to_twist_stamped, + joy, + joy_to_twist, + ] + + +def generate_launch_description() -> LaunchDescription: + return LaunchDescription( + [ + load_gazebo_ui_arg.declaration, + use_rviz_arg.declaration, + use_velodyne_arg.declaration, + OpaqueFunction(function=launch_setup), + ] + ) diff --git a/ros2_ws/src/rcdt_sensors/config/collision_monitor_params.yaml b/ros2_ws/src/rcdt_sensors/config/collision_monitor_params.yaml new file mode 100644 index 00000000..7e3f127b --- /dev/null +++ b/ros2_ws/src/rcdt_sensors/config/collision_monitor_params.yaml @@ -0,0 +1,63 @@ +collision_monitor: + ros__parameters: + use_sim_time: True + base_frame_id: "base_link" + odom_frame_id: "odom" + cmd_vel_in_topic: "joy_cmd_vel" + cmd_vel_out_topic: "coll_mntr_cmd_vel" + transform_tolerance: 0.5 + source_timeout: 5.0 + base_shift_correction: True + stop_pub_timeout: 2.0 + # Polygons represent zone around the robot for "stop" and "slowdown" action types, + # and robot footprint for "approach" action type. + # Footprint could be "polygon" type with dynamically set footprint from footprint_topic + # or "circle" type with static footprint set by radius. "footprint_topic" parameter + # to be ignored in circular case. + polygons: ["PolygonSlower", "PolygonSlow"] + PolygonStop: + type: "polygon" + points: [-0.7, 0.55, 0.7, 0.55, 0.7, -0.55, -0.7, -0.55] + action_type: "stop" + max_points: 5 + visualize: True + polygon_pub_topic: "polygon_stop" + enabled: True + PolygonSlower: + type: "polygon" + points: [-0.7, 0.55, 0.7, 0.55, 0.7, -0.55, -0.7, -0.55] + action_type: "slowdown" + max_points: 10 + slowdown_ratio: 0.1 + visualize: True + polygon_pub_topic: "polygon_slower" + enabled: True + PolygonSlow: + type: "polygon" + points: [-1.2, 0.75, 1.2, 0.75, 1.2, -0.75, -1.2, -0.75] + action_type: "slowdown" + max_points: 10 + slowdown_ratio: 0.3 + visualize: True + polygon_pub_topic: "polygon_slow" + enabled: True + FootprintApproach: + type: "polygon" + action_type: "approach" + footprint_topic: "/local_costmap/published_footprint" + time_before_collision: 2.0 + simulation_time_step: 0.1 + max_points: 5 + visualize: False + enabled: True + observation_sources: ["pointcloud"] + scan: + type: "scan" + topic: "/scan" + enabled: True + pointcloud: + type: "pointcloud" + topic: "/scan/points" + min_height: 0.1 + max_height: 1.0 + enabled: True diff --git a/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro b/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro index c0ab4479..060334a7 100644 --- a/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro +++ b/ros2_ws/src/rcdt_sensors/urdf/rcdt_velodyne.urdf.xacro @@ -22,7 +22,7 @@ Based on: update_rate:=10 lasers:=16 samples:=1875 - min_range:=0.3 + min_range:=0.7 max_range:=130.0 min_angle:=-${M_PI} max_angle:=${M_PI} diff --git a/ros2_ws/src/rcdt_utilities/src_py/joy_to_twist.py b/ros2_ws/src/rcdt_utilities/src_py/joy_to_twist.py index f078aa29..159fae1a 100755 --- a/ros2_ws/src/rcdt_utilities/src_py/joy_to_twist.py +++ b/ros2_ws/src/rcdt_utilities/src_py/joy_to_twist.py @@ -5,7 +5,7 @@ # SPDX-License-Identifier: Apache-2.0 import rclpy -from geometry_msgs.msg import TwistStamped +from geometry_msgs.msg import Twist from rcdt_utilities.launch_utils import get_file_path, get_yaml, spin_node from rclpy import logging from rclpy.node import Node @@ -18,14 +18,12 @@ class JoyToTwist(Node): def __init__(self) -> bool: super().__init__("joy_to_twist_node") self.declare_parameter("sub_topic", "/joy") - self.declare_parameter("pub_topic", "") + self.declare_parameter("pub_topic", "/joy_cmd_vel") self.declare_parameter("config_pkg", "") - self.declare_parameter("pub_frame", "") sub_topic = self.get_parameter("sub_topic").get_parameter_value().string_value pub_topic = self.get_parameter("pub_topic").get_parameter_value().string_value config_pkg = self.get_parameter("config_pkg").get_parameter_value().string_value - pub_frame = self.get_parameter("pub_frame").get_parameter_value().string_value if sub_topic == "": self.get_logger().warn("No subscriber topic was specified. Exiting.") @@ -47,9 +45,8 @@ def __init__(self) -> bool: return self.create_subscription(Joy, sub_topic, self.handle_input, 10) - self.pub = self.create_publisher(TwistStamped, pub_topic, 10) - self.pub_msg = TwistStamped() - self.pub_msg.header.frame_id = pub_frame + self.pub = self.create_publisher(Twist, pub_topic, 10) + self.pub_msg = Twist() self.profile = "A" def handle_input(self, sub_msg: Joy) -> None: @@ -72,9 +69,8 @@ def handle_input(self, sub_msg: Joy) -> None: continue value *= -1 if config.get("flip", False) else 1 direction = config["direction"] - vector = getattr(self.pub_msg.twist, movement) + vector = getattr(self.pub_msg, movement) setattr(vector, direction, value) - self.pub_msg.header.stamp = self.get_clock().now().to_msg() self.pub.publish(self.pub_msg) diff --git a/ros2_ws/src/rcdt_utilities/src_py/joy_to_twist_stamped.py b/ros2_ws/src/rcdt_utilities/src_py/joy_to_twist_stamped.py new file mode 100755 index 00000000..854dbb9b --- /dev/null +++ b/ros2_ws/src/rcdt_utilities/src_py/joy_to_twist_stamped.py @@ -0,0 +1,88 @@ +#!/usr/bin/env python3 + +# SPDX-FileCopyrightText: Alliander N. V. +# +# SPDX-License-Identifier: Apache-2.0 + +import rclpy +from geometry_msgs.msg import TwistStamped +from rcdt_utilities.launch_utils import get_file_path, get_yaml, spin_node +from rclpy import logging +from rclpy.node import Node +from sensor_msgs.msg import Joy + +ros_logger = logging.get_logger(__name__) + + +class JoyToTwistStamped(Node): + def __init__(self) -> bool: + super().__init__("joy_to_twist_stamped_node") + self.declare_parameter("sub_topic", "/joy") + self.declare_parameter("pub_topic", "") + self.declare_parameter("config_pkg", "") + self.declare_parameter("pub_frame", "") + + sub_topic = self.get_parameter("sub_topic").get_parameter_value().string_value + pub_topic = self.get_parameter("pub_topic").get_parameter_value().string_value + config_pkg = self.get_parameter("config_pkg").get_parameter_value().string_value + pub_frame = self.get_parameter("pub_frame").get_parameter_value().string_value + + if sub_topic == "": + self.get_logger().warn("No subscriber topic was specified. Exiting.") + return + + if pub_topic == "": + self.get_logger().warn("No publisher topic was specified. Exiting.") + return + + if config_pkg == "": + self.get_logger().warn("No package for config file was specified. Exiting.") + return + + config = get_file_path(config_pkg, ["config"], "gamepad_mapping.yaml") + self.mapping = get_yaml(config) + + if self.mapping == "": + self.get_logger().warn(f"No mapping was found in {config}. Exiting.") + return + + self.create_subscription(Joy, sub_topic, self.handle_input, 10) + self.pub = self.create_publisher(TwistStamped, pub_topic, 10) + self.pub_msg = TwistStamped() + self.pub_msg.header.frame_id = pub_frame + self.profile = "A" + + def handle_input(self, sub_msg: Joy) -> None: + for idx in range(len(sub_msg.axes)): + value = sub_msg.axes[idx] + if idx not in self.mapping["axes"]: + continue + axis: dict = self.mapping["axes"][idx] + + if axis == "switch_profile": + self.profile = "A" if value > -1 else "B" + continue + + for movement in ["angular", "linear"]: + config: dict = axis.get(movement) + if config is None: + continue + profile = config.get("profile", self.profile) + if profile != self.profile: + continue + value *= -1 if config.get("flip", False) else 1 + direction = config["direction"] + vector = getattr(self.pub_msg.twist, movement) + setattr(vector, direction, value) + self.pub_msg.header.stamp = self.get_clock().now().to_msg() + self.pub.publish(self.pub_msg) + + +def main(args: str = None) -> None: + rclpy.init(args=args) + node = JoyToTwistStamped() + spin_node(node) + + +if __name__ == "__main__": + main() diff --git a/ros2_ws/src/rcdt_utilities/src_py/twist_to_twist_stamped.py b/ros2_ws/src/rcdt_utilities/src_py/twist_to_twist_stamped.py index 0ec8a444..ae54d14a 100755 --- a/ros2_ws/src/rcdt_utilities/src_py/twist_to_twist_stamped.py +++ b/ros2_ws/src/rcdt_utilities/src_py/twist_to_twist_stamped.py @@ -16,7 +16,7 @@ class TwistToTwistStamped(Node): def __init__(self) -> bool: super().__init__("twist_to_twist_stamped_node") - self.declare_parameter("sub_topic", "/cmd_vel") + self.declare_parameter("sub_topic", "/coll_mntr_cmd_vel") self.declare_parameter("pub_topic", "/diff_drive_controller/cmd_vel") sub_topic = self.get_parameter("sub_topic").get_parameter_value().string_value