机器人 避障规划
flyfish
#include <ros/ros.h>#include <moveit/robot_model_loader/robot_model_loader.h>#include <moveit/planning_scene/planning_scene.h>#include <moveit/kinematic_constraints/utils.h>#include <moveit/move_group_interface/move_group_interface.h>#include <moveit/planning_scene_interface/planning_scene_interface.h>#include <eigen_conversions/eigen_msg.h>#include <vector>#include <tf/transform_broadcaster.h>#include <vector>moveit_msgs::ObjectColor setColor(std::string name, double r, double g, double b, double a = 0.9)
{
moveit_msgs::ObjectColor color;
color.id = name;
color.color.r = r;
color.color.g = g;
color.color.b = b;
color.color.a = a;return color;
}
moveit_msgs::PlanningScene sendColors(std::map<std::string, moveit_msgs::ObjectColor> colors)
{
moveit_msgs::PlanningScene p;
p.is_diff = true;for (const auto &i : colors)
{
p.object_colors.push_back(i.second);
}return p;
}int main(int argc, char **argv)
{
ros::init (argc, argv, "moveit_obstacles_demo",ros::init_options::AnonymousName);
ros::AsyncSpinner spinner(1);
spinner.start();
ros::NodeHandle nh;//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////初始化需要使用move group控制的机械臂中的arm groupmoveit::planning_interface::MoveGroupInterface arm("arm");
ROS_INFO("Planning reference frame: %s", arm.getPlanningFrame().c_str());
ROS_INFO("End effector reference frame: %s", arm.getEndEffectorLink().c_str());//获取终端link的名称std::string end_effector_link=arm.getEndEffectorLink();//设置目标位置所使用的参考坐标系std::string reference_frame="base_link";
arm.setPoseReferenceFrame(reference_frame);//当运动规划失败后，允许重新规划armarm.allowReplanning(true);//设置位置(单位：米)和姿态（单位：弧度）的允许误差arm.setGoalPositionTolerance( 0.01);
arm.setGoalOrientationTolerance( 0.05);//设置每次运动规划的时间限制：5sarm.setPlanningTime(5);//////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////初始化场景对象moveit::planning_interface::PlanningSceneInterface scene;
ros::Publisher scene_pub = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 5);
sleep(3);
std::string table_id = "table";
std::string box1_id = "box1";
std::string box2_id = "box2";
std::vector<std::string> object_ids_delete;
object_ids_delete.push_back(table_id);
object_ids_delete.push_back(box1_id);
object_ids_delete.push_back(box2_id);
scene.removeCollisionObjects(object_ids_delete);double table_size[3] = {0.2, 0.7, 0.01};double box1_size[3] = {0.1, 0.05, 0.05};double box2_size[3] = {0.05, 0.05, 0.15};//设置桌面的高度double table_ground = 0.25;//桌子moveit_msgs::CollisionObject table;
table.id = table_id;
shape_msgs::SolidPrimitive primitive;
primitive.type = primitive.BOX;
primitive.dimensions.resize(3);
primitive.dimensions[0] = 0.2;
primitive.dimensions[1] = 0.7;
primitive.dimensions[2] = 0.01;
geometry_msgs::PoseStamped table_pose ;
table_pose.header.frame_id = reference_frame;
table_pose.pose.position.x = 0.26;
table_pose.pose.position.y = 0.0;
table_pose.pose.position.z = table_ground + table_size[2] / 2.0;
table_pose.pose.orientation.w = 1.0;
table.primitives.push_back(primitive);
table.primitive_poses.push_back(table_pose.pose);
table.operation = table.ADD;//盒子1moveit_msgs::CollisionObject box1;
box1.id=box1_id;
shape_msgs::SolidPrimitive box1_primitive;
box1_primitive.type = box1_primitive.BOX;
box1_primitive.dimensions.resize(3);
box1_primitive.dimensions[0] = 0.1;
box1_primitive.dimensions[1] = 0.05;
box1_primitive.dimensions[2] = 0.05;
geometry_msgs::PoseStamped box1_pose;
box1_pose.header.frame_id = reference_frame;
box1_pose.pose.position.x = 0.21;
box1_pose.pose.position.y = -0.1;
box1_pose.pose.position.z = table_ground + table_size[2] + box1_size[2] / 2.0;
box1_pose.pose.orientation.w = 1.0 ;
box1.primitives.push_back(box1_primitive);
box1.primitive_poses.push_back(box1_pose.pose);
box1.operation=box1.ADD;//盒子2moveit_msgs::CollisionObject box2;
box2.id=box2_id;
shape_msgs::SolidPrimitive box2_primitive;
box2_primitive.type = box2_primitive.BOX;
box2_primitive.dimensions.resize(3);
box2_primitive.dimensions[0] = 0.05;
box2_primitive.dimensions[1] = 0.05;
box2_primitive.dimensions[2] = 0.15;
geometry_msgs::PoseStamped box2_pose;
box2_pose.header.frame_id = reference_frame;
box2_pose.pose.position.x = 0.19;
box2_pose.pose.position.y = 0.15;
box2_pose.pose.position.z = table_ground + table_size[2] + box2_size[2] / 2.0;
box2_pose.pose.orientation.w = 1.0 ;
box2.primitives.push_back(box2_primitive);
box2.primitive_poses.push_back(box2_pose.pose);
box2.operation=box2.ADD;
std::vector<moveit_msgs::CollisionObject> collision_objects;
collision_objects.push_back(table);
collision_objects.push_back(box1);
collision_objects.push_back(box2);
scene.addCollisionObjects(collision_objects);
sleep(3);//颜色处理moveit_msgs::ObjectColor table_id_color = setColor(table_id, 0.8, 0, 0, 1.0);
moveit_msgs::ObjectColor box1_id_color = setColor(box1_id, 0.8, 0.4, 0, 1.0);
moveit_msgs::ObjectColor box2_id_color = setColor(box2_id, 0.7, 0.9, 0.4, 1.0);//python dictstd::map<std::string, moveit_msgs::ObjectColor> colors;
colors.insert({table_id, table_id_color});
colors.insert({box1_id, box1_id_color});
colors.insert({box2_id, box2_id_color});////////////////////////////////////////////////////////////////////////////////////////////moveit_msgs::PlanningScene p = sendColors(colors);//发布场景物体颜色设置scene_pub.publish(p);
geometry_msgs::PoseStamped target_pose;
target_pose.header.frame_id = reference_frame;
target_pose.pose.position.x = 0.2;
target_pose.pose.position.y = 0.0;
target_pose.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05;
target_pose.pose.orientation.w=1.0;
arm.setPoseTarget(target_pose, end_effector_link);
arm.move();
sleep(2);
geometry_msgs::PoseStamped target_pose2;
target_pose2.header.frame_id = reference_frame;
target_pose2.pose.position.x = 0.2;
target_pose2.pose.position.y = -0.25;
target_pose2.pose.position.z = table_pose.pose.position.z + table_size[2] + 0.05;
target_pose2.pose.orientation.w=1.0;
arm.setPoseTarget(target_pose2, end_effector_link);
arm.move();
sleep(2);//////////////////////////////////////////////////////////////////////////////////////////////控制机械臂先回到初始化位置arm.setNamedTarget("home");
arm.move();
sleep(2);
}