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 #include "rotors_gazebo_plugins/external/gazebo_geotagged_images_plugin.h"
00019
00020 #include <math.h>
00021 #include <string>
00022 #include <iostream>
00023
00024 #include <boost/filesystem.hpp>
00025 #include <cv.h>
00026 #include <highgui.h>
00027 #include <opencv2/opencv.hpp>
00028
00029 #include "rotors_gazebo_plugins/common.h"
00030
00031 using namespace std;
00032 using namespace gazebo;
00033 using namespace cv;
00034
00035 GZ_REGISTER_SENSOR_PLUGIN(GeotaggedImagesPlugin)
00036
00037
00038 GeotaggedImagesPlugin::GeotaggedImagesPlugin() :
00039 SensorPlugin(), width_(0), height_(0), depth_(0), imageCounter_(0) {}
00040
00041 GeotaggedImagesPlugin::~GeotaggedImagesPlugin()
00042 {
00043 this->parentSensor_.reset();
00044 this->camera_.reset();
00045 }
00046
00047 void GeotaggedImagesPlugin::Load(sensors::SensorPtr sensor, sdf::ElementPtr sdf)
00048 {
00049 if(kPrintOnPluginLoad) {
00050 gzdbg << __FUNCTION__ << "() called." << std::endl;
00051 }
00052
00053 if (!sensor)
00054 gzerr << "Invalid sensor pointer.\n";
00055
00056 this->parentSensor_ =
00057 std::dynamic_pointer_cast<sensors::CameraSensor>(sensor);
00058
00059 if (!this->parentSensor_)
00060 {
00061 gzerr << "GeotaggedImagesPlugin requires a CameraSensor.\n";
00062 }
00063
00064 this->camera_ = this->parentSensor_->Camera();
00065
00066 if (!this->parentSensor_)
00067 {
00068 gzerr << "GeotaggedImagesPlugin not attached to a camera sensor\n";
00069 return;
00070 }
00071 scene_ = camera_->GetScene();
00072 lastImageTime_ = scene_->SimTime();
00073
00074 this->width_ = this->camera_->ImageWidth();
00075 this->height_ = this->camera_->ImageHeight();
00076 this->depth_ = this->camera_->ImageDepth();
00077 this->format_ = this->camera_->ImageFormat();
00078
00079 if (sdf->HasElement("robotNamespace")) {
00080 namespace_ = sdf->GetElement("robotNamespace")->Get<std::string>();
00081 } else {
00082 gzwarn << "[gazebo_geotagging_images_camera_plugin] Please specify a robotNamespace.\n";
00083 }
00084
00085 this->storeIntervalSec_ = 1;
00086 if (sdf->HasElement("interval")) {
00087 this->storeIntervalSec_ = sdf->GetElement("interval")->Get<float>();
00088 }
00089
00090 destWidth_ = width_;
00091 if (sdf->HasElement("width")) {
00092 destWidth_ = sdf->GetElement("width")->Get<int>();
00093 }
00094 destHeight_ = height_;
00095 if (sdf->HasElement("height")) {
00096 destHeight_ = sdf->GetElement("height")->Get<int>();
00097 }
00098
00099
00100
00101 if (system("exiftool -ver &>/dev/null") != 0) {
00102 gzerr << "exiftool not found. geotagging_images plugin will be disabled" << endl;
00103 gzerr << "On Ubuntu, use 'sudo apt-get install libimage-exiftool-perl' to install" << endl;
00104 return;
00105 }
00106
00107 node_handle_ = transport::NodePtr(new transport::Node());
00108 node_handle_->Init(namespace_);
00109
00110 this->parentSensor_->SetActive(true);
00111
00112 this->newFrameConnection_ = this->camera_->ConnectNewImageFrame(
00113 boost::bind(&GeotaggedImagesPlugin::OnNewFrame, this, _1));
00114
00115
00117 gpsSub_ = node_handle_->Subscribe("~/gps_position", &GeotaggedImagesPlugin::OnNewGpsPosition, this);
00118
00119 storageDir_ = "frames";
00120 boost::filesystem::remove_all(storageDir_);
00121 boost::filesystem::create_directory(storageDir_);
00122 }
00123
00124 void GeotaggedImagesPlugin::OnNewGpsPosition(ConstVector3dPtr& v)
00125 {
00126 lastGpsPosition_ = *v;
00127
00128 }
00129
00130 void GeotaggedImagesPlugin::OnNewFrame(const unsigned char * image)
00131 {
00132
00133 image = this->camera_->ImageData(0);
00134
00135 common::Time currentTime = scene_->SimTime();
00136 if (currentTime.Double() - lastImageTime_.Double() < storeIntervalSec_) {
00137 return;
00138 }
00139
00140 Mat frame = Mat(height_, width_, CV_8UC3);
00141 Mat frameBGR = Mat(height_, width_, CV_8UC3);
00142 frame.data = (uchar*)image;
00143 cvtColor(frame, frameBGR, CV_RGB2BGR);
00144
00145 char file_name[256];
00146 snprintf(file_name, sizeof(file_name), "%s/DSC%05i.jpg", storageDir_.c_str(), imageCounter_);
00147
00148 if (destWidth_ != width_ || destHeight_ != height_) {
00149 Mat frameResized;
00150 cv::Size size(destWidth_, destHeight_);
00151 cv::resize(frameBGR, frameResized, size);
00152 imwrite(file_name, frameResized);
00153 } else {
00154 imwrite(file_name, frameBGR);
00155 }
00156
00157 char gps_tag_command[1024];
00158 double lat = lastGpsPosition_.x();
00159 char north_south = 'N', east_west = 'E';
00160 double lon = lastGpsPosition_.y();
00161 if (lat < 0.) {
00162 lat = -lat;
00163 north_south = 'S';
00164 }
00165 if (lon < 0.) {
00166 lon = -lon;
00167 east_west = 'W';
00168 }
00169 snprintf(gps_tag_command, sizeof(gps_tag_command),
00170 "exiftool -gpslatituderef=%c -gpslongituderef=%c -gpsaltituderef=above -gpslatitude=%.9lf -gpslongitude=%.9lf"
00171
00172 " -datetimeoriginal=now -gpsdop=0.8"
00173 " -gpsmeasuremode=3-d -gpssatellites=13 -gpsaltitude=%.3lf -overwrite_original %s &>/dev/null",
00174 north_south, east_west, lat, lon, lastGpsPosition_.z(), file_name);
00175
00176 system(gps_tag_command);
00177
00178 ++imageCounter_;
00179 lastImageTime_ = currentTime;
00180
00181 }
rotors_gazebo_plugins
Author(s): Fadri Furrer, Michael Burri, Mina Kamel, Janosch Nikolic, Markus Achtelik
autogenerated on Thu Apr 18 2019 02:43:43