rgbd_assembler: ping_rgbd_assembler

00001 
00002 /*********************************************************************
00003 *
00004 *  Copyright (c) 2009, Willow Garage, Inc.
00005 *  All rights reserved.
00006 *
00007 *  Redistribution and use in source and binary forms, with or without
00008 *  modification, are permitted provided that the following conditions
00009 *  are met:
00010 *
00011 *   * Redistributions of source code must retain the above copyright
00012 *     notice, this list of conditions and the following disclaimer.
00013 *   * Redistributions in binary form must reproduce the above
00014 *     copyright notice, this list of conditions and the following
00015 *     disclaimer in the documentation and/or other materials provided
00016 *     with the distribution.
00017 *   * Neither the name of the Willow Garage nor the names of its
00018 *     contributors may be used to endorse or promote products derived
00019 *     from this software without specific prior written permission.
00020 *
00021 *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022 *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023 *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024 *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025 *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026 *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027 *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028 *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029 *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030 *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031 *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032 *  POSSIBILITY OF SUCH DAMAGE.
00033 *********************************************************************/
00034 
00035 // Author(s): Jeannette Bohg
00036 
00037 #include <ros/ros.h>
00038 
00039 #include <string>
00040 #include <vector>
00041 
00042 #include "rgbd_assembler/RgbdAssembly.h"
00043 
00044 #include "rgbd_assembler/utils.h"
00045 
00046 #include <visualization_msgs/Marker.h>
00047 #include <image_transport/image_transport.h>
00048 
00049 visualization_msgs::Marker getCloudMarker(const sensor_msgs::PointCloud2 &cloud)
00050 {
00051   static bool first_time = true;
00052   if (first_time) {
00053     srand ( time(NULL) );
00054     first_time = false;
00055   }
00056 
00057   //create the marker
00058   visualization_msgs::Marker marker;
00059   marker.action = visualization_msgs::Marker::ADD;
00060   marker.lifetime = ros::Duration();
00061 
00062   marker.type = visualization_msgs::Marker::POINTS;
00063   marker.scale.x = 0.002;
00064   marker.scale.y = 0.002;
00065   marker.scale.z = 1.0;
00066 
00067   marker.color.r = ((double)rand())/RAND_MAX;
00068   marker.color.g = ((double)rand())/RAND_MAX;
00069   marker.color.b = ((double)rand())/RAND_MAX;
00070   marker.color.a = 1.0;
00071   
00072   int w = cloud.width;
00073   int h = cloud.height;
00074   
00075   for (int i=0; i<h; i++) {
00076     for (int j=0; j<w; j++)  {
00077       int adr = i * w + j;
00078       float nan;
00079       memcpy (&nan, &cloud.data[adr * cloud.point_step + cloud.fields[0].offset], sizeof (float));
00080       if(!isnan(nan)){
00081         geometry_msgs::Point p;
00082         float x, y, z;
00083         memcpy (&x, &cloud.data[adr * cloud.point_step + cloud.fields[0].offset], sizeof (float));
00084         memcpy (&y, &cloud.data[adr * cloud.point_step + cloud.fields[1].offset], sizeof (float));
00085         memcpy (&z, &cloud.data[adr * cloud.point_step + cloud.fields[2].offset], sizeof (float));
00086         p.x = x;
00087         p.y = y;
00088         p.z = z;
00089         
00090         marker.points.push_back(p);
00091 
00092       }
00093     }
00094   }
00095 
00096   return marker;
00097 }
00098 
00099 void publishClusterMarkers(const sensor_msgs::PointCloud2 &cloud, const ros::Publisher &marker_pub)
00100 {
00101   int current_cluster_marker = 1;
00102   visualization_msgs::Marker cloud_marker =  getCloudMarker(cloud);
00103   cloud_marker.header = cloud.header;
00104   cloud_marker.pose.orientation.w = 1;
00105   cloud_marker.ns = "tabletop_node";
00106   cloud_marker.id = current_cluster_marker;
00107   marker_pub.publish(cloud_marker);
00108 }
00109 
00110 void fillRgbImage(sensor_msgs::Image &rgb_img, 
00111                   const sensor_msgs::PointCloud2 &point_cloud)
00112 {
00113 
00114   rgb_img.header = point_cloud.header;
00115   rgb_img.height = point_cloud.height;
00116   rgb_img.width =  point_cloud.width;
00117   rgb_img.encoding = enc::RGB8;
00118   rgb_img.is_bigendian = false;
00119   rgb_img.step = 3 * rgb_img.width;
00120   size_t size = rgb_img.step * rgb_img.height;
00121   rgb_img.data.resize(size);
00122     
00123   for(unsigned int x=0; x<rgb_img.width; ++x){
00124     for(unsigned int y=0; y<rgb_img.height; ++y){
00125       int i = y * rgb_img.width + x;
00126         
00127       float rgb;
00128       memcpy ( &rgb, &point_cloud.data[i * point_cloud.point_step + point_cloud.fields[3].offset], sizeof (float));
00129       float r, g, b;
00130       rgbd_assembler::transformRGB(rgb, r, g, b);
00131           
00132       int wide_i = y * rgb_img.step + x*3;
00133       rgb_img.data[wide_i+0] = round(r*255.0f);
00134       rgb_img.data[wide_i+1] = round(g*255.0f);
00135       rgb_img.data[wide_i+2] = round(b*255.0f);
00136 
00137     }
00138   }
00139 }
00140 
00142 int main(int argc, char **argv)
00143 {
00144   ros::init(argc, argv, "ping_rgbd_assembler_node");
00145   ros::NodeHandle nh_;
00146 
00148   ros::Publisher marker_pub_;
00149 
00151   image_transport::ImageTransport it_(nh_);
00152   
00153   marker_pub_ = nh_.advertise<visualization_msgs::Marker>(nh_.resolveName("tabletop_segmentation_markers"), 10);
00154   image_transport::Publisher image_pub_ = it_.advertise("rgb_out", 1);
00155   
00156 
00157   std::string service_name("/rgbd_assembly");
00158   while ( !ros::service::waitForService(service_name, ros::Duration().fromSec(3.0)) && nh_.ok() ) {
00159     ROS_INFO("Waiting for service %s...", service_name.c_str());
00160   }
00161   if (!nh_.ok()) exit(0);
00162 
00163   rgbd_assembler::RgbdAssembly rgbd_assembler_srv;
00164   if (!ros::service::call(service_name, rgbd_assembler_srv)) {
00165       ROS_ERROR("Call to rgbd service failed");
00166       exit(0);
00167   }
00168   if (rgbd_assembler_srv.response.result != rgbd_assembler_srv.response.SUCCESS) {
00169     ROS_ERROR("RGB-D Assembly service returned error %d", rgbd_assembler_srv.response.result);
00170     exit(0);
00171   }
00172   
00173   publishClusterMarkers(rgbd_assembler_srv.response.point_cloud, marker_pub_);
00174 
00175   sensor_msgs::Image rgb_img;
00176   fillRgbImage(rgb_img, rgbd_assembler_srv.response.point_cloud);
00177   
00178   ros::Rate loop_rate(5);
00179   while (nh_.ok()) {
00180     image_pub_.publish(rgb_img);
00181     ros::spinOnce();
00182     loop_rate.sleep();
00183   }
00184 
00185   return true;
00186 }
ping_rgbd_assembler_node.cpp
