From 8175ae4b47abe634648c944ec4349b6bccf024de Mon Sep 17 00:00:00 2001 From: Huang Zhenbiao Date: Sat, 29 Mar 2025 10:59:00 +0800 Subject: [PATCH] add unitree hardware network config --- .../hardware_unitree_mujoco/CMakeLists.txt | 6 ++ hardwares/hardware_unitree_mujoco/README.md | 18 +++- .../hardware_unitree_mujoco/HardwareUnitree.h | 2 + .../launch/visualize.launch.py | 100 ++++++++++++++++++ .../src/HardwareUnitree.cpp | 12 ++- 5 files changed, 135 insertions(+), 3 deletions(-) create mode 100644 hardwares/hardware_unitree_mujoco/launch/visualize.launch.py diff --git a/hardwares/hardware_unitree_mujoco/CMakeLists.txt b/hardwares/hardware_unitree_mujoco/CMakeLists.txt index 7ff3324..16743b6 100644 --- a/hardwares/hardware_unitree_mujoco/CMakeLists.txt +++ b/hardwares/hardware_unitree_mujoco/CMakeLists.txt @@ -74,4 +74,10 @@ endif () ament_export_dependencies(${dependencies}) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) + +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME}/ +) + ament_package() diff --git a/hardwares/hardware_unitree_mujoco/README.md b/hardwares/hardware_unitree_mujoco/README.md index fba9834..8d50c2d 100644 --- a/hardwares/hardware_unitree_mujoco/README.md +++ b/hardwares/hardware_unitree_mujoco/README.md @@ -37,5 +37,21 @@ Tested environment: Build Command: ```bash cd ~/ros2_ws -colcon build --packages-up-to hardware_unitree_mujoco +colcon build --packages-up-to hardware_unitree_mujoco --symlink-install +``` + +## 3. Config network and domain +Since the real unitree robot has different network and domain name, you need to set the network and domain name in the xacro file. +```xml + + hardware_unitree_mujoco/HardwareUnitree + 1 + lo + +``` + +After modified the config, you can tried to visualize the robot info from real robot by following command: +```bash +source ~/ros2_ws/install/setup.bash +ros2 launch hardware_unitree_mujoco visualize.launch.py ``` \ No newline at end of file diff --git a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h index 7baad49..b37e92d 100644 --- a/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h +++ b/hardwares/hardware_unitree_mujoco/include/hardware_unitree_mujoco/HardwareUnitree.h @@ -58,6 +58,8 @@ protected: unitree_go::msg::dds_::LowState_ low_state_{}; // default init unitree_go::msg::dds_::SportModeState_ high_state_{}; // default init + std::string network_interface_ = "lo"; + int domain_ = 1; /*publisher*/ unitree::robot::ChannelPublisherPtr low_cmd_publisher_; /*subscriber*/ diff --git a/hardwares/hardware_unitree_mujoco/launch/visualize.launch.py b/hardwares/hardware_unitree_mujoco/launch/visualize.launch.py new file mode 100644 index 0000000..cc643a3 --- /dev/null +++ b/hardwares/hardware_unitree_mujoco/launch/visualize.launch.py @@ -0,0 +1,100 @@ +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 + + +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 = xacro.process_file(xacro_file).toxml() + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare(package_description), + "config", + "robot_control.yaml", + ] + ) + + 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', + name='robot_state_publisher', + parameters=[ + { + 'publish_frequency': 20.0, + 'use_tf_static': True, + 'robot_description': robot_description, + 'ignore_timestamp': True + } + ], + ) + + controller_manager = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_controllers], + remappings=[ + ("~/robot_description", "/robot_description"), + ], + output="both", + ) + + 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"], + ) + + return [ + rviz, + robot_state_publisher, + controller_manager, + joint_state_publisher, + RegisterEventHandler( + event_handler=OnProcessExit( + target_action=joint_state_publisher, + on_exit=[imu_sensor_broadcaster], + ) + ) + ] + + +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), + ]) diff --git a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp index 302e8b4..4c6c20a 100644 --- a/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp +++ b/hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp @@ -3,7 +3,6 @@ // #include "hardware_unitree_mujoco/HardwareUnitree.h" -#include #include "crc32.h" #define TOPIC_LOWCMD "rt/lowcmd" @@ -39,7 +38,16 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn Hardwa } } - ChannelFactory::Instance()->Init(1, "lo"); + + if (const auto network_interface_param = info.hardware_parameters.find("network_interface"); network_interface_param != info.hardware_parameters.end()) { + network_interface_ = network_interface_param->second; + } + if (const auto domain_param = info.hardware_parameters.find("domain"); domain_param != info.hardware_parameters.end()) { + domain_ = std::stoi(domain_param->second); + } + + RCLCPP_INFO(get_logger()," network_interface: %s, domain: %d", network_interface_.c_str(), domain_); + ChannelFactory::Instance()->Init(domain_, network_interface_); low_cmd_publisher_ = std::make_shared >(