Jazzy port (#9)

MarqRazz · Dec 5, 2024 · 212e88c · 212e88c
1 parent a9ada4c
commit 212e88c
Show file tree

Hide file tree

Showing 74 changed files with 2,627 additions and 2,117 deletions.
diff --git a/.docker/Dockerfile b/.docker/Dockerfile
@@ -1,13 +1,7 @@
-FROM osrf/ros:humble-desktop-full-jammy
+FROM osrf/ros:jazzy-desktop-full
 
 SHELL ["/bin/bash", "-c", "-o", "pipefail"]
 
-# Add the latest binaries for MoveIt2
-RUN echo "deb [trusted=yes] https://raw.githubusercontent.com/moveit/moveit2_packages/jammy-humble/ ./" \
-    | tee /etc/apt/sources.list.d/moveit_moveit2_packages.list
-RUN echo "yaml https://raw.githubusercontent.com/moveit/moveit2_packages/jammy-humble/local.yaml humble" \
-    | tee /etc/ros/rosdep/sources.list.d/1-moveit_moveit2_packages.list
-
 # Bring the container up to date to get the latest ROS2 humble sync
 # hadolint ignore=DL3008, DL3013
 RUN apt-get update && apt-get upgrade -y && rosdep update \
@@ -63,16 +57,6 @@ RUN apt-get update && apt-get install -y --no-install-recommends \
     ros-humble-rmw-cyclonedds-cpp \
     && rm -rf /var/lib/apt/lists/*
 
-# Upgrade binaries for Gazebo to get working RGBD camera
-# hadolint ignore=DL3008
-RUN echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable jammy main" > /etc/apt/sources.list.d/gazebo-stable.list && \
-    wget -q http://packages.osrfoundation.org/gazebo.key && \
-    apt-key add gazebo.key && \
-    apt-get update && apt-get install --no-install-recommends -y \
-    libsdformat12 \
-    libignition-sensors6=6.7.1-1~jammy \
-    && rm -rf /var/lib/apt/lists/*
-
 # Copy over the source code to rosdep install other dependencies.
 # The source code in this workspace will be mounted from the host at runtime.
 RUN mkdir -p /root/c3pzero_ws/src

diff --git a/README.md b/README.md
@@ -2,18 +2,18 @@
 
 [![Format Status](https://github.com/MarqRazz/c3pzero/actions/workflows/format.yaml/badge.svg)](https://github.com/MarqRazz/c3pzero/actions/workflows/format.yaml)
 
-Description: This is a ROS 2 package for the C3pzero robot which consists of a Permobile C300 diff drive base and Kinova Gen3 manipulator. This Package support the physical hardware, Ignition Gazebo and Nvidia Isaac simulation.
+Description: This is a ROS 2 package for the c3pzero robot which consists of a Permobile C300 diff drive base and dual arm manipulators. This Package support the physical hardware, Gazebo and Nvidia Isaac simulation.
 
-<img src="doc/c3pzero.png"  width="50%" >
+<img src="doc/c3pzero_urdf.png"  width="50%" >
 
 <img src="doc/c300_isaac.png"  width="60%" >
 
 https://github.com/MarqRazz/c3pzero/assets/25058794/35301ba1-e443-44ff-b6ba-0fabae138205
 
 ## Documentation
 
-- [Installation](doc/installation.md)
+- [Setup](doc/setup.md)
 - [User Guide](doc/user.md)
 - [Developers Guide](doc/developer.md)
-- [c3pzero Simulation hand hardware Bringup Guide](c3pzero_bringup/README.md)
+- [c3pzero Mobile Manipulator Guide](c3pzero/README.md)
 - [C300 Mobile Base Guide](c300/README.md)
diff --git a/c300/README.md b/c300/README.md
@@ -14,4 +14,4 @@ ros2 launch c300_bringup gazebo_c300.launch.py
 ``` bash
 ros2 launch c300_driver teleop.launch.py
 ```
-> NOTE: in simulation the `cmd_vel` topic is on `/diff_drive_base_controller/cmd_vel_unstamped`
+> NOTE: in simulation the `cmd_vel` topic is on `/cmd_vel_nav` to work with the ROS Navigation stack.
diff --git a/c300/c300_bringup/CMakeLists.txt b/c300/c300_bringup/CMakeLists.txt
@@ -1,31 +1,12 @@
 cmake_minimum_required(VERSION 3.5)
 project(c300_bringup)
 
-# Default to C99
-if(NOT CMAKE_C_STANDARD)
-  set(CMAKE_C_STANDARD 99)
-endif()
-
-# Default to C++17
-if(NOT CMAKE_CXX_STANDARD)
-  set(CMAKE_CXX_STANDARD 17)
-endif()
-
-if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
-  add_compile_options(-Wall -Wextra -Wpedantic)
-endif()
-
-# find dependencies
 find_package(ament_cmake REQUIRED)
 
 install(
   DIRECTORY config launch rviz worlds
   DESTINATION share/${PROJECT_NAME}
 )
 
-if(BUILD_TESTING)
-  find_package(ament_lint_auto REQUIRED)
-  ament_lint_auto_find_test_dependencies()
-endif()
 
 ament_package()
diff --git a/c300/c300_bringup/config/c300_gz_controllers.yaml b/c300/c300_bringup/config/c300_gz_controllers.yaml
@@ -14,7 +14,7 @@ diff_drive_base_controller:
     right_wheel_names: ["drivewhl_r_joint"]
     wheels_per_side: 1
 
-    wheel_separation: 0.61 # outside distance between the wheels
+    wheel_separation: 0.6 # Shortest distance between the left and right wheels
     wheel_radius: 0.1715
 
     wheel_separation_multiplier: 1.0
@@ -33,7 +33,6 @@ diff_drive_base_controller:
 
     cmd_vel_timeout: 0.5
     #publish_limited_velocity: true
-    use_stamped_vel: false
     #velocity_rolling_window_size: 10
 
     # Velocity and acceleration limits

diff --git a/c300/c300_bringup/launch/gazebo_c300.launch.py b/c300/c300_bringup/launch/gazebo_c300.launch.py
@@ -1,11 +1,11 @@
 # -*- coding: utf-8 -*-
 # Author: Marq Rasmussen
 
-from launch import LaunchDescription
+from launch import LaunchDescription, LaunchContext
 from launch.actions import (
     DeclareLaunchArgument,
-    ExecuteProcess,
     IncludeLaunchDescription,
+    OpaqueFunction,
     RegisterEventHandler,
 )
 from launch.event_handlers import OnProcessExit
@@ -19,95 +19,53 @@
 )
 from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
+from ament_index_python.packages import get_package_share_directory
+
+
+def launch_gz(context: LaunchContext):
+    gz_world_file = get_package_share_directory("c300_bringup") + "/worlds/depot.sdf"
+    # -r is to run the simulation on start
+    # -v is the verbose level
+    #  0: No output, 1: Error, 2: Error and warning, 3: Error, warning, and info, 4: Error, warning, info, and debug.
+    sim_options = "-r -v 3"
+    if LaunchConfiguration("headless").perform(context) == "true":
+        sim_options += " -s"  # -s is to only run the server (headless mode).
+    gz_launch_description = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            [FindPackageShare("ros_gz_sim"), "/launch/gz_sim.launch.py"]
+        ),
+        launch_arguments=[("gz_args", [f"{sim_options} {gz_world_file}"])],
+    )
+    return [gz_launch_description]
 
 
 def generate_launch_description():
     declared_arguments = []
-    # Simulation specific arguments
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "sim_ignition",
-            default_value="true",
-            description="Use Ignition for simulation",
-        )
-    )
-    # General arguments
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "runtime_config_package",
-            default_value="c300_bringup",
-            description='Package with the controller\'s configuration in "config" folder. \
-        Usually the argument is not set, it enables use of a custom setup.',
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_package",
-            default_value="c300_description",
-            description="Description package with robot URDF/XACRO files. Usually the argument \
-        is not set, it enables use of a custom description.",
-        )
-    )
     declared_arguments.append(
-        DeclareLaunchArgument(
-            "description_file",
-            default_value="c300_base.urdf",
-            description="URDF/XACRO description file with the robot.",
-        )
+        DeclareLaunchArgument("rviz", default_value="false", description="Launch RViz?")
     )
     declared_arguments.append(
         DeclareLaunchArgument(
-            "robot_name",
-            default_value="c300",
-            description="Robot name.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "diff_drive_controller",
-            default_value="diff_drive_base_controller",
-            description="Diff drive base controller to start.",
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "launch_rviz", default_value="true", description="Launch RViz?"
-        )
-    )
-    declared_arguments.append(
-        DeclareLaunchArgument(
-            "use_sim_time",
-            default_value="True",
-            description="Use simulation (Gazebo) clock if true",
+            "headless", default_value="true", description="Launch Gazebo headless?"
         )
     )
 
     # Initialize Arguments
-    sim_ignition = LaunchConfiguration("sim_ignition")
-    # General arguments
-    runtime_config_package = LaunchConfiguration("runtime_config_package")
-    description_package = LaunchConfiguration("description_package")
-    description_file = LaunchConfiguration("description_file")
-    robot_name = LaunchConfiguration("robot_name")
-    prefix = LaunchConfiguration("prefix")
-    diff_drive_controller = LaunchConfiguration("diff_drive_controller")
-    launch_rviz = LaunchConfiguration("launch_rviz")
-    use_sim_time = LaunchConfiguration("use_sim_time")
+    launch_rviz = LaunchConfiguration("rviz")
 
     rviz_config_file = PathJoinSubstitution(
-        [FindPackageShare(runtime_config_package), "rviz", "bringup_config.rviz"]
+        [FindPackageShare("c300_bringup"), "rviz", "c300.rviz"]
     )
 
     robot_description_content = Command(
         [
             PathJoinSubstitution([FindExecutable(name="xacro")]),
             " ",
             PathJoinSubstitution(
-                [FindPackageShare(description_package), "urdf", description_file]
+                [FindPackageShare("c300_description"), "urdf", "c300.urdf.xacro"]
             ),
             " ",
-            "sim_ignition:=",
-            sim_ignition,
+            "sim_gazebo:=true",
             " ",
         ]
     )
@@ -118,7 +76,7 @@ def generate_launch_description():
         output="both",
         parameters=[
             {
-                "use_sim_time": use_sim_time,
+                "use_sim_time": True,
                 "robot_description": robot_description_content,
             }
         ],
@@ -129,44 +87,40 @@ def generate_launch_description():
         executable="rviz2",
         name="rviz2",
         output="log",
+        parameters=[{"use_sim_time": True}],
         arguments=["-d", rviz_config_file],
         condition=IfCondition(launch_rviz),
     )
 
-    joint_state_broadcaster_spawner = Node(
+    controller_spawner = Node(
         package="controller_manager",
         executable="spawner",
-        parameters=[{"use_sim_time": use_sim_time}],
+        parameters=[{"use_sim_time": True}],
         arguments=[
+            "diff_drive_base_controller",
             "joint_state_broadcaster",
             "--controller-manager",
             "/controller_manager",
         ],
     )
 
-    # Delay rviz start after `joint_state_broadcaster`
-    delay_rviz_after_joint_state_broadcaster_spawner = RegisterEventHandler(
+    # Delay rviz start after `controller_spawner`
+    delay_rviz_after_controller_spawner = RegisterEventHandler(
         event_handler=OnProcessExit(
-            target_action=joint_state_broadcaster_spawner,
+            target_action=controller_spawner,
             on_exit=[rviz_node],
         )
     )
 
-    diff_drive_controller_spawner = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=[diff_drive_controller, "-c", "/controller_manager"],
-    )
-
-    ignition_spawn_entity = Node(
+    gazebo_spawn_entity = Node(
         package="ros_gz_sim",
         executable="create",
         output="screen",
         arguments=[
             "-string",
             robot_description_content,
             "-name",
-            robot_name,
+            "c300",
             "-allow_renaming",
             "true",
             "-x",
@@ -182,42 +136,27 @@ def generate_launch_description():
             "-Y",
             "0.0",
         ],
-        condition=IfCondition(sim_ignition),
-    )
-
-    ignition_launch_description = IncludeLaunchDescription(
-        PythonLaunchDescriptionSource(
-            [FindPackageShare("ros_gz_sim"), "/launch/gz_sim.launch.py"]
-        ),
-        # TODO (marqrazz): fix the hardcoded path to the gazebo world
-        # -v is the verbose level
-        #  0: No output, 1: Error, 2: Error and warning, 3: Error, warning, and info, 4: Error, warning, info, and debug.
-        # -s launches Gazebo headless
-        launch_arguments={
-            "gz_args": " -r -v 3 -s /root/c3pzero_ws/src/c3pzero/c300/c300_bringup/worlds/depot.sdf"
-        }.items(),
-        condition=IfCondition(sim_ignition),
     )
 
     # Bridge
     gazebo_bridge = Node(
         package="ros_gz_bridge",
         executable="parameter_bridge",
-        parameters=[{"use_sim_time": use_sim_time}],
+        parameters=[{"use_sim_time": True}],
         arguments=[
-            "/scan@sensor_msgs/msg/LaserScan[ignition.msgs.LaserScan",
-            "/clock@rosgraph_msgs/msg/Clock[ignition.msgs.Clock",
+            "/scan@sensor_msgs/msg/LaserScan[gz.msgs.LaserScan",
+            "/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock",
+            "/imu@sensor_msgs/msg/Imu[ignition.msgs.IMU",
         ],
         output="screen",
     )
 
     nodes_to_start = [
         robot_state_publisher_node,
-        joint_state_broadcaster_spawner,
-        delay_rviz_after_joint_state_broadcaster_spawner,
-        diff_drive_controller_spawner,
-        ignition_launch_description,
-        ignition_spawn_entity,
+        controller_spawner,
+        delay_rviz_after_controller_spawner,
+        OpaqueFunction(function=launch_gz),
+        gazebo_spawn_entity,
         gazebo_bridge,
     ]
 

diff --git a/c300/c300_bringup/package.xml b/c300/c300_bringup/package.xml
@@ -12,7 +12,10 @@
   <exec_depend>rviz2</exec_depend>
   <exec_depend>urg_node</exec_depend>
   <exec_depend>controller_manager</exec_depend>
-  <exec_depend>ign_ros2_control</exec_depend>
+  <exec_depend>ros2controlcli</exec_depend>
+  <exec_depend>gz_ros2_control</exec_depend>
+  <exec_depend>ros_gz_bridge</exec_depend>
+  <exec_depend>ros_gz_sim</exec_depend>
   <exec_depend>joint_state_broadcaster</exec_depend>
   <exec_depend>diff_drive_controller</exec_depend>