续学习记录1,进行不同规划器的避障效果比较
Moveit!避障实验
通过更换不同的规划器,比较不同路径规划算法的避障效果
过程如下
1.首先在moveitServerCpp.cpp文件下,设置障碍物
在create_table函数中添加和修改即可
我一共设置了两个物体,一个绿色的底板,用于防止机械臂穿模到地下;另外设置了一个障碍物,代码如下
void create_table() { 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::PlanningSceneInterface planning_scene_interface;moveit_msgs::PlanningScene planning_scene;moveit_msgs::CollisionObject collision_object;collision_object.header.frame_id = arm_->getPlanningFrame();// The id of the object is used to identify it.collision_object.id = "table";// Define a box to add to the world.shape_msgs::SolidPrimitive primitive;primitive.type = primitive.BOX;primitive.dimensions.resize(3);primitive.dimensions[primitive.BOX_X] = 2; //长宽2米,高0.01米primitive.dimensions[primitive.BOX_Y] = 2;primitive.dimensions[primitive.BOX_Z] = 0.01;// Define a pose for the box (specified relative to frame_id)geometry_msgs::Pose box_pose;box_pose.orientation.w = 1.0; //设置box的位姿box_pose.position.x = 0.0;box_pose.position.y = 0.0;box_pose.position.z = -0.01/2 -0.02;collision_object.primitives.push_back(primitive);collision_object.primitive_poses.push_back(box_pose);collision_object.operation = collision_object.ADD; //若要放置两个物体,把从这往上到Define a box to add to the world再复制一份planning_scene.world.collision_objects.push_back(collision_object);planning_scene.is_diff = true;planning_scene_diff_publisher.publish(planning_scene);ROS_INFO("Added an table into the world");// Define a box to add to the world.shape_msgs::SolidPrimitive primitive1;primitive1.type = primitive1.BOX;primitive1.dimensions.resize(3);primitive1.dimensions[primitive1.BOX_X] = 0.2; //长宽2米,高0.01米primitive1.dimensions[primitive1.BOX_Y] = 0.2;primitive1.dimensions[primitive1.BOX_Z] = 0.2;// Define a pose for the box (specified relative to frame_id)geometry_msgs::Pose box_pose1;box_pose1.orientation.w = 1.0; //设置box的位姿box_pose1.position.x = 0.4;box_pose1.position.y = 0.0;box_pose1.position.z = -0.01/2 ;collision_object.primitives.push_back(primitive1);collision_object.primitive_poses.push_back(box_pose1);collision_object.operation = collision_object.ADD; //若要放置两个物体,把从这往上到Define a box to add to the world再复制一份planning_scene.world.collision_objects.push_back(collision_object);planning_scene.is_diff = true;planning_scene_diff_publisher.publish(planning_scene);ROS_INFO("Added an table into the world");}
2.更改cmakelist
add_executable(moveitServerCpp src/moveitServerCpp.cpp)
target_link_libraries(moveitServerCpp ${catkin_LIBRARIES})
然后编译
3.启动模型的rviz看效果
启动后,运行cpp文件可以看到路径规划的避障效果
4.更换不同的规划器
在moveit_control类中更换即可
更换后重新编译再运行
比较不同的避障效果
规划成功
如果规划失败,如下图
可以更换规划器,如果还不行,就是你设置的那个点确实到不了,换点试试