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 }