add unitree hardware network config

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()

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>
+    <plugin>hardware_unitree_mujoco/HardwareUnitree</plugin>
+    <param name="domain">1</param>
+    <param name="network_interface">lo</param>
+</hardware>
+```
+
+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
 ```

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<unitree_go::msg::dds_::LowCmd_> low_cmd_publisher_;
     /*subscriber*/

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),
+    ])

hardwares/hardware_unitree_mujoco/src/HardwareUnitree.cpp

@@ -3,7 +3,6 @@
 //
 
 #include "hardware_unitree_mujoco/HardwareUnitree.h"
-#include <rclcpp/logging.hpp>
 #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<ChannelPublisher<unitree_go::msg::dds_::LowCmd_> >(