#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit/planning_scene_interface/planning_scene_interface.h>
#include <moveit_msgs/CollisionObject.h>
int main(int argc, char **argv)
{
ros::init(argc, argv, "full_demo");
ros::NodeHandle nh;
ros::AsyncSpinner spin(1);
spin.start();
// 创建运动规划的情景,等待创建完成
ros::Publisher planning_scene_diff_publisher = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
ros::WallDuration sleep_t(0.5);
while (planning_scene_diff_publisher.getNumSubscribers() < 1)
{
sleep_t.sleep();
}
moveit::planning_interface::MoveGroupInterface arm("arm");
//无障碍的动作
std::vector<double> joints={0.7,-1.57,0,0,0};
arm.setJointValueTarget(joints);
moveit::planning_interface::MoveGroupInterface::Plan my_plan;
moveit::planning_interface::MoveItErrorCode success = arm.plan(my_plan);
if(success) {
arm.execute(my_plan);
}
joints={-0.7,-1.57,0,0,0};
arm.setJointValueTarget(joints);
success = arm.plan(my_plan);
if(success) {
arm.execute(my_plan);
}
moveit_msgs::PlanningScene planning_scene;
std::string id_1="1";
std::string id_2="2";
// // 第一个障碍物
moveit_msgs::CollisionObject cylinder;
cylinder.header.frame_id = "base_link";
cylinder.id=id_1;
//定义物体形状尺寸
shape_msgs::SolidPrimitive primitive;
primitive.type=primitive.CYLINDER;
primitive.dimensions.resize(3); //dimensions是一个vector,为其分配3个元素空间
primitive.dimensions[0] =0.6; //圆柱体高度
primitive.dimensions[1] =0.05; //半径
geometry_msgs::Pose pose;
pose.orientation.w =1.0;
pose.position.x=0;
pose.position.y=0.2;
pose.position.z=0;
cylinder.primitives.push_back(primitive);
cylinder.primitive_poses.push_back(pose);
//定义操作为添加
cylinder.operation = cylinder.ADD;
planning_scene.world.collision_objects.emplace_back(cylinder);
//第二个障碍物
cylinder.header.frame_id = "base_link";
cylinder.id=id_2;
//定义物体形状尺寸
primitive.type=primitive.CYLINDER;
primitive.dimensions.resize(3); //dimensions是一个vector,为其分配3个元素空间
primitive.dimensions[0] =0.3; //圆柱体高度
primitive.dimensions[1] =0.05; //半径
pose.orientation.w =1.0;
pose.position.x=-0.07;
pose.position.y=0.2;
pose.position.z=0;
cylinder.primitives.push_back(primitive);
cylinder.primitive_poses.push_back(pose);
cylinder.operation = cylinder.ADD;
planning_scene.world.collision_objects.emplace_back(cylinder);
//发布
planning_scene.is_diff = true;
planning_scene_diff_publisher.publish(planning_scene);
//避障运动
joints={0.7,-1.57,0,0,0};
arm.setJointValueTarget(joints);
success = arm.plan(my_plan);
if(success) {
arm.execute(my_plan);
}
//移除障碍物
moveit::planning_interface::PlanningSceneInterface current_scene;
std::vector<std::string> object_ids;
object_ids.emplace_back(id_1);
object_ids.emplace_back(id_2);
current_scene.removeCollisionObjects(object_ids);
while (ros::ok()){
}
return 0;
}