gazebo_ros_baro.cpp
Go to the documentation of this file.
00001 //=================================================================================================
00002 // Copyright (c) 2012, Johannes Meyer, TU Darmstadt
00003 // All rights reserved.
00004 
00005 // Redistribution and use in source and binary forms, with or without
00006 // modification, are permitted provided that the following conditions are met:
00007 //     * Redistributions of source code must retain the above copyright
00008 //       notice, this list of conditions and the following disclaimer.
00009 //     * Redistributions in binary form must reproduce the above copyright
00010 //       notice, this list of conditions and the following disclaimer in the
00011 //       documentation and/or other materials provided with the distribution.
00012 //     * Neither the name of the Flight Systems and Automatic Control group,
00013 //       TU Darmstadt, nor the names of its contributors may be used to
00014 //       endorse or promote products derived from this software without
00015 //       specific prior written permission.
00016 
00017 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND
00018 // ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00019 // WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00020 // DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER BE LIABLE FOR ANY
00021 // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00022 // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00023 // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00024 // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00025 // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00026 // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00027 //=================================================================================================
00028 
00029 #include <hector_quadrotor_gazebo_plugins/gazebo_ros_baro.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032 
00033 static const double DEFAULT_ELEVATION = 0.0;
00034 static const double DEFAULT_QNH       = 1013.25;
00035 
00036 namespace gazebo {
00037 
00038 GazeboRosBaro::GazeboRosBaro()
00039 {
00040 }
00041 
00043 // Destructor
00044 GazeboRosBaro::~GazeboRosBaro()
00045 {
00046   updateTimer.Disconnect(updateConnection);
00047   node_handle_->shutdown();
00048   delete node_handle_;
00049 }
00050 
00052 // Load the controller
00053 void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00054 {
00055   world = _model->GetWorld();
00056 
00057   // load parameters
00058   if (!_sdf->HasElement("robotNamespace"))
00059     namespace_.clear();
00060   else
00061     namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00062 
00063   if (!_sdf->HasElement("bodyName"))
00064   {
00065     link = _model->GetLink();
00066     link_name_ = link->GetName();
00067   }
00068   else {
00069     link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00070     link = _model->GetLink(link_name_);
00071   }
00072 
00073   if (!link)
00074   {
00075     ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00076     return;
00077   }
00078 
00079   if (!_sdf->HasElement("frameId"))
00080     frame_id_ = link->GetName();
00081   else
00082     frame_id_ = _sdf->GetElement("frameId")->GetValueString();
00083 
00084   if (!_sdf->HasElement("topicName"))
00085     height_topic_ = "pressure_height";
00086   else
00087     height_topic_ = _sdf->GetElement("topicName")->GetValueString();
00088 
00089   if (!_sdf->HasElement("altimeterTopicName"))
00090     altimeter_topic_ = "altimeter";
00091   else
00092     altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->GetValueString();
00093 
00094   if (!_sdf->HasElement("elevation"))
00095     elevation_ = DEFAULT_ELEVATION;
00096   else
00097     elevation_ = _sdf->GetElement("elevation")->GetValueDouble();
00098 
00099   if (!_sdf->HasElement("qnh"))
00100     qnh_ = DEFAULT_QNH;
00101   else
00102     qnh_ = _sdf->GetElement("qnh")->GetValueDouble();
00103 
00104   sensor_model_.Load(_sdf);
00105   height_.header.frame_id = frame_id_;
00106 
00107   // start ros node
00108   if (!ros::isInitialized())
00109   {
00110     int argc = 0;
00111     char **argv = NULL;
00112     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00113   }
00114 
00115   node_handle_ = new ros::NodeHandle(namespace_);
00116   if (!height_topic_.empty()) {
00117 #ifdef USE_MAV_MSGS
00118     height_publisher_ = node_handle_->advertise<mav_msgs::Height>(height_topic_, 10);
00119 #else
00120     height_publisher_ = node_handle_->advertise<geometry_msgs::PointStamped>(height_topic_, 10);
00121 #endif
00122   }
00123 
00124   if (!altimeter_topic_.empty()) {
00125     altimeter_publisher_ = node_handle_->advertise<hector_uav_msgs::Altimeter>(altimeter_topic_, 10);
00126   }
00127 
00128   Reset();
00129 
00130   // connect Update function
00131   updateTimer.Load(world, _sdf);
00132   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosBaro::Update, this));
00133 }
00134 
00135 void GazeboRosBaro::Reset()
00136 {
00137   updateTimer.Reset();
00138   sensor_model_.reset();
00139 }
00140 
00142 // Update the controller
00143 void GazeboRosBaro::Update()
00144 {
00145   common::Time sim_time = world->GetSimTime();
00146   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00147 
00148   math::Pose pose = link->GetWorldPose();
00149   double height = sensor_model_(pose.pos.z, dt);
00150 
00151   if (height_publisher_) {
00152 #ifdef USE_MAV_MSGS
00153     double previous_height = height_.height;
00154     height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00155     height_.height = height;
00156     height_.height_variance = sensor_model_.gaussian_noise*sensor_model_.gaussian_noise + sensor_model_.drift*sensor_model_.drift;
00157     height_.climb = dt > 0.0 ? (height_.height - previous_height) / dt : 0.0;
00158     height_.climb_variance = sensor_model_.gaussian_noise*sensor_model_.gaussian_noise;
00159 #else
00160     height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00161     height_.point.z = height;
00162 #endif
00163 
00164     height_publisher_.publish(height_);
00165   }
00166 
00167   if (altimeter_publisher_) {
00168     altimeter_.header = height_.header;
00169     altimeter_.altitude = height + elevation_;
00170     altimeter_.pressure = pow((1.0 - altimeter_.altitude / 44330.0), 5.263157) * qnh_;
00171     altimeter_.qnh = qnh_;
00172     altimeter_publisher_.publish(altimeter_);
00173   }
00174 }
00175 
00176 // Register this plugin with the simulator
00177 GZ_REGISTER_MODEL_PLUGIN(GazeboRosBaro)
00178 
00179 } // namespace gazebo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_quadrotor_gazebo_plugins
Author(s): Johannes Meyer
autogenerated on Mon Jul 15 2013 16:48:25