gazebo_ros_prosilica.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 /*
19  @mainpage
20  Desc: GazeboRosProsilica plugin for simulating Prosilica cameras in Gazebo
21  Author: John Hsu
22  Date: 24 Sept 2008
23 */
24 
25 #include <algorithm>
26 #include <assert.h>
27 
29 
30 #include <gazebo/physics/World.hh>
31 #include <gazebo/physics/HingeJoint.hh>
32 #include <gazebo/sensors/Sensor.hh>
33 #include <gazebo/common/Exception.hh>
34 #include <gazebo/sensors/CameraSensor.hh>
35 #include <gazebo/sensors/SensorTypes.hh>
36 #include <gazebo/rendering/Camera.hh>
37 
38 #include <sdf/sdf.hh>
39 #include <sdf/Param.hh>
40 
41 #include <sensor_msgs/Image.h>
42 #include <sensor_msgs/CameraInfo.h>
43 #include <sensor_msgs/fill_image.h>
45 #include <sensor_msgs/RegionOfInterest.h>
46 
47 #include <opencv/cv.h>
48 #include <opencv/highgui.h>
49 
50 #include <opencv/cvwimage.h>
51 
52 #include <boost/scoped_ptr.hpp>
53 #include <boost/bind.hpp>
54 #include <boost/tokenizer.hpp>
55 #include <boost/thread.hpp>
56 #include <boost/thread/recursive_mutex.hpp>
57 #include <string>
58 
59 namespace gazebo
60 {
61 
63 // Constructor
65 {
66 }
67 
69 // Destructor
71 {
72  // Finalize the controller
73  this->poll_srv_.shutdown();
74 }
75 
77 // Load the controller
78 void GazeboRosProsilica::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
79 {
80 
81  CameraPlugin::Load(_parent, _sdf);
82  this->parentSensor_ = this->parentSensor;
83  this->width_ = this->width;
84  this->height_ = this->height;
85  this->depth_ = this->depth;
86  this->format_ = this->format;
87  this->camera_ = this->camera;
88  GazeboRosCameraUtils::Load(_parent, _sdf);
89 
91 }
92 
94 {
95  // camera mode for prosilica:
96  // prosilica::AcquisitionMode mode_; /// @todo Make this property of Camera
97 
98  //ROS_ERROR_NAMED("prosilica", "before trigger_mode %s %s",this->mode_param_name.c_str(),this->mode_.c_str());
99 
100  if (!this->rosnode_->searchParam("trigger_mode",this->mode_param_name))
101  this->mode_param_name = "trigger_mode";
102 
103  if (!this->rosnode_->getParam(this->mode_param_name,this->mode_))
104  this->mode_ = "streaming";
105 
106  ROS_INFO_NAMED("prosilica", "trigger_mode %s %s",this->mode_param_name.c_str(),this->mode_.c_str());
107 
108 
109  if (this->mode_ == "polled")
110  {
112  }
113  else if (this->mode_ == "streaming")
114  {
115  ROS_DEBUG_NAMED("prosilica", "do nothing here,mode: %s",this->mode_.c_str());
116  }
117  else
118  {
119  ROS_ERROR_NAMED("prosilica", "trigger_mode is invalid: %s, using streaming mode",this->mode_.c_str());
120  }
121 }
122 
124 // Update the controller
125 void GazeboRosProsilica::OnNewImageFrame(const unsigned char *_image,
126  unsigned int _width, unsigned int _height, unsigned int _depth,
127  const std::string &_format)
128 {
129  if (!this->rosnode_->getParam(this->mode_param_name,this->mode_))
130  this->mode_ = "streaming";
131 
132  // should do nothing except turning camera on/off, as we are using service.
134  common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
135 
136  // as long as ros is connected, parent is active
137  //ROS_ERROR_NAMED("prosilica", "debug image count %d",this->image_connect_count_);
138  if (!this->parentSensor->IsActive())
139  {
140  if ((*this->image_connect_count_) > 0)
141  // do this first so there's chance for sensor to run 1 frame after activate
142  this->parentSensor->SetActive(true);
143  }
144  else
145  {
146  //ROS_ERROR_NAMED("prosilica", "camera_ new frame %s %s",this->parentSensor_->Name().c_str(),this->frame_name_.c_str());
147 
148  if (this->mode_ == "streaming")
149  {
150  if ((*this->image_connect_count_) > 0)
151  {
152  if (sensor_update_time - this->last_update_time_ >= this->update_period_)
153  {
154  this->PutCameraData(_image, sensor_update_time);
155  this->PublishCameraInfo(sensor_update_time);
156  this->last_update_time_ = sensor_update_time;
157  }
158  }
159  }
160 
161  }
162 }
163 
164 
166 // new prosilica interface.
167 void GazeboRosProsilica::pollCallback(polled_camera::GetPolledImage::Request& req,
168  polled_camera::GetPolledImage::Response& rsp,
169  sensor_msgs::Image& image, sensor_msgs::CameraInfo& info)
170 {
171  if (!this->rosnode_->getParam(this->mode_param_name,this->mode_))
172  this->mode_ = "streaming";
173 
177 
178  if (this->mode_ != "polled")
179  {
180  rsp.success = false;
181  rsp.status_message = "Camera is not in triggered mode";
182  return;
183  }
184 
185  if (req.binning_x > 1 || req.binning_y > 1)
186  {
187  rsp.success = false;
188  rsp.status_message = "Gazebo Prosilica plugin does not support binning";
189  return;
190  }
191 
192  // get region from request
193  if (req.roi.x_offset <= 0 || req.roi.y_offset <= 0 || req.roi.width <= 0 || req.roi.height <= 0)
194  {
195  req.roi.x_offset = 0;
196  req.roi.y_offset = 0;
197  req.roi.width = this->width_;
198  req.roi.height = this->height;
199  }
200  const unsigned char *src = NULL;
201  ROS_ERROR_NAMED("prosilica", "roidebug %d %d %d %d", req.roi.x_offset, req.roi.y_offset, req.roi.width, req.roi.height);
202 
203  // signal sensor to start update
204  (*this->image_connect_count_)++;
205 
206  // wait until an image has been returned
207  while(!src)
208  {
209  {
210  // Get a pointer to image data
211  src = this->parentSensor->Camera()->ImageData(0);
212 
213  if (src)
214  {
215 
216  // fill CameraInfo
217  this->roiCameraInfoMsg = &info;
218  this->roiCameraInfoMsg->header.frame_id = this->frame_name_;
219 
220  common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime();
221  this->roiCameraInfoMsg->header.stamp.sec = roiLastRenderTime.sec;
222  this->roiCameraInfoMsg->header.stamp.nsec = roiLastRenderTime.nsec;
223 
224  this->roiCameraInfoMsg->width = req.roi.width; //this->parentSensor->ImageWidth() ;
225  this->roiCameraInfoMsg->height = req.roi.height; //this->parentSensor->ImageHeight();
226  // distortion
227 #if ROS_VERSION_MINIMUM(1, 3, 0)
228  this->roiCameraInfoMsg->distortion_model = "plumb_bob";
229  this->roiCameraInfoMsg->D.resize(5);
230 #endif
231  this->roiCameraInfoMsg->D[0] = this->distortion_k1_;
232  this->roiCameraInfoMsg->D[1] = this->distortion_k2_;
233  this->roiCameraInfoMsg->D[2] = this->distortion_k3_;
234  this->roiCameraInfoMsg->D[3] = this->distortion_t1_;
235  this->roiCameraInfoMsg->D[4] = this->distortion_t2_;
236  // original camera matrix
237  this->roiCameraInfoMsg->K[0] = this->focal_length_;
238  this->roiCameraInfoMsg->K[1] = 0.0;
239  this->roiCameraInfoMsg->K[2] = this->cx_ - req.roi.x_offset;
240  this->roiCameraInfoMsg->K[3] = 0.0;
241  this->roiCameraInfoMsg->K[4] = this->focal_length_;
242  this->roiCameraInfoMsg->K[5] = this->cy_ - req.roi.y_offset;
243  this->roiCameraInfoMsg->K[6] = 0.0;
244  this->roiCameraInfoMsg->K[7] = 0.0;
245  this->roiCameraInfoMsg->K[8] = 1.0;
246  // rectification
247  this->roiCameraInfoMsg->R[0] = 1.0;
248  this->roiCameraInfoMsg->R[1] = 0.0;
249  this->roiCameraInfoMsg->R[2] = 0.0;
250  this->roiCameraInfoMsg->R[3] = 0.0;
251  this->roiCameraInfoMsg->R[4] = 1.0;
252  this->roiCameraInfoMsg->R[5] = 0.0;
253  this->roiCameraInfoMsg->R[6] = 0.0;
254  this->roiCameraInfoMsg->R[7] = 0.0;
255  this->roiCameraInfoMsg->R[8] = 1.0;
256  // camera projection matrix (same as camera matrix due to lack of distortion/rectification) (is this generated?)
257  this->roiCameraInfoMsg->P[0] = this->focal_length_;
258  this->roiCameraInfoMsg->P[1] = 0.0;
259  this->roiCameraInfoMsg->P[2] = this->cx_ - req.roi.x_offset;
260  this->roiCameraInfoMsg->P[3] = -this->focal_length_ * this->hack_baseline_;
261  this->roiCameraInfoMsg->P[4] = 0.0;
262  this->roiCameraInfoMsg->P[5] = this->focal_length_;
263  this->roiCameraInfoMsg->P[6] = this->cy_ - req.roi.y_offset;
264  this->roiCameraInfoMsg->P[7] = 0.0;
265  this->roiCameraInfoMsg->P[8] = 0.0;
266  this->roiCameraInfoMsg->P[9] = 0.0;
267  this->roiCameraInfoMsg->P[10] = 1.0;
268  this->roiCameraInfoMsg->P[11] = 0.0;
270 
271  // copy data into image_msg_, then convert to roiImageMsg(image)
272  this->image_msg_.header.frame_id = this->frame_name_;
273 
274  common::Time lastRenderTime = this->parentSensor_->LastMeasurementTime();
275  this->image_msg_.header.stamp.sec = lastRenderTime.sec;
276  this->image_msg_.header.stamp.nsec = lastRenderTime.nsec;
277 
278  //unsigned char dst[this->width_*this->height];
279 
281 
282  // copy from src to image_msg_
283  fillImage(this->image_msg_,
284  this->type_,
285  this->height_,
286  this->width_,
287  this->skip_*this->width_,
288  (void*)src );
289 
291 
292  this->image_pub_.publish(this->image_msg_);
293 
294  {
295  // copy data into ROI image
296  this->roiImageMsg = &image;
297  this->roiImageMsg->header.frame_id = this->frame_name_;
298  common::Time roiLastRenderTime = this->parentSensor_->LastMeasurementTime();
299  this->roiImageMsg->header.stamp.sec = roiLastRenderTime.sec;
300  this->roiImageMsg->header.stamp.nsec = roiLastRenderTime.nsec;
301 
302  // convert image_msg_ to a CvImage using cv_bridge
304  img_bridge_ = cv_bridge::toCvCopy(this->image_msg_);
305 
306  // for debug
307  //cvNamedWindow("showme",CV_WINDOW_AUTOSIZE);
308  //cvSetMouseCallback("showme", &GazeboRosProsilica::mouse_cb, this);
309  //cvStartWindowThread();
310  //cvShowImage("showme",img_bridge_.toIpl());
311 
312  // crop image to roi
313  cv::Mat roi(img_bridge_->image,
314  cv::Rect(req.roi.x_offset, req.roi.y_offset,
315  req.roi.width, req.roi.height));
316  img_bridge_->image = roi;
317 
318  // copy roi'd image into roiImageMsg
319  img_bridge_->toImageMsg(*this->roiImageMsg);
320  }
321  }
322  }
323  usleep(100000);
324  }
325  (*this->image_connect_count_)--;
326  rsp.success = true;
327  return;
328 }
329 
330 
331 /*
332 void GazeboRosProsilica::OnStats( const boost::shared_ptr<msgs::WorldStatistics const> &_msg)
333 {
334  this->simTime = msgs::Convert( _msg->sim_time() );
335 
336  ignition::math::Pose3d pose;
337  pose.Pos().X() = 0.5*sin(0.01*this->simTime.Double());
338  gzdbg << "plugin simTime [" << this->simTime.Double() << "] update pose [" << pose.Pos().X() << "]\n";
339 }
340 */
341 
342 // Register this plugin with the simulator
343 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosProsilica)
344 
345 
346 }
polled_camera::PublicationServer poll_srv_
image_transport
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
sensor_msgs::CameraInfo * roiCameraInfoMsg
event::ConnectionPtr load_connection_
image_transport::Publisher image_pub_
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
sensor_msgs::Image * roiImageMsg
ros message
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
virtual ~GazeboRosProsilica()
Destructor.
sensor_msgs::Image image_msg_
ROS image message.
#define ROS_DEBUG_NAMED(name,...)
ros::Publisher camera_info_pub_
camera info
PublicationServer advertise(ros::NodeHandle &nh, const std::string &service, const PublicationServer::DriverCallback &cb, const ros::VoidPtr &tracked_object=ros::VoidPtr())
void pollCallback(polled_camera::GetPolledImage::Request &req, polled_camera::GetPolledImage::Response &rsp, sensor_msgs::Image &image, sensor_msgs::CameraInfo &info)
event::ConnectionPtr OnLoad(const boost::function< void()> &)
void publish(const sensor_msgs::Image &message) const
bool searchParam(const std::string &key, std::string &result) const
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::string frame_name_
ROS frame transform name to use in the image message header. This should typically match the link nam...
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
std::string type_
size of image buffer
std::string pollServiceName
ROS image topic name.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Mar 26 2019 02:31:27