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/sensors/SensorTypes.hh>
00057
00058 #include <sensor_msgs/image_encodings.h>
00059
00060 #include <gazebo/gazebo_config.h>
00061
00062 namespace gazebo
00063 {
00064
00066
00067 template <class Base>
00068 GazeboRosThermalCamera_<Base>::GazeboRosThermalCamera_()
00069 {
00070 }
00071
00073
00074 template <class Base>
00075 GazeboRosThermalCamera_<Base>::~GazeboRosThermalCamera_()
00076 {
00077 }
00078
00079 template <class Base>
00080 void GazeboRosThermalCamera_<Base>::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
00081 {
00082 Base::Load(_parent, _sdf);
00083
00084 this->parentSensor_ = this->parentSensor;
00085 this->width_ = this->width;
00086 this->height_ = this->height;
00087 this->depth_ = this->depth;
00088 this->format_ = this->format;
00089
00090 this->image_connect_count_ = boost::shared_ptr<int>(new int);
00091 *this->image_connect_count_ = 0;
00092 this->image_connect_count_lock_ = boost::shared_ptr<boost::mutex>(new boost::mutex);
00093 this->was_active_ = boost::shared_ptr<bool>(new bool);
00094 *this->was_active_ = false;
00095
00096 LoadImpl(_parent, _sdf);
00097 GazeboRosCameraUtils::Load(_parent, _sdf);
00098 }
00099
00101
00102 template <class Base>
00103 void GazeboRosThermalCamera_<Base>::OnNewFrame(const unsigned char *_image,
00104 unsigned int _width, unsigned int _height, unsigned int _depth,
00105 const std::string &_format)
00106 {
00107 if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00108 return;
00109
00110 #if (GAZEBO_MAJOR_VERSION > 6)
00111 this->sensor_update_time_ = this->parentSensor_->LastUpdateTime();
00112 #else
00113 this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
00114 #endif
00115
00116 if (!this->parentSensor->IsActive())
00117 {
00118 if ((*this->image_connect_count_) > 0)
00119
00120 this->parentSensor->SetActive(true);
00121 }
00122 else
00123 {
00124 if ((*this->image_connect_count_) > 0)
00125 {
00126 common::Time cur_time = this->world_->GetSimTime();
00127 if (cur_time - this->last_update_time_ >= this->update_period_)
00128 {
00129 this->PutCameraData(_image);
00130 this->PublishCameraInfo();
00131 this->last_update_time_ = cur_time;
00132 }
00133 }
00134 }
00135 }
00136
00137 template <class Base>
00138 void GazeboRosThermalCamera_<Base>::OnNewImageFrame(const unsigned char *_image,
00139 unsigned int _width, unsigned int _height, unsigned int _depth,
00140 const std::string &_format)
00141 {
00142 OnNewFrame(_image, _width, _height, _depth, _format);
00143 }
00144
00146
00147 template <class Base>
00148 void GazeboRosThermalCamera_<Base>::PutCameraData(const unsigned char *_src, common::Time &last_update_time)
00149 {
00150 this->sensor_update_time_ = last_update_time;
00151 this->PutCameraData(_src);
00152 }
00153
00154 template <class Base>
00155 void GazeboRosThermalCamera_<Base>::PutCameraData(const unsigned char *_src)
00156 {
00157 if (!this->initialized_ || this->height_ <=0 || this->width_ <=0)
00158 return;
00159
00160 this->lock_.lock();
00161
00162
00163 this->image_msg_.header.frame_id = this->frame_name_;
00164 this->image_msg_.header.stamp.sec = this->sensor_update_time_.sec;
00165 this->image_msg_.header.stamp.nsec = this->sensor_update_time_.nsec;
00166
00168 if ((*this->image_connect_count_) > 0)
00169 {
00170 this->image_msg_.width = this->width_;
00171 this->image_msg_.height = this->height_;
00172 this->image_msg_.encoding = sensor_msgs::image_encodings::MONO8;
00173 this->image_msg_.step = this->image_msg_.width;
00174
00175 size_t size = this->width_ * this->height_;
00176
00177 std::vector<uint8_t>& data (this->image_msg_.data);
00178 data.resize(size);
00179
00180 size_t img_index = 0;
00181
00182 for (size_t i = 0; i < size; ++i){
00183 if ((_src[img_index] >254) && (_src[img_index+1] < 1) && (_src[img_index+2] < 1)){
00184
00185 data[i]= 255;
00186 }else{
00187
00188 data[i]= (_src[img_index] + _src[img_index+1] + _src[img_index+2]) /8 ;
00189 }
00190 img_index += 3;
00191 }
00192
00193
00194 this->image_pub_.publish(this->image_msg_);
00195 }
00196
00197 this->lock_.unlock();
00198 }
00199
00200 }