进行ros封装

src/humanoid_3d_animation/ros2/launch/mujoco_ros2.launch.py

@@ -0,0 +1,21 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='mujoco_ros2', 
+            executable='mujoco_ros2_sim',  
+            name='mujoco_ros2_sim',
+            output='screen',
+            parameters=[],  
+        ),
+
+        Node(
+            package='mujoco_ros2', 
+            executable='robot_controller', 
+            name='robot_controller',
+            output='screen',
+            parameters=[], 
+        )
+    ])

src/humanoid_3d_animation/ros2/mujoco_ros2_cpp/mujoco_ros2_sim.py

@@ -0,0 +1,75 @@
+import rclpy
+from rclpy.node import Node
+import numpy as np
+import mujoco
+import mujoco.viewer
+from my_interfaces.srv import RobotCtrl
+from my_interfaces.msg import RobotState
+import time
+current_action='stop'
+class MujocoRos2Sim(Node):
+    def __init__(self,name):
+        super().__init__(name)
+        self.get_logger().info("节点已启动：%s!" % name)
+        # 加载数据,模型,轨迹数据
+        self.model=mujoco.MjModel.from_xml_path("RobotH.xml")
+        self.data=mujoco.MjData(self.model)
+        self.data_walk=np.load("walk.npz")
+        self.data_squat=np.load("squat.npz")
+        # 创建viewer
+        self.viewer=mujoco.viewer.launch_passive(self.model, self.data)
+
+        #创建服务用来控制model运动
+        self.service=self.create_service(RobotCtrl, 'robot_control', self.control_callback)
+        #创建话题用来发布当前状态
+        self.pub=self.create_publisher(RobotState, 'robot_state', 10)
+
+    def control_callback(self, request, response):
+
+        self.get_logger().info(f"Received command: {request.command}")
+        if request.command == "squat":
+            current_action="squat"
+        elif request.command == "walk":
+            current_action="walk"
+        elif request.command == "stop":
+            current_action="stop"
+        response.result=True
+        return response
+
+    def publish_status(self):
+        # 发布当前状态
+        msg=RobotState()
+        msg.action=current_action
+        self.pub.publish(msg)
+
+    def run_sim(self):
+        while True:
+            # 控制动作
+            if current_action=="squat":
+                self.data.qpos[0]=self.data_squat["qpos"][0]
+                self.data.qvel[0]=self.data_squat["qvel"][0]
+            elif current_action=="walk":
+                self.data.qpos[0]=self.data_walk["qpos"][0]
+                self.data.qvel[0]=self.data_walk["qvel"][0]
+            elif current_action=="stop":
+                self.data.qpos[0]=0
+                self.data.qvel[0]=0
+            # 模拟一步
+            mujoco.mj_step(self.model, self.data)
+            # 实时渲染
+            self.viewer.sync()
+            # 发布状态
+            self.publish_status()
+
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    sim=MujocoRos2Sim("mujoco_ros2_sim")
+    sim.run_sim()
+    rclpy.spin(sim)
+    sim.destroy_node()
+    rclpy.shutdown()
+
+if __name__=='__main__':
+    main()

src/humanoid_3d_animation/ros2/mujoco_ros2_cpp/robot_controller.cpp

@@ -0,0 +1,121 @@
+#include "rclcpp/rclcpp.hpp"
+#include "my_interfaces/srv/robot_ctrl.hpp"
+#include <termios.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <string>
+
+// 全局变量
+std::string current_action_key = "stop";
+bool action_switch_flag = false;
+
+// 非阻塞键盘读取
+char read_keypress_non_blocking() {
+    struct termios oldt, newt;
+    int ch;
+    int oldf;
+
+    // 获取终端设置
+    tcgetattr(STDIN_FILENO, &oldt);
+    newt = oldt;
+    // 禁用 canonical 模式和回显
+    newt.c_lflag &= ~(ICANON | ECHO);
+    tcsetattr(STDIN_FILENO, TCSANOW, &newt);
+    // 设置非阻塞模式
+    oldf = fcntl(STDIN_FILENO, F_GETFL, 0);
+    fcntl(STDIN_FILENO, F_SETFL, oldf | O_NONBLOCK);
+
+    ch = getchar();
+
+    // 恢复终端设置
+    tcsetattr(STDIN_FILENO, TCSANOW, &oldt);
+    fcntl(STDIN_FILENO, F_SETFL, oldf);
+
+    return (ch != EOF) ? ch : 0;
+}
+
+class RobotController : public rclcpp::Node {
+public:
+    RobotController() : Node("robot_controller") {
+        // 创建服务客户端
+        client_ = this->create_client<my_interfaces::srv::RobotCtrl>("robot_control");
+
+        // 等待服务可用
+        while (!client_->wait_for_service(std::chrono::seconds(1))) {
+            if (!rclcpp::ok()) {
+                RCLCPP_ERROR(this->get_logger(), "等待服务时被中断");
+                return;
+            }
+            RCLCPP_INFO(this->get_logger(), "等待服务 'robot_control' 启动...");
+        }
+
+        RCLCPP_INFO(this->get_logger(), "操作提示：w=行走 | s=下蹲 | q=退出 | 其他键=停止");
+    }
+
+    void send_request(const std::string &action) {
+        auto request = std::make_shared<my_interfaces::srv::RobotCtrl::Request>();
+        request->command = action;
+
+        // 发送异步请求
+        auto result_future = client_->async_send_request(
+            request,
+            std::bind(&RobotController::result_callback, this, std::placeholders::_1)
+        );
+        RCLCPP_INFO(this->get_logger(), "已发送指令：%s", action.c_str());
+    }
+
+private:
+    rclcpp::Client<my_interfaces::srv::RobotCtrl>::SharedPtr client_;
+
+    void result_callback(rclcpp::Client<my_interfaces::srv::RobotCtrl>::SharedFuture future) {
+        auto response = future.get();
+        RCLCPP_INFO(this->get_logger(), "指令执行成功：%s", response->message.c_str());
+    }
+};
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<RobotController>();
+
+    // 初始发送停止指令
+    node->send_request(current_action_key);
+
+    try {
+        while (rclcpp::ok()) {
+            // 读取键盘输入
+            char key = read_keypress_non_blocking();
+            if (key) {
+                if (key == 'w' && current_action_key != "walk") {
+                    current_action_key = "walk";
+                    action_switch_flag = true;
+                } else if (key == 's' && current_action_key != "squat") {
+                    current_action_key = "squat";
+                    action_switch_flag = true;
+                } else if (key == 'q') {  // 退出
+                    RCLCPP_INFO(node->get_logger(), "收到退出指令，程序终止");
+                    break;
+                } else if (key != 'w' && key != 's') {  // 其他键停止
+                    if (current_action_key != "stop") {
+                        current_action_key = "stop";
+                        action_switch_flag = true;
+                    }
+                }
+            }
+
+            // 发送指令
+            if (action_switch_flag) {
+                node->send_request(current_action_key);
+                action_switch_flag = false;
+            }
+
+            // 处理ROS事件
+            rclcpp::spin_some(node);
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
+    } catch (const std::exception &e) {
+        RCLCPP_ERROR(node->get_logger(), "异常：%s", e.what());
+    }
+
+    rclcpp::shutdown();
+    return 0;
+}

src/humanoid_3d_animation/ros2/mujoco_ros2_py/mujoco_ros2_sim.cpp

@@ -0,0 +1,139 @@
+#include "rclcpp/rclcpp.hpp"
+#include "my_interfaces/srv/robot_ctrl.hpp"
+#include "my_interfaces/msg/robot_state.hpp"
+#include <mujoco/mujoco.h>
+#include <fstream>
+#include <string>
+#include <thread>
+
+// 全局变量
+std::string current_action = "stop";
+
+class MujocoRos2Sim : public rclcpp::Node {
+public:
+    MujocoRos2Sim() : Node("mujoco_ros2_sim") {
+        RCLCPP_INFO(this->get_logger(), "节点已启动：mujoco_ros2_sim!");
+
+        // 加载模型
+        char error[1000];
+        model_ = mj_loadXML("RobotH.xml", nullptr, error, 1000);
+        if (!model_) {
+            RCLCPP_ERROR(this->get_logger(), "模型加载失败: %s", error);
+            exit(1);
+        }
+        data_ = mj_makeData(model_);
+
+        // 加载轨迹数据（这里简化处理，实际需要根据npz格式解析）
+        load_trajectory_data();
+
+        // 创建viewer
+        viewer_ = mjViewerCreate();
+        mjv_defaultCamera(&cam_);
+        mjv_defaultOption(&opt_);
+        mjr_defaultContext(&con_);
+        mjv_makeScene(model_, &scene_, 1000);
+        mjr_makeContext(model_, &con_, mjFONTSCALE_150);
+        mjViewerSetup(viewer_, &scene_, &cam_, &opt_, &con_);
+
+        // 创建服务
+        service_ = this->create_service<my_interfaces::srv::RobotCtrl>(
+            "robot_control",
+            std::bind(&MujocoRos2Sim::control_callback, this, std::placeholders::_1, std::placeholders::_2)
+        );
+
+        // 创建发布者
+        publisher_ = this->create_publisher<my_interfaces::msg::RobotState>("robot_state", 10);
+
+        // 启动模拟线程
+        sim_thread_ = std::thread(&MujocoRos2Sim::run_sim, this);
+    }
+
+    ~MujocoRos2Sim() {
+        if (sim_thread_.joinable()) {
+            sim_thread_.join();
+        }
+        mjViewerClose(viewer_);
+        mjr_freeContext(&con_);
+        mjv_freeScene(&scene_);
+        mj_deleteData(data_);
+        mj_deleteModel(model_);
+    }
+
+private:
+    mjModel* model_ = nullptr;
+    mjData* data_ = nullptr;
+    mjViewer* viewer_ = nullptr;
+    mjvScene scene_;
+    mjvCamera cam_;
+    mjvOption opt_;
+    mjrContext con_;
+    rclcpp::Service<my_interfaces::srv::RobotCtrl>::SharedPtr service_;
+    rclcpp::Publisher<my_interfaces::msg::RobotState>::SharedPtr publisher_;
+    std::thread sim_thread_;
+
+    // 轨迹数据（示例）
+    std::vector<double> walk_qpos, walk_qvel;
+    std::vector<double> squat_qpos, squat_qvel;
+
+    void load_trajectory_data() {
+        // 实际应用中需要解析npz文件
+        // 这里仅做示例初始化
+        walk_qpos.push_back(0.0);
+        walk_qvel.push_back(0.1);
+        squat_qpos.push_back(-0.5);
+        squat_qvel.push_back(0.0);
+    }
+
+    void control_callback(
+        const std::shared_ptr<my_interfaces::srv::RobotCtrl::Request> request,
+        std::shared_ptr<my_interfaces::srv::RobotCtrl::Response> response
+    ) {
+        RCLCPP_INFO(this->get_logger(), "收到指令: %s", request->command.c_str());
+        current_action = request->command;
+        response->result = true;
+        response->message = "指令已接收";
+    }
+
+    void publish_status() {
+        auto msg = my_interfaces::msg::RobotState();
+        msg.action = current_action;
+        publisher_->publish(msg);
+    }
+
+    void run_sim() {
+        while (rclcpp::ok()) {
+            // 控制动作
+            if (current_action == "squat" && !squat_qpos.empty()) {
+                data_->qpos[0] = squat_qpos[0];
+                data_->qvel[0] = squat_qvel[0];
+            } else if (current_action == "walk" && !walk_qpos.empty()) {
+                data_->qpos[0] = walk_qpos[0];
+                data_->qvel[0] = walk_qvel[0];
+            } else if (current_action == "stop") {
+                data_->qpos[0] = 0;
+                data_->qvel[0] = 0;
+            }
+
+            // 模拟一步
+            mj_step(model_, data_);
+
+            // 渲染
+            mjv_updateScene(model_, data_, &opt_, nullptr, &cam_, mjCAT_ALL, &scene_);
+            mjViewerRender(viewer_, &scene_, &con_);
+
+            // 发布状态
+            publish_status();
+
+            // 小延迟
+            std::this_thread::sleep_for(std::chrono::milliseconds(10));
+        }
+    }
+};
+
+int main(int argc, char *argv[]) {
+    rclcpp::init(argc, argv);
+    auto node = std::make_shared<MujocoRos2Sim>();
+    rclcpp::spin(node);
+    rclcpp::shutdown();
+    return 0;
+}