I had a problem with using MoveGroupInterface (see [this](http://answers.ros.org/question/231025/moveit-planning-not-continue-after-successful-planning/)), so I decide to use the c++ API (from [this tutorial](http://docs.ros.org/indigo/api/pr2_moveit_tutorials/html/planning/src/doc/motion_planning_api_tutorial.html). It's all working fine now as I can pass the trajectory to gazebo. However, when I want to insert the obstacles into the environment, I tried following [this answer](http://answers.ros.org/question/204770/insert-obstacle-in-step-format-into-moveit-via-c-node/) and come up with the following code:
class collisionObjectAdder
{
protected:
ros::Publisher add_collision_object_pub;
moveit_msgs::CollisionObject co;
public:
collisionObjectAdder(ros::NodeHandle& n)
{
add_collision_object_pub = n.advertise("collision_object", 1000);
}
void addCollisionObject(std::string modelpath, double x = 0, double y = 0, double z = 0, double ow = 1, double ox = 0, double oy = 0, double oz = 0)
{
shapes::Mesh* m = shapes::createMeshFromResource(modelpath);
ROS_INFO("mesh loaded: %i %s ",m->triangle_count,modelpath.c_str());
shape_msgs::Mesh co_mesh;
shapes::ShapeMsg co_mesh_msg;
shapes::constructMsgFromShape(m, co_mesh_msg);
co_mesh = boost::get(co_mesh_msg);
co.meshes.push_back(co_mesh);
int pos = co.meshes.size() - 1;
co.header.frame_id = "world";
moveit_msgs::CollisionObject::_mesh_poses_type mesh_pose;
mesh_pose.resize(1);
mesh_pose[0].position.x = x;
mesh_pose[0].position.y = y;
mesh_pose[0].position.z = z;
mesh_pose[0].orientation.w= ow;
mesh_pose[0].orientation.x= ox;
mesh_pose[0].orientation.y= oy;
mesh_pose[0].orientation.z= oz;
co.mesh_poses.push_back(mesh_pose[0]);
co.operation = co.ADD;
}
void publishCollisionObjects()
{
sleep(1.0); // To make sure the node can publish
add_collision_object_pub.publish(co);
ROS_INFO("Collision object published");
}
};
and I call this in the main function:
ROS_INFO("Adding collision objects into the world");
collisionObjectAdder coa(n);
coa.addCollisionObject("package://myrobot_description/meshes/wall.dae",0.35,0,1.6);
coa.publishCollisionObjects();
ros::WallDuration sleep_time(2.0);
sleep_time.sleep();
This works with the move group example as the planner will try to avoid this obstacle, but it doesn't work with my api example. I had a feeling the tutorial example does not know to fetch /collision_object topic to insert in the planning_scene (need to manually add a callback function?). Any one can suggest me what is missing in my implementation?
↧