MultiCameraPlugin.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright 2013 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 #include <gazebo/sensors/DepthCameraSensor.hh>
00018 #include <gazebo/sensors/CameraSensor.hh>
00019 #include <gazebo_plugins/MultiCameraPlugin.h>
00020 #include <gazebo_plugins/gazebo_ros_utils.h>
00021 
00022 using namespace gazebo;
00023 GZ_REGISTER_SENSOR_PLUGIN(MultiCameraPlugin)
00024 
00025 
00026 MultiCameraPlugin::MultiCameraPlugin() : SensorPlugin()
00027 {
00028 }
00029 
00031 MultiCameraPlugin::~MultiCameraPlugin()
00032 {
00033   this->parentSensor.reset();
00034   this->camera.clear();
00035 }
00036 
00038 void MultiCameraPlugin::Load(sensors::SensorPtr _sensor,
00039   sdf::ElementPtr /*_sdf*/)
00040 {
00041   if (!_sensor)
00042     gzerr << "Invalid sensor pointer.\n";
00043 
00044   GAZEBO_SENSORS_USING_DYNAMIC_POINTER_CAST;
00045   this->parentSensor =
00046     dynamic_pointer_cast<sensors::MultiCameraSensor>(_sensor);
00047 
00048   if (!this->parentSensor)
00049   {
00050     gzerr << "MultiCameraPlugin requires a CameraSensor.\n";
00051     if (dynamic_pointer_cast<sensors::DepthCameraSensor>(_sensor))
00052       gzmsg << "It is a depth camera sensor\n";
00053     if (dynamic_pointer_cast<sensors::CameraSensor>(_sensor))
00054       gzmsg << "It is a camera sensor\n";
00055   }
00056 
00057   if (!this->parentSensor)
00058   {
00059     gzerr << "MultiCameraPlugin not attached to a camera sensor\n";
00060     return;
00061   }
00062 
00063 # if GAZEBO_MAJOR_VERSION >= 7
00064   for (unsigned int i = 0; i < this->parentSensor->CameraCount(); ++i)
00065 # else
00066   for (unsigned int i = 0; i < this->parentSensor->GetCameraCount(); ++i)
00067 # endif
00068   {
00069 # if GAZEBO_MAJOR_VERSION >= 7
00070     this->camera.push_back(this->parentSensor->Camera(i));
00071 # else
00072     this->camera.push_back(this->parentSensor->GetCamera(i));
00073 # endif
00074 
00075     // save camera attributes
00076 # if GAZEBO_MAJOR_VERSION >= 7
00077     this->width.push_back(this->camera[i]->ImageWidth());
00078     this->height.push_back(this->camera[i]->ImageHeight());
00079     this->depth.push_back(this->camera[i]->ImageDepth());
00080     this->format.push_back(this->camera[i]->ImageFormat());
00081 # else
00082     this->width.push_back(this->camera[i]->GetImageWidth());
00083     this->height.push_back(this->camera[i]->GetImageHeight());
00084     this->depth.push_back(this->camera[i]->GetImageDepth());
00085     this->format.push_back(this->camera[i]->GetImageFormat());
00086 # endif
00087 
00088 # if GAZEBO_MAJOR_VERSION >= 7
00089     std::string cameraName = this->parentSensor->Camera(i)->Name();
00090 # else
00091     std::string cameraName = this->parentSensor->GetCamera(i)->GetName();
00092 # endif
00093     // gzdbg << "camera(" << i << ") name [" << cameraName << "]\n";
00094 
00095     // FIXME: hardcoded 2 camera support only
00096     if (cameraName.find("left") != std::string::npos)
00097     {
00098       this->newFrameConnection.push_back(this->camera[i]->ConnectNewImageFrame(
00099         boost::bind(&MultiCameraPlugin::OnNewFrameLeft,
00100         this, _1, _2, _3, _4, _5)));
00101     }
00102     else if (cameraName.find("right") != std::string::npos)
00103     {
00104       this->newFrameConnection.push_back(this->camera[i]->ConnectNewImageFrame(
00105         boost::bind(&MultiCameraPlugin::OnNewFrameRight,
00106         this, _1, _2, _3, _4, _5)));
00107     }
00108   }
00109 
00110   this->parentSensor->SetActive(true);
00111 }
00112 
00114 void MultiCameraPlugin::OnNewFrameLeft(const unsigned char * /*_image*/,
00115                               unsigned int /*_width*/,
00116                               unsigned int /*_height*/,
00117                               unsigned int /*_depth*/,
00118                               const std::string &/*_format*/)
00119 {
00120   /*rendering::Camera::SaveFrame(_image, this->width,
00121     this->height, this->depth, this->format,
00122     "/tmp/camera/me.jpg");
00123     */
00124 }
00125 
00127 void MultiCameraPlugin::OnNewFrameRight(const unsigned char * /*_image*/,
00128                               unsigned int /*_width*/,
00129                               unsigned int /*_height*/,
00130                               unsigned int /*_depth*/,
00131                               const std::string &/*_format*/)
00132 {
00133   /*rendering::Camera::SaveFrame(_image, this->width,
00134     this->height, this->depth, this->format,
00135     "/tmp/camera/me.jpg");
00136     */
00137 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:23