diff --git a/README.md b/README.md index e3ee431..5881c74 100644 --- a/README.md +++ b/README.md @@ -23,27 +23,62 @@ Video for OCS2 Quadruped Controller: [![](http://i0.hdslb.com/bfs/archive/e758ce019587032449a153cf897a543443b64bba.jpg)](https://www.bilibili.com/video/BV1UcxieuEmH/) -## Quick Start +## 1. Quick Start * rosdep -```bash -cd ~/ros2_ws -rosdep install --from-paths src --ignore-src -r -y -``` + ```bash + cd ~/ros2_ws + rosdep install --from-paths src --ignore-src -r -y + ``` * Compile the package -```bash -colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input hardware_unitree_mujoco -``` + ```bash + colcon build --packages-up-to unitree_guide_controller go2_description keyboard_input hardware_unitree_mujoco + ``` + +### 1.1 Mujoco Simulator * Launch the unitree mujoco go2 simulation * Launch the ros2-control -```bash -source ~/ros2_ws/install/setup.bash -ros2 launch go2_description unitree_guide.launch.py -``` + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch unitree_guide_controller mujoco.launch.py + ``` * Run the keyboard control node -```bash -source ~/ros2_ws/install/setup.bash -ros2 run keyboard_input keyboard_input -``` + ```bash + source ~/ros2_ws/install/setup.bash + ros2 run keyboard_input keyboard_input + ``` + +### 1.2 Gazebo Classic Simulator (ROS2 Humble) +* Compile Leg PD Controller + ```bash + colcon build --packages-up-to leg_pd_controller + ``` +* Launch the ros2-control + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch unitree_guide_controller gazebo.launch.py + ``` +* Run the keyboard control node + ```bash + source ~/ros2_ws/install/setup.bash + ros2 run keyboard_input keyboard_input + ``` + +### 1.3 Gazebo Harmonic Simulator (ROS2 Jazzy) +* Compile Leg PD Controller + ```bash + colcon build --packages-up-to leg_pd_controller + ``` +* Launch the ros2-control + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch unitree_guide_controller gazebo_classic.launch.py + ``` +* Run the keyboard control node + ```bash + source ~/ros2_ws/install/setup.bash + ros2 run keyboard_input keyboard_input + ``` + For more details, please refer to the [unitree guide controller](controllers/unitree_guide_controller/) and [go2 description](descriptions/unitree/go2_description/). ## Reference 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 06ecc8b..9cb3fe9 100644 --- a/controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h +++ b/controllers/leg_pd_controller/include/leg_pd_controller/LegPdController.h @@ -5,10 +5,13 @@ #ifndef LEGPDCONTROLLER_H #define LEGPDCONTROLLER_H #include +#include "realtime_tools/realtime_buffer.h" #include +#include "std_msgs/msg/float64_multi_array.hpp" namespace leg_pd_controller { + using DataType = std_msgs::msg::Float64MultiArray; class LegPdController final : public controller_interface::ChainableControllerInterface { public: controller_interface::CallbackReturn on_init() override; @@ -34,8 +37,10 @@ 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) override; + // controller_interface::return_type update_reference_from_subscribers( + // const rclcpp::Time &time, const rclcpp::Duration &period); + + controller_interface::return_type update_reference_from_subscribers() override; std::vector joint_effort_command_; std::vector joint_position_command_; @@ -47,6 +52,7 @@ namespace leg_pd_controller { std::vector state_interface_types_; std::vector reference_interface_types_; + realtime_tools::RealtimeBuffer> rt_buffer_ptr_; std::vector > joint_effort_command_interface_; diff --git a/controllers/leg_pd_controller/src/LegPdController.cpp b/controllers/leg_pd_controller/src/LegPdController.cpp index de5ba0c..33ad2cd 100644 --- a/controllers/leg_pd_controller/src/LegPdController.cpp +++ b/controllers/leg_pd_controller/src/LegPdController.cpp @@ -4,17 +4,22 @@ #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; } @@ -29,22 +34,27 @@ 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); } } @@ -52,24 +62,28 @@ namespace leg_pd_controller { } controller_interface::CallbackReturn LegPdController::on_configure( - const rclcpp_lifecycle::State & /*previous_state*/) { + const rclcpp_lifecycle::State& /*previous_state*/) + { + reference_interfaces_.resize(joint_names_.size()*5, std::numeric_limits::quiet_NaN()); 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); } @@ -77,55 +91,65 @@ 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::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() << std::endl; - std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() << std::endl; + std::cout << "joint_position_state_interface_.size() = " << joint_position_state_interface_.size() << + std::endl; + std::cout << "joint_velocity_state_interface_.size() = " << joint_velocity_state_interface_.size() << + std::endl; + std::cout << "joint_effort_command_interface_.size() = " << joint_effort_command_interface_.size() << + 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", &joint_velocities_command_[ind]); @@ -138,8 +162,13 @@ namespace leg_pd_controller { return reference_interfaces; } - controller_interface::return_type LegPdController::update_reference_from_subscribers( - const rclcpp::Time & /*time*/, const rclcpp::Duration & /*period*/) { + // 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() + { return controller_interface::return_type::OK; } } diff --git a/controllers/unitree_guide_controller/CMakeLists.txt b/controllers/unitree_guide_controller/CMakeLists.txt index 7046c84..bd87bfc 100644 --- a/controllers/unitree_guide_controller/CMakeLists.txt +++ b/controllers/unitree_guide_controller/CMakeLists.txt @@ -77,6 +77,11 @@ install( RUNTIME DESTINATION bin ) +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) + ament_export_dependencies(${CONTROLLER_INCLUDE_DEPENDS}) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) diff --git a/descriptions/deep_robotics/x30_description/launch/unitree_guide.launch.py b/controllers/unitree_guide_controller/launch/gazebo.launch.py similarity index 51% rename from descriptions/deep_robotics/x30_description/launch/unitree_guide.launch.py rename to controllers/unitree_guide_controller/launch/gazebo.launch.py index db3906b..33f7974 100644 --- a/descriptions/deep_robotics/x30_description/launch/unitree_guide.launch.py +++ b/controllers/unitree_guide_controller/launch/gazebo.launch.py @@ -8,35 +8,36 @@ from launch.event_handlers import OnProcessExit from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource -package_description = "x30_description" +package_description = "go2_description" -def process_xacro(): + +def launch_setup(context, *args, **kwargs): + + package_description = context.launch_configurations['pkg_description'] 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) - return robot_description_config.toxml() + robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true'}).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() - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] ) - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", + 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( @@ -67,27 +68,54 @@ def generate_launch_description(): "--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] - ), + return [ + rviz, robot_state_publisher, - controller_manager, - joint_state_publisher, + 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=joint_state_publisher, - on_exit=[imu_sensor_broadcaster, unitree_guide_controller], + target_action=leg_pd_controller, + on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller], ) ), - ]) + ] + + +def generate_launch_description(): + pkg_description = DeclareLaunchArgument( + 'pkg_description', + default_value='go2_description', + description='package for robot description' + ) + + return LaunchDescription([ + pkg_description, + OpaqueFunction(function=launch_setup), + ]) \ No newline at end of file diff --git a/descriptions/unitree/aliengo_description/launch/unitree_guide.launch.py b/controllers/unitree_guide_controller/launch/gazebo_classic.launch.py similarity index 58% rename from descriptions/unitree/aliengo_description/launch/unitree_guide.launch.py rename to controllers/unitree_guide_controller/launch/gazebo_classic.launch.py index fdfe7c4..2962525 100644 --- a/descriptions/unitree/aliengo_description/launch/unitree_guide.launch.py +++ b/controllers/unitree_guide_controller/launch/gazebo_classic.launch.py @@ -8,26 +8,42 @@ from launch.event_handlers import OnProcessExit from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource -package_description = "aliengo_description" +package_description = "go2_description" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] - 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={'robot_type': robot_type_value}) - return (robot_description_config.toxml(), robot_type_value) - def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] + + package_description = context.launch_configurations['pkg_description'] + pkg_path = os.path.join(get_package_share_directory(package_description)) + + xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') + robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true', 'CLASSIC': 'true'}).toxml() + + rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") + + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"])] + ), + launch_arguments={"verbose": "false"}.items(), + ) + + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=["-topic", "robot_description", "-entity", "robot", "-z", "0.5"], + output="screen", ) robot_state_publisher = Node( @@ -44,13 +60,6 @@ def launch_setup(context, *args, **kwargs): ], ) - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - joint_state_publisher = Node( package="controller_manager", executable="spawner", @@ -65,6 +74,13 @@ def launch_setup(context, *args, **kwargs): "--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", @@ -72,41 +88,28 @@ def launch_setup(context, *args, **kwargs): ) return [ + rviz, robot_state_publisher, - controller_manager, - joint_state_publisher, + gazebo, + spawn_entity, + leg_pd_controller, RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster], - ) - ), - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=imu_sensor_broadcaster, - on_exit=[unitree_guide_controller], + target_action=leg_pd_controller, + on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller], ) ), ] def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='aliengo', - description='Type of the robot' + pkg_description = DeclareLaunchArgument( + 'pkg_description', + default_value='go2_description', + description='package for robot description' ) - rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") - return LaunchDescription([ - robot_type_arg, + pkg_description, OpaqueFunction(function=launch_setup), - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ) - ]) + ]) \ No newline at end of file diff --git a/descriptions/unitree/a1_description/launch/unitree_guide.launch.py b/controllers/unitree_guide_controller/launch/mujoco.launch.py similarity index 79% rename from descriptions/unitree/a1_description/launch/unitree_guide.launch.py rename to controllers/unitree_guide_controller/launch/mujoco.launch.py index 07a5f66..60c084e 100644 --- a/descriptions/unitree/a1_description/launch/unitree_guide.launch.py +++ b/controllers/unitree_guide_controller/launch/mujoco.launch.py @@ -9,19 +9,18 @@ from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare -package_description = "a1_description" +package_description = "go2_description" -def process_xacro(context): - robot_type_value = context.launch_configurations['robot_type'] - 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={'robot_type': robot_type_value}) - return (robot_description_config.toxml(), robot_type_value) - def launch_setup(context, *args, **kwargs): - (robot_description, robot_type) = process_xacro(context) + + package_description = context.launch_configurations['pkg_description'] + pkg_path = os.path.join(get_package_share_directory(package_description)) + + xacro_file = os.path.join(pkg_path, 'xacro', 'robot.xacro') + robot_description = xacro.process_file(xacro_file).toxml() + robot_controllers = PathJoinSubstitution( [ FindPackageShare(package_description), @@ -30,6 +29,16 @@ def launch_setup(context, *args, **kwargs): ] ) + rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") + + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] + ) + robot_state_publisher = Node( package='robot_state_publisher', executable='robot_state_publisher', @@ -48,6 +57,9 @@ def launch_setup(context, *args, **kwargs): package="controller_manager", executable="ros2_control_node", parameters=[robot_controllers], + remappings=[ + ("~/robot_description", "/robot_description"), + ], output="both", ) @@ -72,6 +84,7 @@ def launch_setup(context, *args, **kwargs): ) return [ + rviz, robot_state_publisher, controller_manager, joint_state_publisher, @@ -91,22 +104,13 @@ def launch_setup(context, *args, **kwargs): def generate_launch_description(): - robot_type_arg = DeclareLaunchArgument( - 'robot_type', - default_value='a1', - description='Type of the robot' + pkg_description = DeclareLaunchArgument( + 'pkg_description', + default_value='go2_description', + description='package for robot description' ) - rviz_config_file = os.path.join(get_package_share_directory(package_description), "config", "visualize_urdf.rviz") - return LaunchDescription([ - robot_type_arg, + pkg_description, OpaqueFunction(function=launch_setup), - Node( - package='rviz2', - executable='rviz2', - name='rviz_ocs2', - output='screen', - arguments=["-d", rviz_config_file] - ) - ]) + ]) \ No newline at end of file diff --git a/descriptions/README.md b/descriptions/README.md index fb30ada..468a108 100644 --- a/descriptions/README.md +++ b/descriptions/README.md @@ -14,7 +14,7 @@ This folder contains the URDF and SRDF files for the quadruped robot. * [Lite 3](deep_robotics/lite3_description/) * [X30](deep_robotics/x30_description/) -## Steps to transfer urdf to Mujoco model +## 1. Steps to transfer urdf to Mujoco model * Install [Mujoco](https://github.com/google-deepmind/mujoco) * Transfer the mesh files to mujoco supported format, like stl. @@ -29,9 +29,9 @@ This folder contains the URDF and SRDF files for the quadruped robot. compile robot.urdf robot.xml ``` -## Dependencies for Gazebo Simulation +## 2. Dependencies for Gazebo Simulation -Gazebo Simulation only tested on ROS2 Jazzy. I didn't add support for ROS2 Humble because the package name is different. +Gazebo Simulation only tested on ROS2 Jazzy. It add support for ROS2 Humble because the package name is different. * Gazebo Harmonic ```bash @@ -41,6 +41,24 @@ Gazebo Simulation only tested on ROS2 Jazzy. I didn't add support for ROS2 Humbl ```bash sudo apt-get install ros-jazzy-gz-ros2-control ``` +* Legged PD Controller + ```bash + cd ~/ros2_ws + colcon build --packages-up-to leg_pd_controller + ``` + +## 2. Dependencies for Gazebo Classic Simulation + +Gazebo Classic (Gazebo11) Simulation only tested on ROS2 Humble. + +* Gazebo Classic + ```bash + sudo apt-get install ros-humble-gazebo-ros + ``` +* Ros2-Control for Gazebo + ```bash + sudo apt-get install ros-humble-gazebo-ros2-control + ``` * Legged PD Controller ```bash cd ~/ros2_ws diff --git a/descriptions/deep_robotics/lite3_description/README.md b/descriptions/deep_robotics/lite3_description/README.md index 5b0411b..fde657f 100644 --- a/descriptions/deep_robotics/lite3_description/README.md +++ b/descriptions/deep_robotics/lite3_description/README.md @@ -25,7 +25,7 @@ ros2 launch lite3_description visualize.launch.py * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch lite3_description unitree_guide.launch.py + ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=lite3_description ``` * OCS2 Quadruped Controller ```bash @@ -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_unitree_guide.launch.py + ros2 launch lite3_description gazebo.launch.py ``` * Legged Gym Controller ```bash diff --git a/descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py b/descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py deleted file mode 100644 index 51bce4a..0000000 --- a/descriptions/deep_robotics/lite3_description/launch/unitree_guide.launch.py +++ /dev/null @@ -1,93 +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.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) - 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() - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - - 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"], - ) - - 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] - ), - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster, unitree_guide_controller], - ) - ), - ]) diff --git a/descriptions/deep_robotics/x30_description/README.md b/descriptions/deep_robotics/x30_description/README.md index aa27226..0d6b0eb 100644 --- a/descriptions/deep_robotics/x30_description/README.md +++ b/descriptions/deep_robotics/x30_description/README.md @@ -21,12 +21,18 @@ ros2 launch x30_description visualize.launch.py ``` ## Launch ROS2 Control +### Mujoco Simulator +* Unitree Guide Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=x30_description + ``` ### Gazebo Simulator * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch x30_description gazebo_unitree_guide.launch.py + ros2 launch x30_description gazebo.launch.py ``` * Legged Gym Controller ```bash diff --git a/descriptions/unitree/a1_description/README.md b/descriptions/unitree/a1_description/README.md index b98a840..eba37f7 100644 --- a/descriptions/unitree/a1_description/README.md +++ b/descriptions/unitree/a1_description/README.md @@ -25,7 +25,7 @@ ros2 launch a1_description visualize.launch.py * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch a1_description unitree_guide.launch.py + ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=a1_description ``` * OCS2 Quadruped Controller ```bash @@ -38,11 +38,18 @@ ros2 launch a1_description visualize.launch.py ros2 launch a1_description rl_control.launch.py ``` -### Gazebo Simulator +### Gazebo Classic 11 (ROS2 Humble) * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch a1_description gazebo_unitree_guide.launch.py + ros2 launch unitree_guide_controller gazebo_classic.launch.py pkg_description:=a1_description + ``` + +### Gazebo Harmonic (ROS2 Jazzy) +* Unitree Guide Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch a1_description gazebo.launch.py ``` * RL Quadruped Controller ```bash diff --git a/descriptions/unitree/a1_description/config/gazebo.yaml b/descriptions/unitree/a1_description/config/gazebo.yaml index 8756437..92d13cd 100644 --- a/descriptions/unitree/a1_description/config/gazebo.yaml +++ b/descriptions/unitree/a1_description/config/gazebo.yaml @@ -26,6 +26,7 @@ imu_sensor_broadcaster: leg_pd_controller: ros__parameters: + update_rate: 500 # Hz joints: - FR_hip_joint - FR_thigh_joint @@ -50,6 +51,7 @@ leg_pd_controller: unitree_guide_controller: ros__parameters: command_prefix: "leg_pd_controller" + update_rate: 500 # Hz joints: - FR_hip_joint - FR_thigh_joint diff --git a/descriptions/unitree/a1_description/xacro/gazebo_classic.xacro b/descriptions/unitree/a1_description/xacro/gazebo_classic.xacro new file mode 100644 index 0000000..042cf71 --- /dev/null +++ b/descriptions/unitree/a1_description/xacro/gazebo_classic.xacro @@ -0,0 +1,139 @@ + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find go2_description)/config/gazebo.yaml + + + + + 0.6 + 0.6 + 1 + + + + true + + true + 1000 + true + __default_topic__ + + trunk_imu + imu_link + 1000.0 + 0.0 + 0 0 0 + 0 0 0 + imu_link + + 0 0 0 0 0 0 + + + + diff --git a/descriptions/unitree/a1_description/xacro/robot.xacro b/descriptions/unitree/a1_description/xacro/robot.xacro index 451fa5a..a16f00f 100644 --- a/descriptions/unitree/a1_description/xacro/robot.xacro +++ b/descriptions/unitree/a1_description/xacro/robot.xacro @@ -4,6 +4,7 @@ + @@ -11,7 +12,12 @@ - + + + + + + diff --git a/descriptions/unitree/aliengo_description/README.md b/descriptions/unitree/aliengo_description/README.md index f564828..8ae34c1 100644 --- a/descriptions/unitree/aliengo_description/README.md +++ b/descriptions/unitree/aliengo_description/README.md @@ -32,7 +32,7 @@ ros2 launch aliengo_description visualize.launch.py * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch aliengo_description unitree_guide.launch.py + ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=aliengo_description ``` * OCS2 Quadruped Controller ```bash @@ -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_unitree_guide.launch.py + ros2 launch aliengo_description gazebo.launch.py ``` \ No newline at end of file diff --git a/descriptions/unitree/b2_description/README.md b/descriptions/unitree/b2_description/README.md index 48463df..db8d3bf 100644 --- a/descriptions/unitree/b2_description/README.md +++ b/descriptions/unitree/b2_description/README.md @@ -27,7 +27,7 @@ ros2 launch b2_description visualize.launch.py * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch b2_description unitree_guide.launch.py + ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=b2_description ``` * OCS2 Quadruped Controller ```bash @@ -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_unitree_guide.launch.py + ros2 launch b2_description gazebo.launch.py ``` diff --git a/descriptions/unitree/b2_description/launch/unitree_guide.launch.py b/descriptions/unitree/b2_description/launch/unitree_guide.launch.py deleted file mode 100644 index de8088d..0000000 --- a/descriptions/unitree/b2_description/launch/unitree_guide.launch.py +++ /dev/null @@ -1,93 +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.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) - 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() - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - - 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"], - ) - - 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] - ), - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster, unitree_guide_controller], - ) - ), - ]) diff --git a/descriptions/unitree/go1_description/README.md b/descriptions/unitree/go1_description/README.md index 8c79972..f26b0dd 100644 --- a/descriptions/unitree/go1_description/README.md +++ b/descriptions/unitree/go1_description/README.md @@ -27,7 +27,7 @@ ros2 launch go1_description visualize.launch.py * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch go1_description unitree_guide.launch.py + ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=go1_description ``` * OCS2 Quadruped Controller ```bash @@ -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_unitree_guide.launch.py + ros2 launch go1_description gazebo.launch.py ``` \ No newline at end of file diff --git a/descriptions/unitree/go1_description/launch/unitree_guide.launch.py b/descriptions/unitree/go1_description/launch/unitree_guide.launch.py deleted file mode 100644 index 8e1fdd5..0000000 --- a/descriptions/unitree/go1_description/launch/unitree_guide.launch.py +++ /dev/null @@ -1,93 +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.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) - 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() - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - - 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"], - ) - - 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] - ), - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster, unitree_guide_controller], - ) - ), - ]) diff --git a/descriptions/unitree/go2_description/README.md b/descriptions/unitree/go2_description/README.md index 8b4a319..4b47955 100644 --- a/descriptions/unitree/go2_description/README.md +++ b/descriptions/unitree/go2_description/README.md @@ -34,7 +34,7 @@ ros2 launch go2_description visualize.launch.py * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch go2_description unitree_guide.launch.py + ros2 launch unitree_guide_controller mujoco.launch.py ``` * OCS2 Quadruped Controller ```bash @@ -47,12 +47,19 @@ ros2 launch go2_description visualize.launch.py ros2 launch go2_description rl_control.launch.py ``` -### Gazebo Simulator +### Gazebo Classic 11 (ROS2 Humble) +* Unitree Guide Controller + ```bash + source ~/ros2_ws/install/setup.bash + ros2 launch unitree_guide_controller gazebo_classic.launch.py + ``` + +### Gazebo Harmonic (ROS2 Jazzy) * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch go2_description gazebo_unitree_guide.launch.py + ros2 launch unitree_guide_controller gazebo.launch.py ``` * RL Quadruped Controller ```bash diff --git a/descriptions/unitree/go2_description/config/gazebo.yaml b/descriptions/unitree/go2_description/config/gazebo.yaml index 5f0da8d..f2cc666 100644 --- a/descriptions/unitree/go2_description/config/gazebo.yaml +++ b/descriptions/unitree/go2_description/config/gazebo.yaml @@ -26,6 +26,7 @@ imu_sensor_broadcaster: leg_pd_controller: ros__parameters: + update_rate: 500 # Hz joints: - FR_hip_joint - FR_thigh_joint @@ -49,6 +50,7 @@ leg_pd_controller: unitree_guide_controller: ros__parameters: + update_rate: 500 # Hz command_prefix: "leg_pd_controller" joints: - FR_hip_joint diff --git a/descriptions/unitree/go2_description/launch/unitree_guide.launch.py b/descriptions/unitree/go2_description/launch/gazebo_classic.launch.py similarity index 56% rename from descriptions/unitree/go2_description/launch/unitree_guide.launch.py rename to descriptions/unitree/go2_description/launch/gazebo_classic.launch.py index f26ec81..2962525 100644 --- a/descriptions/unitree/go2_description/launch/unitree_guide.launch.py +++ b/descriptions/unitree/go2_description/launch/gazebo_classic.launch.py @@ -8,38 +8,42 @@ from launch.event_handlers import OnProcessExit from launch.substitutions import PathJoinSubstitution from launch_ros.actions import Node from launch_ros.substitutions import FindPackageShare +from launch.launch_description_sources import PythonLaunchDescriptionSource package_description = "go2_description" -def process_xacro(): + +def launch_setup(context, *args, **kwargs): + + package_description = context.launch_configurations['pkg_description'] 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) - return robot_description_config.toxml() + robot_description = xacro.process_file(xacro_file, mappings={'GAZEBO': 'true', 'CLASSIC': 'true'}).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() - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] + rviz = Node( + package='rviz2', + executable='rviz2', + name='rviz_ocs2', + output='screen', + arguments=["-d", rviz_config_file] ) - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - remappings=[ - ("~/robot_description", "/robot_description"), - ], + gazebo = IncludeLaunchDescription( + PythonLaunchDescriptionSource( + [PathJoinSubstitution([FindPackageShare("gazebo_ros"), "launch", "gazebo.launch.py"])] + ), + launch_arguments={"verbose": "false"}.items(), + ) + + spawn_entity = Node( + package="gazebo_ros", + executable="spawn_entity.py", + arguments=["-topic", "robot_description", "-entity", "robot", "-z", "0.5"], + output="screen", ) robot_state_publisher = Node( @@ -70,27 +74,42 @@ def generate_launch_description(): "--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] - ), + return [ + rviz, robot_state_publisher, - controller_manager, - joint_state_publisher, + gazebo, + spawn_entity, + leg_pd_controller, RegisterEventHandler( event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster, unitree_guide_controller], + target_action=leg_pd_controller, + on_exit=[imu_sensor_broadcaster, joint_state_publisher, unitree_guide_controller], ) ), - ]) + ] + + +def generate_launch_description(): + pkg_description = DeclareLaunchArgument( + 'pkg_description', + default_value='go2_description', + description='package for robot description' + ) + + return LaunchDescription([ + pkg_description, + OpaqueFunction(function=launch_setup), + ]) \ No newline at end of file diff --git a/descriptions/unitree/go2_description/xacro/gazebo_classic.xacro b/descriptions/unitree/go2_description/xacro/gazebo_classic.xacro new file mode 100644 index 0000000..042cf71 --- /dev/null +++ b/descriptions/unitree/go2_description/xacro/gazebo_classic.xacro @@ -0,0 +1,139 @@ + + + + + + gazebo_ros2_control/GazeboSystem + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + $(find go2_description)/config/gazebo.yaml + + + + + 0.6 + 0.6 + 1 + + + + true + + true + 1000 + true + __default_topic__ + + trunk_imu + imu_link + 1000.0 + 0.0 + 0 0 0 + 0 0 0 + imu_link + + 0 0 0 0 0 0 + + + + diff --git a/descriptions/unitree/go2_description/xacro/robot.xacro b/descriptions/unitree/go2_description/xacro/robot.xacro index 6ad195d..a54ea76 100755 --- a/descriptions/unitree/go2_description/xacro/robot.xacro +++ b/descriptions/unitree/go2_description/xacro/robot.xacro @@ -4,13 +4,19 @@ + - + + + + + + diff --git a/descriptions/xiaomi/cyberdog_description/README.md b/descriptions/xiaomi/cyberdog_description/README.md index 2ac8274..29e9f6b 100644 --- a/descriptions/xiaomi/cyberdog_description/README.md +++ b/descriptions/xiaomi/cyberdog_description/README.md @@ -28,7 +28,7 @@ ros2 launch cyberdog_description visualize.launch.py * Unitree Guide Controller ```bash source ~/ros2_ws/install/setup.bash - ros2 launch cyberdog_description unitree_guide.launch.py + ros2 launch unitree_guide_controller mujoco.launch.py pkg_description:=cyberdog_description ``` * OCS2 Quadruped Controller ```bash @@ -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_unitree_guide.launch.py + ros2 launch cyberdog_description gazebo.launch.py ``` \ No newline at end of file diff --git a/descriptions/xiaomi/cyberdog_description/launch/unitree_guide.launch.py b/descriptions/xiaomi/cyberdog_description/launch/unitree_guide.launch.py deleted file mode 100644 index 81cdd91..0000000 --- a/descriptions/xiaomi/cyberdog_description/launch/unitree_guide.launch.py +++ /dev/null @@ -1,93 +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.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) - 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() - - robot_controllers = PathJoinSubstitution( - [ - FindPackageShare(package_description), - "config", - "robot_control.yaml", - ] - ) - - controller_manager = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[robot_controllers], - output="both", - ) - - 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"], - ) - - 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] - ), - robot_state_publisher, - controller_manager, - joint_state_publisher, - RegisterEventHandler( - event_handler=OnProcessExit( - target_action=joint_state_publisher, - on_exit=[imu_sensor_broadcaster, unitree_guide_controller], - ) - ), - ])