gazebo_ros_camera_utils.h
Go to the documentation of this file.
1 /*
2  * Copyright 2012 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 #ifndef GAZEBO_ROS_CAMERA_UTILS_HH
19 #define GAZEBO_ROS_CAMERA_UTILS_HH
20 
21 #include <string>
22 // boost stuff
23 #include <boost/thread.hpp>
24 #include <boost/thread/mutex.hpp>
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/PointCloud.h>
33 #include <sensor_msgs/Image.h>
34 #include <sensor_msgs/CameraInfo.h>
35 #include <std_msgs/Empty.h>
36 #include <std_msgs/Float64.h>
39 
40 // dynamic reconfigure stuff
41 #include <gazebo_plugins/GazeboRosCameraConfig.h>
42 #include <dynamic_reconfigure/server.h>
43 
44 // Gazebo
45 #include <gazebo/physics/physics.hh>
46 #include <gazebo/transport/TransportTypes.hh>
47 #include <gazebo/msgs/MessageTypes.hh>
48 #include <gazebo/common/Time.hh>
49 #include <gazebo/sensors/SensorTypes.hh>
50 #include <gazebo/plugins/CameraPlugin.hh>
52 
53 namespace gazebo
54 {
55  class GazeboRosMultiCamera;
56  class GazeboRosTriggeredMultiCamera;
58  {
61  public: GazeboRosCameraUtils();
62 
64  public: ~GazeboRosCameraUtils();
65 
70  public: void Load(sensors::SensorPtr _parent,
71  sdf::ElementPtr _sdf,
72  const std::string &_camera_name_suffix = "");
73 
79  public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf,
80  const std::string &_camera_name_suffix,
81  double _hack_baseline);
82 
83  public: event::ConnectionPtr OnLoad(const boost::function<void()>&);
84 
85  private: void Init();
86 
88  protected: void PutCameraData(const unsigned char *_src);
89  protected: void PutCameraData(const unsigned char *_src,
90  common::Time &last_update_time);
91 
96  protected: void ImageConnect();
97  protected: void ImageDisconnect();
98 
103 
105  private: void SetHFOV(const std_msgs::Float64::ConstPtr& hfov);
106  private: void SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate);
107 
113 
115  protected: sensor_msgs::Image image_msg_;
116 
118  private: std::string robot_namespace_;
119 
121  private: std::string camera_name_;
122 
124  private: std::string tf_prefix_;
125 
127  protected: std::string image_topic_name_;
128 
130  protected: void PublishCameraInfo(ros::Publisher camera_info_publisher);
131  protected: void PublishCameraInfo(common::Time &last_update_time);
132  protected: void PublishCameraInfo();
134  private: void InfoConnect();
135  private: void InfoDisconnect();
138  protected: std::string camera_info_topic_name_;
139  protected: common::Time last_info_update_time_;
140 
143  protected: std::string frame_name_;
145  protected: double update_rate_;
146  protected: double update_period_;
147  protected: common::Time last_update_time_;
148 
149  protected: double cx_prime_;
150  protected: double cx_;
151  protected: double cy_;
152  protected: double focal_length_;
153  protected: double hack_baseline_;
154  protected: double distortion_k1_;
155  protected: double distortion_k2_;
156  protected: double distortion_k3_;
157  protected: double distortion_t1_;
158  protected: double distortion_t2_;
159 
160  protected: bool auto_distortion_;
161  protected: bool border_crop_;
162 
164 
165 
168  protected: boost::mutex lock_;
169 
171  protected: std::string type_;
172  protected: int skip_;
173 
176 
177  // Time last published, refrain from publish unless new image has
178  // been rendered
179  // Allow dynamic reconfiguration of camera params
180  dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
182  void configCallback(gazebo_plugins::GazeboRosCameraConfig &config,
183  uint32_t level);
184 
186  protected: void CameraQueueThread();
187  protected: boost::thread callback_queue_thread_;
188 
189 
190  // copied from CameraPlugin
191  protected: unsigned int width_, height_, depth_;
192  protected: std::string format_;
193 
194  protected: sensors::SensorPtr parentSensor_;
195  protected: rendering::CameraPtr camera_;
196 
197  // Pointer to the world
198  protected: physics::WorldPtr world_;
199 
200  private: event::ConnectionPtr newFrameConnection_;
201 
202  protected: common::Time sensor_update_time_;
203 
204  // maintain for one more release for backwards compatibility
205  protected: physics::WorldPtr world;
206 
207  // deferred load in case ros is blocking
208  private: sdf::ElementPtr sdf;
209  private: void LoadThread();
210  private: boost::thread deferred_load_thread_;
211  private: event::EventT<void()> load_event_;
212 
213  // make a trigger function that the child classes can override
214  // and a function that returns bool to indicate whether the trigger
215  // should be used
216  protected: virtual void TriggerCamera();
217  protected: virtual bool CanTriggerCamera();
218  private: void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy);
220 
222  protected: std::string trigger_topic_name_;
223 
225  protected: bool initialized_;
226 
227  friend class GazeboRosMultiCamera;
229  };
230 }
231 #endif
std::string image_topic_name_
ROS image topic name.
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
dynamic_reconfigure::Server< gazebo_plugins::GazeboRosCameraConfig > * dyn_srv_
boost::shared_ptr< camera_info_manager::CameraInfoManager > camera_info_manager_
std::string robot_namespace_
for setting ROS name space
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_
image_transport::Publisher image_pub_
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
std::string camera_name_
ROS camera name.
void SetHFOV(const std_msgs::Float64::ConstPtr &hfov)
: Camera modification functions
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy)
sensor_msgs::Image image_msg_
ROS image message.
ros::Publisher camera_info_pub_
camera info
event::ConnectionPtr OnLoad(const boost::function< void()> &)
event::ConnectionPtr newFrameConnection_
bool initialized_
True if camera util is initialized.
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...
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
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.
void InfoConnect()
Keep track of number of connctions for CameraInfo.
std::string trigger_topic_name_
ROS trigger topic name.
void configCallback(gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level)
std::string type_
size of image buffer
boost::shared_ptr< boost::mutex > image_connect_count_lock_
A mutex to lock access to image_connect_count_.


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