Moveit 避障路径规划 demo

最新推荐文章于 2025-10-12 15:23:15 发布

原创

最新推荐文章于 2025-10-12 15:23:15 发布 · 1.8k 阅读

45 ·

CC 4.0 BY-SA版权

文章标签：

#ros

本文展示了如何使用ROS MoveIt!库在C++中为机器人手臂实现无障碍动作，并通过规划场景管理添加和移除碰撞物体以实现避障。代码演示了如何创建规划场景、添加障碍物、执行避障运动及清理障碍的过程。

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>


int main(int argc, char **argv)
{
    ros::init(argc, argv, "full_demo");
    ros::NodeHandle nh;
    ros::AsyncSpinner spin(1);
    spin.start();

    //    创建运动规划的情景，等待创建完成
    ros::Publisher planning_scene_diff_publisher = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
    ros::WallDuration sleep_t(0.5);
    while (planning_scene_diff_publisher.getNumSubscribers() < 1)
    {
        sleep_t.sleep();
    }

    moveit::planning_interface::MoveGroupInterface arm("arm");
    //无障碍的动作
    std::vector<double> joints={0.7,-1.57,0,0,0};
    arm.setJointValueTarget(joints);
    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
    moveit::planning_interface::MoveItErrorCode success = arm.plan(my_plan);
    if(success) {