29 #include <gazebo/sensors/Sensor.hh> 30 #include <gazebo/sensors/CameraSensor.hh> 31 #include <gazebo/sensors/SensorTypes.hh> 36 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosCamera)
57 <<
"Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
61 CameraPlugin::Load(_parent, _sdf);
64 this->
width_ = this->width;
66 this->
depth_ = this->depth;
76 unsigned int _width,
unsigned int _height,
unsigned int _depth,
77 const std::string &_format)
79 common::Time sensor_update_time = this->
parentSensor_->LastMeasurementTime();
81 if (!this->parentSensor->IsActive())
85 this->parentSensor->SetActive(
true);
93 ROS_WARN_NAMED(
"camera",
"Negative sensor update time difference detected.");
94 this->last_update_time_ = sensor_update_time;
104 if (sensor_update_time - this->last_update_time_ >= this->
update_period_)
108 this->last_update_time_ = sensor_update_time;
#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.
rendering::CameraPtr camera_
~GazeboRosCamera()
Destructor.
common::Time last_update_time_
sensors::SensorPtr parentSensor_