error_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 "error_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 ErrorDisparityPublisher::ErrorDisparityPublisher(ros::NodeHandle &nh,
00044                                                  std::string frame_id_prefix,
00045                                                  double _scale)
00046         : GenICam2RosPublisher(frame_id_prefix)
00047 {
00048   scale=_scale;
00049 
00050   pub=nh.advertise<sensor_msgs::Image>("error_disparity", 1);
00051 }
00052 
00053 bool ErrorDisparityPublisher::used()
00054 {
00055   return pub.getNumSubscribers() > 0;
00056 }
00057 
00058 void ErrorDisparityPublisher::publish(const rcg::Buffer *buffer, uint64_t pixelformat)
00059 {
00060   if (pub.getNumSubscribers() > 0)
00061   {
00062     if (pixelformat == Error8)
00063     {
00064       // create image and initialize header
00065 
00066       sensor_msgs::ImagePtr im=boost::make_shared<sensor_msgs::Image>();
00067 
00068       const uint64_t freq=1000000000ul;
00069       uint64_t time=buffer->getTimestampNS();
00070 
00071       im->header.seq=seq++;
00072       im->header.stamp.sec=time/freq;
00073       im->header.stamp.nsec=time-freq*im->header.stamp.sec;
00074       im->header.frame_id=frame_id;
00075 
00076       // set image size
00077 
00078       im->width=static_cast<uint32_t>(buffer->getWidth());
00079       im->height=static_cast<uint32_t>(buffer->getHeight());
00080 
00081       // get pointer to image data in buffer
00082 
00083       size_t px=buffer->getXPadding();
00084       const uint8_t *ps=static_cast<const uint8_t *>(buffer->getBase())+buffer->getImageOffset();
00085 
00086       // convert image data
00087 
00088       im->encoding=sensor_msgs::image_encodings::TYPE_32FC1;
00089       im->is_bigendian=rcg::isHostBigEndian();
00090       im->step=im->width*sizeof(float);
00091 
00092       im->data.resize(im->step*im->height);
00093       float *pt=reinterpret_cast<float *>(&im->data[0]);
00094 
00095       for (uint32_t k=0; k<im->height; k++)
00096       {
00097         for (uint32_t i=0; i<im->width; i++)
00098         {
00099           *pt++=scale **ps++;
00100         }
00101 
00102         ps+=px;
00103       }
00104 
00105       // publish message
00106 
00107       pub.publish(im);
00108     }
00109   }
00110 }
00111 
00112 }


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