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