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 (this->camera[i]->GetName().find("left") != std::string::npos)
00081     {
00082       // FIXME: hardcoded, left hack_baseline_ 0
00083       util->Load(_parent, _sdf, "/left", 0.0);
00084     }
00085     else if (this->camera[i]->GetName().find("right") != std::string::npos)
00086     {
00087       double hackBaseline = 0.0;
00088       if (_sdf->HasElement("hackBaseline"))
00089         hackBaseline = _sdf->Get<double>("hackBaseline");
00090       util->Load(_parent, _sdf, "/right", hackBaseline);
00091     }
00092     this->utils.push_back(util);
00093   }
00094 }
00095 
00097 // Update the controller
00098 void GazeboRosMultiCamera::OnNewFrameLeft(const unsigned char *_image,
00099     unsigned int _width, unsigned int _height, unsigned int _depth,
00100     const std::string &_format)
00101 {
00102   GazeboRosCameraUtils* util = this->utils[0];
00103   util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime();
00104 
00105   if (util->parentSensor_->IsActive())
00106   {
00107     common::Time cur_time = util->world_->GetSimTime();
00108     if (cur_time - util->last_update_time_ >= util->update_period_)
00109     {
00110       util->PutCameraData(_image);
00111       util->PublishCameraInfo();
00112       util->last_update_time_ = cur_time;
00113     }
00114   }
00115 }
00116 
00118 // Update the controller
00119 void GazeboRosMultiCamera::OnNewFrameRight(const unsigned char *_image,
00120     unsigned int _width, unsigned int _height, unsigned int _depth,
00121     const std::string &_format)
00122 {
00123   GazeboRosCameraUtils* util = this->utils[1];
00124   util->sensor_update_time_ = util->parentSensor_->GetLastUpdateTime();
00125 
00126   if (util->parentSensor_->IsActive())
00127   {
00128     common::Time cur_time = util->world_->GetSimTime();
00129     if (cur_time - util->last_update_time_ >= util->update_period_)
00130     {
00131       util->PutCameraData(_image);
00132       util->PublishCameraInfo();
00133       util->last_update_time_ = cur_time;
00134     }
00135   }
00136 }
00137 }


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25