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   node_handle_->shutdown();
00050   delete node_handle_;
00051 }
00052 
00054 // Load the controller
00055 void GazeboRosMagnetic::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00056 {
00057   world = _model->GetWorld();
00058 
00059   // load parameters
00060   if (!_sdf->HasElement("robotNamespace"))
00061     namespace_.clear();
00062   else
00063     namespace_ = _sdf->GetElement("robotNamespace")->GetValueString() + "/";
00064 
00065   if (!_sdf->HasElement("topicName"))
00066     topic_ = "magnetic";
00067   else
00068     topic_ = _sdf->GetElement("topicName")->GetValueString();
00069 
00070   if (!_sdf->HasElement("bodyName"))
00071   {
00072     link = _model->GetLink();
00073     link_name_ = link->GetName();
00074   }
00075   else {
00076     link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00077     link = _model->GetLink(link_name_);
00078   }
00079 
00080   if (!link)
00081   {
00082     ROS_FATAL("GazeboRosMagnetic plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00083     return;
00084   }
00085 
00086   if (!_sdf->HasElement("frameId"))
00087     frame_id_ = link_name_;
00088   else
00089     frame_id_ = _sdf->GetElement("frameId")->GetValueString();
00090 
00091   if (!_sdf->HasElement("magnitude"))
00092     magnitude_ = DEFAULT_MAGNITUDE;
00093   else
00094     magnitude_ = _sdf->GetElement("magnitude")->GetValueDouble();
00095 
00096   if (!_sdf->HasElement("referenceHeading"))
00097     reference_heading_ = DEFAULT_REFERENCE_HEADING * M_PI/180.0;
00098   else
00099     reference_heading_ = _sdf->GetElement("referenceHeading")->GetValueDouble() * M_PI/180.0;
00100 
00101   if (!_sdf->HasElement("declination"))
00102     declination_ = DEFAULT_DECLINATION * M_PI/180.0;
00103   else
00104     declination_ = _sdf->GetElement("declination")->GetValueDouble() * M_PI/180.0;
00105 
00106   if (!_sdf->HasElement("inclination"))
00107     inclination_ = DEFAULT_INCLINATION * M_PI/180.0;
00108   else
00109     inclination_ = _sdf->GetElement("inclination")->GetValueDouble() * M_PI/180.0;
00110 
00111   // Note: Gazebo uses NorthWestUp coordinate system, heading and declination are compass headings
00112   magnetic_field_.header.frame_id = frame_id_;
00113   magnetic_field_world_.x = magnitude_ *  cos(inclination_) * cos(reference_heading_ - declination_);
00114   magnetic_field_world_.y = magnitude_ *  sin(reference_heading_ - declination_);
00115   magnetic_field_world_.z = magnitude_ * -sin(inclination_) * cos(reference_heading_ - declination_);
00116 
00117   sensor_model_.Load(_sdf);
00118 
00119   // start ros node
00120   if (!ros::isInitialized())
00121   {
00122     int argc = 0;
00123     char** argv = NULL;
00124     ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00125   }
00126 
00127   node_handle_ = new ros::NodeHandle(namespace_);
00128   publisher_ = node_handle_->advertise<geometry_msgs::Vector3Stamped>(topic_, 1);
00129 
00130   Reset();
00131 
00132   // connect Update function
00133   updateTimer.Load(world, _sdf);
00134   updateConnection = updateTimer.Connect(boost::bind(&GazeboRosMagnetic::Update, this));
00135 }
00136 
00137 void GazeboRosMagnetic::Reset()
00138 {
00139   updateTimer.Reset();
00140   sensor_model_.reset();
00141 }
00142 
00144 // Update the controller
00145 void GazeboRosMagnetic::Update()
00146 {
00147   common::Time sim_time = world->GetSimTime();
00148   double dt = updateTimer.getTimeSinceLastUpdate().Double();
00149 
00150   math::Pose pose = link->GetWorldPose();
00151   math::Vector3 field = sensor_model_(pose.rot.RotateVectorReverse(magnetic_field_world_), dt);
00152 
00153   magnetic_field_.header.stamp = ros::Time(sim_time.sec, sim_time.nsec);
00154   magnetic_field_.vector.x = field.x;
00155   magnetic_field_.vector.y = field.y;
00156   magnetic_field_.vector.z = field.z;
00157 
00158   publisher_.publish(magnetic_field_);
00159 }
00160 
00161 // Register this plugin with the simulator
00162 GZ_REGISTER_MODEL_PLUGIN(GazeboRosMagnetic)
00163 
00164 } // namespace gazebo
 All Classes Namespaces Files Functions Variables Typedefs Enumerations Properties Friends


hector_gazebo_plugins
Author(s): Stefan Kohlbrecher and Johannes Meyer
autogenerated on Mon Jul 15 2013 16:46:30