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 <gazebo/sdf/interface/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 this->type_ = sensor_msgs::image_encodings::MONO8;
00092 this->skip_ = 1;
00093 }
00094
00096
00097 template <class Base>
00098 void GazeboRosThermalCamera_<Base>::OnNewFrame(const unsigned char *_image,
00099 unsigned int _width, unsigned int _height, unsigned int _depth,
00100 const std::string &_format)
00101 {
00102 this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
00103
00104 if (!this->parentSensor->IsActive())
00105 {
00106 if (this->image_connect_count_ > 0)
00107
00108 this->parentSensor->SetActive(true);
00109 }
00110 else
00111 {
00112 if (this->image_connect_count_ > 0)
00113 {
00114 common::Time cur_time = this->world_->GetSimTime();
00115 if (cur_time - this->last_update_time_ >= this->update_period_)
00116 {
00117 this->PutCameraData(_image);
00118 this->last_update_time_ = cur_time;
00119 }
00120 }
00121 }
00122 }
00123
00124 template <class Base>
00125 void GazeboRosThermalCamera_<Base>::OnNewImageFrame(const unsigned char *_image,
00126 unsigned int _width, unsigned int _height, unsigned int _depth,
00127 const std::string &_format)
00128 {
00129 OnNewFrame(_image, _width, _height, _depth, _format);
00130 }
00131
00133
00134 template <class Base>
00135 void GazeboRosThermalCamera_<Base>::PutCameraData(const unsigned char *_src, common::Time &last_update_time)
00136 {
00137 this->sensor_update_time_ = last_update_time;
00138 this->PutCameraData(_src);
00139 }
00140
00141 template <class Base>
00142 void GazeboRosThermalCamera_<Base>::PutCameraData(const unsigned char *_src)
00143 {
00144 this->lock_.lock();
00145
00146
00147 this->image_msg_.header.frame_id = this->frame_name_;
00148 this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
00149 this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00150
00152 if (this->image_connect_count_ > 0)
00153 {
00154 this->image_msg_.encoding = this->type_;
00155 this->image_msg_.width = this->width_;
00156 this->image_msg_.height = this->height_;
00157
00158 size_t size = this->width_ * this->height_;
00159
00160 std::vector<uint8_t>& data (this->image_msg_.data);
00161 data.resize(size);
00162
00163 size_t img_index = 0;
00164
00165 for (size_t i = 0; i < size; ++i){
00166 if (_src[img_index] >254 && _src[img_index+1] < 1 && _src[img_index+2 < 1]){
00167
00168 data[i]= 255;
00169 }else{
00170
00171 data[i]= (_src[img_index] + _src[img_index+1] + _src[img_index+2]) /8 ;
00172 }
00173 img_index += 3;
00174 }
00175
00176
00177 this->image_pub_.publish(this->image_msg_);
00178 }
00179
00180 this->lock_.unlock();
00181 }
00182
00183 }