【附带源码】机械臂MoveIt2极简教程（六）、第三个demo -机械臂的避障规划

系列文章目录

【附带源码】机械臂MoveIt2极简教程（一）、moveit2安装
【附带源码】机械臂MoveIt2极简教程（二）、move_group交互
【附带源码】机械臂MoveIt2极简教程（三）、URDF/SRDF介绍
【附带源码】机械臂MoveIt2极简教程（四）、第一个入门demo
【附带源码】机械臂MoveIt2极简教程（五）、第二个demo - rviz可视化
【附带源码】机械臂MoveIt2极简教程（六）、第三个demo -机械臂的避障规划

本节实现的效果就是让机械臂绕过侧面的障碍物。

1. 新建C++代码

#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_visual_tools/moveit_visual_tools.h>

#include <memory>
#include <rclcpp/rclcpp.hpp>
#include <thread>

int main(int argc, char *argv[])
{
   
    // Initialize ROS and create the Node
    rclcpp::init(argc, argv);
    auto const node = std::make_shared<rclcpp::Node>(
        "planning_around_objects", rclcpp::NodeOptions().automatically_declare_parameters_from_overrides(true));

    // Create a ROS logger
    auto const logger = rclcpp::get_logger("planning_around_objects");

    RCLCPP_INFO(logger, "========================> start");

    // We spin up a SingleThreadedExecutor for the current state monitor to get
    // information about the robot's state.
    rclcpp::executors::SingleThreadedExecutor executor;
    executor.add_node(node);
    auto spinner = std::thread([&executor]()
                               {
    executor.spin(); });

    // Create the MoveIt MoveGroup Interface
    using moveit::planning_interface::MoveGroupInterface;
    auto move_group_interface = MoveGroupInterface(node, "panda_arm");

    // Construct and initialize MoveItVisualTools
    auto moveit_visual_tools =
        moveit_visual_tools::MoveItVisualTools{
   node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC,
                                               move_group_interface.getRobotModel()};
    moveit_vis