ros_msg_conversions.h
Go to the documentation of this file.00001 00060 #ifndef ROS_MSG_CONV_H_ 00061 #define ROS_MSG_CONV_H_ 00062 00063 #include <cob_3d_mapping_msgs/Shape.h> 00064 #include <cob_3d_mapping_msgs/ShapeArray.h> 00065 #include "cob_3d_mapping_common/polygon.h" 00066 #include "cob_3d_mapping_common/cylinder.h" 00067 00068 namespace cob_3d_mapping 00069 { 00080 inline void 00081 toROSMsg(const Polygon& p, cob_3d_mapping_msgs::Shape& s) 00082 { 00083 s.id = p.id; 00084 s.type = cob_3d_mapping_msgs::Shape::POLYGON; 00085 00086 s.params.resize(4); 00087 s.params[0] = p.normal(0); 00088 s.params[1] = p.normal(1); 00089 s.params[2] = p.normal(2); 00090 s.params[3] = p.d; 00091 s.centroid.x = p.centroid(0); 00092 s.centroid.y = p.centroid(1); 00093 s.centroid.z = p.centroid(2); 00094 s.color.r = p.color[0]; 00095 s.color.g = p.color[1]; 00096 s.color.b = p.color[2]; 00097 s.color.a = p.color[3]; 00098 00099 s.points.resize(p.contours.size()); 00100 s.holes.resize(p.holes.size()); 00101 for(unsigned int i=0; i<p.contours.size(); i++) 00102 { 00103 s.holes[i] = p.holes[i]; 00104 //s.points[i].points.resize(p.contours[i].size()); 00105 pcl::PointCloud<pcl::PointXYZ> cloud; 00106 for(unsigned int j=0; j<p.contours[i].size(); j++) 00107 { 00108 pcl::PointXYZ pt; 00109 pt.x = p.contours[i][j](0); 00110 pt.y = p.contours[i][j](1); 00111 pt.z = p.contours[i][j](2); 00112 cloud.points.push_back(pt); 00113 /*s.points[i].points[j].x = p.contours[i][j](0); 00114 s.points[i].points[j].y = p.contours[i][j](1); 00115 s.points[i].points[j].z = p.contours[i][j](2);*/ 00116 } 00117 sensor_msgs::PointCloud2 cloud_msg; 00118 pcl::toROSMsg(cloud, cloud_msg); 00119 s.points[i]= cloud_msg; 00120 } 00121 } 00122 00133 inline bool 00134 fromROSMsg(const cob_3d_mapping_msgs::Shape& s, Polygon& p) 00135 { 00136 p.id = s.id; 00137 p.centroid(0) = s.centroid.x; 00138 p.centroid(1) = s.centroid.y; 00139 p.centroid(2) = s.centroid.z; 00140 p.normal(0) = s.params[0]; 00141 p.normal(1) = s.params[1]; 00142 p.normal(2) = s.params[2]; 00143 p.d = s.params[3]; 00144 //std::cout << "normal: " << p.normal(0) << "," << p.normal(1) << "," << p.normal(2) << std::endl; 00145 //std::cout << "d: " << p.d << std::endl << std::endl; 00146 p.merged = 1; 00147 p.color[0] = s.color.r; 00148 p.color[1] = s.color.g; 00149 p.color[2] = s.color.b; 00150 p.color[3] = s.color.a; 00151 //p.contours.resize(p.polygons.size()); 00152 for(unsigned int i=0; i<s.points.size(); i++) 00153 { 00154 p.holes.push_back(s.holes[i]); 00155 if(s.points[i].data.size()) 00156 { 00157 pcl::PointCloud<pcl::PointXYZ> cloud; 00158 pcl::fromROSMsg(s.points[i], cloud); 00159 std::vector<Eigen::Vector3f> pts; 00160 pts.resize(cloud.points.size()); 00161 for(unsigned int j=0; j<cloud.points.size(); j++) 00162 { 00163 /*pts[j] = Eigen::Vector3f(p.polygons[i].points[j].x, 00164 p.polygons[i].points[j].y, 00165 p.polygons[i].points[j].z);*/ 00166 pts[j](0) = cloud.points[j].x; 00167 pts[j](1) = cloud.points[j].y; 00168 pts[j](2) = cloud.points[j].z; 00169 } 00170 p.contours.push_back(pts); 00171 } 00172 else 00173 { 00174 std::cout << "shape has no points" << std::endl; 00175 return false; 00176 } 00177 } 00178 return true; 00179 } 00180 00181 inline void 00182 toROSMsg(const Cylinder& c, cob_3d_mapping_msgs::Shape& s) 00183 { 00184 00185 00186 s.params.resize(10); 00187 // x axis 00188 00189 00190 s.params[0]=c.normal[0]; 00191 s.params[1]=c.normal[1]; 00192 s.params[2]=c.normal[2]; 00193 00194 s.params[3]=c.sym_axis[0]; 00195 s.params[4]=c.sym_axis[1]; 00196 s.params[5]=c.sym_axis[2]; 00197 00198 s.params[6]=c.origin_[0]; 00199 s.params[7]=c.origin_[1]; 00200 s.params[8]=c.origin_[2]; 00201 00202 s.params[9]=c.r_; 00203 00204 s.centroid.x = c.centroid[0]; 00205 s.centroid.y = c.centroid[1]; 00206 s.centroid.z = c.centroid[2]; 00207 00208 00209 00210 00211 00212 //std::cout << "normal: " << p.normal(0) << "," << p.normal(1) << "," << p.normal(2) << std::endl; 00213 //std::cout << "d: " << p.d << std::endl << std::endl; 00214 00215 s.id=c.id; 00216 s.type = cob_3d_mapping_msgs::Shape::CYLINDER; 00217 s.color.r=c.color[0]; 00218 s.color.g=c.color[1]; 00219 s.color.b=c.color[2]; 00220 s.color.a=c.color[3]; 00221 00222 00223 s.points.resize(c.contours.size()); 00224 s.holes.resize(c.holes.size()); 00225 for(unsigned int i=0; i<c.contours.size(); i++) 00226 { 00227 s.holes[i] = c.holes[i]; 00228 //s.points[i].points.resize(p.contours[i].size()); 00229 pcl::PointCloud<pcl::PointXYZ> cloud; 00230 for(unsigned int j=0; j<c.contours[i].size(); j++) 00231 { 00232 pcl::PointXYZ pt; 00233 pt.x = c.contours[i][j](0); 00234 pt.y = c.contours[i][j](1); 00235 pt.z = c.contours[i][j](2); 00236 cloud.points.push_back(pt); 00237 00238 } 00239 sensor_msgs::PointCloud2 cloud_msg; 00240 00241 pcl::toROSMsg(cloud, cloud_msg); 00242 s.points[i]= cloud_msg; 00243 } 00244 00245 00246 00247 00248 } 00249 00250 00251 00252 inline bool 00253 fromROSMsg(const cob_3d_mapping_msgs::Shape& s,Cylinder& c){ 00254 00255 00256 00257 c.id = s.id; 00258 // c.centroid(0) = s.centroid.x; 00259 // c.centroid(1) = s.centroid.y; 00260 // c.centroid(2) = s.centroid.z; 00261 00262 c.normal[0] = s.params[0]; 00263 c.normal[1] = s.params[1]; 00264 c.normal[2] = s.params[2]; 00265 00266 c.sym_axis[0]= s.params[3]; 00267 c.sym_axis[1] = s.params[4]; 00268 c.sym_axis[2] = s.params[5]; 00269 00270 c.origin_[0] = s.params[6]; 00271 c.origin_[1] = s.params[7]; 00272 c.origin_[2] = s.params[8]; 00273 c.r_ = s.params[9]; 00274 00275 c.centroid[0]=s.centroid.x; 00276 c.centroid[1]=s.centroid.y; 00277 c.centroid[2]=s.centroid.z; 00278 00279 00280 //std::cout << "normal: " << p.normal(0) << "," << p.normal(1) << "," << p.normal(2) << std::endl; 00281 //std::cout << "d: " << p.d << std::endl << std::endl; 00282 c.merged = 1; 00283 c.color[0] = s.color.r; 00284 c.color[1] = s.color.g; 00285 c.color[2] = s.color.b; 00286 c.color[3] = s.color.a; 00287 //p.contours.resize(p.polygons.size()); 00288 for(unsigned int i=0; i<s.points.size(); i++) 00289 { 00290 c.holes.push_back(s.holes[i]); 00291 if(s.points[i].data.size()) 00292 { 00293 pcl::PointCloud<pcl::PointXYZ> cloud; 00294 pcl::fromROSMsg(s.points[i], cloud); 00295 00296 00297 std::vector<Eigen::Vector3f> pts; 00298 pts.resize(cloud.points.size()); 00299 for(unsigned int j=0; j<cloud.points.size(); j++) 00300 { 00301 /*pts[j] = Eigen::Vector3f(p.polygons[i].points[j].x, 00302 p.polygons[i].points[j].y, 00303 p.polygons[i].points[j].z);*/ 00304 pts[j](0) = cloud.points[j].x; 00305 pts[j](1) = cloud.points[j].y; 00306 pts[j](2) = cloud.points[j].z; 00307 } 00308 c.contours.push_back(pts); 00309 } 00310 else 00311 { 00312 std::cout << "cylinder has no points" << std::endl; 00313 return false; 00314 } 00315 } 00316 return true; 00317 00318 00319 00320 } 00321 00322 00323 00324 00325 } 00326 00327 #endif 00328