视频讲解

创建add_cylinder的package

ros2 pkg create add_cylinder --build-type ament_cmake --dependencies rclcpp control_msgs moveit_ros_planning_interface

在src中添加add_cylinder.cpp，如下

 #include <rclcpp/rclcpp.hpp>
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>

#include <tf2/LinearMath/Quaternion.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>

#include <cmath>

// 定义一个将度数转换为弧度的函数
double degreesToRadians(double degrees) {
    return degrees * M_PI / 180.0;
}

int main(int argc, char** argv)
{
    rclcpp::init(argc, argv);
    auto node = rclcpp::Node::make_shared("add_cylinder");

    // 创建MoveGroupInterface对象，指定规划组名称
    moveit::planning_interface::MoveGroupInterface move_group(node, "panda_arm");

    // 创建PlanningSceneInterface对象
    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;

    // 添加圆柱体到规划场景
    moveit_msgs::msg::CollisionObject collision_object;
    collision_object.header.frame_id = move_group.getPlanningFrame();
    collision_object.id = "cylinder";

    shape_msgs::msg::SolidPrimitive primitive;
    primitive.type = primitive.CYLINDER;
    primitive.dimensions.resize(2);
    primitive.dimensions[0] = 0.05;  // 高度
    primitive.dimensions[1] = 0.01;   // 半径

    geometry_msgs::msg::Pose cylinder_pose;
    cylinder_pose.orientation.w = 1.0;
    cylinder_pose.position.x = 0.5;
    cylinder_pose.position.y = 0.0;
    cylinder_pose.position.z = 0.0;

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

    std::vector<moveit_msgs::msg::CollisionObject> collision_objects;
    collision_objects.push_back(collision_object);
    planning_scene_interface.applyCollisionObjects(collision_objects);

    rclcpp::shutdown();
    return 0;
}

CMakeLists.txt文件如下

cmake_minimum_required(VERSION 3.8)
project(add_cylinder)

if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
  add_compile_options(-Wall -Wextra -Wpedantic)
endif()

# 查找依赖项
find_package(ament_cmake REQUIRED)
find_package(rclcpp REQUIRED)
find_package(moveit_ros_planning_interface REQUIRED)

# 添加可执行文件
add_executable(add_cylinder src/add_cylinder.cpp)
ament_target_dependencies(add_cylinder rclcpp moveit_ros_planning_interface)

# 安装可执行文件
install(TARGETS
  add_cylinder
  DESTINATION lib/${PROJECT_NAME}
)

ament_package()

编译

cd ~/ws_moveit 
colcon build --packages-select add_cylinder

先启动Rviz2的panda机械臂场景

ros2 launch moveit2_tutorials demo.launch.py rviz_config:=panda_moveit_config_demo_empty.rviz

在其一个终端，输入如下指令

source install/setup.bash 
ros2 run add_cylinder add_cylinder