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 #ifdef ENABLE_PROFILER
32 #include <ignition/common/Profiler.hh>
33 #endif
34 
35 namespace gazebo
36 {
37 // Register this plugin with the simulator
38 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosMultiCamera)
39 
40 // Constructor
43 {
44 }
45 
47 // Destructor
49 {
50 }
51 
52 void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent,
53  sdf::ElementPtr _sdf)
54 {
55  MultiCameraPlugin::Load(_parent, _sdf);
56 
57  // Make sure the ROS node for Gazebo has already been initialized
58  if (!ros::isInitialized())
59  {
60  ROS_FATAL_STREAM_NAMED("multicamera", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
61  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
62  return;
63  }
64 
65  // initialize shared_ptr members
66  this->image_connect_count_ = boost::shared_ptr<int>(new int(0));
68  this->was_active_ = boost::shared_ptr<bool>(new bool(false));
69 
70  // copying from CameraPlugin into GazeboRosCameraUtils
71  for (unsigned i = 0; i < this->camera.size(); ++i)
72  {
74  util->parentSensor_ = this->parentSensor;
75  util->width_ = this->width[i];
76  util->height_ = this->height[i];
77  util->depth_ = this->depth[i];
78  util->format_ = this->format[i];
79  util->camera_ = this->camera[i];
80  // Set up a shared connection counter
83  util->was_active_ = this->was_active_;
84  if (this->camera[i]->Name().find("left") != std::string::npos)
85  {
86  // FIXME: hardcoded, left hack_baseline_ 0
87  util->Load(_parent, _sdf, "/left", 0.0);
88  }
89  else if (this->camera[i]->Name().find("right") != std::string::npos)
90  {
91  double hackBaseline = 0.0;
92  if (_sdf->HasElement("hackBaseline"))
93  hackBaseline = _sdf->Get<double>("hackBaseline");
94  util->Load(_parent, _sdf, "/right", hackBaseline);
95  }
96  this->utils.push_back(util);
97  }
98 }
99 
101 
102 void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image,
103  GazeboRosCameraUtils* util)
104 {
105 #ifdef ENABLE_PROFILER
106  IGN_PROFILE("GazeboRosMultiCamera::OnNewFrame");
107 #endif
108 # if GAZEBO_MAJOR_VERSION >= 7
109  common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime();
110 # else
111  common::Time sensor_update_time = util->parentSensor_->GetLastMeasurementTime();
112 # endif
113 
114  if (util->parentSensor_->IsActive())
115  {
116  if (sensor_update_time - util->last_update_time_ >= util->update_period_)
117  {
118 #ifdef ENABLE_PROFILER
119  IGN_PROFILE_BEGIN("PutCameraData");
120 #endif
121  util->PutCameraData(_image, sensor_update_time);
122 #ifdef ENABLE_PROFILER
123  IGN_PROFILE_END();
124  IGN_PROFILE_BEGIN("PublishCameraInfo");
125 #endif
126  util->PublishCameraInfo(sensor_update_time);
127 #ifdef ENABLE_PROFILER
128  IGN_PROFILE_END();
129 #endif
130  util->last_update_time_ = sensor_update_time;
131  }
132  }
133 }
134 
135 // Update the controller
136 void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
137  unsigned int _width, unsigned int _height, unsigned int _depth,
138  const std::string &_format)
139 {
140  OnNewFrame(_image, this->utils[0]);
141 }
142 
144 // Update the controller
145 void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image,
146  unsigned int _width, unsigned int _height, unsigned int _depth,
147  const std::string &_format)
148 {
149  OnNewFrame(_image, this->utils[1]);
150 }
151 }
gazebo::MultiCameraPlugin::Load
virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
Definition: MultiCameraPlugin.cpp:38
gazebo::GazeboRosMultiCamera::OnNewFrame
void OnNewFrame(const unsigned char *_image, GazeboRosCameraUtils *util)
Definition: gazebo_ros_multicamera.cpp:102
boost::shared_ptr< int >
gazebo::GazeboRosCameraUtils::parentSensor_
sensors::SensorPtr parentSensor_
Definition: gazebo_ros_camera_utils.h:193
gazebo
gazebo::GazeboRosMultiCamera::image_connect_count_lock_
boost::shared_ptr< boost::mutex > image_connect_count_lock_
Definition: gazebo_ros_multicamera.h:60
gazebo::GazeboRosMultiCamera::image_connect_count_
boost::shared_ptr< int > image_connect_count_
Definition: gazebo_ros_multicamera.h:59
gazebo::GazeboRosMultiCamera
Definition: gazebo_ros_multicamera.h:30
gazebo::GazeboRosCameraUtils::PutCameraData
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
Definition: gazebo_ros_camera_utils.cpp:639
gazebo::MultiCameraPlugin::width
std::vector< unsigned int > width
Definition: MultiCameraPlugin.h:48
gazebo::MultiCameraPlugin::depth
std::vector< unsigned int > depth
Definition: MultiCameraPlugin.h:48
gazebo::GazeboRosCameraUtils::last_update_time_
common::Time last_update_time_
Definition: gazebo_ros_camera_utils.h:146
gazebo::GazeboRosMultiCamera::utils
std::vector< GazeboRosCameraUtils * > utils
Definition: gazebo_ros_multicamera.h:43
gazebo::GazeboRosCameraUtils::width_
unsigned int width_
Definition: gazebo_ros_camera_utils.h:190
gazebo::MultiCameraPlugin::parentSensor
sensors::MultiCameraSensorPtr parentSensor
Definition: MultiCameraPlugin.h:46
gazebo::GazeboRosCameraUtils::PublishCameraInfo
void PublishCameraInfo(ros::Publisher camera_info_publisher)
Publish CameraInfo to the ROS topic.
Definition: gazebo_ros_camera_utils.cpp:690
gazebo::GazeboRosCameraUtils
Definition: gazebo_ros_camera_utils.h:56
gazebo::GazeboRosCameraUtils::image_connect_count_lock_
boost::shared_ptr< boost::mutex > image_connect_count_lock_
A mutex to lock access to image_connect_count_.
Definition: gazebo_ros_camera_utils.h:94
gazebo::GazeboRosCameraUtils::height_
unsigned int height_
Definition: gazebo_ros_camera_utils.h:190
gazebo::GazeboRosMultiCamera::OnNewFrameLeft
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.
Definition: gazebo_ros_multicamera.cpp:136
gazebo::GazeboRosMultiCamera::was_active_
boost::shared_ptr< bool > was_active_
Definition: gazebo_ros_multicamera.h:61
gazebo::GazeboRosMultiCamera::~GazeboRosMultiCamera
~GazeboRosMultiCamera()
Destructor.
Definition: gazebo_ros_multicamera.cpp:48
ROS_FATAL_STREAM_NAMED
#define ROS_FATAL_STREAM_NAMED(name, args)
gazebo::MultiCameraPlugin::camera
std::vector< rendering::CameraPtr > camera
Definition: MultiCameraPlugin.h:51
ros::isInitialized
ROSCPP_DECL bool isInitialized()
gazebo::GazeboRosCameraUtils::image_connect_count_
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
Definition: gazebo_ros_camera_utils.h:92
gazebo::GazeboRosCameraUtils::update_period_
double update_period_
Definition: gazebo_ros_camera_utils.h:145
gazebo::GazeboRosCameraUtils::depth_
unsigned int depth_
Definition: gazebo_ros_camera_utils.h:190
gazebo::GazeboRosMultiCamera::OnNewFrameRight
virtual void OnNewFrameRight(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Definition: gazebo_ros_multicamera.cpp:145
gazebo::GazeboRosCameraUtils::Load
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
Definition: gazebo_ros_camera_utils.cpp:104
gazebo_ros_multicamera.h
gazebo::GazeboRosCameraUtils::was_active_
boost::shared_ptr< bool > was_active_
Keep track when we activate this camera through ros subscription, was it already active?...
Definition: gazebo_ros_camera_utils.h:101
gazebo::GazeboRosMultiCamera::Load
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
Definition: gazebo_ros_multicamera.cpp:52
gazebo::MultiCameraPlugin::format
std::vector< std::string > format
Definition: MultiCameraPlugin.h:49
gazebo::GazeboRosCameraUtils::format_
std::string format_
Definition: gazebo_ros_camera_utils.h:191
gazebo::MultiCameraPlugin::height
std::vector< unsigned int > height
Definition: MultiCameraPlugin.h:48
gazebo::GazeboRosCameraUtils::camera_
rendering::CameraPtr camera_
Definition: gazebo_ros_camera_utils.h:194


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Apr 23 2021 02:48:24