fix leg controller in jazzy, simplified gazebo launch file

controllers/leg_pd_controller/CMakeLists.txt

@@ -21,6 +21,15 @@ foreach (Dependency IN ITEMS ${CONTROLLER_INCLUDE_DEPENDS})
     find_package(${Dependency} REQUIRED)
 endforeach ()
 
+# check ros2 control version
+find_package(controller_manager 3.0.0)
+# Handle the case where the required version is not found
+if(NOT controller_manager_FOUND)
+    add_definitions(-DROS2_CONTROL_VERSION_LT_3)
+    message(FATAL_ERROR "ros2_control version below 3.0.0. "
+            "Change the implementation to support ros2_control version 2.")
+endif()
+
 add_library(${PROJECT_NAME} SHARED
         src/LegPdController.cpp
 )

controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h

@@ -37,10 +37,14 @@ namespace leg_pd_controller {
     protected:
         std::vector<hardware_interface::CommandInterface> on_export_reference_interfaces() override;
 
-        // controller_interface::return_type update_reference_from_subscribers(
+        #ifdef ROS2_CONTROL_VERSION_LT_3
-        //     const rclcpp::Time &time, const rclcpp::Duration &period);
-
         controller_interface::return_type update_reference_from_subscribers() override;
+        #else
+        controller_interface::return_type update_reference_from_subscribers(
+            const rclcpp::Time &time, const rclcpp::Duration &period);
+        #endif
+
+
 
         std::vector<double> joint_effort_command_;
         std::vector<double> joint_position_command_;

controllers/leg_pd_controller/src/LegPdController.cpp

@@ -4,22 +4,17 @@
 
 #include "leg_pd_controller/LegPdController.h"
 
-namespace leg_pd_controller
+namespace leg_pd_controller {
-{
     using config_type = controller_interface::interface_configuration_type;
 
-    controller_interface::CallbackReturn LegPdController::on_init()
+    controller_interface::CallbackReturn LegPdController::on_init() {
-    {
+        try {
-        try
+            joint_names_ = auto_declare<std::vector<std::string> >("joints", joint_names_);
-        {
-            joint_names_ = auto_declare<std::vector<std::string>>("joints", joint_names_);
             reference_interface_types_ =
-                auto_declare<std::vector<std::string>>("reference_interfaces", reference_interface_types_);
+                    auto_declare<std::vector<std::string> >("reference_interfaces", reference_interface_types_);
             state_interface_types_ = auto_declare<std::vector<
-                std::string>>("state_interfaces", state_interface_types_);
+                std::string> >("state_interfaces", state_interface_types_);
-        }
+        } catch (const std::exception &e) {
-        catch (const std::exception& e)
-        {
             fprintf(stderr, "Exception thrown during init stage with message: %s \n", e.what());
             return controller_interface::CallbackReturn::ERROR;
         }
@@ -34,27 +29,22 @@ namespace leg_pd_controller
         return CallbackReturn::SUCCESS;
     }
 
-    controller_interface::InterfaceConfiguration LegPdController::command_interface_configuration() const
+    controller_interface::InterfaceConfiguration LegPdController::command_interface_configuration() const {
-    {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
 
         conf.names.reserve(joint_names_.size());
-        for (const auto& joint_name : joint_names_)
+        for (const auto &joint_name: joint_names_) {
-        {
             conf.names.push_back(joint_name + "/effort");
         }
 
         return conf;
     }
 
-    controller_interface::InterfaceConfiguration LegPdController::state_interface_configuration() const
+    controller_interface::InterfaceConfiguration LegPdController::state_interface_configuration() const {
-    {
         controller_interface::InterfaceConfiguration conf = {config_type::INDIVIDUAL, {}};
         conf.names.reserve(joint_names_.size() * state_interface_types_.size());
-        for (const auto& joint_name : joint_names_)
+        for (const auto &joint_name: joint_names_) {
-        {
+            for (const auto &interface_type: state_interface_types_) {
-            for (const auto& interface_type : state_interface_types_)
-            {
                 conf.names.push_back(joint_name + "/" += interface_type);
             }
         }
@@ -62,28 +52,26 @@ namespace leg_pd_controller
     }
 
     controller_interface::CallbackReturn LegPdController::on_configure(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
+#ifdef ROS2_CONTROL_VERSION_LT_3
-        reference_interfaces_.resize(joint_names_.size()*5, std::numeric_limits<double>::quiet_NaN());
+        reference_interfaces_.resize(joint_names_.size() * 5, std::numeric_limits<double>::quiet_NaN());
+#endif
         return CallbackReturn::SUCCESS;
     }
 
     controller_interface::CallbackReturn LegPdController::on_activate(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
         joint_effort_command_interface_.clear();
         joint_position_state_interface_.clear();
         joint_velocity_state_interface_.clear();
 
         // assign effort command interface
-        for (auto& interface : command_interfaces_)
+        for (auto &interface: command_interfaces_) {
-        {
             joint_effort_command_interface_.emplace_back(interface);
         }
 
         // assign state interfaces
-        for (auto& interface : state_interfaces_)
+        for (auto &interface: state_interfaces_) {
-        {
             state_interface_map_[interface.get_interface_name()]->push_back(interface);
         }
 
@@ -91,27 +79,23 @@ namespace leg_pd_controller
     }
 
     controller_interface::CallbackReturn LegPdController::on_deactivate(
-        const rclcpp_lifecycle::State& /*previous_state*/)
+        const rclcpp_lifecycle::State & /*previous_state*/) {
-    {
         release_interfaces();
         return CallbackReturn::SUCCESS;
     }
 
-    bool LegPdController::on_set_chained_mode(bool /*chained_mode*/)
+    bool LegPdController::on_set_chained_mode(bool /*chained_mode*/) {
-    {
         return true;
     }
 
     controller_interface::return_type LegPdController::update_and_write_commands(
-        const rclcpp::Time& /*time*/, const rclcpp::Duration& /*period*/)
+        const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
-    {
         if (joint_names_.size() != joint_effort_command_.size() ||
             joint_names_.size() != joint_kp_command_.size() ||
             joint_names_.size() != joint_position_command_.size() ||
             joint_names_.size() != joint_position_state_interface_.size() ||
             joint_names_.size() != joint_velocity_state_interface_.size() ||
-            joint_names_.size() != joint_effort_command_interface_.size())
+            joint_names_.size() != joint_effort_command_interface_.size()) {
-        {
             std::cout << "joint_names_.size() = " << joint_names_.size() << std::endl;
             std::cout << "joint_effort_command_.size() = " << joint_effort_command_.size() << std::endl;
             std::cout << "joint_kp_command_.size() = " << joint_kp_command_.size() << std::endl;
@@ -126,8 +110,7 @@ namespace leg_pd_controller
             throw std::runtime_error("Mismatch in vector sizes in update_and_write_commands");
         }
 
-        for (size_t i = 0; i < joint_names_.size(); ++i)
+        for (size_t i = 0; i < joint_names_.size(); ++i) {
-        {
             // PD Controller
             const double torque = joint_effort_command_[i] + joint_kp_command_[i] * (
                                       joint_position_command_[i] - joint_position_state_interface_[i].get().get_value())
@@ -141,14 +124,12 @@ namespace leg_pd_controller
         return controller_interface::return_type::OK;
     }
 
-    std::vector<hardware_interface::CommandInterface> LegPdController::on_export_reference_interfaces()
+    std::vector<hardware_interface::CommandInterface> LegPdController::on_export_reference_interfaces() {
-    {
         std::vector<hardware_interface::CommandInterface> reference_interfaces;
 
         int ind = 0;
         std::string controller_name = get_node()->get_name();
-        for (const auto& joint_name : joint_names_)
+        for (const auto &joint_name: joint_names_) {
-        {
             std::cout << joint_name << std::endl;
             reference_interfaces.emplace_back(controller_name, joint_name + "/position", &joint_position_command_[ind]);
             reference_interfaces.emplace_back(controller_name, joint_name + "/velocity",
@@ -162,15 +143,16 @@ namespace leg_pd_controller
         return reference_interfaces;
     }
 
-    // controller_interface::return_type LegPdController::update_reference_from_subscribers(
+#ifdef ROS2_CONTROL_VERSION_LT_3
-    //     const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+    controller_interface::return_type LegPdController::update_reference_from_subscribers() {
-    //     return controller_interface::return_type::OK;
-    // }
-
-    controller_interface::return_type LegPdController::update_reference_from_subscribers()
-    {
         return controller_interface::return_type::OK;
     }
+#else
+    controller_interface::return_type LegPdController::update_reference_from_subscribers(
+        const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) {
+        return controller_interface::return_type::OK;
+    }
+#endif
 }
 
 #include <pluginlib/class_list_macros.hpp>

controllers/unitree_guide_controller/launch/gazebo.launch.py

@@ -10,13 +10,10 @@ from launch_ros.actions import Node
 from launch_ros.substitutions import FindPackageShare
 from launch.launch_description_sources import PythonLaunchDescriptionSource
 
-package_description = "go2_description"
-
-
 
 def launch_setup(context, *args, **kwargs):
-
     package_description = context.launch_configurations['pkg_description']
+    init_height = context.launch_configurations['height']
     pkg_path = os.path.join(get_package_share_directory(package_description))
 
     xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
@@ -37,7 +34,7 @@ def launch_setup(context, *args, **kwargs):
         executable='create',
         output='screen',
         arguments=['-topic', 'robot_description', '-name',
-                   'robot', '-allow_renaming', 'true', '-z', '0.5'],
+                   'robot', '-allow_renaming', 'true', '-z', init_height],
     )
 
     robot_state_publisher = Node(
@@ -83,7 +80,6 @@ def launch_setup(context, *args, **kwargs):
 
     return [
         rviz,
-        robot_state_publisher,
         Node(
             package='ros_gz_bridge',
             executable='parameter_bridge',
@@ -115,7 +111,14 @@ def generate_launch_description():
         description='package for robot description'
     )
 
+    height = DeclareLaunchArgument(
+        'height',
+        default_value='0.5',
+        description='Init height in simulation'
+    )
+
     return LaunchDescription([
         pkg_description,
+        height,
         OpaqueFunction(function=launch_setup),
     ])

descriptions/deep_robotics/lite3_description/README.md

@@ -43,7 +43,7 @@ ros2 launch lite3_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch lite3_description gazebo.launch.py
+  ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=lite3_description height:=0.43
   ```
 * Legged Gym Controller
   ```bash

descriptions/deep_robotics/lite3_description/launch/gazebo_unitree_guide.launch.py

@@ -1,106 +0,0 @@
-import os
-
-import xacro
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-package_description = "lite3_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'lite3', '-allow_renaming', 'true', '-z', '0.4'],
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {
-                'publish_frequency': 20.0,
-                'use_tf_static': True,
-                'robot_description': robot_description,
-                'ignore_timestamp': True
-            }
-        ],
-    )
-
-    joint_state_publisher = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    imu_sensor_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["imu_sensor_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    leg_pd_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["leg_pd_controller",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-            output='screen'
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        robot_state_publisher,
-        gz_spawn_entity,
-        leg_pd_controller,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/deep_robotics/x30_description/README.md

@@ -39,7 +39,7 @@ ros2 launch x30_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch x30_description gazebo.launch.py
+  ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=x30_description height:=0.64
   ```
 * Legged Gym Controller
   ```bash

descriptions/deep_robotics/x30_description/launch/gazebo_unitree_guide.launch.py

@@ -1,106 +0,0 @@
-import os
-
-import xacro
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-package_description = "x30_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'x30', '-allow_renaming', 'true', '-z', '0.4'],
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {
-                'publish_frequency': 20.0,
-                'use_tf_static': True,
-                'robot_description': robot_description,
-                'ignore_timestamp': True
-            }
-        ],
-    )
-
-    joint_state_publisher = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    imu_sensor_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["imu_sensor_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    leg_pd_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["leg_pd_controller",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-            output='screen'
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        robot_state_publisher,
-        gz_spawn_entity,
-        leg_pd_controller,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/unitree/a1_description/README.md

@@ -1,27 +1,34 @@
 # Unitree A1 Description
+
 This repository contains the urdf model of A1.
 
 ![A1](../../../.images/a1.png)
 
 Tested environment:
+
 * Ubuntu 24.04
     * ROS2 Jazzy
 
 ## Build
+
 ```bash
 cd ~/ros2_ws
 colcon build --packages-up-to a1_description --symlink-install
 ```
 
 ## Visualize the robot
+
 To visualize and check the configuration of the robot in rviz, simply launch:
+
 ```bash
 source ~/ros2_ws/install/setup.bash
 ros2 launch a1_description visualize.launch.py
 ```
 
 ## Launch ROS2 Control
+
 ### Mujoco Simulator
+
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
@@ -39,6 +46,7 @@ ros2 launch a1_description visualize.launch.py
   ```
 
 ### Gazebo Classic 11 (ROS2 Humble)
+
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
@@ -46,10 +54,11 @@ ros2 launch a1_description visualize.launch.py
   ```
 
 ### Gazebo Harmonic (ROS2 Jazzy)
+
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch a1_description gazebo.launch.py
+  ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=a1_description height:=0.43
   ```
 * RL Quadruped Controller
   ```bash

descriptions/unitree/a1_description/config/gazebo.yaml

@@ -1,7 +1,7 @@
 # Controller Manager configuration
 controller_manager:
   ros__parameters:
-    update_rate: 2000  # Hz
+    update_rate: 1000  # Hz
 
     # Define the available controllers
     joint_state_broadcaster:

descriptions/unitree/a1_description/launch/gazebo_unitree_guide.launch.py

@@ -1,106 +0,0 @@
-import os
-
-import xacro
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-package_description = "a1_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'a1', '-allow_renaming', 'true', '-z', '0.4'],
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {
-                'publish_frequency': 20.0,
-                'use_tf_static': True,
-                'robot_description': robot_description,
-                'ignore_timestamp': True
-            }
-        ],
-    )
-
-    joint_state_publisher = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    imu_sensor_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["imu_sensor_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    leg_pd_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["leg_pd_controller",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-            output='screen'
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        robot_state_publisher,
-        gz_spawn_entity,
-        leg_pd_controller,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/unitree/aliengo_description/README.md

@@ -45,5 +45,5 @@ ros2 launch aliengo_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch aliengo_description gazebo.launch.py
+  ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=aliengo_description height:=0.535
   ```

descriptions/unitree/aliengo_description/launch/gazebo_unitree_guide.launch.py

@@ -1,106 +0,0 @@
-import os
-
-import xacro
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-package_description = "aliengo_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'robot', '-allow_renaming', 'true', '-z', '0.5'],
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {
-                'publish_frequency': 20.0,
-                'use_tf_static': True,
-                'robot_description': robot_description,
-                'ignore_timestamp': True
-            }
-        ],
-    )
-
-    joint_state_publisher = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    imu_sensor_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["imu_sensor_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    leg_pd_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["leg_pd_controller",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-            output='screen'
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        robot_state_publisher,
-        gz_spawn_entity,
-        leg_pd_controller,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/unitree/b2_description/README.md

@@ -40,5 +40,5 @@ ros2 launch b2_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch b2_description gazebo.launch.py
+  ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=b2_description height:=0.68
   ```

descriptions/unitree/b2_description/launch/gazebo_unitree_guide.launch.py

@@ -1,106 +0,0 @@
-import os
-
-import xacro
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-package_description = "b2_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'robot', '-allow_renaming', 'true', '-z', '0.68'],
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {
-                'publish_frequency': 20.0,
-                'use_tf_static': True,
-                'robot_description': robot_description,
-                'ignore_timestamp': True
-            }
-        ],
-    )
-
-    joint_state_publisher = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    imu_sensor_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["imu_sensor_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    leg_pd_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["leg_pd_controller",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-            output='screen'
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        robot_state_publisher,
-        gz_spawn_entity,
-        leg_pd_controller,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/unitree/go1_description/README.md

@@ -40,5 +40,5 @@ ros2 launch go1_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch go1_description gazebo.launch.py
+  ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=go1_description
   ```

descriptions/unitree/go1_description/launch/gazebo_unitree_guide.launch.py

@@ -1,106 +0,0 @@
-import os
-
-import xacro
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-package_description = "go1_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'go2', '-allow_renaming', 'true', '-z', '0.5'],
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {
-                'publish_frequency': 20.0,
-                'use_tf_static': True,
-                'robot_description': robot_description,
-                'ignore_timestamp': True
-            }
-        ],
-    )
-
-    joint_state_publisher = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    imu_sensor_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["imu_sensor_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    leg_pd_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["leg_pd_controller",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-            output='screen'
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        robot_state_publisher,
-        gz_spawn_entity,
-        leg_pd_controller,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/unitree/go2_description/launch/gazebo_unitree_guide.launch.py

@@ -1,106 +0,0 @@
-import os
-
-import xacro
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-package_description = "go2_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'robot', '-allow_renaming', 'true', '-z', '0.5'],
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {
-                'publish_frequency': 20.0,
-                'use_tf_static': True,
-                'robot_description': robot_description,
-                'ignore_timestamp': True
-            }
-        ],
-    )
-
-    joint_state_publisher = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    imu_sensor_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["imu_sensor_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    leg_pd_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["leg_pd_controller",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-            output='screen'
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        robot_state_publisher,
-        gz_spawn_entity,
-        leg_pd_controller,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-    ])

descriptions/xiaomi/cyberdog_description/README.md

@@ -41,5 +41,5 @@ ros2 launch cyberdog_description visualize.launch.py
 * Unitree Guide Controller
   ```bash
   source ~/ros2_ws/install/setup.bash
-  ros2 launch cyberdog_description gazebo.launch.py
+  ros2 launch unitree_guide_controller gazebo.launch.py pkg_description:=cyberdog_description height:=0.31
   ```

descriptions/xiaomi/cyberdog_description/launch/gazebo_unitree_guide.launch.py

@@ -1,106 +0,0 @@
-import os
-
-import xacro
-from ament_index_python.packages import get_package_share_directory
-from launch import LaunchDescription
-from launch.actions import DeclareLaunchArgument, OpaqueFunction, IncludeLaunchDescription, RegisterEventHandler
-from launch.launch_description_sources import PythonLaunchDescriptionSource
-from launch.event_handlers import OnProcessExit
-from launch.substitutions import PathJoinSubstitution
-from launch_ros.actions import Node
-from launch_ros.substitutions import FindPackageShare
-
-package_description = "cyberdog_description"
-
-
-def process_xacro():
-    pkg_path = os.path.join(get_package_share_directory(package_description))
-    xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro')
-    robot_description_config = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'})
-    return robot_description_config.toxml()
-
-
-def generate_launch_description():
-    rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz")
-
-    robot_description = process_xacro()
-
-    gz_spawn_entity = Node(
-        package='ros_gz_sim',
-        executable='create',
-        output='screen',
-        arguments=['-topic', 'robot_description', '-name',
-                   'robot', '-allow_renaming', 'true', '-z', '0.46'],
-    )
-
-    robot_state_publisher = Node(
-        package='robot_state_publisher',
-        executable='robot_state_publisher',
-        name='robot_state_publisher',
-        parameters=[
-            {
-                'publish_frequency': 20.0,
-                'use_tf_static': True,
-                'robot_description': robot_description,
-                'ignore_timestamp': True
-            }
-        ],
-    )
-
-    joint_state_publisher = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["joint_state_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    imu_sensor_broadcaster = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["imu_sensor_broadcaster",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    leg_pd_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["leg_pd_controller",
-                   "--controller-manager", "/controller_manager"],
-    )
-
-    unitree_guide_controller = Node(
-        package="controller_manager",
-        executable="spawner",
-        arguments=["unitree_guide_controller", "--controller-manager", "/controller_manager"],
-    )
-
-    return LaunchDescription([
-        Node(
-            package='rviz2',
-            executable='rviz2',
-            name='rviz_ocs2',
-            output='screen',
-            arguments=["-d", rviz_config_file]
-        ),
-        Node(
-            package='ros_gz_bridge',
-            executable='parameter_bridge',
-            arguments=['/clock@rosgraph_msgs/msg/Clock[gz.msgs.Clock'],
-            output='screen'
-        ),
-        IncludeLaunchDescription(
-            PythonLaunchDescriptionSource(
-                [PathJoinSubstitution([FindPackageShare('ros_gz_sim'),
-                                       'launch',
-                                       'gz_sim.launch.py'])]),
-            launch_arguments=[('gz_args', [' -r -v 4 empty.sdf'])]),
-        robot_state_publisher,
-        gz_spawn_entity,
-        leg_pd_controller,
-        RegisterEventHandler(
-            event_handler=OnProcessExit(
-                target_action=leg_pd_controller,
-                on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller],
-            )
-        ),
-    ])