195 panther add collision monitoring to the panther (#203)

Add collision monitoring to the panther
---------

Signed-off-by: Max Waterhout <[email protected]>
Signed-off-by: Jelmer de Wolde <[email protected]>
Co-authored-by: Jelmer de Wolde <[email protected]>
Co-authored-by: Jelmer de Wolde <[email protected]>

Author: MaxWaterhout

docs/content/panther.md

@@ -154,12 +154,6 @@ A map should be visualized in Rviz and be updated when driving around.
 ros2 launch rcdt_panther panther.launch.py collision_monitor:=True
 ```
 
-Next, start the collision monitor in a second terminal:
-
-```bash
-ros2 launch rcdt_panther collision_monitor.launch.py
-```
-
 Rviz should visualize "slow-down" regions around the Panther and the Panther should slow down when objects are in these regions.
 
 **Navigation:**

ros2_ws/src/rcdt_joystick/launch/joystick.launch.py

@@ -10,6 +10,7 @@
 
 use_sim_arg = LaunchArgument("simulation", True, [True, False])
 robots_arg = LaunchArgument("robots", "")
+use_collision_monitor_arg = LaunchArgument("collision_monitor", False, [True, False])
 scale_speed_arg = LaunchArgument(
     "scale_speed", default_value=0.4, min_value=0.0, max_value=1.0
 )
@@ -26,6 +27,7 @@ def launch_setup(context: LaunchContext) -> list:
     """
     use_sim = use_sim_arg.bool_value(context)
     robots = robots_arg.string_value(context).split(" ")
+    use_collision_monitor = use_collision_monitor_arg.bool_value(context)
     scale_speed = scale_speed_arg.float_value(context)
 
     joy = Node(
@@ -62,12 +64,16 @@ def launch_setup(context: LaunchContext) -> list:
         namespace="franka",
     )
 
+    pub_topic = (
+        "/panther/cmd_vel" if not use_collision_monitor else "/panther/cmd_vel_raw"
+    )
+
     joy_to_twist_panther = Node(
         package="rcdt_joystick",
         executable="joy_to_twist.py",
         parameters=[
             {"sub_topic": "/panther/joy"},
-            {"pub_topic": "/panther/cmd_vel"},
+            {"pub_topic": pub_topic},
             {"config_pkg": "rcdt_panther"},
             {"scale": 1.0 if use_sim else scale_speed},
         ],
@@ -97,6 +103,7 @@ def generate_launch_description() -> LaunchDescription:
         [
             use_sim_arg.declaration,
             robots_arg.declaration,
+            use_collision_monitor_arg.declaration,
             scale_speed_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]

ros2_ws/src/rcdt_panther/config/collision_monitor.yaml

@@ -0,0 +1,9 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+# Based on: https://docs.nav2.org/configuration/packages/configuring-behavior-server.html
+
+behavior_server:
+  ros__parameters:
+    enable_stamped_cmd_vel: true

ros2_ws/src/rcdt_panther/config/nav2/behavior_server.yaml

@@ -0,0 +1,70 @@
+# SPDX-FileCopyrightText: Alliander N. V.
+#
+# SPDX-License-Identifier: Apache-2.0
+
+## Based on https://docs.nav2.org/tutorials/docs/using_collision_monitor.html
+
+collision_monitor:
+  ros__parameters:
+    base_frame_id: "panther/base_footprint"
+    odom_frame_id: "panther/odom"
+    cmd_vel_in_topic: "panther/cmd_vel_raw"
+    cmd_vel_out_topic: "panther/cmd_vel"
+    enable_stamped_cmd_vel: True
+    polygons: ["PolygonSlow", "VelocityPolygonStop"]
+
+    PolygonSlow:
+      type: "polygon"
+      points: "[[-1.5, 0.9], [1.5, 0.9], [1.5, -0.9], [-1.5, -0.9]]"
+      action_type: "slowdown"
+      min_points: 4
+      slowdown_ratio: 0.7
+      visualize: True
+      polygon_pub_topic: "polygon_slower"
+      enabled: True
+
+    VelocityPolygonStop:
+      type: "velocity_polygon"
+      action_type: "stop"
+      min_points: 6
+      visualize: True
+      enabled: True
+      polygon_pub_topic: "velocity_polygon_stop"
+      velocity_polygons: ["rotation", "translation_forward", "translation_backward", "stopped"]
+      holonomic: false
+
+      rotation:
+        points: "[[0.65, 0.65], [0.65, -0.65], [-0.65, -0.65], [-0.65, 0.65]]"
+        linear_min: 0.0
+        linear_max: 0.05
+        theta_min: -1.0
+        theta_max: 1.0
+
+      translation_forward:
+        points: "[[0.90, 0.65], [0.90, -0.65], [-0.50, -0.65], [-0.50, 0.65]]"
+        linear_min: 0.0
+        linear_max: 1.0
+        theta_min: -1.0
+        theta_max: 1.0
+
+      translation_backward:
+        points: "[[0.50, 0.65], [0.50, -0.65], [-0.90, -0.65], [-0.90, 0.65]]"
+        linear_min: -1.0
+        linear_max: 0.0
+        theta_min: -1.0
+        theta_max: 1.0
+
+      stopped:
+        points: "[[0.60, 0.60], [0.60, -0.60], [-0.60, -0.60], [-0.60, 0.60]]"
+        linear_min: -1.0
+        linear_max: 1.0
+        theta_min: -1.0
+        theta_max: 1.0
+
+    observation_sources: ["pointcloud"]
+    pointcloud:
+      type: "pointcloud"
+      topic: "/velodyne/scan/points"
+      min_height: 0.1
+      max_height: 1.0
+      enabled: True

ros2_ws/src/rcdt_panther/config/nav2/collision_monitor.yaml

@@ -8,6 +8,7 @@
 from rcdt_utilities.launch_utils import (
     SKIP,
     LaunchArgument,
+    LaunchPosition,
     get_file_path,
     get_robot_description,
 )
@@ -17,6 +18,7 @@
 child_arg = LaunchArgument("child", "", ["", "franka", "velodyne"])
 load_gazebo_ui_arg = LaunchArgument("load_gazebo_ui", False, [True, False])
 world_arg = LaunchArgument("world", "empty_camera.sdf")
+panther_xyz_arg = LaunchArgument("panther_xyz", "0,0,0.2")
 
 
 def launch_setup(context: LaunchContext) -> list:
@@ -32,6 +34,7 @@ def launch_setup(context: LaunchContext) -> list:
     child = child_arg.string_value(context)
     load_gazebo_ui = load_gazebo_ui_arg.bool_value(context)
     world = world_arg.string_value(context)
+    panther_xyz = LaunchPosition(panther_xyz_arg.string_value(context))
 
     namespace = "panther"
     frame_prefix = namespace + "/" if namespace else ""
@@ -50,10 +53,11 @@ def launch_setup(context: LaunchContext) -> list:
     )
 
     robots = ["panther"]
-    positions = ["0,0,0.2"]
+    positions = [panther_xyz.string]
     if child == "velodyne":
         robots.append("velodyne")
-        positions.append("0.13,-0.13,0.55")
+        velodyne_relative_position = [0.13, -0.13, 0.55]
+        positions.append(panther_xyz.absolute(velodyne_relative_position))
     robot = RegisteredLaunchDescription(
         get_file_path("rcdt_gazebo", ["launch"], "gazebo_robot.launch.py"),
         launch_arguments={
@@ -83,6 +87,7 @@ def generate_launch_description() -> LaunchDescription:
             child_arg.declaration,
             load_gazebo_ui_arg.declaration,
             world_arg.declaration,
+            panther_xyz_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]
     )