gazebo_ros_p3d.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 /*
00019  * Desc: 3D position interface for ground truth.
00020  * Author: Sachin Chitta and John Hsu
00021  * Date: 1 June 2008
00022  */
00023 
00024 #ifndef GAZEBO_ROS_P3D_HH
00025 #define GAZEBO_ROS_P3D_HH
00026 
00027 #include <string>
00028 
00029 #include <boost/thread.hpp>
00030 #include <boost/thread/mutex.hpp>
00031 
00032 #include <ros/ros.h>
00033 #include <nav_msgs/Odometry.h>
00034 
00035 #include <ros/callback_queue.h>
00036 #include <ros/advertise_options.h>
00037 
00038 #include <gazebo/physics/physics.hh>
00039 #include <gazebo/transport/TransportTypes.hh>
00040 #include <gazebo/common/Time.hh>
00041 #include <gazebo/common/Plugin.hh>
00042 #include <gazebo/common/Events.hh>
00043 
00044 #include <gazebo_plugins/PubQueue.h>
00045 
00046 namespace gazebo
00047 {
00048   class GazeboRosP3D : public ModelPlugin
00049   {
00051     public: GazeboRosP3D();
00052 
00054     public: virtual ~GazeboRosP3D();
00055 
00057     public: void Load(physics::ModelPtr _parent, sdf::ElementPtr _sdf);
00058 
00060     protected: virtual void UpdateChild();
00061 
00062     private: physics::WorldPtr world_;
00063     private: physics::ModelPtr model_;
00064 
00066     private: physics::LinkPtr link_;
00067 
00069     private: physics::LinkPtr reference_link_;
00070 
00071 
00073     private: ros::NodeHandle* rosnode_;
00074     private: ros::Publisher pub_;
00075     private: PubQueue<nav_msgs::Odometry>::Ptr pub_Queue;
00076 
00078     private: nav_msgs::Odometry pose_msg_;
00079 
00081     private: std::string link_name_;
00082 
00084     private: std::string topic_name_;
00085 
00088     private: std::string frame_name_;
00089     private: std::string tf_frame_name_;
00090 
00092     private: math::Pose offset_;
00093 
00095     private: boost::mutex lock;
00096 
00098     private: common::Time last_time_;
00099     private: math::Vector3 last_vpos_;
00100     private: math::Vector3 last_veul_;
00101     private: math::Vector3 apos_;
00102     private: math::Vector3 aeul_;
00103     private: math::Vector3 last_frame_vpos_;
00104     private: math::Vector3 last_frame_veul_;
00105     private: math::Vector3 frame_apos_;
00106     private: math::Vector3 frame_aeul_;
00107 
00108     // rate control
00109     private: double update_rate_;
00110 
00112     private: double gaussian_noise_;
00113 
00115     private: double GaussianKernel(double mu, double sigma);
00116 
00118     private: std::string robot_namespace_;
00119 
00120     private: ros::CallbackQueue p3d_queue_;
00121     private: void P3DQueueThread();
00122     private: boost::thread callback_queue_thread_;
00123 
00124     // Pointer to the update event connection
00125     private: event::ConnectionPtr update_connection_;
00126 
00127     private: unsigned int seed;
00128 
00129     // ros publish multi queue, prevents publish() blocking
00130     private: PubMultiQueue pmq;
00131   };
00132 }
00133 #endif


gazebo_plugins
Author(s): John Hsu
autogenerated on Thu Feb 23 2017 03:43:23