gazebo_ros_camera.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 /*
00019  @mainpage
00020    Desc: GazeboRosCamera plugin for simulating cameras in Gazebo
00021    Author: John Hsu
00022    Date: 24 Sept 2008
00023 */
00024 
00025 #include "gazebo_plugins/gazebo_ros_camera.h"
00026 
00027 #include <string>
00028 
00029 #include <gazebo/sensors/Sensor.hh>
00030 #include <gazebo/sensors/CameraSensor.hh>
00031 #include <gazebo/sensors/SensorTypes.hh>
00032 
00033 namespace gazebo
00034 {
00035 // Register this plugin with the simulator
00036 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosCamera)
00037 
00038 
00039 // Constructor
00040 GazeboRosCamera::GazeboRosCamera()
00041 {
00042 }
00043 
00045 // Destructor
00046 GazeboRosCamera::~GazeboRosCamera()
00047 {
00048   ROS_DEBUG_STREAM_NAMED("camera","Unloaded");
00049 }
00050 
00051 void GazeboRosCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _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   CameraPlugin::Load(_parent, _sdf);
00062   // copying from CameraPlugin into GazeboRosCameraUtils
00063   this->parentSensor_ = this->parentSensor;
00064   this->width_ = this->width;
00065   this->height_ = this->height;
00066   this->depth_ = this->depth;
00067   this->format_ = this->format;
00068   this->camera_ = this->camera;
00069 
00070   GazeboRosCameraUtils::Load(_parent, _sdf);
00071 }
00072 
00074 // Update the controller
00075 void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
00076     unsigned int _width, unsigned int _height, unsigned int _depth,
00077     const std::string &_format)
00078 {
00079 # if GAZEBO_MAJOR_VERSION >= 7
00080   common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
00081 # else
00082   common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
00083 # endif
00084 
00085   if (!this->parentSensor->IsActive())
00086   {
00087     if ((*this->image_connect_count_) > 0)
00088       // do this first so there's chance for sensor to run once after activated
00089       this->parentSensor->SetActive(true);
00090   }
00091   else
00092   {
00093     if ((*this->image_connect_count_) > 0)
00094     {
00095       // OnNewFrame is triggered at the gazebo sensor <update_rate>
00096       // while there is also a plugin <updateRate> that can throttle the
00097       // rate down further (but then why not reduce the sensor rate?
00098       // what is the use case?).
00099       // Setting the <updateRate> to zero will make this plugin
00100       // update at the gazebo sensor <update_rate>, update_period_ will be
00101       // zero and the conditional always will be true.
00102       if (sensor_update_time - this->last_update_time_ >= this->update_period_)
00103       {
00104         this->PutCameraData(_image, sensor_update_time);
00105         this->PublishCameraInfo(sensor_update_time);
00106         this->last_update_time_ = sensor_update_time;
00107       }
00108     }
00109   }
00110 }
00111 }


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