24 #include "gazebo/sensors/DepthCameraSensor.hh" 33 #include <boost/algorithm/string.hpp> 41 OpticalFlowPlugin::OpticalFlowPlugin()
43 : SensorPlugin(), width(0), height(0), depth(0), timer_()
51 this->parentSensor.reset();
59 gzdbg << __FUNCTION__ <<
"() called." << std::endl;
63 gzerr <<
"Invalid sensor pointer.\n";
66 std::dynamic_pointer_cast<sensors::CameraSensor>(_sensor);
68 if (!this->parentSensor)
70 gzerr <<
"OpticalFlowPlugin requires a CameraSensor.\n";
71 if (std::dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
72 gzmsg <<
"It is a depth camera sensor\n";
75 this->camera = this->parentSensor->Camera();
77 if (!this->parentSensor)
79 gzerr <<
"OpticalFlowPlugin not attached to a camera sensor\n";
83 this->width = this->camera->ImageWidth();
84 this->height = this->camera->ImageHeight();
85 this->depth = this->camera->ImageDepth();
86 this->
format = this->camera->ImageFormat();
88 if (this->width != 64 || this->height != 64) {
89 gzerr <<
"[gazebo_optical_flow_plugin] Incorrect image size, must by 64 x 64.\n";
92 if (_sdf->HasElement(
"robotNamespace"))
93 namespace_ = _sdf->GetElement(
"robotNamespace")->Get<std::string>();
95 gzwarn <<
"[gazebo_optical_flow_plugin] Please specify a robotNamespace.\n";
97 node_handle_ = transport::NodePtr(
new transport::Node());
100 const string scopedName = _sensor->ParentName();
102 string topicName =
"~/" + scopedName +
"/opticalFlow";
103 boost::replace_all(topicName,
"::",
"/");
105 opticalFlow_pub_ =
node_handle_->Advertise<opticalFlow_msgs::msgs::opticalFlow>(topicName, 10);
107 hfov =
float(this->camera->HFOV().Radian());
108 first_frame_time = this->camera->LastRenderWallTime().Double();
110 old_frame_time = first_frame_time;
111 focal_length = (this->width/2)/
tan(hfov/2);
113 this->newFrameConnection = this->camera->ConnectNewImageFrame(
116 this->parentSensor->SetActive(
true);
119 const int ouput_rate = 20;
120 _optical_flow =
new OpticalFlowOpenCV(focal_length, focal_length, ouput_rate);
128 unsigned int _height,
130 const std::string &_format)
133 rate = this->camera->AvgFPS();
134 _image = this->camera->ImageData(0);
135 frame_time = this->camera->LastRenderWallTime().Double();
137 frame_time_us = (frame_time - first_frame_time) * 1e6;
141 float flow_x_ang = 0;
142 float flow_y_ang = 0;
144 int quality = _optical_flow->calcFlow((uint8_t *)_image, frame_time_us, dt_us, flow_x_ang, flow_y_ang);
150 opticalFlow_message.set_time_usec(0);
151 opticalFlow_message.set_sensor_id(2.0);
152 opticalFlow_message.set_integration_time_us(dt_us);
153 opticalFlow_message.set_integrated_x(flow_x_ang);
154 opticalFlow_message.set_integrated_y(flow_y_ang);
155 opticalFlow_message.set_integrated_xgyro(0.0);
156 opticalFlow_message.set_integrated_ygyro(0.0);
157 opticalFlow_message.set_integrated_zgyro(0.0);
158 opticalFlow_message.set_temperature(20.0);
159 opticalFlow_message.set_quality(quality);
160 opticalFlow_message.set_time_delta_distance_us(0.0);
161 opticalFlow_message.set_distance(0.0);
163 opticalFlow_pub_->Publish(opticalFlow_message);
std::string format(const std::string &fmt, Args ... args)
GZ_REGISTER_SENSOR_PLUGIN(GazeboGpsPlugin)
virtual void OnNewFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
static const bool kPrintOnPluginLoad
virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
virtual ~OpticalFlowPlugin()
INLINE Rall1d< T, V, S > tan(const Rall1d< T, V, S > &arg)
gazebo::transport::NodePtr node_handle_