gazebo_ros_multicamera.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 /*
00018  * Desc: Syncronizes shutters across multiple cameras
00019  * Author: John Hsu
00020  * Date: 10 June 2013
00021  */
00022 
00023 #include <string>
00024 
00025 #include <gazebo/sensors/Sensor.hh>
00026 #include <gazebo/sensors/MultiCameraSensor.hh>
00027 #include <gazebo/sensors/SensorTypes.hh>
00028 
00029 #include "gazebo_plugins/gazebo_ros_multicamera.h"
00030 
00031 namespace gazebo
00032 {
00033 // Register this plugin with the simulator
00034 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosMultiCamera)
00035 
00036 
00037 // Constructor
00038 GazeboRosMultiCamera::GazeboRosMultiCamera()
00039 {
00040 }
00041 
00043 // Destructor
00044 GazeboRosMultiCamera::~GazeboRosMultiCamera()
00045 {
00046 }
00047 
00048 void GazeboRosMultiCamera::Load(sensors::SensorPtr _parent,
00049   sdf::ElementPtr _sdf)
00050 {
00051   MultiCameraPlugin::Load(_parent, _sdf);
00052 
00053   // Make sure the ROS node for Gazebo has already been initialized
00054   if (!ros::isInitialized())
00055   {
00056     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00057       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00058     return;
00059   }
00060 
00061   // initialize shared_ptr members
00062   this->image_connect_count_ = boost::shared_ptr<int>(new int(0));
00063   this->image_connect_count_lock_ = boost::shared_ptr<boost::mutex>(new boost::mutex);
00064   this->was_active_ = boost::shared_ptr<bool>(new bool(false));
00065 
00066   // copying from CameraPlugin into GazeboRosCameraUtils
00067   for (unsigned i = 0; i < this->camera.size(); ++i)
00068   {
00069     GazeboRosCameraUtils* util = new GazeboRosCameraUtils();
00070     util->parentSensor_ = this->parentSensor;
00071     util->width_   = this->width[i];
00072     util->height_  = this->height[i];
00073     util->depth_   = this->depth[i];
00074     util->format_  = this->format[i];
00075     util->camera_  = this->camera[i];
00076     // Set up a shared connection counter
00077     util->image_connect_count_ = this->image_connect_count_;
00078     util->image_connect_count_lock_ = this->image_connect_count_lock_;
00079     util->was_active_ = this->was_active_;
00080 # if GAZEBO_MAJOR_VERSION >= 7
00081     if (this->camera[i]->Name().find("left") != std::string::npos)
00082 # else
00083     if (this->camera[i]->GetName().find("left") != std::string::npos)
00084 # endif
00085     {
00086       // FIXME: hardcoded, left hack_baseline_ 0
00087       util->Load(_parent, _sdf, "/left", 0.0);
00088     }
00089 # if GAZEBO_MAJOR_VERSION >= 7
00090     else if (this->camera[i]->Name().find("right") != std::string::npos)
00091 # else
00092     else if (this->camera[i]->GetName().find("right") != std::string::npos)
00093 # endif
00094     {
00095       double hackBaseline = 0.0;
00096       if (_sdf->HasElement("hackBaseline"))
00097         hackBaseline = _sdf->Get<double>("hackBaseline");
00098       util->Load(_parent, _sdf, "/right", hackBaseline);
00099     }
00100     this->utils.push_back(util);
00101   }
00102 }
00103 
00105 
00106 void GazeboRosMultiCamera::OnNewFrame(const unsigned char *_image,
00107     GazeboRosCameraUtils* util)
00108 {
00109 # if GAZEBO_MAJOR_VERSION >= 7
00110   common::Time sensor_update_time = util->parentSensor_->LastMeasurementTime();
00111 # else
00112   common::Time sensor_update_time = util->parentSensor_->GetLastMeasurementTime();
00113 # endif
00114 
00115   if (util->parentSensor_->IsActive())
00116   {
00117     if (sensor_update_time - util->last_update_time_ >= util->update_period_)
00118     {
00119       util->PutCameraData(_image, sensor_update_time);
00120       util->PublishCameraInfo(sensor_update_time);
00121       util->last_update_time_ = sensor_update_time;
00122     }
00123   }
00124 }
00125 
00126 // Update the controller
00127 void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
00128     unsigned int _width, unsigned int _height, unsigned int _depth,
00129     const std::string &_format)
00130 {
00131   OnNewFrame(_image, this->utils[0]);
00132 }
00133 
00135 // Update the controller
00136 void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image,
00137     unsigned int _width, unsigned int _height, unsigned int _depth,
00138     const std::string &_format)
00139 {
00140   OnNewFrame(_image, this->utils[1]);
00141 }
00142 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Jun 6 2019 18:41:09