gazebo_ros_multicamera.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  * Desc: Syncronizes shutters across multiple cameras
19  * Author: John Hsu
20  * Date: 10 June 2013
21  */
22 
23 #include <string>
24 
25 #include <gazebo/sensors/Sensor.hh>
26 #include <gazebo/sensors/MultiCameraSensor.hh>
27 #include <gazebo/sensors/SensorTypes.hh>
28 
30 
31 namespace gazebo
32 {
33 // Register this plugin with the simulator
34 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosMultiCamera)
35 
36 // Constructor
39 {
40 }
41 
43 // Destructor
45 {
46 }
47 
48 void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent,
49  sdf::ElementPtr _sdf)
50 {
51  MultiCameraPlugin::Load(_parent, _sdf);
52 
53  // Make sure the ROS node for Gazebo has already been initialized
54  if (!ros::isInitialized())
55  {
56  ROS_FATAL_STREAM_NAMED("multicamera", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
57  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
58  return;
59  }
60 
61  // initialize shared_ptr members
62  this->image_connect_count_ = boost::shared_ptr<int>(new int(0));
64  this->was_active_ = boost::shared_ptr<bool>(new bool(false));
65 
66  // copying from CameraPlugin into GazeboRosCameraUtils
67  for (unsigned i = 0; i < this->camera.size(); ++i)
68  {
70  util->parentSensor_ = this->parentSensor;
71  util->width_ = this->width[i];
72  util->height_ = this->height[i];
73  util->depth_ = this->depth[i];
74  util->format_ = this->format[i];
75  util->camera_ = this->camera[i];
76  // Set up a shared connection counter
79  util->was_active_ = this->was_active_;
80  if (this->camera[i]->Name().find("left") != std::string::npos)
81  {
82  // FIXME: hardcoded, left hack_baseline_ 0
83  util->Load(_parent, _sdf, "/left", 0.0);
84  }
85  else if (this->camera[i]->Name().find("right") != std::string::npos)
86  {
87  double hackBaseline = 0.0;
88  if (_sdf->HasElement("hackBaseline"))
89  hackBaseline = _sdf->Get<double>("hackBaseline");
90  util->Load(_parent, _sdf, "/right", hackBaseline);
91  }
92  this->utils.push_back(util);
93  }
94 }
95 
97 
98 void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image,
100 {
101 # if GAZEBO_MAJOR_VERSION >= 7
102  common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime();
103 # else
104  common::Time sensor_update_time = util->parentSensor_->GetLastMeasurementTime();
105 # endif
106 
107  if (util->parentSensor_->IsActive())
108  {
109  if (sensor_update_time - util->last_update_time_ >= util->update_period_)
110  {
111  util->PutCameraData(_image, sensor_update_time);
112  util->PublishCameraInfo(sensor_update_time);
113  util->last_update_time_ = sensor_update_time;
114  }
115  }
116 }
117 
118 // Update the controller
119 void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
120  unsigned int _width, unsigned int _height, unsigned int _depth,
121  const std::string &_format)
122 {
123  OnNewFrame(_image, this->utils[0]);
124 }
125 
127 // Update the controller
128 void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image,
129  unsigned int _width, unsigned int _height, unsigned int _depth,
130  const std::string &_format)
131 {
132  OnNewFrame(_image, this->utils[1]);
133 }
134 }
std::vector< rendering::CameraPtr > camera
virtual void OnNewFrameLeft(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller FIXME: switch to function vectors.
ROSCPP_DECL bool isInitialized()
boost::shared_ptr< bool > was_active_
Keep track when we activate this camera through ros subscription, was it already active? resume state when unsubscribed.
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
std::vector< GazeboRosCameraUtils * > utils
std::vector< unsigned int > width
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
boost::shared_ptr< boost::mutex > image_connect_count_lock_
virtual void OnNewFrameRight(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
sensors::MultiCameraSensorPtr parentSensor
#define ROS_FATAL_STREAM_NAMED(name, args)
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
std::vector< unsigned int > depth
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
boost::shared_ptr< int > image_connect_count_
std::vector< unsigned int > height
boost::shared_ptr< bool > was_active_
void PublishCameraInfo(ros::Publisher camera_info_publisher)
Publish CameraInfo to the ROS topic.
void OnNewFrame(const unsigned char *_image, GazeboRosCameraUtils *util)
std::vector< std::string > format
boost::shared_ptr< boost::mutex > image_connect_count_lock_
A mutex to lock access to image_connect_count_.


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52