30 #include <gazebo/physics/World.hh> 31 #include <gazebo/physics/HingeJoint.hh> 32 #include <gazebo/sensors/Sensor.hh> 33 #include <gazebo/common/Exception.hh> 34 #include <gazebo/sensors/CameraSensor.hh> 35 #include <gazebo/sensors/SensorTypes.hh> 36 #include <gazebo/rendering/Camera.hh> 39 #include <sdf/Param.hh> 41 #include <sensor_msgs/Image.h> 42 #include <sensor_msgs/CameraInfo.h> 45 #include <sensor_msgs/RegionOfInterest.h> 47 #include <opencv/cv.h> 48 #include <opencv/highgui.h> 50 #include <opencv/cvwimage.h> 52 #include <boost/scoped_ptr.hpp> 53 #include <boost/bind.hpp> 54 #include <boost/tokenizer.hpp> 55 #include <boost/thread.hpp> 56 #include <boost/thread/recursive_mutex.hpp> 81 CameraPlugin::Load(_parent, _sdf);
83 this->
width_ = this->width;
85 this->
depth_ = this->depth;
104 this->
mode_ =
"streaming";
109 if (this->
mode_ ==
"polled")
113 else if (this->
mode_ ==
"streaming")
119 ROS_ERROR_NAMED(
"prosilica",
"trigger_mode is invalid: %s, using streaming mode",this->
mode_.c_str());
126 unsigned int _width,
unsigned int _height,
unsigned int _depth,
127 const std::string &_format)
130 this->
mode_ =
"streaming";
134 common::Time sensor_update_time = this->
parentSensor_->LastMeasurementTime();
138 if (!this->parentSensor->IsActive())
142 this->parentSensor->SetActive(
true);
148 if (this->
mode_ ==
"streaming")
168 polled_camera::GetPolledImage::Response& rsp,
169 sensor_msgs::Image& image, sensor_msgs::CameraInfo& info)
172 this->
mode_ =
"streaming";
178 if (this->
mode_ !=
"polled")
181 rsp.status_message =
"Camera is not in triggered mode";
185 if (req.binning_x > 1 || req.binning_y > 1)
188 rsp.status_message =
"Gazebo Prosilica plugin does not support binning";
193 if (req.roi.x_offset <= 0 || req.roi.y_offset <= 0 || req.roi.width <= 0 || req.roi.height <= 0)
195 req.roi.x_offset = 0;
196 req.roi.y_offset = 0;
197 req.roi.width = this->
width_;
198 req.roi.height = this->height;
200 const unsigned char *src = NULL;
201 ROS_ERROR_NAMED(
"prosilica",
"roidebug %d %d %d %d", req.roi.x_offset, req.roi.y_offset, req.roi.width, req.roi.height);
211 src = this->parentSensor->Camera()->ImageData(0);
220 common::Time roiLastRenderTime = this->
parentSensor_->LastMeasurementTime();
227 #if ROS_VERSION_MINIMUM(1, 3, 0) 274 common::Time lastRenderTime = this->
parentSensor_->LastMeasurementTime();
275 this->
image_msg_.header.stamp.sec = lastRenderTime.sec;
276 this->
image_msg_.header.stamp.nsec = lastRenderTime.nsec;
298 common::Time roiLastRenderTime = this->
parentSensor_->LastMeasurementTime();
299 this->
roiImageMsg->header.stamp.sec = roiLastRenderTime.sec;
300 this->
roiImageMsg->header.stamp.nsec = roiLastRenderTime.nsec;
313 cv::Mat roi(img_bridge_->image,
314 cv::Rect(req.roi.x_offset, req.roi.y_offset,
315 req.roi.width, req.roi.height));
316 img_bridge_->image = roi;
polled_camera::PublicationServer poll_srv_
image_transport
#define ROS_INFO_NAMED(name,...)
void publish(const boost::shared_ptr< M > &message) const
sensor_msgs::CameraInfo * roiCameraInfoMsg
event::ConnectionPtr load_connection_
image_transport::Publisher image_pub_
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
sensor_msgs::Image * roiImageMsg
ros message
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
virtual ~GazeboRosProsilica()
Destructor.
sensor_msgs::Image image_msg_
ROS image message.
#define ROS_DEBUG_NAMED(name,...)
ros::Publisher camera_info_pub_
camera info
PublicationServer advertise(ros::NodeHandle &nh, const std::string &service, const PublicationServer::DriverCallback &cb, const ros::VoidPtr &tracked_object=ros::VoidPtr())
void pollCallback(polled_camera::GetPolledImage::Request &req, polled_camera::GetPolledImage::Response &rsp, sensor_msgs::Image &image, sensor_msgs::CameraInfo &info)
event::ConnectionPtr OnLoad(const boost::function< void()> &)
void publish(const sensor_msgs::Image &message) const
bool searchParam(const std::string &key, std::string &result) const
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
CvImagePtr toCvCopy(const sensor_msgs::ImageConstPtr &source, const std::string &encoding=std::string())
std::string mode_param_name
std::string frame_name_
ROS frame transform name to use in the image message header. This should typically match the link nam...
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
rendering::CameraPtr camera_
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
common::Time last_update_time_
bool getParam(const std::string &key, std::string &s) const
#define ROS_ERROR_NAMED(name,...)
GazeboRosProsilica()
Constructor.
sensors::SensorPtr parentSensor_
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the controller.
std::string type_
size of image buffer
std::string pollServiceName
ROS image topic name.