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   this->sensor_update_time_ = this->parentSensor_->GetLastUpdateTime();
00080 
00081   if (!this->parentSensor->IsActive())
00082   {
00083     if ((*this->image_connect_count_) > 0)
00084       // do this first so there's chance for sensor to run once after activated
00085       this->parentSensor->SetActive(true);
00086   }
00087   else
00088   {
00089     if ((*this->image_connect_count_) > 0)
00090     {
00091       common::Time cur_time = this->world_->GetSimTime();
00092       if (cur_time - this->last_update_time_ >= this->update_period_)
00093       {
00094         this->PutCameraData(_image);
00095         this->PublishCameraInfo();
00096         this->last_update_time_ = cur_time;
00097       }
00098     }
00099   }
00100 }
00101 }


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