MultiCameraPlugin.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 #include <gazebo/sensors/DepthCameraSensor.hh>
18 #include <gazebo/sensors/CameraSensor.hh>
21 
22 using namespace gazebo;
23 GZ_REGISTER_SENSOR_PLUGIN(MultiCameraPlugin)
24 
25 MultiCameraPlugin::MultiCameraPlugin() : SensorPlugin()
27 {
28 }
29 
31 MultiCameraPlugin::~MultiCameraPlugin()
32 {
33  this->parentSensor.reset();
34  this->camera.clear();
35 }
36 
38 void MultiCameraPlugin::Load(sensors::SensorPtr _sensor,
39  sdf::ElementPtr /*_sdf*/)
40 {
41  if (!_sensor)
42  gzerr << "Invalid sensor pointer.\n";
43 
45  this->parentSensor =
46  dynamic_pointer_cast<sensors::MultiCameraSensor>(_sensor);
47 
48  if (!this->parentSensor)
49  {
50  gzerr << "MultiCameraPlugin requires a CameraSensor.\n";
51  if (dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
52  gzmsg << "It is a depth camera sensor\n";
53  if (dynamic_pointer_cast<sensors::CameraSensor>(_sensor))
54  gzmsg << "It is a camera sensor\n";
55  }
56 
57  if (!this->parentSensor)
58  {
59  gzerr << "MultiCameraPlugin not attached to a camera sensor\n";
60  return;
61  }
62 
63  for (unsigned int i = 0; i < this->parentSensor->CameraCount(); ++i)
64  {
65  this->camera.push_back(this->parentSensor->Camera(i));
66 
67  // save camera attributes
68  this->width.push_back(this->camera[i]->ImageWidth());
69  this->height.push_back(this->camera[i]->ImageHeight());
70  this->depth.push_back(this->camera[i]->ImageDepth());
71  this->format.push_back(this->camera[i]->ImageFormat());
72 
73  std::string cameraName = this->parentSensor->Camera(i)->Name();
74  // gzdbg << "camera(" << i << ") name [" << cameraName << "]\n";
75 
76  // FIXME: hardcoded 2 camera support only
77  if (cameraName.find("left") != std::string::npos)
78  {
79  this->newFrameConnection.push_back(this->camera[i]->ConnectNewImageFrame(
81  this, _1, _2, _3, _4, _5)));
82  }
83  else if (cameraName.find("right") != std::string::npos)
84  {
85  this->newFrameConnection.push_back(this->camera[i]->ConnectNewImageFrame(
87  this, _1, _2, _3, _4, _5)));
88  }
89  }
90 
91  this->parentSensor->SetActive(true);
92 }
93 
95 void MultiCameraPlugin::OnNewFrameLeft(const unsigned char * /*_image*/,
96  unsigned int /*_width*/,
97  unsigned int /*_height*/,
98  unsigned int /*_depth*/,
99  const std::string &/*_format*/)
100 {
101  /*rendering::Camera::SaveFrame(_image, this->width,
102  this->height, this->depth, this->format,
103  "/tmp/camera/me.jpg");
104  */
105 }
106 
108 void MultiCameraPlugin::OnNewFrameRight(const unsigned char * /*_image*/,
109  unsigned int /*_width*/,
110  unsigned int /*_height*/,
111  unsigned int /*_depth*/,
112  const std::string &/*_format*/)
113 {
114  /*rendering::Camera::SaveFrame(_image, this->width,
115  this->height, this->depth, this->format,
116  "/tmp/camera/me.jpg");
117  */
118 }
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)
std::vector< unsigned int > width
sensors::MultiCameraSensorPtr parentSensor
virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
std::vector< unsigned int > depth
std::vector< unsigned int > height
#define GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST
std::vector< std::string > format
std::vector< event::ConnectionPtr > newFrameConnection
virtual void OnNewFrameRight(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)


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