当前位置：首页 > news >正文

规划后的轨迹，如何发给 moveit_servo 执行

news 2026/5/12 18:53:59

moveit_servo 不执行 “已经规划好的轨迹”

moveit_servo 只接收 “实时末端速度 / 角速度指令”

也就是：你不能直接把轨迹点发给 servo，必须把轨迹点转成实时速度指令发给它。

我给你最简单、可直接运行的方法

方法：轨迹点 → 转成末端速度 → 发给 servo

你只需要发布话题/servo_node/delta_twist_cmds类型：geometry_msgs/msg/TwistStamped

我直接给你完整可运行代码（加到你的程序里就能用）

cpp

运行

#include <rclcpp/rclcpp.hpp> #include <moveit_servo/servo_parameters.h> #include <moveit_servo/servo.h> #include <moveit/planning_scene_monitor/planning_scene_monitor.h> #include <geometry_msgs/msg/pose_stamped.hpp> #include <geometry_msgs/msg/twist_stamped.hpp> // <--- 加这个 #include <tf2_ros/transform_listener.h> #include <Eigen/Geometry> using namespace std::chrono_literals; static const rclcpp::Logger LOGGER = rclcpp::get_logger("servo_demo_node"); // 轨迹点结构体（和你 fanuc_driver 一致） struct TrajPoint { float x, y, z; float rx, ry, rz; }; int main(int argc, char** argv) { rclcpp::init(argc, argv); auto node = std::make_shared<rclcpp::Node>("servo_demo_node"); // ----------------------------- // 你的原有代码（不变） // ----------------------------- auto tf_buffer = std::make_shared<tf2_ros::Buffer>(node->get_clock()); auto planning_scene_monitor = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>( node, "robot_description", tf_buffer); planning_scene_monitor->startStateMonitor("/joint_states"); planning_scene_monitor->startPublishingPlanningScene( planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE); planning_scene_monitor->startSceneMonitor(); auto servo_params = moveit_servo::ServoParameters::makeServoParameters(node); auto servo = std::make_unique<moveit_servo::Servo>(node, servo_params, planning_scene_monitor); servo->start(); // ----------------------------- // ✅ 创建 servo 速度发布器 // ----------------------------- auto twist_pub = node->create_publisher<geometry_msgs::msg::TwistStamped>( "/servo_node/delta_twist_cmds", 10); // ----------------------------- // 【示例】你规划好的轨迹 // ----------------------------- std::vector<TrajPoint> trajectory; trajectory.push_back({0.5, 0.0, 0.2, 0, 0, 0}); trajectory.push_back({0.5, 0.1, 0.2, 0, 0, 0}); trajectory.push_back({0.5, 0.2, 0.2, 0, 0, 0}); trajectory.push_back({0.5, 0.2, 0.3, 0, 0, 0}); // ----------------------------- // ✅ 轨迹执行线程：发给 servo // ----------------------------- std::thread execute_thread([&]() { rclcpp::Rate rate(10); // 10Hz 发送速度 for (const auto& point : trajectory) { if (!rclcpp::ok()) break; // 1. 获取当前末端位置 auto current_state = planning_scene_monitor->getStateMonitor()->getCurrentState(); if (!current_state) { rate.sleep(); continue; } Eigen::Isometry3d current_pose = current_state->getGlobalLinkTransform("YB_Link"); double cx = current_pose.translation().x(); double cy = current_pose.translation().y(); double cz = current_pose.translation().z(); // 2. 计算速度（简单P控制器） double k = 0.5; // 速度系数，调小更稳 double vx = (point.x - cx) * k; double vy = (point.y - cy) * k; double vz = (point.z - cz) * k; // 3. 构建速度指令 geometry_msgs::msg::TwistStamped twist; twist.header.stamp = node->now(); twist.header.frame_id = "base_link"; twist.twist.linear.x = vx; twist.twist.linear.y = vy; twist.twist.linear.z = vz; twist.twist.angular.x = 0; twist.twist.angular.y = 0; twist.twist.angular.z = 0; // 4. ✅ 发给 servo 执行 twist_pub->publish(twist); RCLCPP_INFO(node->get_logger(), "执行轨迹点: x=%.3f y=%.3f z=%.3f", point.x, point.y, point.z); rate.sleep(); } // 停止 geometry_msgs::msg::TwistStamped stop_twist; stop_twist.header.frame_id = "base_link"; twist_pub->publish(stop_twist); RCLCPP_INFO(node->get_logger(), "✅ 轨迹执行完成"); }); // ----------------------------- // 你的原有末端显示（不变） // ----------------------------- const std::string EE_FRAME = "YB_Link"; auto ee_pose_timer = node->create_wall_timer(100ms, [&]() { auto current_state = planning_scene_monitor->getStateMonitor()->getCurrentState(); if (!current_state) return; Eigen::Isometry3d ee_tf = current_state->getGlobalLinkTransform(EE_FRAME); double x = ee_tf.translation().x(); double y = ee_tf.translation().y(); double z = ee_tf.translation().z(); RCLCPP_INFO_STREAM_THROTTLE(node->get_logger(), *node->get_clock(), 1000, "📌 末端坐标 → X: " << x << " Y: " << y << " Z: " << z); }); // ----------------------------- // 自旋 // ----------------------------- auto executor = std::make_unique<rclcpp::executors::MultiThreadedExecutor>(); executor->add_node(node); executor->spin(); execute_thread.join(); rclcpp::shutdown(); return 0; }