cob_arm_navigation: addObject.cpp Source File

00001 
00061 #include <ros/ros.h>
00062 #include <stdio.h>
00063 #include <stdlib.h>
00064 
00065 #include <mapping_msgs/CollisionObject.h>
00066 //#include <tf/tf.h>
00067 #include <gazebo/GetModelState.h>
00068 
00069 
00070 
00071 int main(int argc, char** argv) {
00072 
00073   ros::init(argc, argv, "addObject");
00074   //const std::string frame_id = "/base_footprint";
00075   const std::string frame_id = "/map";
00076 
00077   ros::NodeHandle nh;
00078 
00079   ros::Publisher object_in_map_pub_;
00080   object_in_map_pub_  = nh.advertise<mapping_msgs::CollisionObject>("collision_object", 20);
00081   
00082 
00083   if (argc != 3){
00084     ROS_ERROR("Need a parameter name (1) and a model_name (2)");
00085     return -1;
00086   }
00087   std::string parameter_name = argv[1];
00088   std::string model_name = argv[2];
00089   ROS_INFO("Model-Name: %s", model_name.c_str());
00090 
00091   
00092   while(!nh.hasParam(parameter_name))
00093   {
00094           ROS_WARN("waiting for parameter \"%s\"... ", parameter_name.c_str());
00095           ros::Duration(0.5).sleep();
00096   }
00097   
00098   std::string model_parameter;
00099   if (nh.getParam(parameter_name, model_parameter))
00100   {
00101         ROS_INFO("Getting parameter successful!");
00102         ROS_DEBUG("Parameter: %s", model_parameter.c_str());
00103   }
00104 
00105 
00106   //parsing-scheme only valid for "box" (size: x,y,z)
00107   std::string pattern;
00108   std::size_t found_box, found_size, found_p, found_x, found_y, found_z;
00109   
00110   pattern = "geom:box";
00111   found_box=model_parameter.find(pattern);
00112   if (found_box!=std::string::npos)
00113     ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_box));
00114   else
00115   {
00116           ROS_ERROR("%s not found", pattern.c_str());
00117           ROS_ERROR("I can't parse this model. Aborting!");
00118           return -1;
00119   }
00120   
00121   
00122   
00123   pattern = "size";
00124   found_size=model_parameter.find(pattern);
00125   if (found_size!=std::string::npos)
00126     ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_size));
00127   else
00128         ROS_ERROR("%s not found", pattern.c_str());
00129 
00130   pattern = ">";
00131   found_p=model_parameter.find(pattern, found_size);
00132   if (found_p!=std::string::npos)
00133     ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_p));
00134   else
00135         ROS_ERROR("%s not found", pattern.c_str());
00136 
00137   pattern = " ";
00138   found_x=model_parameter.find_first_not_of(pattern, found_p+1);
00139   if (found_x!=std::string::npos)
00140     ROS_DEBUG("first not \"%s\" found at: %d", pattern.c_str(), int(found_x));
00141   else
00142         ROS_ERROR("%s not found", pattern.c_str());
00143 
00144   pattern = " ";        
00145   found_p=model_parameter.find_first_of(pattern, found_x);
00146   if (found_p!=std::string::npos)
00147     ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_p));
00148   else
00149         ROS_ERROR("%s not found", pattern.c_str());
00150 
00151   size_t length_x = found_p - found_x;
00152   std::string x = model_parameter.substr(found_x, length_x);
00153   ROS_DEBUG("x: %s, real_length_x: %d", x.c_str(), int(length_x));
00154 
00155   double x_d = strtod(x.c_str(), NULL);
00156   ROS_INFO("x as double: %f", x_d);
00157  
00158  
00159  
00160   pattern = " ";
00161   found_y=model_parameter.find_first_not_of(pattern, found_p);
00162   if (found_y!=std::string::npos)
00163     ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_y));
00164   else
00165         ROS_ERROR("%s not found", pattern.c_str());
00166         
00167   found_p=model_parameter.find_first_of(pattern, found_y);
00168   if (found_p!=std::string::npos)
00169     ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_p));
00170   else
00171         ROS_ERROR("%s not found", pattern.c_str());
00172   
00173   size_t length_y = found_p - found_y;
00174   std::string y = model_parameter.substr(found_y, length_y);
00175   ROS_DEBUG("y: %s, real_length_y: %d", y.c_str(), int(length_y));
00176 
00177   double y_d = strtod(y.c_str(), NULL);
00178   ROS_INFO("y as double: %f", y_d);
00179   
00180   
00181   
00182   
00183   pattern = " ";
00184   found_z=model_parameter.find_first_not_of(pattern, found_p);
00185   if (found_z!=std::string::npos)
00186     ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_z));
00187   else
00188         ROS_ERROR("%s not found", pattern.c_str());
00189         
00190   pattern = "<";        
00191   found_p=model_parameter.find_first_of(pattern, found_z);
00192   if (found_p!=std::string::npos)
00193     ROS_DEBUG("first \"%s\" found at: %d", pattern.c_str(), int(found_p));
00194   else
00195         ROS_ERROR("%s not found", pattern.c_str());
00196   
00197   size_t length_z = found_p - found_z;
00198   std::string z = model_parameter.substr(found_z, length_z);
00199   ROS_DEBUG("z: %s, real_length_z: %d", z.c_str(), int(length_z));
00200 
00201   double z_d = strtod(z.c_str(), NULL);
00202   ROS_INFO("z as double: %f", z_d);
00203   
00204   
00205   
00206   
00207   
00208   
00209   
00210   
00211   //rosservice call /gazebo/get_model_state milk_box cob3::base_link
00212   ROS_INFO("waiting for service /gazebo/get_model_state....");
00213   ros::service::waitForService("/gazebo/get_model_state");
00214   
00215   ros::ServiceClient state_client = nh.serviceClient<gazebo::GetModelState>("/gazebo/get_model_state");
00216   
00217   gazebo::GetModelState state_srv;
00218 
00219   state_srv.request.model_name = model_name;
00220   if (state_client.call(state_srv))
00221   {
00222         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);
00223   }
00224   else
00225   {
00226         ROS_ERROR("Failed to call service get_model_state");
00227         ros::shutdown();
00228   }
00229   
00230   
00231   
00232   
00233   //add to environment_server as a known obstacle
00234   mapping_msgs::CollisionObject collision_object;
00235   collision_object.id = model_name + "_object";
00236   collision_object.operation.operation = mapping_msgs::CollisionObjectOperation::ADD;
00237   //collision_object.operation.operation = mapping_msgs::CollisionObjectOperation::REMOVE;
00238   collision_object.header.frame_id = frame_id;
00239   collision_object.header.stamp = ros::Time::now();
00240   collision_object.shapes.resize(1);
00241   collision_object.poses.resize(1);
00242   
00243   //ToDo: figure out how *.model-size and *.urdf-extend are related
00244   //ToDo: figure out where the *.model origin is located (top,center,bottom?)
00245   collision_object.shapes[0].type = geometric_shapes_msgs::Shape::BOX;
00246   collision_object.shapes[0].dimensions.push_back(x_d/2.0);
00247   collision_object.shapes[0].dimensions.push_back(y_d/2.0);
00248   collision_object.shapes[0].dimensions.push_back(z_d/2.0);
00249   
00250   collision_object.poses[0].position.x = state_srv.response.pose.position.x;
00251   collision_object.poses[0].position.y = state_srv.response.pose.position.y;
00252   collision_object.poses[0].position.z = state_srv.response.pose.position.z;
00253   collision_object.poses[0].orientation.x = state_srv.response.pose.orientation.x;
00254   collision_object.poses[0].orientation.y = state_srv.response.pose.orientation.y;
00255   collision_object.poses[0].orientation.z = state_srv.response.pose.orientation.z;
00256   collision_object.poses[0].orientation.w = state_srv.response.pose.orientation.w;
00257   
00258   
00259   
00260   
00261   object_in_map_pub_.publish(collision_object);
00262         
00263   ROS_INFO("Should have published");
00264  
00265   ros::shutdown();
00266 }
00267 
00268 
00269