ROS2 Rviz 实战：给 panda 机械臂场景塞个圆柱体

视频讲解

ROS2 Rviz 实战：给 panda 机械臂场景塞个圆柱体

创建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(TARGETSadd_cylinderDESTINATION 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