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

系列文章目录

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

---

---

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

moveit2机械臂绕障

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_visual_tools.deleteAllMarkers();
    moveit_visual_tools.loadRemoteControl();

    // Create a closure for updating the text in rviz
    auto const draw_title = [&moveit_visual_tools](auto text)
    {
        auto const text_pose = []
        {
            auto msg = Eigen::Isometry3d::Identity();
            msg.translation().z() = 1.0;
            return msg;
        }();
        moveit_visual_tools.publishText(text_pose, text, rviz_visual_tools::WHITE, rviz_visual_tools::XLARGE);
    };
    auto const prompt = [&moveit_visual_tools](auto text)
    { moveit_visual_tools.prompt(text); };
    auto const draw_trajectory_tool_path =
        [&moveit_visual_tools, jmg = move_group_interface.getRobotModel()->getJointModelGroup("panda_arm")](
            auto const trajectory)
    { moveit_visual_tools.publishTrajectoryLine(trajectory, jmg); };

    // Set a target Pose with updated values !!!
    auto const target_pose = []
    {
        geometry_msgs::msg::Pose msg;
        msg.orientation.y = 0.8;
        msg.orientation.w = 0.6;
        msg.position.x = 0.1;
        msg.position.y = 0.4;
        msg.position.z = 0.4;
        return msg;
    }();
    move_group_interface.setPoseTarget(target_pose);

    // Create collision object for the robot to avoid
    auto const collision_object = [frame_id = move_group_interface.getPlanningFrame()]
    {
        moveit_msgs::msg::CollisionObject collision_object;
        collision_object.header.frame_id = frame_id;
        collision_object.id = "box1";
        shape_msgs::msg::SolidPrimitive primitive;

        // Define the size of the box in meters
        primitive.type = primitive.BOX;
        primitive.dimensions.resize(3);
        primitive.dimensions[primitive.BOX_X] = 0.5;
        primitive.dimensions[primitive.BOX_Y] = 0.1;
        primitive.dimensions[primitive.BOX_Z] = 0.5;

        // Define the pose of the box (relative to the frame_id)
        geometry_msgs::msg::Pose box_pose;
        box_pose.orientation.w = 1.0;
        box_pose.position.x = 0.2;
        box_pose.position.y = 0.2;
        box_pose.position.z = 0.25;

        collision_object.primitives.push_back(primitive);
        collision_object.primitive_poses.push_back(box_pose);
        collision_object.operation = collision_object.ADD;

        return collision_object;
    }();

    // Add the collision object to the scene
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
    planning_scene_interface.applyCollisionObject(collision_object);

    // Create a plan to that target pose
    prompt("Press 'next' in the RvizVisualToolsGui window to plan");
    draw_title("Planning");
    moveit_visual_tools.trigger();
    auto const [success, plan] = [&move_group_interface]
    {
        moveit::planning_interface::MoveGroupInterface::Plan msg;
        auto const ok = static_cast<bool>(move_group_interface.plan(msg));
        return std::make_pair(ok, msg);
    }();

    // Execute the plan
    if (success)
    {
        draw_trajectory_tool_path(plan.trajectory_);
        moveit_visual_tools.trigger();
        prompt("Press 'next' in the RvizVisualToolsGui window to execute");
        draw_title("Executing");
        moveit_visual_tools.trigger();
        move_group_interface.execute(plan);
    }
    else
    {
        draw_title("Planning Failed!");
        moveit_visual_tools.trigger();
        RCLCPP_ERROR(logger, "Planning failed!");
    }

    // Shutdown ROS
    rclcpp::shutdown();
    spinner.join();
    return 0;
}

2. 修改launch文件

沿用上一节的launch文件demo_moveit.launch.py

import os
import yaml
from launch import LaunchDescription
from launch_ros.actions import Node
from ament_index_python.packages import get_package_share_directory
import xacro

def load_file(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return file.read()
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def load_yaml(package_name, file_path):
    package_path = get_package_share_directory(package_name)
    absolute_file_path = os.path.join(package_path, file_path)

    try:
        with open(absolute_file_path, "r") as file:
            return yaml.safe_load(file)
    except EnvironmentError:  # parent of IOError, OSError *and* WindowsError where available
        return None


def generate_launch_description():
    robot_description_config = xacro.process_file(
        os.path.join(
            get_package_share_directory("moveit_resources_panda_moveit_config"),
            "config",
            "panda.urdf.xacro",
        )
    )
    robot_description = {"robot_description": robot_description_config.toxml()}

    robot_description_semantic_config = load_file(
        "moveit_resources_panda_moveit_config", "config/panda.srdf"
    )
    robot_description_semantic = {
        "robot_description_semantic": robot_description_semantic_config
    }

    kinematics_yaml = load_yaml(
        "moveit_resources_panda_moveit_config", "config/kinematics.yaml"
    )

    # 第四节
    # move_group_demo = Node(
    #     package="demo_moveit",
    #     executable="demo_moveit",
    #     name="demo_moveit",
    #     output="screen",
    #     parameters=[robot_description, 
    #                 robot_description_semantic, 
    #                 kinematics_yaml,
    #                 ],
    # )

    # 第五节
    # move_group_demo = Node(
    #     package="demo_moveit",
    #     executable="visualize_in_rviz",
    #     name="visualize_in_rviz",
    #     output="screen",
    #     parameters=[robot_description, 
    #                 robot_description_semantic, 
    #                 kinematics_yaml,
    #                 ],
    # )

    # 第六节
    move_group_demo = Node(
        package="demo_moveit",
        executable="planning_around_objects",
        name="planning_around_objects",
        output="screen",
        parameters=[robot_description, 
                    robot_description_semantic, 
                    kinematics_yaml,
                    ],
    )

    return LaunchDescription([move_group_demo])

3. 修改CMakeLists.txt

依旧沿用前一节的CMakeLists.txt，额外添加了新的节点

cmake_minimum_required(VERSION 3.10.2)
project(demo_moveit)

# Common cmake code applied to all moveit packages
find_package(moveit_common REQUIRED)
moveit_package()

find_package(Eigen3 REQUIRED)
find_package(Boost REQUIRED system filesystem date_time thread)
find_package(ament_cmake REQUIRED)
find_package(control_msgs REQUIRED)
find_package(moveit_core REQUIRED)
find_package(moveit_ros_planning REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)
find_package(moveit_ros_perception REQUIRED)
find_package(moveit_servo REQUIRED)
find_package(interactive_markers REQUIRED)
find_package(rviz_visual_tools REQUIRED)
find_package(moveit_visual_tools REQUIRED)
find_package(geometric_shapes REQUIRED)
find_package(rclcpp REQUIRED)
find_package(rclcpp_action REQUIRED)
find_package(pluginlib REQUIRED)
find_package(tf2_ros REQUIRED)
find_package(tf2_eigen REQUIRED)
find_package(tf2_geometry_msgs REQUIRED)

set(THIS_PACKAGE_INCLUDE_DIRS
  doc/interactivity/include
)

set(THIS_PACKAGE_INCLUDE_DEPENDS
  ament_cmake
  rclcpp
  rclcpp_action
  tf2_geometry_msgs
  tf2_ros
  moveit_core
  rviz_visual_tools
  moveit_visual_tools
  moveit_ros_planning_interface
  interactive_markers
  tf2_geometry_msgs
  moveit_ros_planning
  pluginlib
  Eigen3
  Boost
  control_msgs
  moveit_servo
)

include_directories(${THIS_PACKAGE_INCLUDE_DIRS})


ament_export_dependencies(
  ${THIS_PACKAGE_INCLUDE_DEPENDS}
)

ament_export_include_directories(include)


# -------------------------------------------------------------- executable
add_executable(demo_moveit
  src/demo_moveit.cpp)

target_include_directories(demo_moveit
    PUBLIC include)

ament_target_dependencies(demo_moveit
    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)

# -------------------------------------------------------------- executable
add_executable(visualize_in_rviz
  src/visualize_in_rviz.cpp)

target_include_directories(visualize_in_rviz
    PUBLIC include)

ament_target_dependencies(visualize_in_rviz
    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)

# -------------------------------------------------------------- executable
add_executable(planning_around_objects
  src/planning_around_objects.cpp)

target_include_directories(planning_around_objects
    PUBLIC include)

ament_target_dependencies(planning_around_objects
    ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
# -------------------------------------------------------------- install
  
install(TARGETS demo_moveit visualize_in_rviz planning_around_objects
  DESTINATION lib/${PROJECT_NAME}
)
install(DIRECTORY launch
  DESTINATION share/${PROJECT_NAME}
)


ament_package()

4. 运行

• 进入工作空间，编译项目

cd ~/ws_moveit

colcon build --cmake-args -DCMAKE_BUILD_TYPE=Release --packages-select demo_moveit

• 启动panda_arm机械臂

ros2 launch moveit2_tutorials demo.launch.py

1. 记得先把MotionPlanning关闭，因为目前用不到;
2. 添加插件RvizVisualToolsGui

• 运行本节节点

ros2 launch demo_moveit demo_moveit.launch.py