Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 #include <hector_gazebo_thermal_camera/gazebo_ros_thermal_camera.h>
00054
00055 #include <gazebo/sensors/Sensor.hh>
00056 #include <sdf/sdf.hh>
00057 #include <gazebo/sensors/SensorTypes.hh>
00058
00059 #include <sensor_msgs/image_encodings.h>
00060
00061 namespace gazebo
00062 {
00063
00065
00066 template <class Base>
00067 GazeboRosThermalCamera_<Base>::GazeboRosThermalCamera_()
00068 {
00069 }
00070
00072
00073 template <class Base>
00074 GazeboRosThermalCamera_<Base>::~GazeboRosThermalCamera_()
00075 {
00076 }
00077
00078 template <class Base>
00079 void GazeboRosThermalCamera_<Base>::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00080 {
00081 Base::Load(_parent, _sdf);
00082
00083 this->parentSensor_ = this->parentSensor;
00084 this->width_ = this->width;
00085 this->height_ = this->height;
00086 this->depth_ = this->depth;
00087 this->format_ = this->format;
00088 LoadImpl(_parent, _sdf);
00089 GazeboRosCameraUtils::Load(_parent, _sdf);
00090 }
00091
00093
00094 template <class Base>
00095 void GazeboRosThermalCamera_<Base>::OnNewFrame(const unsigned char *_image,
00096 unsigned int _width, unsigned int _height, unsigned int _depth,
00097 const std::string &_format)
00098 {
00099
00100
00101
00102 if (!callback_queue_thread_.joinable()
00103 || this->height_ <=0 || this->width_ <=0)
00104 return;
00105
00106 this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
00107
00108 if (!this->parentSensor->IsActive())
00109 {
00110 if (this->image_connect_count_ > 0)
00111
00112 this->parentSensor->SetActive(true);
00113 }
00114 else
00115 {
00116 if (this->image_connect_count_ > 0)
00117 {
00118 common::Time cur_time = this->world_->GetSimTime();
00119 if (cur_time - this->last_update_time_ >= this->update_period_)
00120 {
00121 this->PutCameraData(_image);
00122 this->PublishCameraInfo();
00123 this->last_update_time_ = cur_time;
00124 }
00125 }
00126 }
00127 }
00128
00129 template <class Base>
00130 void GazeboRosThermalCamera_<Base>::OnNewImageFrame(const unsigned char *_image,
00131 unsigned int _width, unsigned int _height, unsigned int _depth,
00132 const std::string &_format)
00133 {
00134 OnNewFrame(_image, _width, _height, _depth, _format);
00135 }
00136
00138
00139 template <class Base>
00140 void GazeboRosThermalCamera_<Base>::PutCameraData(const unsigned char *_src, common::Time &last_update_time)
00141 {
00142 this->sensor_update_time_ = last_update_time;
00143 this->PutCameraData(_src);
00144 }
00145
00146 template <class Base>
00147 void GazeboRosThermalCamera_<Base>::PutCameraData(const unsigned char *_src)
00148 {
00149
00150
00151
00152 if (!callback_queue_thread_.joinable()
00153 || this->height_ <=0 || this->width_ <=0)
00154 return;
00155
00156 this->lock_.lock();
00157
00158
00159 this->image_msg_.header.frame_id = this->frame_name_;
00160 this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
00161 this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00162
00164 if (this->image_connect_count_ > 0)
00165 {
00166 this->image_msg_.width = this->width_;
00167 this->image_msg_.height = this->height_;
00168 this->image_msg_.encoding = sensor_msgs::image_encodings::MONO8;
00169 this->image_msg_.step = this->image_msg_.width;
00170
00171 size_t size = this->width_ * this->height_;
00172
00173 std::vector<uint8_t>& data (this->image_msg_.data);
00174 data.resize(size);
00175
00176 size_t img_index = 0;
00177
00178 for (size_t i = 0; i < size; ++i){
00179 if (_src[img_index] >254 && _src[img_index+1] < 1 && _src[img_index+2 < 1]){
00180
00181 data[i]= 255;
00182 }else{
00183
00184 data[i]= (_src[img_index] + _src[img_index+1] + _src[img_index+2]) /8 ;
00185 }
00186 img_index += 3;
00187 }
00188
00189
00190 this->image_pub_.publish(this->image_msg_);
00191 }
00192
00193 this->lock_.unlock();
00194 }
00195
00196 }