【附带源码】机械臂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 Noderclcpp::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 loggerauto 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 Interfaceusing moveit::planning_interface::MoveGroupInterface;auto move_group_interface = MoveGroupInterface(node, "panda_arm");// Construct and initialize MoveItVisualToolsauto moveit_visual_tools =moveit_visual_tools::MoveItVisualTools{node, "panda_link0", rviz_visual_tools::RVIZ_MARKER_TOPIC,move_group_interface.getRobotModel()};moveit_vis