$search
00001 /* 00002 * gazebo thermal camera emulator plugin 00003 * Copyright (C) 2012 00004 * Stefan Kohlbrecher, TU Darmstadt 00005 * 00006 * This program is free software; you can redistribute it and/or modify 00007 * it under the terms of the GNU General Public License as published by 00008 * the Free Software Foundation; either version 2 of the License, or 00009 * (at your option) any later version. 00010 * 00011 * This program is distributed in the hope that it will be useful, 00012 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00013 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00014 * GNU General Public License for more details. 00015 * 00016 * You should have received a copy of the GNU General Public License 00017 * along with this program; if not, write to the Free Software 00018 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00019 * 00020 */ 00021 00022 //based on: 00023 /* 00024 * Gazebo - Outdoor Multi-Robot Simulator 00025 * Copyright (C) 2003 00026 * Nate Koenig & Andrew Howard 00027 * 00028 * This program is free software; you can redistribute it and/or modify 00029 * it under the terms of the GNU General Public License as published by 00030 * the Free Software Foundation; either version 2 of the License, or 00031 * (at your option) any later version. 00032 * 00033 * This program is distributed in the hope that it will be useful, 00034 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00035 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00036 * GNU General Public License for more details. 00037 * 00038 * You should have received a copy of the GNU General Public License 00039 * along with this program; if not, write to the Free Software 00040 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00041 * 00042 */ 00043 /* 00044 * Desc: A dynamic controller plugin that publishes ROS image_raw camera_info topic for generic camera sensor. 00045 * Author: John Hsu 00046 * Date: 24 Sept 2008 00047 * SVN: $Id$ 00048 */ 00049 #ifndef GAZEBO_ROS_CAMERA_HH 00050 #define GAZEBO_ROS_CAMERA_HH 00051 00052 #define USE_CBQ 00053 #ifdef USE_CBQ 00054 #include <ros/callback_queue.h> 00055 #include <ros/advertise_options.h> 00056 #endif 00057 00058 #include <ros/ros.h> 00059 #include "boost/thread/mutex.hpp" 00060 #include <sensor_msgs/Image.h> 00061 #include <sensor_msgs/CameraInfo.h> 00062 #include <std_msgs/Float64.h> 00063 #include <gazebo/Param.hh> 00064 #include <gazebo/Controller.hh> 00065 00066 #include <hector_gazebo_thermal_camera/GazeboRosThermalCameraConfig.h> 00067 #include <dynamic_reconfigure/server.h> 00068 #include <nodelet/loader.h> 00069 00070 #include <pcl_ros/point_cloud.h> 00071 #include <pcl/point_types.h> 00072 #include <Eigen/Core> 00073 00074 #include "image_transport/image_transport.h" 00075 00076 namespace gazebo 00077 { 00078 class MonoCameraSensor; 00079 00082 00134 class GazeboRosThermalCamera : public Controller 00135 { 00138 public: GazeboRosThermalCamera(Entity *parent); 00139 00141 public: virtual ~GazeboRosThermalCamera(); 00142 00145 protected: virtual void LoadChild(XMLConfigNode *node); 00146 00148 protected: virtual void InitChild(); 00149 00151 protected: virtual void UpdateChild(); 00152 00154 protected: virtual void FiniChild(); 00155 00157 private: void PutCameraData(); 00159 private: void PublishCameraInfo(); 00160 00162 private: int imageConnectCount; 00163 private: void ImageConnect(); 00164 private: void ImageDisconnect(); 00165 00167 private: int pointCloudConnectCount; 00168 private: void PointCloudConnect(); 00169 private: void PointCloudDisconnect(); 00170 00172 private: int infoConnectCount; 00173 private: void InfoConnect(); 00174 private: void InfoDisconnect(); 00175 private: void SetHFOV(const std_msgs::Float64::ConstPtr& hfov); 00176 private: void SetUpdateRate(const std_msgs::Float64::ConstPtr& update_rate); 00177 00178 // convert image from gazebo sensor format to desired format 00179 private: void convertImageFormat(unsigned char *dst, const unsigned char *src); 00180 00181 private: bool fillDepthImage(pcl::PointCloud<pcl::PointXYZ>& point_cloud, 00182 uint32_t rows_arg, uint32_t cols_arg, 00183 uint32_t step_arg, void* data_arg); 00184 00186 private: MonoCameraSensor *myParent; 00187 00189 private: ros::NodeHandle* rosnode_; 00190 private: ros::NodeHandle* managernode_; 00191 private: nodelet::Loader* manager_; 00192 private: ros::Publisher point_cloud_pub_,camera_info_pub_; 00193 private: image_transport::Publisher image_pub_; 00194 private: image_transport::ImageTransport* itnode_; 00195 00197 private: sensor_msgs::Image imageMsg; 00198 private: sensor_msgs::CameraInfo cameraInfoMsg; 00199 private: pcl::PointCloud<pcl::PointXYZ> pointCloudMsg; 00200 00202 private: ParamT<std::string> *imageTopicNameP; // deprecated 00203 private: ParamT<std::string> *cameraInfoTopicNameP; // deprecated 00204 private: ParamT<std::string> *pointCloudTopicNameP; 00205 private: ParamT<std::string> *cameraNameP; 00206 private: ParamT<std::string> *frameNameP; 00207 00208 private: ParamT<double> *CxPrimeP; // rectified optical center x, for sim, CxPrime == Cx 00209 private: ParamT<double> *CxP; // optical center x 00210 private: ParamT<double> *CyP; // optical center y 00211 private: ParamT<double> *focal_lengthP; // also known as focal length 00212 private: ParamT<double> *hack_baselineP; // also known as focal length 00213 private: ParamT<double> *pointCloudCutoffP; // linear distortion 00214 private: ParamT<double> *distortion_k1P; // linear distortion 00215 private: ParamT<double> *distortion_k2P; // quadratic distortion 00216 private: ParamT<double> *distortion_k3P; // cubic distortion 00217 private: ParamT<double> *distortion_t1P; // tangential distortion 00218 private: ParamT<double> *distortion_t2P; // tangential distortion 00219 00221 private: ParamT<std::string> *robotNamespaceP; 00222 private: std::string robotNamespace; 00223 00225 private: std::string cameraName; 00227 private: std::string imageTopicName; 00229 private: std::string cameraInfoTopicName; 00231 private: std::string pointCloudTopicName; 00234 private: std::string frameName; 00235 00236 private: double CxPrime; 00237 private: double Cx; 00238 private: double Cy; 00239 private: double focal_length; 00240 private: double hack_baseline; 00241 private: double pointCloudCutoff; 00242 private: double distortion_k1; 00243 private: double distortion_k2; 00244 private: double distortion_k3; 00245 private: double distortion_t1; 00246 private: double distortion_t2; 00247 00249 private: boost::mutex lock; 00250 00252 private: int height, width, depth; 00253 private: std::string type; 00254 private: int skip; 00255 00256 private: ros::Subscriber cameraHFOVSubscriber_; 00257 private: ros::Subscriber cameraUpdateRateSubscriber_; 00258 00259 // Time last published, refrain from publish unless new image has been rendered 00260 Time last_point_cloud_pub_time_, last_image_pub_time_, last_camera_info_pub_time_; 00261 00262 // Allow dynamic reconfiguration of camera params 00263 dynamic_reconfigure::Server<hector_gazebo_thermal_camera::GazeboRosThermalCameraConfig> *dyn_srv_; 00264 00265 void configCallback(hector_gazebo_thermal_camera::GazeboRosThermalCameraConfig &config, uint32_t level); 00266 00267 // Name of camera 00268 std::string dynamicReconfigureName; 00269 00270 #ifdef USE_CBQ 00271 private: ros::CallbackQueue camera_queue_; 00272 private: void CameraQueueThread(); 00273 private: boost::thread callback_queue_thread_; 00274 #endif 00275 00276 public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 00277 }; 00278 00280 00281 00282 } 00283 #endif 00284