版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/flyfish1986/article/details/81295374
机器人 避障规划
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 group
moveit::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);
//当运动规划失败后,允许重新规划arm
arm.allowReplanning(true);
//设置位置(单位:米)和姿态(单位:弧度)的允许误差
arm.setGoalPositionTolerance( 0.01);
arm.setGoalOrientationTolerance( 0.05);
//设置每次运动规划的时间限制:5s
arm.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;
//盒子1
moveit_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;
//盒子2
moveit_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 dict
std::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);
}