ROS electric arm navigation programmatically adding collision object

edit

Basically what you want to do is load the mesh object, create an arm_navigation_msgs::CollisionObject from that mesh object, and publish it on the '/collision_object' topic. The environment_server listens to this topic and then will include the mesh object in the planning scene, making MoveArm plan around it.

In a bit more detail, here's a sketch of some code:

#include <geometric_shapes/shape_operations.h>
#include <planning_environment/util/construct_object.h>

btVector3 scale(1.0,1.0,1.0);
//filename is either an absolute path or a package:// address
shapes::Mesh* mesh = shapes::createMeshFromFilename(filename, &scale);

arm_navigation_msgs::Shape object;
if(!planning_environment::constructObjectMsg(mesh, object)) {
  ROS_WARN_STREAM("Object construction fails");
}
delete mesh;
arm_navigation_msgs::CollisionObject collision_object;
collision_object.operation.operation = arm_navigation_msgs::CollisionObjectOperation::ADD;
collision_object.header.stamp = ros::Time::now();
collision_object.header.frame_id = WORLD_FRAME_ID;
collision_object.id = "my_object";
collision_object.shapes.push_back(object);
//pose comes from elsewhere and must be set
geometry_msgs::Pose object_recognition_pose;
collision_object.poses.push_back(pose);

//publish on /collision_object topic
collision_object_publisher.publish(collision_object)