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 
00048   dynamic_reconfigure_server_.reset();
00049 
00050   node_handle_->shutdown();
00051   delete node_handle_;
00052 }
00053 
00055 // Load the controller
00056 void GazeboRosBaro::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00057 {
00058   world = _model->GetWorld();
00059   link = _model->GetLink();
00060   link_name_ = link->GetName();
00061 
00062   if (_sdf->HasElement("bodyName"))
00063   {
00064     link_name_ = _sdf->GetElement("bodyName")->Get<std::string>();
00065     link = _model->GetLink(link_name_);
00066   }
00067 
00068   if (!link)
00069   {
00070     ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00071     return;
00072   }
00073 
00074   // default parameters
00075   namespace_.clear();
00076   frame_id_ = link->GetName();
00077   height_topic_ = "pressure_height";
00078   altimeter_topic_ = "altimeter";
00079   elevation_ = DEFAULT_ELEVATION;
00080   qnh_ = DEFAULT_QNH;
00081 
00082   // load parameters
00083   if (_sdf->HasElement("robotNamespace"))     namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00084   if (_sdf->HasElement("frameId"))            frame_id_ = _sdf->GetElement("frameId")->Get<std::string>();
00085   if (_sdf->HasElement("topicName"))          height_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
00086   if (_sdf->HasElement("altimeterTopicName")) altimeter_topic_ = _sdf->GetElement("altimeterTopicName")->Get<std::string>();
00087   if (_sdf->HasElement("elevation"))          elevation_ = _sdf->GetElement("elevation")->Get<double>();
00088   if (_sdf->HasElement("qnh"))                qnh_ = _sdf->GetElement("qnh")->Get<double>();
00089 
00090   // load sensor model
00091   sensor_model_.Load(_sdf);
00092 
00093   height_.header.frame_id = frame_id_;
00094 
00095   // Make sure the ROS node for Gazebo has already been initialized
00096   if (!ros::isInitialized())
00097   {
00098     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00099       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00100     return;
00101   }
00102 
00103   node_handle_ = new ros::NodeHandle(namespace_);
00104 
00105   // advertise height
00106   if (!height_topic_.empty()) {
00107     height_publisher_ = node_handle_->advertise<geometry_msgs::PointStamped>(height_topic_, 10);
00108   }
00109 
00110   // advertise altimeter
00111   if (!altimeter_topic_.empty()) {
00112     altimeter_publisher_ = node_handle_->advertise<hector_uav_msgs::Altimeter>(altimeter_topic_, 10);
00113   }
00114 
00115   // setup dynamic_reconfigure server
00116   dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, altimeter_topic_)));
00117   dynamic_reconfigure_server_->setCallback(boost::bind(&SensorModel::dynamicReconfigureCallback, &sensor_model_, _1, _2));
00118 
00119   Reset();
00120 
00121   // connect Update function
00122   updateTimer.Load(world, _sdf);
00123   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosBaro::Update, this));
00124 }
00125 
00126 void GazeboRosBaro::Reset()
00127 {
00128   updateTimer.Reset();
00129   sensor_model_.reset();
00130 }
00131 
00133 // Update the controller
00134 void GazeboRosBaro::Update()
00135 {
00136   common::Time sim_time = world->GetSimTime();
00137   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00138 
00139   math::Pose pose = link->GetWorldPose();
00140   double height = sensor_model_(pose.pos.z, dt);
00141 
00142   if (height_publisher_) {
00143     height_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00144     height_.point.z = height;
00145     height_publisher_.publish(height_);
00146   }
00147 
00148   if (altimeter_publisher_) {
00149     altimeter_.header = height_.header;
00150     altimeter_.altitude = height + elevation_;
00151     altimeter_.pressure = pow((1.0 - altimeter_.altitude / 44330.0), 5.263157) * qnh_;
00152     altimeter_.qnh = qnh_;
00153     altimeter_publisher_.publish(altimeter_);
00154   }
00155 }
00156 
00157 // Register this plugin with the simulator
00158 GZ_REGISTER_MODEL_PLUGIN(GazeboRosBaro)
00159 
00160 } // namespace gazebo


hector_quadrotor_gazebo_plugins
Author(s): Johannes Meyer
autogenerated on Sun Jul 10 2016 04:50:04