gazebo_ros_depth_camera.h
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2012-2014 Open Source Robotics Foundation
3  *
4  * Licensed under the Apache License, Version 2.0 (the "License");
5  * you may not use this file except in compliance with the License.
6  * You may obtain a copy of the License at
7  *
8  * http://www.apache.org/licenses/LICENSE-2.0
9  *
10  * Unless required by applicable law or agreed to in writing, software
11  * distributed under the License is distributed on an "AS IS" BASIS,
12  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13  * See the License for the specific language governing permissions and
14  * limitations under the License.
15  *
16 */
17 /*
18  * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor.
19  * Author: John Hsu
20  * Date: 24 Sept 2008
21  */
22 
23 #ifndef GAZEBO_ROS_DEPTH_CAMERA_HH
24 #define GAZEBO_ROS_DEPTH_CAMERA_HH
25 
26 // ros stuff
27 #include <ros/ros.h>
28 #include <ros/callback_queue.h>
29 #include <ros/advertise_options.h>
30 
31 // ros messages stuff
32 #include <sensor_msgs/PointCloud2.h>
33 #include <sensor_msgs/Image.h>
34 #include <sensor_msgs/CameraInfo.h>
35 #include <sensor_msgs/fill_image.h>
36 #include <std_msgs/Float64.h>
38 
39 // gazebo stuff
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>
47 
48 // dynamic reconfigure stuff
49 #include <gazebo_plugins/GazeboRosCameraConfig.h>
50 #include <dynamic_reconfigure/server.h>
51 
52 // boost stuff
53 #include <boost/thread/mutex.hpp>
54 
55 // camera stuff
57 
58 namespace gazebo
59 {
60  class GazeboRosDepthCamera : public DepthCameraPlugin, GazeboRosCameraUtils
61  {
64  public: GazeboRosDepthCamera();
65 
67  public: ~GazeboRosDepthCamera();
68 
71  public: virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf);
72 
74  public: virtual void Advertise();
75 
77  protected: virtual void OnNewDepthFrame(const float *_image,
78  unsigned int _width, unsigned int _height,
79  unsigned int _depth, const std::string &_format);
80 
82  protected: virtual void OnNewRGBPointCloud(const float *_pcd,
83  unsigned int _width, unsigned int _height,
84  unsigned int _depth, const std::string &_format);
85 
87  protected: virtual void OnNewImageFrame(const unsigned char *_image,
88  unsigned int _width, unsigned int _height,
89  unsigned int _depth, const std::string &_format);
90 
92  private: void FillPointdCloud(const float *_src);
93 
95  private: void FillDepthImage(const float *_src);
96 
99  private: void PointCloudConnect();
100  private: void PointCloudDisconnect();
101 
104  private: void DepthImageConnect();
105  private: void DepthImageDisconnect();
107 
108  private: bool FillPointCloudHelper(sensor_msgs::PointCloud2 &point_cloud_msg,
109  uint32_t rows_arg, uint32_t cols_arg,
110  uint32_t step_arg, void* data_arg);
111 
112  private: bool FillDepthImageHelper( sensor_msgs::Image& image_msg,
113  uint32_t rows_arg, uint32_t cols_arg,
114  uint32_t step_arg, void* data_arg);
115 
119 
121  private: sensor_msgs::PointCloud2 point_cloud_msg_;
122  private: sensor_msgs::Image depth_image_msg_;
123 
124  private: double point_cloud_cutoff_;
125 
127  private: std::string point_cloud_topic_name_;
128 
129  private: void InfoConnect();
130  private: void InfoDisconnect();
131 
133  protected: virtual void PublishCameraInfo();
134 
136  private: std::string depth_image_topic_name_;
139  private: void DepthInfoConnect();
140  private: void DepthInfoDisconnect();
141 
142  // overload with our own
143  private: common::Time depth_sensor_update_time_;
145 
146  private: event::ConnectionPtr load_connection_;
147  };
148 
149 }
150 #endif
151 
virtual void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf)
Load the plugin.
std::string depth_image_topic_name_
image where each pixel contains the depth information
std::string point_cloud_topic_name_
ROS image topic name.
bool FillDepthImageHelper(sensor_msgs::Image &image_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 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
virtual void Advertise()
Advertise point cloud and depth image.
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)


gazebo_plugins
Author(s): John Hsu
autogenerated on Wed Aug 24 2022 02:47:52