gazebo_ros_thermal_camera.cpp
Go to the documentation of this file.
1 //=================================================================================================
2 // Copyright (c) 2012, Stefan Kohlbrecher, TU Darmstadt
3 // All rights reserved.
4 
5 // Redistribution and use in source and binary forms, with or without
6 // modification, are permitted provided that the following conditions are met:
7 // * Redistributions of source code must retain the above copyright
8 // notice, this list of conditions and the following disclaimer.
9 // * Redistributions in binary form must reproduce the above copyright
10 // notice, this list of conditions and the following disclaimer in the
11 // documentation and/or other materials provided with the distribution.
12 // * Neither the name of the Simulation, Systems Optimization and Robotics
13 // group, TU Darmstadt nor the names of its contributors may be used to
14 // endorse or promote products derived from this software without
15 // specific prior written permission.
16 
17 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
18 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
19 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
20 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
21 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
22 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
23 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
24 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
25 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
26 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
27 //=================================================================================================
32 /*
33  * Gazebo - Outdoor Multi-Robot Simulator
34  * Copyright (C) 2003
35  * Nate Koenig & Andrew Howard
36  *
37  * This program is free software; you can redistribute it and/or modify
38  * it under the terms of the GNU General Public License as published by
39  * the Free Software Foundation; either version 2 of the License, or
40  * (at your option) any later version.
41  *
42  * This program is distributed in the hope that it will be useful,
43  * but WITHOUT ANY WARRANTY; without even the implied warranty of
44  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
45  * GNU General Public License for more details.
46  *
47  * You should have received a copy of the GNU General Public License
48  * along with this program; if not, write to the Free Software
49  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
50  *
51  */
52 
54 
55 #include <gazebo/sensors/Sensor.hh>
56 #include <gazebo/sensors/SensorTypes.hh>
57 
59 
60 #include <gazebo/gazebo_config.h>
61 
62 namespace gazebo
63 {
64 
66 // Constructor
67 template <class Base>
69 {
70 }
71 
73 // Destructor
74 template <class Base>
76 {
77 }
78 
79 template <class Base>
80 void GazeboRosThermalCamera_<Base>::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
81 {
82  Base::Load(_parent, _sdf);
83  // copying from CameraPlugin into GazeboRosCameraUtils
84  this->parentSensor_ = this->parentSensor;
85  this->width_ = this->width;
86  this->height_ = this->height;
87  this->depth_ = this->depth;
88  this->format_ = this->format;
89 
90  this->image_connect_count_ = boost::shared_ptr<int>(new int);
91  *this->image_connect_count_ = 0;
92  this->image_connect_count_lock_ = boost::shared_ptr<boost::mutex>(new boost::mutex);
93  this->was_active_ = boost::shared_ptr<bool>(new bool);
94  *this->was_active_ = false;
95 
96  LoadImpl(_parent, _sdf);
97  GazeboRosCameraUtils::Load(_parent, _sdf);
98 }
99 
101 // Update the controller
102 template <class Base>
103 void GazeboRosThermalCamera_<Base>::OnNewFrame(const unsigned char *_image,
104  unsigned int _width, unsigned int _height, unsigned int _depth,
105  const std::string &_format)
106 {
107  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
108  return;
109 
110 #if (GAZEBO_MAJOR_VERSION > 6)
111  this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
112 #else
113  this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
114 #endif
115 
116  if (!this->parentSensor->IsActive())
117  {
118  if ((*this->image_connect_count_) > 0)
119  // do this first so there's chance for sensor to run 1 frame after activate
120  this->parentSensor->SetActive(true);
121  }
122  else
123  {
124  if ((*this->image_connect_count_) > 0)
125  {
126 #if (GAZEBO_MAJOR_VERSION >= 8)
127  common::Time cur_time = this->world_->SimTime();
128 #else
129  common::Time cur_time = this->world_->GetSimTime();
130 #endif
131  if (cur_time - this->last_update_time_ >= this->update_period_)
132  {
133  this->PutCameraData(_image);
134  this->PublishCameraInfo();
135  this->last_update_time_ = cur_time;
136  }
137  }
138  }
139 }
140 
141 template <class Base>
142 void GazeboRosThermalCamera_<Base>::OnNewImageFrame(const unsigned char *_image,
143  unsigned int _width, unsigned int _height, unsigned int _depth,
144  const std::string &_format)
145 {
146  OnNewFrame(_image, _width, _height, _depth, _format);
147 }
148 
150 // Put camera_ data to the interface
151 template <class Base>
152 void GazeboRosThermalCamera_<Base>::PutCameraData(const unsigned char *_src, common::Time &last_update_time)
153 {
154  this->sensor_update_time_ = last_update_time;
155  this->PutCameraData(_src);
156 }
157 
158 template <class Base>
159 void GazeboRosThermalCamera_<Base>::PutCameraData(const unsigned char *_src)
160 {
161  if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
162  return;
163 
164  this->lock_.lock();
165 
166  // copy data into image
167  this->image_msg_.header.frame_id = this->frame_name_;
168  this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
169  this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
170 
172  if ((*this->image_connect_count_) > 0)
173  {
174  this->image_msg_.width = this->width_;
175  this->image_msg_.height = this->height_;
176  this->image_msg_.encoding = sensor_msgs::image_encodings::MONO8;
177  this->image_msg_.step = this->image_msg_.width;
178 
179  size_t size = this->width_ * this->height_;
180 
181  std::vector<uint8_t>& data (this->image_msg_.data);
182  data.resize(size);
183 
184  size_t img_index = 0;
185 
186  for (size_t i = 0; i < size; ++i){
187  if ((_src[img_index] >254) && (_src[img_index+1] < 1) && (_src[img_index+2] < 1)){
188  //RGB [255,0,0] translates to white (white hot)
189  data[i]= 255;
190  }else{
191  //Everything else is written to the MONO8 output image much darker
192  data[i]= (_src[img_index] + _src[img_index+1] + _src[img_index+2]) /8 ;
193  }
194  img_index += 3;
195  }
196 
197  // publish to ros
198  this->image_pub_.publish(this->image_msg_);
199  }
200 
201  this->lock_.unlock();
202 }
203 
204 }
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
data
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
virtual void OnNewFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.


hector_gazebo_thermal_camera
Author(s): Stefan Kohlbrecher
autogenerated on Fri Feb 5 2021 03:48:36