disparity_publisher.cc
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2017 Roboception GmbH
00003  * All rights reserved
00004  *
00005  * Author: Heiko Hirschmueller
00006  *
00007  * Redistribution and use in source and binary forms, with or without
00008  * modification, are permitted provided that the following conditions are met:
00009  *
00010  * 1. Redistributions of source code must retain the above copyright notice,
00011  * this list of conditions and the following disclaimer.
00012  *
00013  * 2. Redistributions in binary form must reproduce the above copyright notice,
00014  * this list of conditions and the following disclaimer in the documentation
00015  * and/or other materials provided with the distribution.
00016  *
00017  * 3. Neither the name of the copyright holder nor the names of its contributors
00018  * may be used to endorse or promote products derived from this software without
00019  * specific prior written permission.
00020  *
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00022  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00023  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00024  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00025  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00026  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00027  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00028  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00029  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00030  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00031  * POSSIBILITY OF SUCH DAMAGE.
00032  */
00033 
00034 #include "disparity_publisher.h"
00035 
00036 #include <rc_genicam_api/pixel_formats.h>
00037 
00038 #include <sensor_msgs/image_encodings.h>
00039 
00040 namespace rc
00041 {
00042 
00043 DisparityPublisher::DisparityPublisher(ros::NodeHandle &nh,
00044                                        std::string frame_id_prefix, double _f,
00045                                        double _t, double _scale)
00046         : GenICam2RosPublisher(frame_id_prefix)
00047 {
00048   seq=0;
00049   f=_f;
00050   t=_t;
00051   scale=_scale;
00052   disprange=0;
00053 
00054   pub=nh.advertise<stereo_msgs::DisparityImage>("disparity", 1);
00055 }
00056 
00057 void DisparityPublisher::setDisprange(int _disprange)
00058 {
00059   disprange=_disprange;
00060 }
00061 
00062 bool DisparityPublisher::used()
00063 {
00064   return pub.getNumSubscribers() > 0;
00065 }
00066 
00067 void DisparityPublisher::publish(const rcg::Buffer *buffer, uint64_t pixelformat)
00068 {
00069   if (pub.getNumSubscribers() > 0 && pixelformat == Coord3D_C16)
00070   {
00071     // allocate new image message and set meta information
00072 
00073     stereo_msgs::DisparityImagePtr p=boost::make_shared<stereo_msgs::DisparityImage>();
00074 
00075     const uint64_t freq=1000000000ul;
00076     uint64_t time=buffer->getTimestampNS();
00077 
00078     p->header.seq=seq++;
00079     p->header.stamp.sec=time/freq;
00080     p->header.stamp.nsec=time-freq*p->header.stamp.sec;
00081     p->header.frame_id=frame_id;
00082 
00083     // prepare size and format of outgoing image
00084 
00085     p->image.header=p->header;
00086     p->image.width=static_cast<uint32_t>(buffer->getWidth());
00087     p->image.height=static_cast<uint32_t>(buffer->getHeight());
00088     p->image.encoding=sensor_msgs::image_encodings::TYPE_32FC1;
00089     p->image.is_bigendian=rcg::isHostBigEndian();
00090     p->image.step=p->image.width*sizeof(float);
00091 
00092     size_t px=buffer->getXPadding();
00093     const uint8_t *ps=static_cast<const uint8_t *>(buffer->getBase())+buffer->getImageOffset();
00094 
00095     // convert image information
00096 
00097     p->image.data.resize(p->image.step*p->image.height);
00098 
00099     float *pt=reinterpret_cast<float *>(&p->image.data[0]);
00100     float dmax=0;
00101 
00102     bool bigendian=buffer->isBigEndian();
00103 
00104     for (uint32_t k=0; k<p->image.height; k++)
00105     {
00106       if (bigendian)
00107       {
00108         for (uint32_t i=0; i<p->image.width; i++)
00109         {
00110           uint16_t d=(ps[0]<<8)|ps[1];
00111 
00112           *pt=-1.0f;
00113 
00114           if (d != 0)
00115           {
00116             *pt=scale*d;
00117             dmax=std::max(dmax, *pt);
00118           }
00119 
00120           ps+=2;
00121           pt++;
00122         }
00123       }
00124       else
00125       {
00126         for (uint32_t i=0; i<p->image.width; i++)
00127         {
00128           uint16_t d=(ps[1]<<8)|ps[0];
00129 
00130           *pt=-1.0f;
00131 
00132           if (d != 0)
00133           {
00134             *pt=scale*d;
00135             dmax=std::max(dmax, *pt);
00136           }
00137 
00138           ps+=2;
00139           pt++;
00140         }
00141       }
00142 
00143       ps+=px;
00144     }
00145 
00146     p->f=f*p->image.width;
00147     p->T=t;
00148     p->valid_window.x_offset=0;
00149     p->valid_window.y_offset=0;
00150     p->valid_window.width=p->image.width;
00151     p->valid_window.height=p->image.height;
00152     p->min_disparity=0;
00153     p->max_disparity=std::max(dmax, static_cast<float>(disprange));
00154     p->delta_d=scale;
00155 
00156     // publish message
00157 
00158     pub.publish(p);
00159   }
00160 }
00161 
00162 }


rc_visard_driver
Author(s): Heiko Hirschmueller , Christian Emmerich , Felix Ruess
autogenerated on Thu Jun 6 2019 20:43:02