gazebo_ros_magnetic.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_gazebo_plugins/gazebo_ros_magnetic.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032 
00033 static const double DEFAULT_MAGNITUDE           = 1.0;
00034 static const double DEFAULT_REFERENCE_HEADING   = 0.0;
00035 static const double DEFAULT_DECLINATION         = 0.0;
00036 static const double DEFAULT_INCLINATION         = 60.0;
00037 
00038 namespace gazebo {
00039 
00040 GazeboRosMagnetic::GazeboRosMagnetic()
00041 {
00042 }
00043 
00045 // Destructor
00046 GazeboRosMagnetic::~GazeboRosMagnetic()
00047 {
00048   updateTimer.Disconnect(updateConnection);
00049 
00050   dynamic_reconfigure_server_.reset();
00051 
00052   node_handle_->shutdown();
00053   delete node_handle_;
00054 }
00055 
00057 // Load the controller
00058 void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00059 {
00060   world = _model->GetWorld();
00061 
00062   // load parameters
00063   if (_sdf->HasElement("robotNamespace"))
00064     namespace_ = _sdf->GetElement("robotNamespace")->GetValue()->GetAsString();
00065   else
00066     namespace_.clear();
00067 
00068   if (!_sdf->HasElement("topicName"))
00069     topic_ = "magnetic";
00070   else
00071     topic_ = _sdf->GetElement("topicName")->Get<std::string>();
00072 
00073 
00074   if (_sdf->HasElement("bodyName"))
00075   {
00076     link_name_ = _sdf->GetElement("bodyName")->GetValue()->GetAsString();
00077     link = _model->GetLink(link_name_);
00078   }
00079   else
00080   {
00081     link = _model->GetLink();
00082     link_name_ = link->GetName();
00083   }
00084 
00085   if (!link)
00086   {
00087     ROS_FATAL("GazeboRosMagnetic plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00088     return;
00089   }
00090 
00091   // default parameters
00092   frame_id_ = link_name_;
00093   magnitude_ = DEFAULT_MAGNITUDE;
00094   reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
00095   declination_ = DEFAULT_DECLINATION * M_PI/180.0;
00096   inclination_ = DEFAULT_INCLINATION * M_PI/180.0;
00097 
00098   if (_sdf->HasElement("frameId"))
00099     frame_id_ = _sdf->GetElement("frameId")->GetValue()->GetAsString();
00100 
00101   if (_sdf->HasElement("magnitude"))
00102     _sdf->GetElement("magnitude")->GetValue()->Get(magnitude_);
00103 
00104   if (_sdf->HasElement("referenceHeading"))
00105     if (_sdf->GetElement("referenceHeading")->GetValue()->Get(reference_heading_))
00106       reference_heading_ *= M_PI/180.0;
00107 
00108   if (_sdf->HasElement("declination"))
00109     if (_sdf->GetElement("declination")->GetValue()->Get(declination_))
00110       declination_ *= M_PI/180.0;
00111 
00112   if (_sdf->HasElement("inclination"))
00113     if (_sdf->GetElement("inclination")->GetValue()->Get(inclination_))
00114       inclination_ *= M_PI/180.0;
00115 
00116   // Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
00117   magnetic_field_.header.frame_id = frame_id_;
00118   magnetic_field_world_.x = magnitude_ *  cos(inclination_) * cos(reference_heading_ - declination_);
00119   magnetic_field_world_.y = magnitude_ *  cos(inclination_) * sin(reference_heading_ - declination_);
00120   magnetic_field_world_.z = magnitude_ * -sin(inclination_);
00121 
00122   sensor_model_.Load(_sdf);
00123 
00124   // start ros node
00125   if (!ros::isInitialized())
00126   {
00127     int argc = 0;
00128     char** argv = NULL;
00129     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00130   }
00131 
00132   node_handle_ = new ros::NodeHandle(namespace_);
00133   publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(topic_, 1);
00134 
00135   // setup dynamic_reconfigure server
00136   dynamic_reconfigure_server_.reset(new dynamic_reconfigure::Server<SensorModelConfig>(ros::NodeHandle(*node_handle_, topic_)));
00137   dynamic_reconfigure_server_->setCallback(boost::bind(&SensorModel3::dynamicReconfigureCallback, &sensor_model_, _1, _2));
00138 
00139   Reset();
00140 
00141   // connect Update function
00142   updateTimer.Load(world, _sdf);
00143   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosMagnetic::Update, this));
00144 }
00145 
00146 void GazeboRosMagnetic::Reset()
00147 {
00148   updateTimer.Reset();
00149   sensor_model_.reset();
00150 }
00151 
00153 // Update the controller
00154 void GazeboRosMagnetic::Update()
00155 {
00156   common::Time sim_time = world->GetSimTime();
00157   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00158 
00159   math::Pose pose = link->GetWorldPose();
00160   math::Vector3 field = sensor_model_(pose.rot.RotateVectorReverse(magnetic_field_world_), dt);
00161 
00162   magnetic_field_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00163   magnetic_field_.vector.x = field.x;
00164   magnetic_field_.vector.y = field.y;
00165   magnetic_field_.vector.z = field.z;
00166 
00167   publisher_.publish(magnetic_field_);
00168 }
00169 
00170 // Register this plugin with the simulator
00171 GZ_REGISTER_MODEL_PLUGIN(GazeboRosMagnetic)
00172 
00173 } // namespace gazebo


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher , Johannes Meyer
autogenerated on Wed Aug 26 2015 11:44:35