23 #ifndef GAZEBO_ROS_DEPTH_CAMERA_HH 24 #define GAZEBO_ROS_DEPTH_CAMERA_HH 32 #include <sensor_msgs/PointCloud2.h> 33 #include <sensor_msgs/Image.h> 34 #include <sensor_msgs/CameraInfo.h> 36 #include <std_msgs/Float64.h> 40 #include <sdf/Param.hh> 41 #include <gazebo/physics/physics.hh> 42 #include <gazebo/transport/TransportTypes.hh> 43 #include <gazebo/msgs/MessageTypes.hh> 44 #include <gazebo/common/Time.hh> 45 #include <gazebo/sensors/SensorTypes.hh> 46 #include <gazebo/plugins/DepthCameraPlugin.hh> 49 #include <gazebo_plugins/GazeboRosCameraConfig.h> 50 #include <dynamic_reconfigure/server.h> 53 #include <boost/thread/mutex.hpp> 71 public:
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
78 unsigned int _width,
unsigned int _height,
79 unsigned int _depth,
const std::string &_format);
83 unsigned int _width,
unsigned int _height,
84 unsigned int _depth,
const std::string &_format);
88 unsigned int _width,
unsigned int _height,
89 unsigned int _depth,
const std::string &_format);
109 uint32_t rows_arg, uint32_t cols_arg,
110 uint32_t step_arg,
void* data_arg);
113 uint32_t rows_arg, uint32_t cols_arg,
114 uint32_t step_arg,
void* data_arg);
double point_cloud_cutoff_
void PointCloudDisconnect()
~GazeboRosDepthCamera()
Destructor.
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
virtual void PublishCameraInfo()
std::string depth_image_topic_name_
image where each pixel contains the depth information
void DepthImageDisconnect()
std::string point_cloud_topic_name_
ROS image topic name.
common::Time last_depth_image_camera_info_update_time_
common::Time depth_sensor_update_time_
int depth_info_connect_count_
bool FillDepthImageHelper(sensor_msgs::Image &image_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg)
GazeboRosDepthCamera()
Constructor.
sensor_msgs::PointCloud2 point_cloud_msg_
PointCloud2 point cloud message.
ros::Publisher point_cloud_pub_
A pointer to the ROS node. A node will be instantiated if it does not exist.
int point_cloud_connect_count_
Keep track of number of connctions for point clouds.
virtual void OnNewDepthFrame(const float *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
void FillPointdCloud(const float *_src)
Put camera data to the ROS topic.
int depth_image_connect_count_
Keep track of number of connctions for point clouds.
void FillDepthImage(const float *_src)
push depth image data into ros topic
event::ConnectionPtr load_connection_
ros::Publisher depth_image_camera_info_pub_
ros::Publisher depth_image_pub_
virtual void Advertise()
Advertise point cloud and depth image.
std::string depth_image_camera_info_topic_name_
void DepthInfoDisconnect()
sensor_msgs::Image depth_image_msg_
virtual void OnNewRGBPointCloud(const float *_pcd, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
virtual void OnNewImageFrame(const unsigned char *_image, unsigned int _width, unsigned int _height, unsigned int _depth, const std::string &_format)
Update the controller.
bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg, uint32_t rows_arg, uint32_t cols_arg, uint32_t step_arg, void *data_arg)