gazebo_ros_triggered_multicamera.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 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 #include <string>
19 
20 #include <gazebo/sensors/Sensor.hh>
21 #include <gazebo/sensors/MultiCameraSensor.hh>
22 #include <gazebo/sensors/SensorTypes.hh>
23 
25 
26 namespace gazebo
27 {
28 // Register this plugin with the simulator
29 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosTriggeredMultiCamera)
30 
31 // Constructor
34 {
35 }
36 
38 // Destructor
40 {
41 }
42 
43 void GazeboRosTriggeredMultiCamera::Load(sensors::SensorPtr _parent,
44  sdf::ElementPtr _sdf)
45 {
46  MultiCameraPlugin::Load(_parent, _sdf);
47 
48  // Make sure the ROS node for Gazebo has already been initialized
49  if (!ros::isInitialized())
50  {
51  ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
52  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
53  return;
54  }
55 
56  // initialize shared_ptr members
57  this->image_connect_count_ = boost::shared_ptr<int>(new int(0));
59  this->was_active_ = boost::shared_ptr<bool>(new bool(false));
60 
61  // copying from CameraPlugin into GazeboRosCameraUtils
62  for (unsigned i = 0; i < this->camera.size(); ++i)
63  {
65  cam->parentSensor_ = this->parentSensor;
66  cam->width_ = this->width[i];
67  cam->height_ = this->height[i];
68  cam->depth_ = this->depth[i];
69  cam->format_ = this->format[i];
70  cam->camera_ = this->camera[i];
71  // Set up a shared connection counter
74  cam->was_active_ = this->was_active_;
75  if (this->camera[i]->Name().find("left") != std::string::npos)
76  {
77  // FIXME: hardcoded, left hack_baseline_ 0
78  cam->Load(_parent, _sdf, "/left", 0.0);
79  }
80  else if (this->camera[i]->Name().find("right") != std::string::npos)
81  {
82  double hackBaseline = 0.0;
83  if (_sdf->HasElement("hackBaseline"))
84  hackBaseline = _sdf->Get<double>("hackBaseline");
85  cam->Load(_parent, _sdf, "/right", hackBaseline);
86  }
87  this->triggered_cameras.push_back(cam);
88  }
89 }
90 
92 // Update the controller
93 void GazeboRosTriggeredMultiCamera::OnNewFrameLeft(const unsigned char *_image,
94  unsigned int _width, unsigned int _height, unsigned int _depth,
95  const std::string &_format)
96 {
98  cam->OnNewFrame(_image, _width, _height, _depth, _format);
99 }
100 
102 // Update the controller
103 void GazeboRosTriggeredMultiCamera::OnNewFrameRight(const unsigned char *_image,
104  unsigned int _width, unsigned int _height, unsigned int _depth,
105  const std::string &_format)
106 {
108  cam->OnNewFrame(_image, _width, _height, _depth, _format);
109 }
110 }
std::vector< rendering::CameraPtr > camera
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.
std::vector< GazeboRosTriggeredCamera * > triggered_cameras
std::vector< unsigned int > width
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
sensors::MultiCameraSensorPtr parentSensor
#define ROS_FATAL_STREAM(args)
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
boost::shared_ptr< boost::mutex > image_connect_count_lock_
virtual void OnNewFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
std::vector< unsigned int > depth
std::vector< unsigned int > height
virtual void OnNewFrameRight(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
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.
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