Add nav2 to the panther (#198)

* Update environment variables for the meshes

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Enable nav2 navigation to panther.launch

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Add nav2 launch file and integration tests for Panther navigation

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Remove redundant navigation launch instructions from Panther documentation

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Refactor navigation launch setup: replace navigation.launch.py with nav2.launch.py and update launch arguments

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Add collision monitor argument to navigation launch files

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Update build cache reference in reusable-docker.yml to use 'build-cache-latest'

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Update test_panther_nav2.py: Enable RViz and adjust navigation goal position and orientation

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Refactor test_panther_nav2.py: Disable RViz and remove unnecessary sleep in registration test

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Add support for real lidar.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Define velodyne as separate 'robot'.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Fix reuse.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Add velodyne packages to docker image.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Fix parameters.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Fix spawn position of panther.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Set nav2 default to false.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Remove old panther namespace.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Enable nav2 in launch file for test.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Fix for use on real robot.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

* Update transform_tolerance in controller_server parameters to improve navigation stability

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>

* Add velodyne robot model to rviz.

Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

---------

Signed-off-by: Max Waterhout <max.waterhout@hotmail.nl>
Signed-off-by: Jelmer de Wolde <jelmer.de.wolde@alliander.com>

Author: MaxWaterhout

dockerfiles/install_scripts/core_packages.sh

@@ -29,7 +29,9 @@ apt install -y \
     ros-$ROS_DISTRO-rmw-cyclonedds-cpp \
     ros-$ROS_DISTRO-ros-gz \
     ros-$ROS_DISTRO-ros2-controllers \
+    ros-$ROS_DISTRO-rqt-tf-tree \
     ros-$ROS_DISTRO-slam-toolbox \
+    ros-$ROS_DISTRO-velodyne \
     ros-$ROS_DISTRO-velodyne-description
 
 pip install uv --break-system-packages

docs/content/panther.md

@@ -168,12 +168,6 @@ Rviz should visualize "slow-down" regions around the Panther and the Panther sho
 ros2 launch rcdt_panther panther.launch.py nav2:=True
 ```
 
-Next, start navigation in a second terminal:
-
-```bash
-ros2 launch rcdt_panther navigation.launch.py
-```
-
 Now you should be able to place a *2D Goal Pose* in Rviz, somewhere on the map. The robot should plan, visualize and drive a route to this goal pose.
 
 :::{note}

ros2_ws/src/rcdt_gazebo/launch/gazebo_robot.launch.py

@@ -15,9 +15,8 @@
 load_gazebo_ui_arg = LaunchArgument("load_gazebo_ui", False, [True, False])
 world_arg = LaunchArgument("world", "empty_camera.sdf")
 robots_arg = LaunchArgument("robots", "")
-positions_arg = LaunchArgument("positions", "0-0-0")
+positions_arg = LaunchArgument("positions", "0,0,0")
 use_realsense_arg = LaunchArgument("realsense", False, [True, False])
-use_velodyne_arg = LaunchArgument("velodyne", False, [True, False])
 
 
 def launch_setup(context: LaunchContext) -> list:
@@ -37,7 +36,6 @@ def launch_setup(context: LaunchContext) -> list:
     robots = robots_arg.string_value(context).split(" ")
     positions = positions_arg.string_value(context).split(" ")
     use_realsense = use_realsense_arg.bool_value(context)
-    use_velodyne = use_velodyne_arg.bool_value(context)
 
     sdf_file = get_file_path("rcdt_gazebo", ["worlds"], world)
     sdf = ET.parse(sdf_file)
@@ -65,11 +63,11 @@ def launch_setup(context: LaunchContext) -> list:
                 "/franka/realsense/depth/image_rect_raw_float@sensor_msgs/msg/Image@gz.msgs.Image",
             ]
         )
-    if use_velodyne:
+    if "velodyne" in robots:
         bridge_topics.extend(
             [
-                "/panther/velodyne/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan",
-                "/panther/velodyne/scan/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked",
+                "/velodyne/scan@sensor_msgs/msg/LaserScan@gz.msgs.LaserScan",
+                "/velodyne/scan/points@sensor_msgs/msg/PointCloud2@gz.msgs.PointCloudPacked",
             ]
         )
 
@@ -82,7 +80,7 @@ def launch_setup(context: LaunchContext) -> list:
     spawn_robots: list[Node] = []
     for robot, position in zip(robots, positions, strict=False):
         namespace = "" if not robot else f"/{robot}"
-        x, y, z = position.split("-")
+        x, y, z = position.split(",")
         spawn_robot = Node(
             package="ros_gz_sim",
             executable="create",
@@ -143,7 +141,6 @@ def generate_launch_description() -> LaunchDescription:
             robots_arg.declaration,
             positions_arg.declaration,
             use_realsense_arg.declaration,
-            use_velodyne_arg.declaration,
             OpaqueFunction(function=launch_setup),
         ]
     )

ros2_ws/src/rcdt_mobile_manipulator/launch/core.launch.py

@@ -46,8 +46,8 @@ def launch_setup(context: LaunchContext) -> list:
         "panther",
     ]
     positions = [
-        f"0-0-{FRANKA_HEIGHT}",
-        "0-0-0.2",
+        f"0,0,{FRANKA_HEIGHT}",
+        "0,0,0.2",
     ]
     robot = RegisteredLaunchDescription(
         get_file_path("rcdt_gazebo", ["launch"], "gazebo_robot.launch.py"),