34 #ifndef GAZEBO_ROS_DEPTH_CAMERA_HH 35 #define GAZEBO_ROS_DEPTH_CAMERA_HH 43 #include <sensor_msgs/PointCloud2.h> 44 #include <sensor_msgs/Image.h> 45 #include <sensor_msgs/CameraInfo.h> 47 #include <std_msgs/Float64.h> 51 #include <sdf/Param.hh> 52 #include <gazebo/physics/physics.hh> 53 #include <gazebo/transport/TransportTypes.hh> 54 #include <gazebo/msgs/MessageTypes.hh> 55 #include <gazebo/common/Time.hh> 56 #include <gazebo/sensors/SensorTypes.hh> 57 #include <gazebo/plugins/DepthCameraPlugin.hh> 60 #include <gazebo_plugins/GazeboRosCameraConfig.h> 61 #include <dynamic_reconfigure/server.h> 64 #include <boost/thread/mutex.hpp> 69 #include <opencv2/core.hpp> 84 public:
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
91 unsigned int _width,
unsigned int _height,
92 unsigned int _depth,
const std::string &_format);
96 unsigned int _width,
unsigned int _height,
97 unsigned int _depth,
const std::string &_format);
101 unsigned int _width,
unsigned int _height,
102 unsigned int _depth,
const std::string &_format);
140 uint32_t rows_arg, uint32_t cols_arg,
141 uint32_t step_arg,
void* data_arg);
144 uint32_t rows_arg, uint32_t cols_arg,
145 uint32_t step_arg,
void* data_arg);
ros::Publisher depth_image_camera_info_pub_
sensor_msgs::Image multibeam_image_msg_
cv::Mat ConstructSonarImage(cv::Mat &depth, cv::Mat &normals)
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
std::string depth_image_topic_name_
image where each pixel contains the depth information
event::ConnectionPtr newRGBPointCloudConnection
bool FillDepthImageHelper(sensor_msgs::Image &image_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg)
void ComputeSonarImage(const float *_src)
push depth image data into ros topic
virtual void OnNewRGBPointCloud(const float *_pcd, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
sensor_msgs::Image raw_sonar_image_msg_
void RawSonarImageDisconnect()
void ApplySpeckleNoise(cv::Mat &scan, float fov)
sensor_msgs::Image sonar_image_msg_
void ApplySmoothing(cv::Mat &scan, float fov)
double point_cloud_cutoff_
void MultibeamImageConnect()
std::vector< int > angle_nbr_indices_
ros::Publisher multibeam_image_pub_
event::ConnectionPtr load_connection_
common::Time last_depth_image_camera_info_update_time_
void DepthInfoDisconnect()
~GazeboRosImageSonar()
Destructor.
cv::Mat ComputeNormalImage(cv::Mat &depth)
cv::Mat ConstructVisualScanImage(cv::Mat &raw_scan)
rendering::DepthCameraPtr depthCamera
int depth_info_connect_count_
void MultibeamImageDisconnect()
ros::Publisher point_cloud_pub_
A pointer to the ROS node. A node will be instantiated if it does not exist.
void FillDepthImage(const float *_src)
push depth image data into ros topic
std::string point_cloud_topic_name_
ROS image topic name.
virtual void PublishCameraInfo()
common::Time depth_sensor_update_time_
ros::Publisher normal_image_pub_
virtual void OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
virtual void Advertise()
Advertise point cloud and depth image.
void DepthImageDisconnect()
sensors::DepthCameraSensorPtr parentSensor
void RawSonarImageConnect()
void SonarImageDisconnect()
void NormalImageDisconnect()
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
std::default_random_engine generator
bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg)
sensor_msgs::PointCloud2 point_cloud_msg_
PointCloud2 point cloud message.
ros::Publisher raw_sonar_image_pub_
cv::Mat ConstructScanImage(cv::Mat &depth, cv::Mat &SNR)
event::ConnectionPtr newDepthFrameConnection
sensor_msgs::Image depth_image_msg_
GazeboRosImageSonar()
Constructor.
event::ConnectionPtr newImageFrameConnection
void PointCloudDisconnect()
int depth_image_connect_count_
Keep track of number of connctions for point clouds.
sensor_msgs::Image normal_image_msg_
void ApplyMedianFilter(cv::Mat &scan)
void NormalImageConnect()
void FillPointdCloud(const float *_src)
Put camera data to the ROS topic.
std::vector< std::vector< int > > angle_range_indices_
ros::Publisher depth_image_pub_
ros::Publisher sonar_image_pub_
int point_cloud_connect_count_
Keep track of number of connctions for point clouds.
std::string depth_image_camera_info_topic_name_