25 #include <gazebo/sensors/Sensor.hh> 26 #include <gazebo/sensors/MultiCameraSensor.hh> 27 #include <gazebo/sensors/SensorTypes.hh> 34 GZ_REGISTER_SENSOR_PLUGIN(GazeboRosMultiCamera)
56 ROS_FATAL_STREAM_NAMED(
"multicamera",
"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)");
67 for (
unsigned i = 0; i < this->
camera.size(); ++i)
80 if (this->
camera[i]->Name().find(
"left") != std::string::npos)
83 util->
Load(_parent, _sdf,
"/left", 0.0);
85 else if (this->
camera[i]->Name().find(
"right") != std::string::npos)
87 double hackBaseline = 0.0;
88 if (_sdf->HasElement(
"hackBaseline"))
89 hackBaseline = _sdf->Get<
double>(
"hackBaseline");
90 util->
Load(_parent, _sdf,
"/right", hackBaseline);
92 this->
utils.push_back(util);
101 # if GAZEBO_MAJOR_VERSION >= 7 102 common::Time sensor_update_time = util->
parentSensor_->LastMeasurementTime();
104 common::Time sensor_update_time = util->
parentSensor_->GetLastMeasurementTime();
120 unsigned int _width,
unsigned int _height,
unsigned int _depth,
121 const std::string &_format)
129 unsigned int _width,
unsigned int _height,
unsigned int _depth,
130 const std::string &_format)
std::vector< rendering::CameraPtr > camera
virtual void OnNewFrameLeft(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller FIXME: switch to function vectors.
~GazeboRosMultiCamera()
Destructor.
ROSCPP_DECL bool isInitialized()
boost::shared_ptr< bool > was_active_
Keep track when we activate this camera through ros subscription, was it already active? resume state when unsubscribed.
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
std::vector< GazeboRosCameraUtils * > utils
std::vector< unsigned int > width
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
boost::shared_ptr< boost::mutex > image_connect_count_lock_
virtual void OnNewFrameRight(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
sensors::MultiCameraSensorPtr parentSensor
#define ROS_FATAL_STREAM_NAMED(name, args)
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
virtual void Load(sensors::SensorPtr _sensor, sdf::ElementPtr _sdf)
std::vector< unsigned int > depth
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
rendering::CameraPtr camera_
boost::shared_ptr< int > image_connect_count_
std::vector< unsigned int > height
common::Time last_update_time_
boost::shared_ptr< bool > was_active_
void PublishCameraInfo(ros::Publisher camera_info_publisher)
Publish CameraInfo to the ROS topic.
void OnNewFrame(const unsigned char *_image, GazeboRosCameraUtils *util)
std::vector< std::string > format
sensors::SensorPtr parentSensor_
boost::shared_ptr< boost::mutex > image_connect_count_lock_
A mutex to lock access to image_connect_count_.