21 #include <boost/thread/thread.hpp> 22 #include <boost/bind.hpp> 26 #include <sensor_msgs/Image.h> 29 #include <geometry_msgs/Point32.h> 30 #include <sensor_msgs/ChannelFloat32.h> 35 #include <gazebo/physics/World.hh> 36 #include <gazebo/physics/HingeJoint.hh> 37 #include <gazebo/sensors/Sensor.hh> 38 #include <gazebo/common/Exception.hh> 39 #include <gazebo/sensors/CameraSensor.hh> 40 #include <gazebo/sensors/SensorTypes.hh> 41 #include <gazebo/rendering/Camera.hh> 42 #include <gazebo/rendering/Distortion.hh> 62 gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level)
66 ROS_INFO_NAMED(
"camera_utils",
"Reconfigure request for the gazebo ros camera_: %s. New rate: %.2f",
88 const std::string &_camera_name_suffix,
89 double _hack_baseline)
95 this->
Load(_parent, _sdf, _camera_name_suffix);
105 sdf::ElementPtr _sdf,
106 const std::string &_camera_name_suffix)
109 std::string world_name = _parent->WorldName();
112 this->
world_ = physics::get_world(world_name);
121 std::stringstream ss;
125 if (this->
sdf->HasElement(
"imageTopicName"))
129 if (this->
sdf->HasElement(
"triggerTopicName"))
133 if (this->
sdf->HasElement(
"cameraInfoTopicName"))
135 this->
sdf->Get<std::string>(
"cameraInfoTopicName");
137 if (!this->
sdf->HasElement(
"cameraName"))
138 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <cameraName>, default to empty");
146 if (!this->
sdf->HasElement(
"frameName"))
147 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <frameName>, defaults to /world");
151 if (!this->
sdf->HasElement(
"updateRate"))
153 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <updateRate>, defaults to unlimited (0).");
159 if (!this->
sdf->HasElement(
"CxPrime"))
161 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <CxPrime>, defaults to 0");
167 if (!this->
sdf->HasElement(
"Cx"))
169 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <Cx>, defaults to 0");
173 this->
cx_ = this->
sdf->Get<
double>(
"Cx");
175 if (!this->
sdf->HasElement(
"Cy"))
177 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <Cy>, defaults to 0");
181 this->
cy_ = this->
sdf->Get<
double>(
"Cy");
183 if (!this->
sdf->HasElement(
"focalLength"))
185 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <focalLength>, defaults to 0");
191 if (!this->
sdf->HasElement(
"hackBaseline"))
193 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <hackBaseline>, defaults to 0");
199 if (!this->
sdf->HasElement(
"distortionK1"))
201 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <distortionK1>, defaults to 0");
207 if (!this->
sdf->HasElement(
"distortionK2"))
209 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <distortionK2>, defaults to 0");
215 if (!this->
sdf->HasElement(
"distortionK3"))
217 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <distortionK3>, defaults to 0");
223 if (!this->
sdf->HasElement(
"distortionT1"))
225 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <distortionT1>, defaults to 0");
231 if (!this->
sdf->HasElement(
"distortionT2"))
233 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <distortionT2>, defaults to 0");
239 if (!this->
sdf->HasElement(
"borderCrop"))
241 ROS_DEBUG_NAMED(
"camera_utils",
"Camera plugin missing <borderCrop>, defaults to true");
269 gzerr <<
"Not loading plugin since ROS hasn't been " 270 <<
"properly initialized. Try starting gazebo with ros plugin:\n" 271 <<
" gazebo -s libgazebo_ros_api_plugin.so\n";
291 boost::trim_right_if(this->
tf_prefix_,boost::is_any_of(
"/"));
295 ROS_INFO_NAMED(
"camera_utils",
"Camera Plugin (ns = %s) <tf_prefix_>, set to \"%s\"",
302 new dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
304 dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
311 ROS_WARN_NAMED(
"camera_utils",
"dynamic reconfigure is not enabled for this image topic [%s]" 312 " because <cameraName> is not specified",
330 ros::AdvertiseOptions::create<sensor_msgs::CameraInfo>(
356 ros::SubscribeOptions::create<std_msgs::Empty>(
376 const std_msgs::Empty::ConstPtr &dummy)
385 this->
camera_->SetHFOV(ignition::math::Angle(hfov->data));
391 const std_msgs::Float64::ConstPtr& update_rate)
448 else if (this->
format_ ==
"R8G8B8" || this->
format_ ==
"RGB_INT8")
453 else if (this->
format_ ==
"B8G8R8" || this->
format_ ==
"BGR_INT8")
458 else if (this->
format_ ==
"R16G16B16" || this->
format_ ==
"RGB_INT16")
463 else if (this->
format_ ==
"BAYER_RGGB8")
465 ROS_INFO_NAMED(
"camera_utils",
"bayer simulation maybe computationally expensive.");
469 else if (this->
format_ ==
"BAYER_BGGR8")
471 ROS_INFO_NAMED(
"camera_utils",
"bayer simulation maybe computationally expensive.");
475 else if (this->
format_ ==
"BAYER_GBRG8")
477 ROS_INFO_NAMED(
"camera_utils",
"bayer simulation maybe computationally expensive.");
481 else if (this->
format_ ==
"BAYER_GRBG8")
483 ROS_INFO_NAMED(
"camera_utils",
"bayer simulation maybe computationally expensive.");
498 this->
cx_ = (
static_cast<double>(this->
width_) + 1.0) /2.0;
500 this->
cy_ = (
static_cast<double>(this->
height_) + 1.0) /2.0;
503 double hfov = this->
camera_->HFOV().Radian();
504 double computed_focal_length =
505 (
static_cast<double>(this->
width_)) /
506 (2.0 * tan(hfov / 2.0));
515 if (!ignition::math::equal(this->
focal_length_, computed_focal_length))
517 ROS_WARN_NAMED(
"camera_utils",
"The <focal_length>[%f] you have provided for camera_ [%s]" 518 " is inconsistent with specified image_width [%d] and" 519 " HFOV [%f]. Please double check to see that" 520 " focal_length = width_ / (2.0 * tan(HFOV/2.0))," 521 " the expected focal_length value is [%f]," 522 " please update your camera_ model description accordingly.",
525 computed_focal_length);
530 sensor_msgs::CameraInfo camera_info_msg;
532 camera_info_msg.header.frame_id = this->
frame_name_;
534 camera_info_msg.height = this->
height_;
535 camera_info_msg.width = this->
width_;
537 #if ROS_VERSION_MINIMUM(1, 3, 0) 538 camera_info_msg.distortion_model =
"plumb_bob";
539 camera_info_msg.D.resize(5);
544 if(this->
camera_->LensDistortion())
559 camera_info_msg.K[1] = 0.0;
560 camera_info_msg.K[2] = this->
cx_;
561 camera_info_msg.K[3] = 0.0;
563 camera_info_msg.K[5] = this->
cy_;
564 camera_info_msg.K[6] = 0.0;
565 camera_info_msg.K[7] = 0.0;
566 camera_info_msg.K[8] = 1.0;
568 camera_info_msg.R[0] = 1.0;
569 camera_info_msg.R[1] = 0.0;
570 camera_info_msg.R[2] = 0.0;
571 camera_info_msg.R[3] = 0.0;
572 camera_info_msg.R[4] = 1.0;
573 camera_info_msg.R[5] = 0.0;
574 camera_info_msg.R[6] = 0.0;
575 camera_info_msg.R[7] = 0.0;
576 camera_info_msg.R[8] = 1.0;
580 camera_info_msg.P[1] = 0.0;
581 camera_info_msg.P[2] = this->
cx_;
583 camera_info_msg.P[4] = 0.0;
585 camera_info_msg.P[6] = this->
cy_;
586 camera_info_msg.P[7] = 0.0;
587 camera_info_msg.P[8] = 0.0;
588 camera_info_msg.P[9] = 0.0;
589 camera_info_msg.P[10] = 1.0;
590 camera_info_msg.P[11] = 0.0;
605 common::Time &last_update_time)
619 boost::mutex::scoped_lock lock(this->
lock_);
628 this->
skip_*this->
width_, reinterpret_cast<const void*>(_src));
670 camera_info_publisher.
publish(camera_info_msg);
678 static const double timeout = 0.001;
virtual bool CanTriggerCamera()
const std::string BAYER_GRBG8
ros::Subscriber trigger_subscriber_
std::string image_topic_name_
ROS image topic name.
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
std::string getPrefixParam(ros::NodeHandle &nh)
common::Time sensor_update_time_
#define ROS_WARN_NAMED(name,...)
dynamic_reconfigure::Server< gazebo_plugins::GazeboRosCameraConfig > * dyn_srv_
Subscriber subscribe(const std::string &topic, uint32_t queue_size, void(T::*fp)(M), T *obj, const TransportHints &transport_hints=TransportHints())
boost::shared_ptr< camera_info_manager::CameraInfoManager > camera_info_manager_
std::string robot_namespace_
for setting ROS name space
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.
image_transport::ImageTransport * itnode_
boost::thread deferred_load_thread_
image_transport::Publisher image_pub_
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
Publisher advertise(const std::string &base_topic, uint32_t queue_size, bool latch=false)
~GazeboRosCameraUtils()
Destructor.
std::string camera_name_
ROS camera name.
void SetHFOV(const std_msgs::Float64::ConstPtr &hfov)
: Camera modification functions
ros::CallbackQueue camera_queue_
virtual void TriggerCamera()
boost::thread callback_queue_thread_
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
const std::string BAYER_GBRG8
void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy)
std::string resolve(const std::string &prefix, const std::string &frame_name)
sensor_msgs::Image image_msg_
ROS image message.
#define ROS_DEBUG_NAMED(name,...)
ros::Publisher camera_info_pub_
camera info
std::string tf_prefix_
tf prefix
event::ConnectionPtr OnLoad(const boost::function< void()> &)
void publish(const sensor_msgs::Image &message) const
bool initialized_
True if camera util is initialized.
common::Time last_info_update_time_
Publisher advertise(const std::string &topic, uint32_t queue_size, bool latch=false)
double update_rate_
update rate of this sensor
std::string frame_name_
ROS frame transform name to use in the image message header. This should typically match the link nam...
std::string camera_info_topic_name_
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
const std::string BAYER_BGGR8
rendering::CameraPtr camera_
GazeboRosCameraUtils()
Constructor.
uint32_t getNumSubscribers() const
void SetUpdateRate(const std_msgs::Float64::ConstPtr &update_rate)
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
common::Time last_update_time_
std::string trigger_topic_name_
ROS trigger topic name.
event::EventT< void()> load_event_
#define ROS_ERROR_NAMED(name,...)
void configCallback(gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level)
const std::string BAYER_RGGB8
std::string GetRobotNamespace(const sensors::SensorPtr &parent, const sdf::ElementPtr &sdf, const char *pInfo=NULL)
Reads the name space tag of a sensor plugin.
sensors::SensorPtr parentSensor_
std::string type_
size of image buffer
boost::shared_ptr< void > VoidPtr
boost::shared_ptr< boost::mutex > image_connect_count_lock_
A mutex to lock access to image_connect_count_.