00001 #include <ros/ros.h>
00002 #include <mapping_msgs/AttachedCollisionObject.h>
00003 #include <geometric_shapes_msgs/Shape.h>
00004 #include <resource_retriever/retriever.h>
00005 #include <geometric_shapes/shapes.h>
00006 #include <geometric_shapes/shape_operations.h>
00007
00008 bool getTrianglesFromMeshFile(std::string mesh_file, std::vector<int32_t> &triangles, std::vector<geometry_msgs::Point> &vertices)
00009 {
00010 bool retval = false;
00011 shapes::Shape *mesh = NULL;
00012
00013 if (!mesh_file.empty())
00014 {
00015 resource_retriever::Retriever retriever;
00016 resource_retriever::MemoryResource res;
00017 bool ok = true;
00018
00019 try
00020 {
00021 res = retriever.get(mesh_file);
00022 }
00023 catch (resource_retriever::Exception& e)
00024 {
00025 ROS_ERROR("%s", e.what());
00026 ok = false;
00027 }
00028
00029 if (ok)
00030 {
00031 if (res.size == 0)
00032 ROS_WARN("Retrieved empty mesh for resource '%s'", mesh_file.c_str());
00033 else
00034 {
00035 mesh = shapes::createMeshFromBinaryStlData(reinterpret_cast<char*>(res.data.get()), res.size);
00036 if (mesh == NULL)
00037 ROS_ERROR("Failed to load mesh '%s'", mesh_file.c_str());
00038 else
00039 retval = true;
00040 }
00041 }
00042 }
00043 else
00044 ROS_WARN("Empty mesh filename");
00045
00046 if(retval)
00047 {
00048 triangles.resize(static_cast<shapes::Mesh*>(mesh)->triangleCount*3);
00049 for(size_t i=0; i<triangles.size(); ++i)
00050 triangles[i] = static_cast<shapes::Mesh*>(mesh)->triangles[i];
00051
00052 ROS_INFO("vertexCount: %d triangleCount: %d", static_cast<shapes::Mesh*>(mesh)->vertexCount, static_cast<shapes::Mesh*>(mesh)->triangleCount);
00053
00054 vertices.resize((static_cast<shapes::Mesh*>(mesh)->vertexCount));
00055 for(size_t i=0; i<vertices.size(); ++i)
00056 {
00057 vertices[i].x = static_cast<shapes::Mesh*>(mesh)->vertices[3*i];
00058 vertices[i].y = static_cast<shapes::Mesh*>(mesh)->vertices[3*i+1];
00059 vertices[i].z = static_cast<shapes::Mesh*>(mesh)->vertices[3*i+2];
00060 }
00061 }
00062
00063 return retval;
00064 }
00065
00066 int main(int argc, char** argv) {
00067
00068 ros::init(argc, argv, "sbpl_attach_object_right_arm");
00069 double radius=0,length=0;
00070 std::string shape, mesh_file, operation;
00071 ros::Publisher att_object_in_map_pub_;
00072 ros::NodeHandle nh;
00073 geometric_shapes_msgs::Shape object;
00074 geometry_msgs::Pose pose;
00075
00076 att_object_in_map_pub_ = nh.advertise<mapping_msgs::AttachedCollisionObject>("attached_collision_object", 10);
00077 sleep(2);
00078
00079 ros::NodeHandle ph("~");
00080 ph.param<std::string>("shape",shape,"sphere");
00081 ph.param<std::string>("operation",operation,"add");
00082 ph.param<std::string>("mesh_filename",mesh_file,"");
00083 ph.param<double>("radius",radius,0.025);
00084 ph.param<double>("length",length,0.5);
00085
00086
00087 mapping_msgs::AttachedCollisionObject att_object;
00088 att_object.link_name = "r_gripper_r_finger_tip_link";
00089 att_object.touch_links.push_back("r_gripper_palm_link");
00090 att_object.touch_links.push_back("r_gripper_r_finger_link");
00091 att_object.touch_links.push_back("r_gripper_l_finger_link");
00092 att_object.touch_links.push_back("r_gripper_l_finger_tip_link");
00093
00094 att_object.object.id = "sbpl_attached_" + shape;
00095 att_object.object.header.frame_id = "r_gripper_r_finger_tip_link";
00096 att_object.object.header.stamp = ros::Time::now();
00097
00098 if(operation.compare("remove") == 0)
00099 att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::REMOVE;
00100 else
00101 att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00102
00103 if(shape.compare("mesh") == 0)
00104 {
00105 object.type = geometric_shapes_msgs::Shape::MESH;
00106
00107 if(!getTrianglesFromMeshFile(mesh_file, object.triangles, object.vertices))
00108 {
00109 ROS_ERROR("Unable to retrieve mesh file. Exiting");
00110 return 0;
00111 }
00112 }
00113 else if(shape.compare("cylinder") == 0)
00114 {
00115 object.type = geometric_shapes_msgs::Shape::CYLINDER;
00116 object.dimensions.resize(2);
00117 object.dimensions[0] = radius;
00118 object.dimensions[1] = length;
00119 }
00120 else if(shape.compare("box") == 0)
00121 {
00122 ROS_INFO("Box is not yet implemented. Using a sphere.");
00123 object.type = geometric_shapes_msgs::Shape::SPHERE;
00124
00125 }
00126 else
00127 {
00128 object.type = geometric_shapes_msgs::Shape::SPHERE;
00129 object.dimensions.resize(1);
00130 object.dimensions[0] = radius;
00131 }
00132
00133 pose.position.x = 0.0;
00134 pose.position.y = 0.03;
00135 pose.position.z = 0.0;
00136 pose.orientation.x = 0;
00137 pose.orientation.y = 0;
00138 pose.orientation.z = 0;
00139 pose.orientation.w = 1;
00140 att_object.object.shapes.push_back(object);
00141 att_object.object.poses.push_back(pose);
00142
00143 ros::Rate r(0.1);
00144 while(nh.ok())
00145 {
00146 att_object_in_map_pub_.publish(att_object);
00147 r.sleep();
00148 }
00149
00150 ROS_INFO("Should have published");
00151
00152 ros::Duration(2.0).sleep();
00153
00154 ros::shutdown();
00155 return 0;
00156 }
00157