gazebo_ros_triggered_camera.cpp
Go to the documentation of this file.
1 /*
2  * Copyright 2017 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 
19 
20 #include <float.h>
21 #include <string>
22 
23 #include <gazebo/sensors/Sensor.hh>
24 #include <gazebo/sensors/CameraSensor.hh>
25 #include <gazebo/sensors/SensorTypes.hh>
26 
27 namespace gazebo
28 {
29 
30 // Register this plugin with the simulator
31 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosTriggeredCamera)
32 
33 // Constructor
36 {
37 }
38 
40 // Destructor
42 {
43  ROS_DEBUG_STREAM_NAMED("camera","Unloaded");
44 }
45 
46 void GazeboRosTriggeredCamera::Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
47 {
48  // Make sure the ROS node for Gazebo has already been initialized
49  if (!ros::isInitialized())
50  {
51  ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
52  << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
53  return;
54  }
55 
56  CameraPlugin::Load(_parent, _sdf);
57  // copying from CameraPlugin into GazeboRosTriggeredCameraUtils
58  this->parentSensor_ = this->parentSensor;
59  this->width_ = this->width;
60  this->height_ = this->height;
61  this->depth_ = this->depth;
62  this->format_ = this->format;
63  this->camera_ = this->camera;
64 
65  GazeboRosCameraUtils::Load(_parent, _sdf);
66 
67  this->SetCameraEnabled(false);
68  this->preRenderConnection_ =
69  event::Events::ConnectPreRender(
70  std::bind(&GazeboRosTriggeredCamera::PreRender, this));
71 }
72 
73 void GazeboRosTriggeredCamera::Load(sensors::SensorPtr _parent,
74  sdf::ElementPtr _sdf,
75  const std::string &_camera_name_suffix,
76  double _hack_baseline)
77 {
78  GazeboRosCameraUtils::Load(_parent, _sdf, _camera_name_suffix, _hack_baseline);
79 
80  this->SetCameraEnabled(false);
81  this->preRenderConnection_ =
82  event::Events::ConnectPreRender(
83  std::bind(&GazeboRosTriggeredCamera::PreRender, this));
84 }
85 
87 // Update the controller
88 void GazeboRosTriggeredCamera::OnNewFrame(const unsigned char *_image,
89  unsigned int _width, unsigned int _height, unsigned int _depth,
90  const std::string &_format)
91 {
92  this->sensor_update_time_ = this->parentSensor_->LastMeasurementTime();
93 
94  if ((*this->image_connect_count_) > 0)
95  {
96  this->PutCameraData(_image);
97  this->PublishCameraInfo();
98  }
99  this->SetCameraEnabled(false);
100 
101  std::lock_guard<std::mutex> lock(this->mutex);
102  this->triggered = std::max(this->triggered-1, 0);
103 }
104 
106 {
107  std::lock_guard<std::mutex> lock(this->mutex);
108  if (!this->parentSensor_)
109  return;
110  this->triggered++;
111 }
112 
114 {
115  return true;
116 }
117 
119 {
120  std::lock_guard<std::mutex> lock(this->mutex);
121  if (this->triggered > 0)
122  {
123  this->SetCameraEnabled(true);
124  }
125 }
126 
128 {
129  this->parentSensor_->SetActive(_enabled);
130  this->parentSensor_->SetUpdateRate(_enabled ? 0.0 : DBL_MIN);
131 }
132 
133 }
#define ROS_DEBUG_STREAM_NAMED(name, args)
ROSCPP_DECL bool isInitialized()
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
#define ROS_FATAL_STREAM(args)
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
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 PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.


gazebo_plugins
Author(s): John Hsu
autogenerated on Tue Apr 6 2021 02:19:39