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