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>
51 
52 namespace gazebo
53 {
54  class GazeboRosMultiCamera;
55  class GazeboRosTriggeredMultiCamera;
57  {
60  public: GazeboRosCameraUtils();
61 
63  public: ~GazeboRosCameraUtils();
64 
69  public: void Load(sensors::SensorPtr _parent,
70  sdf::ElementPtr _sdf,
71  const std::string &_camera_name_suffix = "");
72 
78  public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf,
79  const std::string &_camera_name_suffix,
80  double _hack_baseline);
81 
82  public: event::ConnectionPtr OnLoad(const boost::function<void()>&);
83 
84  private: void Init();
85 
87  protected: void PutCameraData(const unsigned char *_src);
88  protected: void PutCameraData(const unsigned char *_src,
89  common::Time &last_update_time);
90 
95  protected: void ImageConnect();
96  protected: void ImageDisconnect();
97 
102 
104  private: void SetHFOV(const std_msgs::Float64::ConstPtr& hfov);
105  private: void SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate);
106 
112 
114  protected: sensor_msgs::Image image_msg_;
115 
117  private: std::string robot_namespace_;
118 
120  private: std::string camera_name_;
121 
123  private: std::string tf_prefix_;
124 
126  protected: std::string image_topic_name_;
127 
129  protected: void PublishCameraInfo(ros::Publisher camera_info_publisher);
130  protected: void PublishCameraInfo(common::Time &last_update_time);
131  protected: void PublishCameraInfo();
133  private: void InfoConnect();
134  private: void InfoDisconnect();
137  protected: std::string camera_info_topic_name_;
138  protected: common::Time last_info_update_time_;
139 
142  protected: std::string frame_name_;
144  protected: double update_rate_;
145  protected: double update_period_;
146  protected: common::Time last_update_time_;
147 
148  protected: double cx_prime_;
149  protected: double cx_;
150  protected: double cy_;
151  protected: double focal_length_;
152  protected: double hack_baseline_;
153  protected: double distortion_k1_;
154  protected: double distortion_k2_;
155  protected: double distortion_k3_;
156  protected: double distortion_t1_;
157  protected: double distortion_t2_;
158 
159  protected: bool auto_distortion_;
160  protected: bool border_crop_;
161 
163 
164 
167  protected: boost::mutex lock_;
168 
170  protected: std::string type_;
171  protected: int skip_;
172 
175 
176  // Time last published, refrain from publish unless new image has
177  // been rendered
178  // Allow dynamic reconfiguration of camera params
179  dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
181  void configCallback(gazebo_plugins::GazeboRosCameraConfig &config,
182  uint32_t level);
183 
185  protected: void CameraQueueThread();
186  protected: boost::thread callback_queue_thread_;
187 
188 
189  // copied from CameraPlugin
190  protected: unsigned int width_, height_, depth_;
191  protected: std::string format_;
192 
193  protected: sensors::SensorPtr parentSensor_;
194  protected: rendering::CameraPtr camera_;
195 
196  // Pointer to the world
197  protected: physics::WorldPtr world_;
198 
199  private: event::ConnectionPtr newFrameConnection_;
200 
201  protected: common::Time sensor_update_time_;
202 
203  // maintain for one more release for backwards compatibility
204  protected: physics::WorldPtr world;
205 
206  // deferred load in case ros is blocking
207  private: sdf::ElementPtr sdf;
208  private: void LoadThread();
209  private: boost::thread deferred_load_thread_;
210  private: event::EventT<void()> load_event_;
211 
212  // make a trigger function that the child classes can override
213  // and a function that returns bool to indicate whether the trigger
214  // should be used
215  protected: virtual void TriggerCamera();
216  protected: virtual bool CanTriggerCamera();
217  private: void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy);
219 
221  protected: std::string trigger_topic_name_;
222 
224  protected: bool initialized_;
225 
226  friend class GazeboRosMultiCamera;
228  };
229 }
230 #endif
gazebo::GazeboRosCameraUtils::ImageConnect
void ImageConnect()
Definition: gazebo_ros_camera_utils.cpp:407
gazebo::GazeboRosCameraUtils::initialized_
bool initialized_
True if camera util is initialized.
Definition: gazebo_ros_camera_utils.h:224
gazebo::GazeboRosCameraUtils::itnode_
image_transport::ImageTransport * itnode_
Definition: gazebo_ros_camera_utils.h:111
gazebo::GazeboRosCameraUtils::image_pub_
image_transport::Publisher image_pub_
Definition: gazebo_ros_camera_utils.h:110
gazebo::GazeboRosCameraUtils::InfoDisconnect
void InfoDisconnect()
gazebo::GazeboRosCameraUtils::border_crop_
bool border_crop_
Definition: gazebo_ros_camera_utils.h:160
gazebo::GazeboRosCameraUtils::LoadThread
void LoadThread()
Definition: gazebo_ros_camera_utils.cpp:273
ros::Publisher
image_transport::ImageTransport
gazebo::GazeboRosTriggeredMultiCamera
Definition: gazebo_ros_triggered_multicamera.h:30
boost::shared_ptr< int >
gazebo::GazeboRosCameraUtils::parentSensor_
sensors::SensorPtr parentSensor_
Definition: gazebo_ros_camera_utils.h:193
gazebo::GazeboRosCameraUtils::ImageDisconnect
void ImageDisconnect()
Definition: gazebo_ros_camera_utils.cpp:421
gazebo
gazebo::GazeboRosCameraUtils::CanTriggerCamera
virtual bool CanTriggerCamera()
Definition: gazebo_ros_camera_utils.cpp:375
gazebo::GazeboRosCameraUtils::callback_queue_thread_
boost::thread callback_queue_thread_
Definition: gazebo_ros_camera_utils.h:186
ros.h
gazebo::GazeboRosMultiCamera
Definition: gazebo_ros_multicamera.h:30
gazebo::GazeboRosCameraUtils::PutCameraData
void PutCameraData(const unsigned char *_src)
Put camera data to the ROS topic.
Definition: gazebo_ros_camera_utils.cpp:639
gazebo::GazeboRosCameraUtils::sdf
sdf::ElementPtr sdf
Definition: gazebo_ros_camera_utils.h:207
camera_info_manager.h
gazebo::GazeboRosCameraUtils::camera_info_pub_
ros::Publisher camera_info_pub_
camera info
Definition: gazebo_ros_camera_utils.h:136
gazebo::GazeboRosCameraUtils::cx_
double cx_
Definition: gazebo_ros_camera_utils.h:149
gazebo::GazeboRosCameraUtils::load_event_
event::EventT< void()> load_event_
Definition: gazebo_ros_camera_utils.h:210
gazebo::GazeboRosCameraUtils::last_update_time_
common::Time last_update_time_
Definition: gazebo_ros_camera_utils.h:146
gazebo::GazeboRosCameraUtils::InfoConnect
void InfoConnect()
Keep track of number of connctions for CameraInfo.
gazebo::GazeboRosCameraUtils::cameraUpdateRateSubscriber_
ros::Subscriber cameraUpdateRateSubscriber_
Definition: gazebo_ros_camera_utils.h:174
gazebo::GazeboRosCameraUtils::focal_length_
double focal_length_
Definition: gazebo_ros_camera_utils.h:151
gazebo::GazeboRosCameraUtils::distortion_t2_
double distortion_t2_
Definition: gazebo_ros_camera_utils.h:157
gazebo::GazeboRosCameraUtils::SetUpdateRate
void SetUpdateRate(const std_msgs::Float64::ConstPtr &update_rate)
Definition: gazebo_ros_camera_utils.cpp:399
gazebo::GazeboRosCameraUtils::image_topic_name_
std::string image_topic_name_
ROS image topic name.
Definition: gazebo_ros_camera_utils.h:126
gazebo::GazeboRosCameraUtils::width_
unsigned int width_
Definition: gazebo_ros_camera_utils.h:190
gazebo::GazeboRosCameraUtils::world
physics::WorldPtr world
Definition: gazebo_ros_camera_utils.h:204
gazebo::GazeboRosCameraUtils::rosnode_
ros::NodeHandle * rosnode_
A pointer to the ROS node. A node will be instantiated if it does not exist.
Definition: gazebo_ros_camera_utils.h:109
gazebo::GazeboRosCameraUtils::last_info_update_time_
common::Time last_info_update_time_
Definition: gazebo_ros_camera_utils.h:138
gazebo::GazeboRosCameraUtils::hack_baseline_
double hack_baseline_
Definition: gazebo_ros_camera_utils.h:152
gazebo::GazeboRosCameraUtils::robot_namespace_
std::string robot_namespace_
for setting ROS name space
Definition: gazebo_ros_camera_utils.h:117
gazebo::GazeboRosCameraUtils
Definition: gazebo_ros_camera_utils.h:56
gazebo::GazeboRosCameraUtils::image_connect_count_lock_
boost::shared_ptr< boost::mutex > image_connect_count_lock_
A mutex to lock access to image_connect_count_.
Definition: gazebo_ros_camera_utils.h:94
gazebo::GazeboRosCameraUtils::height_
unsigned int height_
Definition: gazebo_ros_camera_utils.h:190
gazebo::GazeboRosCameraUtils::PublishCameraInfo
void PublishCameraInfo()
Definition: gazebo_ros_camera_utils.cpp:674
gazebo::GazeboRosCameraUtils::type_
std::string type_
size of image buffer
Definition: gazebo_ros_camera_utils.h:170
ros::CallbackQueue
gazebo::GazeboRosCameraUtils::frame_name_
std::string frame_name_
ROS frame transform name to use in the image message header. This should typically match the link nam...
Definition: gazebo_ros_camera_utils.h:142
gazebo::GazeboRosCameraUtils::SetHFOV
void SetHFOV(const std_msgs::Float64::ConstPtr &hfov)
: Camera modification functions
Definition: gazebo_ros_camera_utils.cpp:388
gazebo::GazeboRosCameraUtils::CameraQueueThread
void CameraQueueThread()
Definition: gazebo_ros_camera_utils.cpp:704
gazebo::GazeboRosCameraUtils::cy_
double cy_
Definition: gazebo_ros_camera_utils.h:150
gazebo::GazeboRosCameraUtils::TriggerCamera
virtual void TriggerCamera()
Definition: gazebo_ros_camera_utils.cpp:371
gazebo::GazeboRosCameraUtils::image_connect_count_
boost::shared_ptr< int > image_connect_count_
Keep track of number of image connections.
Definition: gazebo_ros_camera_utils.h:92
gazebo::GazeboRosCameraUtils::update_period_
double update_period_
Definition: gazebo_ros_camera_utils.h:145
gazebo::GazeboRosCameraUtils::cameraHFOVSubscriber_
ros::Subscriber cameraHFOVSubscriber_
Definition: gazebo_ros_camera_utils.h:173
gazebo::GazeboRosCameraUtils::sensor_update_time_
common::Time sensor_update_time_
Definition: gazebo_ros_camera_utils.h:201
gazebo::GazeboRosCameraUtils::GazeboRosCameraUtils
GazeboRosCameraUtils()
Constructor.
Definition: gazebo_ros_camera_utils.cpp:50
gazebo::GazeboRosCameraUtils::distortion_k2_
double distortion_k2_
Definition: gazebo_ros_camera_utils.h:154
gazebo::GazeboRosCameraUtils::depth_
unsigned int depth_
Definition: gazebo_ros_camera_utils.h:190
gazebo::GazeboRosCameraUtils::cx_prime_
double cx_prime_
Definition: gazebo_ros_camera_utils.h:148
gazebo::GazeboRosCameraUtils::trigger_subscriber_
ros::Subscriber trigger_subscriber_
Definition: gazebo_ros_camera_utils.h:218
gazebo::GazeboRosCameraUtils::lock_
boost::mutex lock_
A mutex to lock access to fields that are used in ROS message callbacks.
Definition: gazebo_ros_camera_utils.h:167
image_transport.h
gazebo::GazeboRosCameraUtils::deferred_load_thread_
boost::thread deferred_load_thread_
Definition: gazebo_ros_camera_utils.h:209
gazebo::GazeboRosCameraUtils::configCallback
void configCallback(gazebo_plugins::GazeboRosCameraConfig &config, uint32_t level)
Definition: gazebo_ros_camera_utils.cpp:61
callback_queue.h
gazebo::GazeboRosCameraUtils::Load
void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf, const std::string &_camera_name_suffix="")
Load the plugin.
Definition: gazebo_ros_camera_utils.cpp:104
gazebo::GazeboRosCameraUtils::Init
void Init()
Definition: gazebo_ros_camera_utils.cpp:436
gazebo::GazeboRosCameraUtils::camera_info_manager_
boost::shared_ptr< camera_info_manager::CameraInfoManager > camera_info_manager_
Definition: gazebo_ros_camera_utils.h:162
gazebo::GazeboRosCameraUtils::distortion_k3_
double distortion_k3_
Definition: gazebo_ros_camera_utils.h:155
advertise_options.h
gazebo::GazeboRosCameraUtils::dyn_srv_
dynamic_reconfigure::Server< gazebo_plugins::GazeboRosCameraConfig > * dyn_srv_
Definition: gazebo_ros_camera_utils.h:180
gazebo::GazeboRosCameraUtils::newFrameConnection_
event::ConnectionPtr newFrameConnection_
Definition: gazebo_ros_camera_utils.h:199
gazebo::GazeboRosCameraUtils::was_active_
boost::shared_ptr< bool > was_active_
Keep track when we activate this camera through ros subscription, was it already active?...
Definition: gazebo_ros_camera_utils.h:101
gazebo::GazeboRosCameraUtils::auto_distortion_
bool auto_distortion_
Definition: gazebo_ros_camera_utils.h:159
image_transport::Publisher
gazebo::GazeboRosCameraUtils::update_rate_
double update_rate_
update rate of this sensor
Definition: gazebo_ros_camera_utils.h:144
gazebo::GazeboRosCameraUtils::skip_
int skip_
Definition: gazebo_ros_camera_utils.h:171
gazebo::GazeboRosCameraUtils::camera_name_
std::string camera_name_
ROS camera name.
Definition: gazebo_ros_camera_utils.h:120
gazebo::GazeboRosCameraUtils::~GazeboRosCameraUtils
~GazeboRosCameraUtils()
Destructor.
Definition: gazebo_ros_camera_utils.cpp:74
gazebo::GazeboRosCameraUtils::format_
std::string format_
Definition: gazebo_ros_camera_utils.h:191
gazebo::GazeboRosCameraUtils::image_msg_
sensor_msgs::Image image_msg_
ROS image message.
Definition: gazebo_ros_camera_utils.h:114
gazebo::GazeboRosCameraUtils::world_
physics::WorldPtr world_
Definition: gazebo_ros_camera_utils.h:197
gazebo::GazeboRosCameraUtils::distortion_k1_
double distortion_k1_
Definition: gazebo_ros_camera_utils.h:153
gazebo_ros_utils.h
gazebo::GazeboRosCameraUtils::TriggerCameraInternal
void TriggerCameraInternal(const std_msgs::Empty::ConstPtr &dummy)
Definition: gazebo_ros_camera_utils.cpp:380
gazebo::GazeboRosCameraUtils::distortion_t1_
double distortion_t1_
Definition: gazebo_ros_camera_utils.h:156
gazebo::GazeboRosCameraUtils::camera_info_topic_name_
std::string camera_info_topic_name_
Definition: gazebo_ros_camera_utils.h:137
gazebo::GazeboRosCameraUtils::camera_queue_
ros::CallbackQueue camera_queue_
Definition: gazebo_ros_camera_utils.h:184
gazebo::GazeboRosCameraUtils::camera_
rendering::CameraPtr camera_
Definition: gazebo_ros_camera_utils.h:194
ros::NodeHandle
ros::Subscriber
gazebo::GazeboRosCameraUtils::OnLoad
event::ConnectionPtr OnLoad(const boost::function< void()> &)
Definition: gazebo_ros_camera_utils.cpp:266
gazebo::GazeboRosCameraUtils::tf_prefix_
std::string tf_prefix_
tf prefix
Definition: gazebo_ros_camera_utils.h:123
gazebo::GazeboRosCameraUtils::trigger_topic_name_
std::string trigger_topic_name_
ROS trigger topic name.
Definition: gazebo_ros_camera_utils.h:221


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Nov 23 2023 03:50:28