00001
00033 #ifndef OBJECT_HANDLER_INCLUDED
00034 #define OBJECT_HANDLER_INCLUDED
00035
00036
00037 #include <ros/ros.h>
00038 #include <stdio.h>
00039 #include <stdlib.h>
00040 #include <vector>
00041
00042 #include <cob_arm_navigation/HandleObject.h>
00043 #include <gazebo/GetModelState.h>
00044 #include <mapping_msgs/CollisionObject.h>
00045 #include <mapping_msgs/AttachedCollisionObject.h>
00046 #include <planning_environment_msgs/GetCollisionObjects.h>
00047
00048
00049
00050 const std::string frame_id = "/map";
00051
00052
00053 class Object_Handler
00054 {
00055 private:
00056 ros::NodeHandle rh;
00057
00058 ros::Publisher m_object_in_map_pub;
00059 ros::Publisher m_att_object_in_map_pub;
00060
00061 ros::ServiceClient m_state_client;
00062 ros::ServiceClient m_collision_objects_client;
00063
00064 ros::ServiceServer m_add_object_server;
00065 ros::ServiceServer m_remove_object_server;
00066 ros::ServiceServer m_attach_object_server;
00067 ros::ServiceServer m_detach_object_server;
00068
00069
00070 public:
00071 Object_Handler()
00072 {
00073 ROS_INFO("Object_Handler_constructor called");
00074
00075 ROS_WARN("waiting for services...");
00076 ros::service::waitForService("/gazebo/get_model_state");
00077 ros::service::waitForService("/cob3_environment_server/get_collision_objects");
00078 ROS_INFO("...done!");
00079
00080
00081 m_object_in_map_pub = rh.advertise<mapping_msgs::CollisionObject>("collision_object", 1);
00082 m_att_object_in_map_pub = rh.advertise<mapping_msgs::AttachedCollisionObject>("attached_collision_object", 1);
00083
00084
00085 m_state_client = rh.serviceClient<gazebo::GetModelState>("/gazebo/get_model_state");
00086 m_collision_objects_client = rh.serviceClient<planning_environment_msgs::GetCollisionObjects>("/cob3_environment_server/get_collision_objects");
00087
00088 m_add_object_server = rh.advertiseService("/object_handler/add_object", &Object_Handler::add_object, this);
00089 m_remove_object_server = rh.advertiseService("/object_handler/remove_object", &Object_Handler::remove_object, this);
00090 m_attach_object_server = rh.advertiseService("/object_handler/attach_object", &Object_Handler::attach_object, this);
00091 m_detach_object_server = rh.advertiseService("/object_handler/detach_object", &Object_Handler::detach_object, this);
00092 ROS_INFO("object_handler ready...");
00093 }
00094
00095 void run()
00096 {
00097 ROS_INFO("spinning...");
00098 ros::spin();
00099 }
00100
00101 private:
00102
00103
00104 bool add_object(cob_arm_navigation::HandleObject::Request &req,
00105 cob_arm_navigation::HandleObject::Response &res )
00106 {
00107 ROS_INFO("add_object-service called!");
00108 ROS_INFO("Adding object %s ...",req.object.data.c_str());
00109
00110 std::string parameter_name = req.object.data;
00111 std::string model_name = req.object.data;
00112 ROS_INFO("Model-Name: %s", model_name.c_str());
00113
00114
00115 while(!rh.hasParam(parameter_name))
00116 {
00117 ROS_WARN("waiting for parameter \"%s\"... ", parameter_name.c_str());
00118 ros::Duration(0.5).sleep();
00119 }
00120
00121 std::string model_parameter;
00122 if (rh.getParam(parameter_name, model_parameter))
00123 {
00124 ROS_INFO("Getting parameter successful!");
00125 ROS_DEBUG("Parameter: %s", model_parameter.c_str());
00126 }
00127
00128 mapping_msgs::CollisionObject collision_object;
00129
00130
00131 std::string pattern = "geom:box";
00132 std::size_t found_box = model_parameter.find(pattern);
00133 if (found_box!=std::string::npos)
00134 {
00135 ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_box));
00136 bool success;
00137 std::vector< double > dimensions;
00138
00139 success = parse_box(model_parameter, dimensions);
00140 if(!success)
00141 {
00142 ROS_ERROR("Error while parsing a geom:box model. Aborting!");
00143
00144 res.success.data = false;
00145 res.error_message.data = "Error while parsing a geom:box model.";
00146
00147 return false;
00148 }
00149
00150 success = compose_box(model_name, dimensions, collision_object);
00151
00152 if(!success)
00153 {
00154 ROS_ERROR("Error while composing the collision_object. Aborting!");
00155
00156 res.success.data = false;
00157 res.error_message.data = "Error while composing the collision_object.";
00158
00159 return false;
00160 }
00161 collision_object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00162 }
00163 else
00164 {
00165 ROS_ERROR("%s not found", pattern.c_str());
00166 ROS_ERROR("I can't parse this model. Aborting!");
00167
00168 res.success.data = false;
00169 res.error_message.data = "No parser for the geom:type of this model available yet.";
00170
00171 return false;
00172 }
00173
00174
00175 m_object_in_map_pub.publish(collision_object);
00176
00177 ROS_INFO("Object added to environment server!");
00178
00179 res.success.data = true;
00180 res.error_message.data = "Object added to environment server!";
00181 return true;
00182 }
00183
00184
00185 bool remove_object(cob_arm_navigation::HandleObject::Request &req,
00186 cob_arm_navigation::HandleObject::Response &res )
00187 {
00188 ROS_INFO("remove_object-service called!");
00189 ROS_INFO("Removing object %s ...",req.object.data.c_str());
00190
00191 std::string object_name = req.object.data;
00192
00193 planning_environment_msgs::GetCollisionObjects srv;
00194
00195 srv.request.include_points = false;
00196
00197 if (m_collision_objects_client.call(srv))
00198 {
00199 ROS_INFO("get_collision_objects service_call successfull!");
00200 for(unsigned int i = 0; i < srv.response.collision_objects.size(); i++)
00201 {
00202 if(srv.response.collision_objects[i].id == object_name)
00203 {
00204 ROS_INFO("%s found!", object_name.c_str());
00205 mapping_msgs::CollisionObject collision_object = srv.response.collision_objects[i];
00206
00207 collision_object.operation.operation = mapping_msgs::CollisionObjectOperation::REMOVE;
00208
00209 m_object_in_map_pub.publish(collision_object);
00210
00211 ROS_INFO("Object removed from environment server!");
00212
00213 res.success.data = true;
00214 res.error_message.data = "Object removed from environment server!";
00215 return true;
00216 }
00217 }
00218 ROS_ERROR("Could not find object %s among known objects. Aborting!", object_name.c_str());
00219
00220 res.success.data = false;
00221 res.error_message.data = "Could not find object among known objects.";
00222
00223 return false;
00224 }
00225 else
00226 {
00227 ROS_ERROR("Failed to call service get_collision_objects. Aborting!");
00228
00229 res.success.data = false;
00230 res.error_message.data = "Failed to call service get_collision_objects.";
00231
00232 return false;
00233 }
00234
00235
00236 ROS_ERROR("You shouldn't be here!");
00237
00238 return false;
00239 }
00240
00241
00242 bool attach_object(cob_arm_navigation::HandleObject::Request &req,
00243 cob_arm_navigation::HandleObject::Response &res )
00244 {
00245 ROS_INFO("attach_object-service called!");
00246 ROS_INFO("Attaching object %s ...",req.object.data.c_str());
00247
00248 std::string object_name = req.object.data;
00249
00250 planning_environment_msgs::GetCollisionObjects srv;
00251
00252 srv.request.include_points = false;
00253
00254 if (m_collision_objects_client.call(srv))
00255 {
00256 ROS_INFO("get_collision_objects service_call successfull!");
00257 for(unsigned int i = 0; i < srv.response.collision_objects.size(); i++)
00258 {
00259 if(srv.response.collision_objects[i].id == object_name)
00260 {
00261 ROS_INFO("%s found!", object_name.c_str());
00262
00263 mapping_msgs::AttachedCollisionObject att_object;
00264 att_object.object = srv.response.collision_objects[i];
00265
00266 att_object.link_name = "sdh_palm_link";
00267 att_object.touch_links.push_back("sdh_grasp_link");
00268 att_object.touch_links.push_back("sdh_finger_11_link");
00269 att_object.touch_links.push_back("sdh_finger_12_link");
00270 att_object.touch_links.push_back("sdh_finger_13_link");
00271 att_object.touch_links.push_back("sdh_finger_21_link");
00272 att_object.touch_links.push_back("sdh_finger_22_link");
00273 att_object.touch_links.push_back("sdh_finger_23_link");
00274 att_object.touch_links.push_back("sdh_thumb_1_link");
00275 att_object.touch_links.push_back("sdh_thumb_2_link");
00276 att_object.touch_links.push_back("sdh_thumb_3_link");
00277
00278 att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::ATTACH_AND_REMOVE_AS_OBJECT;
00279
00280 m_att_object_in_map_pub.publish(att_object);
00281
00282 ROS_INFO("Object attached to robot!");
00283
00284 res.success.data = true;
00285 res.error_message.data = "Object attached to robot!";
00286 return true;
00287 }
00288 }
00289 ROS_ERROR("Could not find object %s among known objects. Aborting!", object_name.c_str());
00290
00291 res.success.data = false;
00292 res.error_message.data = "Could not find object among known objects.";
00293
00294 return false;
00295 }
00296 else
00297 {
00298 ROS_ERROR("Failed to call service get_collision_objects. Aborting!");
00299
00300 res.success.data = false;
00301 res.error_message.data = "Failed to call service get_collision_objects.";
00302
00303 return false;
00304 }
00305
00306
00307 ROS_ERROR("You shouldn't be here!");
00308
00309 return false;
00310 }
00311
00312
00313 bool detach_object(cob_arm_navigation::HandleObject::Request &req,
00314 cob_arm_navigation::HandleObject::Response &res )
00315 {
00316 ROS_INFO("detach_object-service called!");
00317 ROS_INFO("Detaching object %s ...",req.object.data.c_str());
00318
00319 std::string object_name = req.object.data;
00320
00321 planning_environment_msgs::GetCollisionObjects srv;
00322
00323 srv.request.include_points = false;
00324
00325 if (m_collision_objects_client.call(srv))
00326 {
00327 ROS_INFO("get_collision_objects service_call successfull!");
00328 for(unsigned int i = 0; i < srv.response.attached_collision_objects.size(); i++)
00329 {
00330 if(srv.response.attached_collision_objects[i].object.id == object_name)
00331 {
00332 ROS_INFO("%s found!", object_name.c_str());
00333
00334 mapping_msgs::AttachedCollisionObject att_object = srv.response.attached_collision_objects[i];
00335
00336 att_object.object.operation.operation = mapping_msgs::CollisionObjectOperation::DETACH_AND_ADD_AS_OBJECT;
00337
00338 m_att_object_in_map_pub.publish(att_object);
00339
00340 ROS_INFO("Object detached from robot!");
00341
00342 res.success.data = true;
00343 res.error_message.data = "Object detached from robot!";
00344 return true;
00345 }
00346 }
00347 ROS_ERROR("Could not find object %s among attached objects. Aborting!", object_name.c_str());
00348
00349 res.success.data = false;
00350 res.error_message.data = "Could not find object among attached objects.";
00351
00352 return false;
00353 }
00354 else
00355 {
00356 ROS_ERROR("Failed to call service get_collision_objects. Aborting!");
00357
00358 res.success.data = false;
00359 res.error_message.data = "Failed to call service get_collision_objects.";
00360
00361 return false;
00362 }
00363
00364
00365 ROS_ERROR("You shouldn't be here!");
00366
00367 return false;
00368 }
00369
00370
00371
00372
00373 bool parse_box(std::string model_parameter, std::vector< double > &dimensions)
00374 {
00375
00376 std::string pattern;
00377 std::size_t found_size, found_p, found_x, found_y, found_z;
00378
00379 pattern = "size";
00380 found_size=model_parameter.find(pattern);
00381 if (found_size!=std::string::npos)
00382 ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_size));
00383 else
00384 {
00385 ROS_ERROR("%s not found", pattern.c_str());
00386 return false;
00387 }
00388
00389 pattern = ">";
00390 found_p=model_parameter.find(pattern, found_size);
00391 if (found_p!=std::string::npos)
00392 ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_p));
00393 else
00394 {
00395 ROS_ERROR("%s not found", pattern.c_str());
00396 return false;
00397 }
00398
00399 pattern = " ";
00400 found_x=model_parameter.find_first_not_of(pattern, found_p+1);
00401 if (found_x!=std::string::npos)
00402 ROS_DEBUG("first not \"%s\" found at: %d", pattern.c_str(), int(found_x));
00403 else
00404 {
00405 ROS_ERROR("%s not found", pattern.c_str());
00406 return false;
00407 }
00408
00409 pattern = " ";
00410 found_p=model_parameter.find_first_of(pattern, found_x);
00411 if (found_p!=std::string::npos)
00412 ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_p));
00413 else
00414 {
00415 ROS_ERROR("%s not found", pattern.c_str());
00416 return false;
00417 }
00418
00419 size_t length_x = found_p - found_x;
00420 std::string x = model_parameter.substr(found_x, length_x);
00421 ROS_DEBUG("x: %s, real_length_x: %d", x.c_str(), int(length_x));
00422
00423 double x_d = strtod(x.c_str(), NULL);
00424 ROS_INFO("x as double: %f", x_d);
00425
00426
00427
00428 pattern = " ";
00429 found_y=model_parameter.find_first_not_of(pattern, found_p);
00430 if (found_y!=std::string::npos)
00431 ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_y));
00432 else
00433 {
00434 ROS_ERROR("%s not found", pattern.c_str());
00435 return false;
00436 }
00437
00438 found_p=model_parameter.find_first_of(pattern, found_y);
00439 if (found_p!=std::string::npos)
00440 ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_p));
00441 else
00442 {
00443 ROS_ERROR("%s not found", pattern.c_str());
00444 return false;
00445 }
00446
00447 size_t length_y = found_p - found_y;
00448 std::string y = model_parameter.substr(found_y, length_y);
00449 ROS_DEBUG("y: %s, real_length_y: %d", y.c_str(), int(length_y));
00450
00451 double y_d = strtod(y.c_str(), NULL);
00452 ROS_INFO("y as double: %f", y_d);
00453
00454
00455
00456
00457 pattern = " ";
00458 found_z=model_parameter.find_first_not_of(pattern, found_p);
00459 if (found_z!=std::string::npos)
00460 ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_z));
00461 else
00462 {
00463 ROS_ERROR("%s not found", pattern.c_str());
00464 return false;
00465 }
00466
00467 pattern = "<";
00468 found_p=model_parameter.find_first_of(pattern, found_z);
00469 if (found_p!=std::string::npos)
00470 ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_p));
00471 else
00472 {
00473 ROS_ERROR("%s not found", pattern.c_str());
00474 return false;
00475 }
00476
00477 size_t length_z = found_p - found_z;
00478 std::string z = model_parameter.substr(found_z, length_z);
00479 ROS_DEBUG("z: %s, real_length_z: %d", z.c_str(), int(length_z));
00480
00481 double z_d = strtod(z.c_str(), NULL);
00482 ROS_INFO("z as double: %f", z_d);
00483
00484 dimensions.push_back(x_d);
00485 dimensions.push_back(y_d);
00486 dimensions.push_back(z_d);
00487
00488 return true;
00489 }
00490
00491
00492 bool compose_box(std::string model_name, std::vector< double > dimensions, mapping_msgs::CollisionObject &collision_object)
00493 {
00494
00495 gazebo::GetModelState state_srv;
00496
00497 state_srv.request.model_name = model_name;
00498 if (m_state_client.call(state_srv))
00499 {
00500 ROS_INFO("ModelPose (x,y,z): (%f,%f,%f)", state_srv.response.pose.position.x, state_srv.response.pose.position.y, state_srv.response.pose.position.z);
00501 }
00502 else
00503 {
00504 ROS_ERROR("Failed to call service get_model_state");
00505 return false;
00506 }
00507
00508 collision_object.id = model_name;
00509
00510
00511 collision_object.header.frame_id = frame_id;
00512 collision_object.header.stamp = ros::Time::now();
00513 collision_object.shapes.resize(1);
00514 collision_object.poses.resize(1);
00515
00516
00517
00518 collision_object.shapes[0].type = geometric_shapes_msgs::Shape::BOX;
00519 collision_object.shapes[0].dimensions.push_back(dimensions[0]/2.0);
00520 collision_object.shapes[0].dimensions.push_back(dimensions[1]/2.0);
00521 collision_object.shapes[0].dimensions.push_back(dimensions[2]/2.0);
00522
00523 collision_object.poses[0].position.x = state_srv.response.pose.position.x;
00524 collision_object.poses[0].position.y = state_srv.response.pose.position.y;
00525 collision_object.poses[0].position.z = state_srv.response.pose.position.z;
00526 collision_object.poses[0].orientation.x = state_srv.response.pose.orientation.x;
00527 collision_object.poses[0].orientation.y = state_srv.response.pose.orientation.y;
00528 collision_object.poses[0].orientation.z = state_srv.response.pose.orientation.z;
00529 collision_object.poses[0].orientation.w = state_srv.response.pose.orientation.w;
00530
00531 return true;
00532 }
00533
00534
00535
00536
00537
00538 };
00539
00540 #endif
