RosVisualization.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2013, Fraunhofer FKIE
00003  *
00004  * Authors: Bastian Gaspers
00005  *
00006  * Redistribution and use in source and binary forms, with or without
00007  * modification, are permitted provided that the following conditions are met:
00008  *
00009  * * Redistributions of source code must retain the above copyright
00010  *   notice, this list of conditions and the following disclaimer.
00011  * * Redistributions in binary form must reproduce the above copyright
00012  *   notice, this list of conditions and the following disclaimer in the
00013  *   documentation and/or other materials provided with the distribution.
00014  * * Neither the name of the Fraunhofer FKIE nor the names of its
00015  *   contributors may be used to endorse or promote products derived from
00016  *   this software without specific prior written permission.
00017  *
00018  * This file is part of the StructureColoring ROS package.
00019  *
00020  * The StructureColoring ROS package is free software:
00021  * you can redistribute it and/or modify it under the terms of the
00022  * GNU Lesser General Public License as published by the Free
00023  * Software Foundation, either version 3 of the License, or
00024  * (at your option) any later version.
00025  *
00026  * The StructureColoring ROS package is distributed in the hope that it will be useful,
00027  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00028  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
00029  * GNU Lesser General Public License for more details.
00030  *
00031  * You should have received a copy of the GNU Lesser General Public License
00032  * along with The StructureColoring ROS package.
00033  * If not, see <http://www.gnu.org/licenses/>.
00034  */
00035 
00036 #include <structureColoring/RosVisualization.h>
00037 #include <visualization_msgs/Marker.h>
00038 #include <sensor_msgs/PointCloud2.h>
00039 #include <pcl/filters/extract_indices.h>
00040 #include <cmath>
00041 
00042 /*****************************************************************************/
00043 
00044 RosVisualization::RosVisualization(ros::NodeHandle& nh){
00045         mHistogramPC2Publisher = nh.advertise<sensor_msgs::PointCloud2>("/segmentation/histogramspc2", 5);
00046         mPC2Publisher = nh.advertise<sensor_msgs::PointCloud2>("/segmentation/pc2", 5);
00047         mSegmentedCloudPublisher = nh.advertise<sensor_msgs::PointCloud2>("/segmentation/segmentedcloud2", 5);
00048         mMarkerPublisher = nh.advertise<visualization_msgs::Marker>("visualization_marker", 100);
00049         mCylinderMarkerPublisher = nh.advertise<visualization_msgs::Marker>("cylinder_marker", 100);
00050         mCylinderPointCloudPublisher = nh.advertise<sensor_msgs::PointCloud2>("/segmentation/cylinderPoints", 5);
00051         mFrameID = "/openni_rgb_optical_frame";
00052 }
00053 
00054 /*****************************************************************************/
00055 
00056 void RosVisualization::getColorByIndex(float *rgb, unsigned int index, unsigned int total){
00057         float t = 1 - (static_cast<float>(index)/static_cast<float>(total));
00058 // set rgb values
00059         if(t == 1.0){
00060                 rgb[0] = 1.0f;
00061                 rgb[1] = 0.0f;
00062                 rgb[2] = 0.0f;
00063         } else if (t < 1.0 && t >= 0.83333) {
00064                 rgb[0] = 1.0f;
00065                 rgb[1] = 1.0f-(t-0.83333)/0.16666;
00066                 rgb[2] = 0.0f;
00067         } else if (t < 0.83333 && t >= 0.66666) {
00068                 rgb[0] = (t-0.66666)/0.16666;
00069                 rgb[1] = 1.0f;
00070                 rgb[2] = 0.0f;
00071         } else if (t < 0.66666 && t >= 0.5) {
00072                 rgb[0] = 0.0f;
00073                 rgb[1] = 1.0f;
00074                 rgb[2] = 1.0f-(t-0.5)/0.16666;
00075         } else if (t < 0.5 && t >= 0.33333){
00076                 rgb[0] = 0.0f;
00077                 rgb[1] = 1.0f-(t-0.33333)/0.16666;
00078                 rgb[2] = 1.0f;
00079         } else if (t < 0.33333 && t >= 0.16666){
00080                 rgb[0] = (t-0.16666)/0.16666;
00081                 rgb[1] = 0.0f;
00082                 rgb[2] = 1.0f;
00083         } else {
00084                 rgb[0] = 1.0f;
00085                 rgb[1] = 0.0f;
00086                 rgb[2] = 1.0f-t/0.16666;
00087         }
00088 }
00089 
00090 /*****************************************************************************/
00091 
00092 void RosVisualization::publishPairMarker(const NodePairs& nodePairs){
00093         if (mCylinderMarkerPublisher.getNumSubscribers() == 0)return;
00094         visualization_msgs::Marker line_list;
00095         line_list.header.frame_id = mFrameID;
00096         line_list.header.stamp = ros::Time::now();
00097         line_list.action = visualization_msgs::Marker::ADD;
00098         line_list.pose.orientation.w = 1.0;
00099         line_list.ns = "pairs";
00100         line_list.id = 0;
00101         line_list.type = visualization_msgs::Marker::LINE_LIST;
00102         line_list.scale.x = 0.01;
00103         line_list.color.r = 1.0;
00104         line_list.color.g = 0.4;
00105         line_list.color.b = 0.0;
00106         line_list.color.a = 1.0;
00107         const Vec3& centerPoint = nodePairs[nodePairs.size() * 0.7].getFirst()->value_.meanPos;
00108         for(NodePairs::const_iterator np_it = nodePairs.begin(); np_it != nodePairs.end(); ++np_it){
00109                 if (np_it->getFirst()->value_.meanPos == centerPoint || np_it->getSecond()->value_.meanPos == centerPoint){
00110                         geometry_msgs::Point pn1, pn2;
00111                         Vec3 meanPos = np_it->getFirst()->value_.meanPos;
00112                         pn1.x = meanPos.x();
00113                         pn1.y = meanPos.y();
00114                         pn1.z = meanPos.z();
00115                         meanPos = np_it->getSecond()->value_.meanPos;
00116                         pn2.x = meanPos.x();
00117                         pn2.y = meanPos.y();
00118                         pn2.z = meanPos.z();
00119                         line_list.points.push_back(pn1);
00120                         line_list.points.push_back(pn2);
00121                 }
00122         }
00123         mCylinderMarkerPublisher.publish(line_list);
00124 }
00125 
00126 /*****************************************************************************/
00127 
00128 void RosVisualization::publishPlaneMarker(const PlanePatches& planes, const std::string& namePrefix){
00129         if (mMarkerPublisher.getNumSubscribers() == 0)return;
00130         visualization_msgs::Marker triangle_list;
00131         triangle_list.header.frame_id = mFrameID;
00132         triangle_list.header.stamp = ros::Time::now();
00133         triangle_list.action = visualization_msgs::Marker::ADD;
00134         triangle_list.pose.orientation.w = 1.0;
00135         triangle_list.id = 1;
00136         triangle_list.type = visualization_msgs::Marker::TRIANGLE_LIST;
00137         triangle_list.scale.x = 1.0;
00138         triangle_list.scale.y = 1.0;
00139         triangle_list.scale.z = 1.0;
00140         triangle_list.color.a = 0.4;
00141         visualization_msgs::Marker line_list;
00142         line_list.header.frame_id = mFrameID;
00143         line_list.header.stamp = ros::Time::now();
00144         line_list.action = visualization_msgs::Marker::ADD;
00145         line_list.pose.orientation.w = 1.0;
00146         line_list.ns = "plane normals";
00147         line_list.id = 0;
00148         line_list.type = visualization_msgs::Marker::LINE_LIST;
00149         line_list.scale.x = 0.01;
00150         line_list.color.r = 0.0;
00151         line_list.color.g = 0.0;
00152         line_list.color.b = 1.0;
00153         line_list.color.a = 1.0;
00154         unsigned int i = 0;
00155         for (PlanePatches::const_iterator planes_it = planes.begin(); planes_it != planes.end(); planes_it++){
00156                 float rgb[3]={0,0,0};
00157                 getColorByIndex(rgb, i, planes.size());
00158                 PlanePatch::Points brVertices=(*planes_it)->getBRVertices();
00159                 geometry_msgs::Point p0, p1, p2, p3;
00160                 p0.x = brVertices[0].x(); p0.y = brVertices[0].y(); p0.z = brVertices[0].z();
00161                 p1.x = brVertices[1].x(); p1.y = brVertices[1].y(); p1.z = brVertices[1].z();
00162                 p2.x = brVertices[2].x(); p2.y = brVertices[2].y(); p2.z = brVertices[2].z();
00163                 p3.x = brVertices[3].x(); p3.y = brVertices[3].y(); p3.z = brVertices[3].z();
00164                 triangle_list.points.push_back(p0);
00165                 triangle_list.points.push_back(p1);
00166                 triangle_list.points.push_back(p2);
00167                 triangle_list.points.push_back(p2);
00168                 triangle_list.points.push_back(p3);
00169                 triangle_list.points.push_back(p0);
00170                 triangle_list.color.r = rgb[0];
00171                 triangle_list.color.g = rgb[1];
00172                 triangle_list.color.b = rgb[2];
00173                 char ns2[255];
00174 //              sprintf(ns2, " %d (%.2f,%.2f,%.2f)", i, rgb[0], rgb[1], rgb[2]);
00175                 sprintf(ns2, " %d", i);
00176                 std::string name = namePrefix;
00177                 name.append(ns2);
00178                 triangle_list.ns = name.c_str();
00179                 mMarkerPublisher.publish(triangle_list);
00180                 triangle_list.points.clear();
00181                 i++;
00182 
00183                 geometry_msgs::Point pn1, pn2;
00184                 pn1.x = (*planes_it)->getPlaneCoG().x();
00185                 pn1.y = (*planes_it)->getPlaneCoG().y();
00186                 pn1.z = (*planes_it)->getPlaneCoG().z();
00187                 const Vec3& planeNormal((*planes_it)->getPlane3D().getPlaneNormal());
00188                 pn2.x = pn1.x + planeNormal.x() * 0.1;
00189                 pn2.y = pn1.y + planeNormal.y() * 0.1;
00190                 pn2.z = pn1.z + planeNormal.z() * 0.1;
00191                 line_list.points.push_back(pn1);
00192                 line_list.points.push_back(pn2);
00193         }
00194         mMarkerPublisher.publish(line_list);
00195 }
00196 
00197 /*****************************************************************************/
00198 
00199 void RosVisualization::publishCylinderMarker(const CylinderPatches& cylinders, const std::string& namePrefix){
00200         if (mCylinderMarkerPublisher.getNumSubscribers() == 0)return;
00201         visualization_msgs::Marker cylinder;
00202         cylinder.header.frame_id = mFrameID;
00203         cylinder.header.stamp = ros::Time::now();
00204         cylinder.action = visualization_msgs::Marker::ADD;
00205         cylinder.id = 0;
00206         cylinder.type = visualization_msgs::Marker::CYLINDER;
00207         cylinder.color.a = 0.7;
00208         unsigned int i = 0;
00209         for (CylinderPatches::const_iterator cit = cylinders.begin(); cit != cylinders.end(); cit++){
00210                 float rgb[3]={0,0,0};
00211                 getColorByIndex(rgb, i, cylinders.size());
00212                 cylinder.color.r = rgb[0];
00213                 cylinder.color.g = rgb[1];
00214                 cylinder.color.b = rgb[2];
00215                 cylinder.scale.x = (*cit)->getRadius() * 2.f;
00216                 cylinder.scale.y = (*cit)->getRadius() * 2.f;
00217                 cylinder.scale.z = (*cit)->getHeight();
00218 //              ROS_ERROR("angle: %f, axis %f %f %f", (*cit)->getRotationAngle(), (*cit)->getRotationAxis().x(), (*cit)->getRotationAxis().y(), (*cit)->getRotationAxis().z() );
00219                 Eigen::Quaternionf q((*cit)->getReTransformRot());
00220                 cylinder.pose.orientation.x = q.x();
00221                 cylinder.pose.orientation.y = q.y();
00222                 cylinder.pose.orientation.z = q.z();
00223                 cylinder.pose.orientation.w = q.w();
00224                 cylinder.pose.position.x = (*cit)->getMidPoint().x();
00225                 cylinder.pose.position.y = (*cit)->getMidPoint().y();
00226                 cylinder.pose.position.z = (*cit)->getMidPoint().z();
00227 //              ROS_INFO("Aufpunkt = (%f, %f, %f)", (*cit)->getPointOnAxis().x(), (*cit)->getPointOnAxis().y(), (*cit)->getPointOnAxis().z());
00228 //              ROS_INFO("Mittelpunkt = (%f, %f, %f)", (*cit)->getMidPoint().x(), (*cit)->getMidPoint().y(), (*cit)->getMidPoint().z());
00229 //              ROS_INFO("TopPoint = (%f, %f, %f)", (*cit)->getTopPoint().x(), (*cit)->getTopPoint().y(), (*cit)->getTopPoint().z());
00230                 char ns2[255];
00231 //              sprintf(ns2, " %d (%.2f,%.2f,%.2f)", i, rgb[0], rgb[1], rgb[2]);
00232                 sprintf(ns2, " %d", i);
00233                 std::string name = namePrefix;
00234                 name.append(ns2);
00235                 cylinder.ns = name.c_str();
00236                 mCylinderMarkerPublisher.publish(cylinder);
00237                 i++;
00238         }
00239 }
00240 
00241 /*****************************************************************************/
00242 
00243 void RosVisualization::publishOctreeNormalMarker(const OcTree& octree){
00244         if (mMarkerPublisher.getNumSubscribers() == 0)return;
00245         visualization_msgs::Marker line_list, cube_list, line_list_red;
00246         line_list.header.frame_id = cube_list.header.frame_id = line_list_red.header.frame_id = mFrameID;
00247         line_list.header.stamp = cube_list.header.stamp = line_list_red.header.stamp = ros::Time::now();
00248         line_list.action = cube_list.action = line_list_red.action = visualization_msgs::Marker::ADD;
00249         line_list.pose.orientation.w = cube_list.pose.orientation.w = line_list_red.pose.orientation.w = 1.0;
00250         line_list.id = 5;
00251         line_list.type = visualization_msgs::Marker::LINE_LIST;
00252         line_list.scale.x = 0.005;
00253         line_list.scale.y = 0.005;
00254         line_list.color.r = 0; line_list.color.g = 1; line_list.color.b = 0;
00255         line_list.color.a = 1;
00256         cube_list.id = 3;
00257         cube_list.type = visualization_msgs::Marker::CUBE_LIST;
00258         cube_list.color.r = 0.4;
00259         cube_list.color.g = 0.4;
00260         cube_list.color.b = 0.4;
00261         cube_list.color.a = 0.1;
00262         line_list_red.id = 7;
00263         line_list_red.type = visualization_msgs::Marker::CUBE_LIST;
00264         line_list_red.scale.x = 0.005;
00265         line_list_red.scale.y = 0.005;
00266         line_list_red.color.r = 1; line_list_red.color.g = 0; line_list_red.color.b = 0;
00267         line_list_red.color.a = 1;
00268         line_list_red.ns = "bad normals";
00269         for (unsigned int d = 0; d <= octree.getMaxDepth(); d++){
00270 //              if (d == 10){
00271 //                      cube_list.color.r = 1.0;
00272 //                      cube_list.color.g = 0.0;
00273 //                      cube_list.color.b = 0.0;
00274 //              }
00275                 const NodePointerList& nodes = octree.getSamplingMap().find(d)->second;
00276 //markers namespaces
00277                 char ns1[255];
00278                 sprintf(ns1, "cubes on depth %u", d);
00279                 cube_list.ns = ns1;
00280                 char ns3[255];
00281                 sprintf(ns3, "normals on depth %u", d);
00282                 line_list.ns = ns3;
00283 
00284                 for (NodePointerList::const_iterator node_it = nodes.begin(); node_it != nodes.end(); ++node_it){
00285             if ((*node_it)->depth_ != (int)d)
00286                 continue;
00287                         float scale = octree.getCellSizeFromDepth(d); //should be the same for all three coordinate axis
00288                         cube_list.scale.x = scale;
00289                         cube_list.scale.y = scale;
00290                         cube_list.scale.z = scale;
00291 
00292                         geometry_msgs::Point p, p1;
00293                         Eigen::Vector4f pos = (*node_it)->pos_key_.getPosition(octree.getOctreePtr());
00294                         p.x = pos(0);
00295                         p.y = pos(1);
00296                         p.z = pos(2);
00297                         cube_list.points.push_back(p);
00298 //center of gravity
00299                         Eigen::Vector3f cog_vec = (*node_it)->value_.meanPos;
00300                         p.x = cog_vec.x();
00301                         p.y = cog_vec.y();
00302                         p.z = cog_vec.z();
00303 //                      cube_list.scale.x = .5f;
00304 //                      cube_list.scale.y = .5f;
00305 //                      cube_list.scale.z = .5f;
00306 //                      cube_list.points.push_back(p);
00307 //                      std_msgs::ColorRGBA c;
00308 //                      c.r = 1.f;//1.f - octreeSamplingMap[d][i]->value.curv;
00309 //                      c.g = 0.f;//octreeSamplingMap[d][i]->value.curv;
00310 //                      c.b = 0.f;
00311 //                      cube_list.colors.push_back(c);
00312                         if ((*node_it)->value_.stable){
00313                                 if ((std::isfinite((*node_it)->value_.normal.x()))&&
00314                                                 (std::isfinite((*node_it)->value_.normal.y()))&&
00315                                                 (std::isfinite((*node_it)->value_.normal.z()))){
00316                                         p1.x = p.x + (*node_it)->value_.normal.x() * scale;
00317                                         p1.y = p.y + (*node_it)->value_.normal.y() * scale;
00318                                         p1.z = p.z + (*node_it)->value_.normal.z() * scale;
00319                                         line_list.points.push_back(p); line_list.points.push_back(p1);
00320                                 }
00321                         } else {
00322                                 if(::calculateNormal(*node_it, Vec3(0.f, 0.f, 0.f), 20, std::numeric_limits<float>::max(), 0.f, 0.f)){
00323                                         if ((std::isfinite((*node_it)->value_.normal.x()))&&
00324                                                         (std::isfinite((*node_it)->value_.normal.y()))&&
00325                                                         (std::isfinite((*node_it)->value_.normal.z()))){
00326                                                 p1.x = p.x + (*node_it)->value_.normal.x() * scale;
00327                                                 p1.y = p.y + (*node_it)->value_.normal.y() * scale;
00328                                                 p1.z = p.z + (*node_it)->value_.normal.z() * scale;
00329                                                 line_list_red.points.push_back(p); line_list_red.points.push_back(p1);
00330                                         }
00331                                 }
00332                         }
00333                 }
00334 //publish marker
00335                 if ((d <= 10) && (!cube_list.points.empty()))
00336                         mMarkerPublisher.publish(cube_list);
00337                 cube_list.points.clear();
00338                 mMarkerPublisher.publish(line_list);
00339                 line_list.points.clear();
00340                 mMarkerPublisher.publish(line_list_red);
00341                 line_list_red.points.clear();
00342         }
00343 
00344 }
00345 
00346 /*****************************************************************************/
00347 
00348 void RosVisualization::publishHistogramMarker(const SphereUniformSampling& HTBins, const float& scale, const Eigen::Vector3f& translation){
00349         if (mMarkerPublisher.getNumSubscribers() == 0)return;
00350         visualization_msgs::Marker arrow;
00351         arrow.header.frame_id = mFrameID;
00352         arrow.header.stamp = ros::Time::now();
00353         arrow.action = visualization_msgs::Marker::ADD;
00354         arrow.pose.orientation.w = 1.0;
00355         arrow.id = 128;
00356         arrow.type = visualization_msgs::Marker::ARROW;
00357         arrow.scale.x = 0.005;
00358         arrow.scale.y = arrow.scale.x;
00359         arrow.color.a = 1;
00360         std::string ns = "histogram arrows";
00361         pcl::PointCloud<pcl::PointXYZRGB>::Ptr pc(new pcl::PointCloud<pcl::PointXYZRGB>);
00362         HTBins.getHistogramAsPointCloud(*pc, scale, Eigen::Vector3f(0.f, 0.f, 0.f));
00363         geometry_msgs::Point p1, p2;
00364         for(size_t pidx = 0; pidx < pc->points.size(); ++pidx){
00365                 char trans[50];
00366                 sprintf(trans, " trans=(%f, %f, %f)", translation.x(), translation.y(), translation.z());
00367                 char dir[50];
00368                 sprintf(dir, " dir=(%f, %f, %f)", pc->points[pidx].x, pc->points[pidx].y, pc->points[pidx].z);
00369                 arrow.ns = ns.append(trans).append(dir);
00370                 float rgbf = pc->points[pidx].rgb;
00371                 unsigned int rgb = *(unsigned int*)&rgbf;
00372                 ROS_ERROR("rgbuint = %u", rgb);
00373                 arrow.color.r = 2.f * ((rgb & 0xff0000) >> 16)/255.0;
00374                 arrow.color.g = 2.f * ((rgb & 0xff00) >> 8)/255.0;
00375                 arrow.color.b = 2.f * (rgb & 0xff)/255.0;
00376                 ROS_ERROR("r=%u g=%u b=%u", rgb & 0xff, rgb & 0xff00, rgb & 0xff0000);
00377                 ROS_ERROR("r=%f g=%f b=%f", arrow.color.r, arrow.color.g, arrow.color.b);
00378                 p1.x = translation.x();
00379                 p1.y = translation.y();
00380                 p1.z = translation.z();
00381                 p2.x = p1.x + 2.f * pc->points[pidx].x;
00382                 p2.y = p1.y + 2.f * pc->points[pidx].y;
00383                 p2.z = p1.z + 2.f * pc->points[pidx].z;
00384                 arrow.points.push_back(p1);
00385                 arrow.points.push_back(p2);
00386                 mMarkerPublisher.publish(arrow);
00387                 arrow.points.clear();
00388         }
00389 }
00390 
00391 /*****************************************************************************/
00392 
00393 void RosVisualization::publishHTBinCloud(const SphereUniformSamplings& HTBins){
00394         if (mHistogramPC2Publisher.getNumSubscribers() == 0)return;
00395         pcl::PointCloud<pcl::PointXYZRGB> newCloud;
00396         for(unsigned int i = 0; i < HTBins.size(); i++){
00397                 HTBins[i].getHistogramAsPointCloud(newCloud, .1f, Eigen::Vector3f(.25f * i, 0, 0));
00398         }
00399         sensor_msgs::PointCloud2 newMsg;
00400         pcl::toROSMsg<pcl::PointXYZRGB>(newCloud, newMsg);
00401         newMsg.header.frame_id=mFrameID;
00402         newMsg.header.stamp=ros::Time::now();
00403         mHistogramPC2Publisher.publish(newMsg);
00404 }
00405 
00406 /*****************************************************************************/
00407 
00408 void RosVisualization::publishHTBinCloud(pcl::PointCloud<pcl::PointXYZRGB> histogramCloud){
00409         if (mHistogramPC2Publisher.getNumSubscribers() == 0)return;
00410         sensor_msgs::PointCloud2 newMsg;
00411         pcl::toROSMsg<pcl::PointXYZRGB>(histogramCloud, newMsg);
00412         newMsg.header.frame_id=mFrameID;
00413         newMsg.header.stamp=ros::Time::now();
00414         mHistogramPC2Publisher.publish(newMsg);
00415 }
00416 
00417 /*****************************************************************************/
00418 
00419 /*****************************************************************************/
00420 
00421 void RosVisualization::publishCylinderPoints(const CylinderPatches& cylinders, const boost::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB> > inputPC){
00422         if (mCylinderPointCloudPublisher.getNumSubscribers() == 0)return;
00423         pcl::PointCloud<pcl::PointXYZRGB> pointCloud;
00424 
00425         pcl::PointCloud<pcl::PointXYZRGB> tmpCloud;
00426         pcl::ExtractIndices<pcl::PointXYZRGB> extract;
00427         extract.setInputCloud(inputPC);
00428         //fill cylinders into PointCloud
00429         unsigned int i = 0;
00430         for(CylinderPatches::const_iterator cit = cylinders.begin(); cit != cylinders.end(); ++cit){
00431                 pcl::PointIndices::Ptr pointIndices(new pcl::PointIndices());
00432                 std::copy((*cit)->getInliers().begin(), (*cit)->getInliers().end(), std::back_inserter(pointIndices->indices));
00433                 extract.setIndices(pointIndices);
00434                 extract.filter(tmpCloud);
00435                 float rgb[3];
00436                 getColorByIndex(rgb, i, cylinders.size());
00437                 unsigned int red = rgb[0] * 255.f + 0.5f;
00438                 unsigned int green = rgb[1] * 255.f + 0.5f;
00439                 unsigned int blue = rgb[2] * 255.f + 0.5f;
00440                 unsigned int color = blue + (green<<8) + (red<<16);
00441                 for(unsigned int pit = 0; pit < tmpCloud.points.size(); ++pit){
00442                         tmpCloud.points[pit].rgb = *(float*)&color;
00443                 }
00444                 pointCloud.header = tmpCloud.header;
00445                 pointCloud += tmpCloud;
00446                 i++;
00447         }
00448 
00449         sensor_msgs::PointCloud2 segMsg;
00450         pcl::toROSMsg<PointT>(pointCloud, segMsg);
00451         segMsg.header.frame_id = mFrameID;
00452         segMsg.header.stamp=ros::Time::now();
00453         mCylinderPointCloudPublisher.publish(segMsg);
00454 }
00455 
00456 /*****************************************************************************/
00457 
00458 void RosVisualization::publishPointCloud(const PointCloud& pointCloud){
00459         if (mPC2Publisher.getNumSubscribers() == 0)return;
00460         sensor_msgs::PointCloud2 segMsg;
00461         pcl::toROSMsg<PointT>(pointCloud, segMsg);
00462         segMsg.header.frame_id=mFrameID;
00463         segMsg.header.stamp=ros::Time::now();
00464         mPC2Publisher.publish(segMsg);
00465 }
00466 
00467 /*****************************************************************************/
00468 
00469 void RosVisualization::publishPoints(const Points& points, const std::string& nameAppendix, const Vec3& color){
00470         if (mMarkerPublisher.getNumSubscribers() == 0)return;
00471         visualization_msgs::Marker point_list;
00472         point_list.header.frame_id = mFrameID;
00473         point_list.header.stamp = ros::Time::now();
00474         point_list.action = visualization_msgs::Marker::ADD;
00475         point_list.pose.orientation.w = 1.0f;
00476         point_list.id = 10;
00477         point_list.type = visualization_msgs::Marker::POINTS;
00478         point_list.scale.x = 0.02f;
00479         point_list.scale.y = 0.02f;
00480         point_list.scale.z = 0.02f;
00481         point_list.color.a = 0.7f;
00482         point_list.color.r = color.x();
00483         point_list.color.g = color.y();
00484         point_list.color.b = color.z();
00485         char ns[255];
00486         sprintf(ns, "PlanePoints %s", nameAppendix.c_str());
00487         point_list.ns = ns;
00488         for (Points::const_iterator pit = points.begin(); pit != points.end(); ++pit){
00489                 geometry_msgs::Point p;
00490                 p.x = pit->x();
00491                 p.y = pit->y();
00492                 p.z = pit->z();
00493                 point_list.points.push_back(p);
00494         }
00495         mMarkerPublisher.publish(point_list);
00496 }
00497 
00498 /*****************************************************************************/
00499 
00500 void RosVisualization::publishCylinderPointsNormals(const PointCloud& partCloud, const pcl::PointCloud<pcl::Normal>& normalCloud, const std::string& name){
00501         if (mMarkerPublisher.getNumSubscribers() == 0)return;
00502         visualization_msgs::Marker line_list;
00503         line_list.header.frame_id = mFrameID;
00504         line_list.header.stamp = ros::Time::now();
00505         line_list.action = visualization_msgs::Marker::ADD;
00506         line_list.pose.orientation.w = 1.0;
00507         line_list.id = 26;
00508         line_list.type = visualization_msgs::Marker::LINE_LIST;
00509         line_list.scale.x = 0.0005;
00510         line_list.scale.y = 0.0005;
00511         line_list.color.r = 0; line_list.color.g = 1; line_list.color.b = 0;
00512         line_list.color.a = 1;
00513         std::string linePrefix = "line ";
00514         linePrefix.append(name);
00515         line_list.ns = linePrefix;
00516         visualization_msgs::Marker point_list;
00517         point_list.header.frame_id = mFrameID;
00518         point_list.header.stamp = ros::Time::now();
00519         point_list.action = visualization_msgs::Marker::ADD;
00520         point_list.pose.orientation.w = 1.0f;
00521         point_list.id = 25;
00522         point_list.type = visualization_msgs::Marker::POINTS;
00523         point_list.scale.x = 0.002f;
00524         point_list.scale.y = 0.002f;
00525         point_list.scale.z = 0.002f;
00526         point_list.color.a = 0.7f;
00527         point_list.color.r = 0.f;
00528         point_list.color.g = 1.f;
00529         point_list.color.b = 0.5f;
00530         std::string pointPrefix = "point ";
00531         pointPrefix.append(name);
00532         point_list.ns = pointPrefix;
00533         for (unsigned int i = 0; i < partCloud.points.size(); ++i){
00534 //              if(i % 20 == 0){
00535                 geometry_msgs::Point p, pn;
00536                 p.x = partCloud.points[i].x;
00537                 p.y = partCloud.points[i].y;
00538                 p.z = partCloud.points[i].z;
00539                 point_list.points.push_back(p);
00540                 pn.x = p.x + normalCloud.points[i].normal_x * 0.02f;
00541                 pn.y = p.y + normalCloud.points[i].normal_y * 0.02f;
00542                 pn.z = p.z + normalCloud.points[i].normal_z * 0.02f;
00543                 line_list.points.push_back(p); line_list.points.push_back(pn);
00544 //              }
00545         }
00546         mMarkerPublisher.publish(point_list);
00547         mMarkerPublisher.publish(line_list);
00548 }
00549 
00550 /*****************************************************************************/
00551 
00552 void RosVisualization::printTopics()
00553 {
00554     ROS_INFO("Fixed Frame: %s\n\tHistogramPC2: %s\n\tInputPC2: %s\n\tSegmentedPlanesPC2: %s\n\tSegmentedCylindersPC2: %s\n\tMarkers: %s\n\tCylinderMarkers: %s", mFrameID.c_str(), mHistogramPC2Publisher.getTopic().c_str(), mPC2Publisher.getTopic().c_str(), mSegmentedCloudPublisher.getTopic().c_str(), mCylinderPointCloudPublisher.getTopic().c_str(), mMarkerPublisher.getTopic().c_str(), mCylinderMarkerPublisher.getTopic().c_str());
00555 }
00556

structure_coloring_fkie
Author(s): Bastian Gaspers
autogenerated on Sun Jan 5 2014 11:38:09