From 4575779eb50343d1f9b424748daacdac38e07a4e Mon Sep 17 00:00:00 2001 From: Zhenbiao Huang Date: Sun, 20 Oct 2024 12:42:34 +0800 Subject: [PATCH] fix leg controller in jazzy, simplified gazebo launch file --- controllers/leg_pd_controller/CMakeLists.txt | 9 ++ .../leg_pd_controller/LegPdController.h | 10 +- .../leg_pd_controller/src/LegPdController.cpp | 102 +++++++---------- .../launch/gazebo.launch.py | 17 +-- .../deep_robotics/lite3_description/README.md | 2 +- .../launch/gazebo_unitree_guide.launch.py | 106 ------------------ .../deep_robotics/x30_description/README.md | 2 +- .../launch/gazebo_unitree_guide.launch.py | 106 ------------------ descriptions/unitree/a1_description/README.md | 11 +- .../unitree/a1_description/config/gazebo.yaml | 2 +- .../launch/gazebo_unitree_guide.launch.py | 106 ------------------ .../unitree/aliengo_description/README.md | 2 +- .../launch/gazebo_unitree_guide.launch.py | 106 ------------------ descriptions/unitree/b2_description/README.md | 4 +- .../launch/gazebo_unitree_guide.launch.py | 106 ------------------ .../unitree/go1_description/README.md | 2 +- .../launch/gazebo_unitree_guide.launch.py | 106 ------------------ .../launch/gazebo_unitree_guide.launch.py | 106 ------------------ .../xiaomi/cyberdog_description/README.md | 2 +- .../launch/gazebo_unitree_guide.launch.py | 106 ------------------ 20 files changed, 86 insertions(+), 927 deletions(-) delete mode 100644 descriptions/deep_robotics/lite3_description/launch/gazebo_unitree_guide.launch.py delete mode 100644 descriptions/deep_robotics/x30_description/launch/gazebo_unitree_guide.launch.py delete mode 100644 descriptions/unitree/a1_description/launch/gazebo_unitree_guide.launch.py delete mode 100644 descriptions/unitree/aliengo_description/launch/gazebo_unitree_guide.launch.py delete mode 100644 descriptions/unitree/b2_description/launch/gazebo_unitree_guide.launch.py delete mode 100644 descriptions/unitree/go1_description/launch/gazebo_unitree_guide.launch.py delete mode 100644 descriptions/unitree/go2_description/launch/gazebo_unitree_guide.launch.py delete mode 100644 descriptions/xiaomi/cyberdog_description/launch/gazebo_unitree_guide.launch.py diff --git a/controllers/leg_pd_controller/CMakeLists.txt b/controllers/leg_pd_controller/CMakeLists.txt index 609f950..0c07275 100644 --- a/controllers/leg_pd_controller/CMakeLists.txt +++ b/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 ) diff --git a/controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h b/controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h index 9cb3fe9..c10203a 100644 --- a/controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h +++ b/controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h @@ -37,10 +37,14 @@ namespace leg_pd_controller { protected: std::vector on_export_reference_interfaces() override; - // controller_interface::return_type update_reference_from_subscribers( - // const rclcpp::Time &time, const rclcpp::Duration &period); - + #ifdef ROS2_CONTROL_VERSION_LT_3 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 joint_effort_command_; std::vector joint_position_command_; diff --git a/controllers/leg_pd_controller/src/LegPdController.cpp b/controllers/leg_pd_controller/src/LegPdController.cpp index 33ad2cd..ffe90b1 100644 --- a/controllers/leg_pd_controller/src/LegPdController.cpp +++ b/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() - { - try - { - joint_names_ = auto_declare>("joints", joint_names_); + controller_interface::CallbackReturn LegPdController::on_init() { + try { + joint_names_ = auto_declare >("joints", joint_names_); reference_interface_types_ = - auto_declare>("reference_interfaces", reference_interface_types_); + auto_declare >("reference_interfaces", reference_interface_types_); state_interface_types_ = auto_declare>("state_interfaces", state_interface_types_); - } - catch (const std::exception& e) - { + std::string> >("state_interfaces", state_interface_types_); + } 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& interface_type : state_interface_types_) - { + for (const auto &joint_name: joint_names_) { + 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*/) - { - reference_interfaces_.resize(joint_names_.size()*5, std::numeric_limits::quiet_NaN()); + const rclcpp_lifecycle::State & /*previous_state*/) { +#ifdef ROS2_CONTROL_VERSION_LT_3 + reference_interfaces_.resize(joint_names_.size() * 5, std::numeric_limits::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,64 +79,57 @@ 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; std::cout << "joint_position_command_.size() = " << joint_position_command_.size() << std::endl; std::cout << "joint_position_state_interface_.size() = " << joint_position_state_interface_.size() << - std::endl; + std::endl; std::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() << - std::endl; + std::endl; std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() << - std::endl; + std::endl; 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()) - + - joint_kd_command_[i] * ( - joint_velocities_command_[i] - joint_velocity_state_interface_[i].get(). - get_value()); + joint_position_command_[i] - joint_position_state_interface_[i].get().get_value()) + + + joint_kd_command_[i] * ( + joint_velocities_command_[i] - joint_velocity_state_interface_[i].get(). + get_value()); joint_effort_command_interface_[i].get().set_value(torque); } return controller_interface::return_type::OK; } - std::vector LegPdController::on_export_reference_interfaces() - { + std::vector LegPdController::on_export_reference_interfaces() { std::vector 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( - // const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { - // return controller_interface::return_type::OK; - // } - - controller_interface::return_type LegPdController::update_reference_from_subscribers() - { +#ifdef ROS2_CONTROL_VERSION_LT_3 + 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 diff --git a/controllers/unitree_guide_controller/launch/gazebo.launch.py b/controllers/unitree_guide_controller/launch/gazebo.launch.py index 33f7974..aac6fb9 100644 --- a/controllers/unitree_guide_controller/launch/gazebo.launch.py +++ b/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), - ]) \ No newline at end of file + ]) diff --git a/descriptions/deep_robotics/lite3_description/README.md b/descriptions/deep_robotics/lite3_description/README.md index f0c7e07..46392b3 100644 --- a/descriptions/deep_robotics/lite3_description/README.md +++ b/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 diff --git a/descriptions/deep_robotics/lite3_description/launch/gazebo_unitree_guide.launch.py b/descriptions/deep_robotics/lite3_description/launch/gazebo_unitree_guide.launch.py deleted file mode 100644 index 22f16bd..0000000 --- a/descriptions/deep_robotics/lite3_description/launch/gazebo_unitree_guide.launch.py +++ /dev/null @@ -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], - ) - ), - ]) diff --git a/descriptions/deep_robotics/x30_description/README.md b/descriptions/deep_robotics/x30_description/README.md index 6376e8f..45e9b3e 100644 --- a/descriptions/deep_robotics/x30_description/README.md +++ b/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 diff --git a/descriptions/deep_robotics/x30_description/launch/gazebo_unitree_guide.launch.py b/descriptions/deep_robotics/x30_description/launch/gazebo_unitree_guide.launch.py deleted file mode 100644 index 4058737..0000000 --- a/descriptions/deep_robotics/x30_description/launch/gazebo_unitree_guide.launch.py +++ /dev/null @@ -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], - ) - ), - ]) diff --git a/descriptions/unitree/a1_description/README.md b/descriptions/unitree/a1_description/README.md index 348b369..4bd6126 100644 --- a/descriptions/unitree/a1_description/README.md +++ b/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 diff --git a/descriptions/unitree/a1_description/config/gazebo.yaml b/descriptions/unitree/a1_description/config/gazebo.yaml index 92d13cd..7e77551 100644 --- a/descriptions/unitree/a1_description/config/gazebo.yaml +++ b/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: diff --git a/descriptions/unitree/a1_description/launch/gazebo_unitree_guide.launch.py b/descriptions/unitree/a1_description/launch/gazebo_unitree_guide.launch.py deleted file mode 100644 index 125e4f9..0000000 --- a/descriptions/unitree/a1_description/launch/gazebo_unitree_guide.launch.py +++ /dev/null @@ -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], - ) - ), - ]) diff --git a/descriptions/unitree/aliengo_description/README.md b/descriptions/unitree/aliengo_description/README.md index 0b0b833..f7993bb 100644 --- a/descriptions/unitree/aliengo_description/README.md +++ b/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 ``` \ No newline at end of file diff --git a/descriptions/unitree/aliengo_description/launch/gazebo_unitree_guide.launch.py b/descriptions/unitree/aliengo_description/launch/gazebo_unitree_guide.launch.py deleted file mode 100644 index c50364c..0000000 --- a/descriptions/unitree/aliengo_description/launch/gazebo_unitree_guide.launch.py +++ /dev/null @@ -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], - ) - ), - ]) diff --git a/descriptions/unitree/b2_description/README.md b/descriptions/unitree/b2_description/README.md index 1bea8c8..559e8a7 100644 --- a/descriptions/unitree/b2_description/README.md +++ b/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 + ``` \ No newline at end of file diff --git a/descriptions/unitree/b2_description/launch/gazebo_unitree_guide.launch.py b/descriptions/unitree/b2_description/launch/gazebo_unitree_guide.launch.py deleted file mode 100644 index b6141f9..0000000 --- a/descriptions/unitree/b2_description/launch/gazebo_unitree_guide.launch.py +++ /dev/null @@ -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], - ) - ), - ]) diff --git a/descriptions/unitree/go1_description/README.md b/descriptions/unitree/go1_description/README.md index 8e08652..d07a430 100644 --- a/descriptions/unitree/go1_description/README.md +++ b/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 ``` \ No newline at end of file diff --git a/descriptions/unitree/go1_description/launch/gazebo_unitree_guide.launch.py b/descriptions/unitree/go1_description/launch/gazebo_unitree_guide.launch.py deleted file mode 100644 index dfb92f7..0000000 --- a/descriptions/unitree/go1_description/launch/gazebo_unitree_guide.launch.py +++ /dev/null @@ -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], - ) - ), - ]) diff --git a/descriptions/unitree/go2_description/launch/gazebo_unitree_guide.launch.py b/descriptions/unitree/go2_description/launch/gazebo_unitree_guide.launch.py deleted file mode 100644 index 3bf34d5..0000000 --- a/descriptions/unitree/go2_description/launch/gazebo_unitree_guide.launch.py +++ /dev/null @@ -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], - ) - ), - ]) diff --git a/descriptions/xiaomi/cyberdog_description/README.md b/descriptions/xiaomi/cyberdog_description/README.md index a62d9be..51a6aee 100644 --- a/descriptions/xiaomi/cyberdog_description/README.md +++ b/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 ``` \ No newline at end of file diff --git a/descriptions/xiaomi/cyberdog_description/launch/gazebo_unitree_guide.launch.py b/descriptions/xiaomi/cyberdog_description/launch/gazebo_unitree_guide.launch.py deleted file mode 100644 index 5a876f2..0000000 --- a/descriptions/xiaomi/cyberdog_description/launch/gazebo_unitree_guide.launch.py +++ /dev/null @@ -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], - ) - ), - ])