gazebo_ros_camera.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2013 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 
18 /*
19  @mainpage
20  Desc: GazeboRosCamera plugin for simulating cameras in Gazebo
21  Author: John Hsu
22  Date: 24 Sept 2008
23 */
24 
26 
27 #include <string>
28 
29 #include <gazebo/sensors/Sensor.hh>
30 #include <gazebo/sensors/CameraSensor.hh>
31 #include <gazebo/sensors/SensorTypes.hh>
32 
33 namespace gazebo
34 {
35 // Register this plugin with the simulator
36 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosCamera)
37 
38 // Constructor
41 {
42 }
43 
45 // Destructor
47 {
48  ROS_DEBUG_STREAM_NAMED("camera","Unloaded");
49 }
50 
51 void GazeboRosCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
52 {
53  // Make sure the ROS node for Gazebo has already been initialized
54  if (!ros::isInitialized())
55  {
56  ROS_FATAL_STREAM_NAMED("camera", "A ROS node for Gazebo has not been initialized, unable to load plugin. "
57  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
58  return;
59  }
60 
61  CameraPlugin::Load(_parent, _sdf);
62  // copying from CameraPlugin into GazeboRosCameraUtils
63  this->parentSensor_ = this->parentSensor;
64  this->width_ = this->width;
65  this->height_ = this->height;
66  this->depth_ = this->depth;
67  this->format_ = this->format;
68  this->camera_ = this->camera;
69 
70  GazeboRosCameraUtils::Load(_parent, _sdf);
71 }
72 
74 // Update the controller
75 void GazeboRosCamera::OnNewFrame(const unsigned char *_image,
76  unsigned int _width, unsigned int _height, unsigned int _depth,
77  const std::string &_format)
78 {
79 # if GAZEBO_MAJOR_VERSION >= 7
80  common::Time sensor_update_time = this->parentSensor_->LastMeasurementTime();
81 # else
82  common::Time sensor_update_time = this->parentSensor_->GetLastMeasurementTime();
83 # endif
84 
85  if (!this->parentSensor->IsActive())
86  {
87  if ((*this->image_connect_count_) > 0)
88  // do this first so there's chance for sensor to run once after activated
89  this->parentSensor->SetActive(true);
90  }
91  else
92  {
93  if ((*this->image_connect_count_) > 0)
94  {
95  if (sensor_update_time < this->last_update_time_)
96  {
97  ROS_WARN_NAMED("camera", "Negative sensor update time difference detected.");
98  this->last_update_time_ = sensor_update_time;
99  }
100 
101  // OnNewFrame is triggered at the gazebo sensor <update_rate>
102  // while there is also a plugin <updateRate> that can throttle the
103  // rate down further (but then why not reduce the sensor rate?
104  // what is the use case?).
105  // Setting the <updateRate> to zero will make this plugin
106  // update at the gazebo sensor <update_rate>, update_period_ will be
107  // zero and the conditional always will be true.
108  if (sensor_update_time - this->last_update_time_ >= this->update_period_)
109  {
110  this->PutCameraData(_image, sensor_update_time);
111  this->PublishCameraInfo(sensor_update_time);
112  this->last_update_time_ = sensor_update_time;
113  }
114  }
115  }
116 }
117 }
#define ROS_DEBUG_STREAM_NAMED(name, args)
#define ROS_WARN_NAMED(name,...)
ROSCPP_DECL bool isInitialized()
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
virtual void OnNewFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
#define ROS_FATAL_STREAM_NAMED(name, args)
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52