gazebo_ros_camera_utils.h
Go to the documentation of this file.
00001 /*
00002  * Copyright 2012 Open Source Robotics Foundation
00003  *
00004  * Licensed under the Apache License, Version 2.0 (the "License");
00005  * you may not use this file except in compliance with the License.
00006  * You may obtain a copy of the License at
00007  *
00008  *     http://www.apache.org/licenses/LICENSE-2.0
00009  *
00010  * Unless required by applicable law or agreed to in writing, software
00011  * distributed under the License is distributed on an "AS IS" BASIS,
00012  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00013  * See the License for the specific language governing permissions and
00014  * limitations under the License.
00015  *
00016 */
00017 
00018 #ifndef GAZEBO_ROS_CAMERA_UTILS_HH
00019 #define GAZEBO_ROS_CAMERA_UTILS_HH
00020 
00021 #include <string>
00022 // boost stuff
00023 #include <boost/thread.hpp>
00024 #include <boost/thread/mutex.hpp>
00025 
00026 // ros stuff
00027 #include <ros/ros.h>
00028 #include <ros/callback_queue.h>
00029 #include <ros/advertise_options.h>
00030 
00031 // ros messages stuff
00032 #include <sensor_msgs/PointCloud.h>
00033 #include <sensor_msgs/Image.h>
00034 #include <sensor_msgs/CameraInfo.h>
00035 #include <std_msgs/Float64.h>
00036 #include <image_transport/image_transport.h>
00037 #include <camera_info_manager/camera_info_manager.h>
00038 
00039 // dynamic reconfigure stuff
00040 #include <gazebo_plugins/GazeboRosCameraConfig.h>
00041 #include <dynamic_reconfigure/server.h>
00042 
00043 // Gazebo
00044 #include <gazebo/physics/physics.hh>
00045 #include <gazebo/transport/TransportTypes.hh>
00046 #include <gazebo/msgs/MessageTypes.hh>
00047 #include <gazebo/common/Time.hh>
00048 #include <gazebo/sensors/SensorTypes.hh>
00049 #include <gazebo/plugins/CameraPlugin.hh>
00050 #include <gazebo_plugins/gazebo_ros_utils.h>
00051 
00052 namespace gazebo
00053 {
00054   class GazeboRosMultiCamera;
00055   class GazeboRosCameraUtils
00056   {
00059     public: GazeboRosCameraUtils();
00060 
00062     public: ~GazeboRosCameraUtils();
00063 
00068     public: void Load(sensors::SensorPtr _parent,
00069                       sdf::ElementPtr _sdf,
00070                       const std::string &_camera_name_suffix = "");
00071 
00077     public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf,
00078                       const std::string &_camera_name_suffix,
00079                       double _hack_baseline);
00080 
00081     public: event::ConnectionPtr OnLoad(const boost::function<void()>&);
00082 
00083     private: void Init();
00084 
00086     protected: void PutCameraData(const unsigned char *_src);
00087     protected: void PutCameraData(const unsigned char *_src,
00088       common::Time &last_update_time);
00089 
00091     protected: boost::shared_ptr<int> image_connect_count_;
00093     protected: boost::shared_ptr<boost::mutex> image_connect_count_lock_;
00094     protected: void ImageConnect();
00095     protected: void ImageDisconnect();
00096 
00100     protected: boost::shared_ptr<bool> was_active_;
00101 
00103     private: void SetHFOV(const std_msgs::Float64::ConstPtr& hfov);
00104     private: void SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate);
00105 
00108     protected: ros::NodeHandle* rosnode_;
00109     protected: image_transport::Publisher image_pub_;
00110     private: image_transport::ImageTransport* itnode_;
00111 
00113     protected: sensor_msgs::Image image_msg_;
00114 
00116     private: std::string robot_namespace_;
00117 
00119     private: std::string camera_name_;
00120 
00122     private: std::string tf_prefix_;
00123     
00125     protected: std::string image_topic_name_;
00126 
00128     protected: void PublishCameraInfo(ros::Publisher camera_info_publisher);
00129     protected: void PublishCameraInfo(common::Time &last_update_time);
00130     protected: void PublishCameraInfo();
00132     private: void InfoConnect();
00133     private: void InfoDisconnect();
00135     protected: ros::Publisher camera_info_pub_;
00136     protected: std::string camera_info_topic_name_;
00137     protected: common::Time last_info_update_time_;
00138 
00141     protected: std::string frame_name_;
00143     protected: double update_rate_;
00144     protected: double update_period_;
00145     protected: common::Time last_update_time_;
00146 
00147     protected: double cx_prime_;
00148     protected: double cx_;
00149     protected: double cy_;
00150     protected: double focal_length_;
00151     protected: double hack_baseline_;
00152     protected: double distortion_k1_;
00153     protected: double distortion_k2_;
00154     protected: double distortion_k3_;
00155     protected: double distortion_t1_;
00156     protected: double distortion_t2_;
00157 
00158     protected: boost::shared_ptr<camera_info_manager::CameraInfoManager> camera_info_manager_;
00159 
00160 
00163     protected: boost::mutex lock_;
00164 
00166     protected: std::string type_;
00167     protected: int skip_;
00168 
00169     private: ros::Subscriber cameraHFOVSubscriber_;
00170     private: ros::Subscriber cameraUpdateRateSubscriber_;
00171 
00172     // Time last published, refrain from publish unless new image has
00173     // been rendered
00174     // Allow dynamic reconfiguration of camera params
00175     dynamic_reconfigure::Server<gazebo_plugins::GazeboRosCameraConfig>
00176       *dyn_srv_;
00177     void configCallback(gazebo_plugins::GazeboRosCameraConfig &config,
00178       uint32_t level);
00179 
00180     protected: ros::CallbackQueue camera_queue_;
00181     protected: void CameraQueueThread();
00182     protected: boost::thread callback_queue_thread_;
00183 
00184 
00185     // copied from CameraPlugin
00186     protected: unsigned int width_, height_, depth_;
00187     protected: std::string format_;
00188 
00189     protected: sensors::SensorPtr parentSensor_;
00190     protected: rendering::CameraPtr camera_;
00191 
00192     // Pointer to the world
00193     protected: physics::WorldPtr world_;
00194 
00195     private: event::ConnectionPtr newFrameConnection_;
00196 
00197     protected: common::Time sensor_update_time_;
00198 
00199     // maintain for one more release for backwards compatibility
00200     protected: physics::WorldPtr world;
00201 
00202     // deferred load in case ros is blocking
00203     private: sdf::ElementPtr sdf;
00204     private: void LoadThread();
00205     private: boost::thread deferred_load_thread_;
00206     private: event::EventT<void()> load_event_;
00207 
00209     protected: bool initialized_;
00210 
00211     friend class GazeboRosMultiCamera;
00212   };
00213 }
00214 #endif


gazebo_plugins
Author(s): John Hsu
autogenerated on Fri Aug 28 2015 10:47:25