gazebo_quadrotor_propulsion.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_quadrotor_propulsion.h>
00030 #include <hector_quadrotor_model/helpers.h>
00031 
00032 #include <gazebo/common/Events.hh>
00033 #include <gazebo/physics/physics.hh>
00034 
00035 #include <rosgraph_msgs/Clock.h>
00036 #include <geometry_msgs/WrenchStamped.h>
00037 
00038 namespace gazebo {
00039 
00040 using namespace common;
00041 using namespace math;
00042 using namespace hector_quadrotor_model;
00043 
00044 GazeboQuadrotorPropulsion::GazeboQuadrotorPropulsion()
00045   : node_handle_(0)
00046 {
00047 }
00048 
00049 GazeboQuadrotorPropulsion::~GazeboQuadrotorPropulsion()
00050 {
00051   event::Events::DisconnectWorldUpdateBegin(updateConnection);
00052   if (node_handle_) {
00053     node_handle_->shutdown();
00054     if (callback_queue_thread_.joinable())
00055       callback_queue_thread_.join();
00056     delete node_handle_;
00057   }
00058 }
00059 
00061 // Load the controller
00062 void GazeboQuadrotorPropulsion::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00063 {
00064   world = _model->GetWorld();
00065   link = _model->GetLink();
00066 
00067   // default parameters
00068   namespace_.clear();
00069   param_namespace_ = "quadrotor_propulsion";
00070   trigger_topic_ = "quadro/trigger";
00071   command_topic_ = "command/motor";
00072   pwm_topic_ = "motor_pwm";
00073   wrench_topic_ = "propulsion/wrench";
00074   supply_topic_ = "supply";
00075   status_topic_ = "motor_status";
00076   control_tolerance_ = ros::Duration();
00077   control_delay_ = ros::Duration();
00078   frame_id_ = link->GetName();
00079 
00080   // load parameters
00081   if (_sdf->HasElement("robotNamespace"))   namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00082   if (_sdf->HasElement("paramNamespace"))   param_namespace_ = _sdf->GetElement("paramNamespace")->Get<std::string>();
00083   if (_sdf->HasElement("triggerTopic"))     trigger_topic_ = _sdf->GetElement("triggerTopic")->Get<std::string>();
00084   if (_sdf->HasElement("topicName"))        command_topic_ = _sdf->GetElement("topicName")->Get<std::string>();
00085   if (_sdf->HasElement("pwmTopicName"))     pwm_topic_ = _sdf->GetElement("pwmTopicName")->Get<std::string>();
00086   if (_sdf->HasElement("wrenchTopic"))      wrench_topic_ = _sdf->GetElement("wrenchTopic")->Get<std::string>();
00087   if (_sdf->HasElement("supplyTopic"))      supply_topic_ = _sdf->GetElement("supplyTopic")->Get<std::string>();
00088   if (_sdf->HasElement("statusTopic"))      status_topic_ = _sdf->GetElement("statusTopic")->Get<std::string>();
00089   if (_sdf->HasElement("frameId"))          frame_id_= _sdf->GetElement("frameId")->Get<std::string>();
00090 
00091   if (_sdf->HasElement("voltageTopicName")) {
00092     gzwarn << "[quadrotor_propulsion] Plugin parameter 'voltageTopicName' is deprecated! Plese change your config to use "
00093            << "'topicName' for MotorCommand messages or 'pwmTopicName' for MotorPWM messages." << std::endl;
00094     pwm_topic_ = _sdf->GetElement("voltageTopicName")->Get<std::string>();
00095   }
00096 
00097   // set control timing parameters
00098   controlTimer.Load(world, _sdf, "control");
00099   motorStatusTimer.Load(world, _sdf, "motorStatus");
00100   if (_sdf->HasElement("controlTolerance")) control_tolerance_.fromSec(_sdf->GetElement("controlTolerance")->Get<double>());
00101   if (_sdf->HasElement("controlDelay"))     control_delay_.fromSec(_sdf->GetElement("controlDelay")->Get<double>());
00102 
00103   // set initial supply voltage
00104   if (_sdf->HasElement("supplyVoltage"))    model_.setInitialSupplyVoltage(_sdf->GetElement("supplyVoltage")->Get<double>());
00105 
00106   // Make sure the ROS node for Gazebo has already been initialized
00107   if (!ros::isInitialized())
00108   {
00109     ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00110       << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00111     return;
00112   }
00113 
00114   node_handle_ = new ros::NodeHandle(namespace_);
00115 
00116   // get model parameters
00117   if (!model_.configure(ros::NodeHandle(*node_handle_, param_namespace_))) {
00118     gzwarn << "[quadrotor_propulsion] Could not properly configure the propulsion plugin. Make sure you loaded the parameter file." << std::endl;
00119     return;
00120   }
00121 
00122   // publish trigger
00123   if (!trigger_topic_.empty())
00124   {
00125     ros::AdvertiseOptions ops;
00126     ops.callback_queue = &callback_queue_;
00127     ops.init<rosgraph_msgs::Clock>(trigger_topic_, 10);
00128     trigger_publisher_ = node_handle_->advertise(ops);
00129   }
00130 
00131   // subscribe voltage command
00132   if (!command_topic_.empty())
00133   {
00134     ros::SubscribeOptions ops;
00135     ops.callback_queue = &callback_queue_;
00136     ops.init<hector_uav_msgs::MotorCommand>(command_topic_, 1, boost::bind(&QuadrotorPropulsion::addCommandToQueue, &model_, _1));
00137     command_subscriber_ = node_handle_->subscribe(ops);
00138   }
00139 
00140   // subscribe pwm command
00141   if (!pwm_topic_.empty())
00142   {
00143     ros::SubscribeOptions ops;
00144     ops.callback_queue = &callback_queue_;
00145     ops.init<hector_uav_msgs::MotorPWM>(pwm_topic_, 1, boost::bind(&QuadrotorPropulsion::addPWMToQueue, &model_, _1));
00146     pwm_subscriber_ = node_handle_->subscribe(ops);
00147   }
00148 
00149   // advertise wrench
00150   if (!wrench_topic_.empty())
00151   {
00152     ros::AdvertiseOptions ops;
00153     ops.callback_queue = &callback_queue_;
00154     ops.init<geometry_msgs::WrenchStamped>(wrench_topic_, 10);
00155     wrench_publisher_ = node_handle_->advertise(ops);
00156   }
00157 
00158   // advertise and latch supply
00159   if (!supply_topic_.empty())
00160   {
00161     ros::AdvertiseOptions ops;
00162     ops.callback_queue = &callback_queue_;
00163     ops.latch = true;
00164     ops.init<hector_uav_msgs::Supply>(supply_topic_, 10);
00165     supply_publisher_ = node_handle_->advertise(ops);
00166     supply_publisher_.publish(model_.getSupply());
00167   }
00168 
00169   // advertise motor_status
00170   if (!status_topic_.empty())
00171   {
00172     ros::AdvertiseOptions ops;
00173     ops.callback_queue = &callback_queue_;
00174     ops.init<hector_uav_msgs::MotorStatus>(status_topic_, 10);
00175     motor_status_publisher_ = node_handle_->advertise(ops);
00176   }
00177 
00178   // callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorPropulsion::QueueThread,this ) );
00179 
00180   Reset();
00181 
00182   // New Mechanism for Updating every World Cycle
00183   // Listen to the update event. This event is broadcast every
00184   // simulation iteration.
00185   updateConnection = event::Events::ConnectWorldUpdateBegin(
00186       boost::bind(&GazeboQuadrotorPropulsion::Update, this));
00187 }
00188 
00190 // Update the controller
00191 void GazeboQuadrotorPropulsion::Update()
00192 {
00193   // Get simulator time
00194   Time current_time = world->GetSimTime();
00195   Time dt = current_time - last_time_;
00196   last_time_ = current_time;
00197   if (dt <= 0.0) return;
00198 
00199   // Send trigger
00200   bool trigger = controlTimer.getUpdatePeriod() > 0.0 ? controlTimer.update() : false;
00201   if (trigger && trigger_publisher_) {
00202     rosgraph_msgs::Clock clock;
00203     clock.clock = ros::Time(current_time.sec, current_time.nsec);
00204     trigger_publisher_.publish(clock);
00205 
00206     ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Sent a trigger message at t = " << current_time.Double() << " (dt = " << (current_time - last_trigger_time_).Double() << ")");
00207     last_trigger_time_ = current_time;
00208   }
00209 
00210   // Get new commands/state
00211   callback_queue_.callAvailable();
00212 
00213   // Process input queue
00214   model_.processQueue(ros::Time(current_time.sec, current_time.nsec), control_tolerance_, control_delay_, (model_.getMotorStatus().on && trigger) ? ros::WallDuration(1.0) : ros::WallDuration(), &callback_queue_);
00215 
00216   // fill input vector u for propulsion model
00217   geometry_msgs::Twist twist;
00218   fromVector(link->GetRelativeLinearVel(), twist.linear);
00219   fromVector(link->GetRelativeAngularVel(), twist.angular);
00220   model_.setTwist(twist);
00221 
00222   // update the model
00223   model_.update(dt.Double());
00224 
00225   // get wrench from model
00226   Vector3 force, torque;
00227   toVector(model_.getWrench().force, force);
00228   toVector(model_.getWrench().torque, torque);
00229 
00230   // publish wrench
00231   if (wrench_publisher_) {
00232     geometry_msgs::WrenchStamped wrench_msg;
00233     wrench_msg.header.stamp = ros::Time(current_time.sec, current_time.nsec);
00234     wrench_msg.header.frame_id = frame_id_;
00235     wrench_msg.wrench = model_.getWrench();
00236     wrench_publisher_.publish(wrench_msg);
00237   }
00238 
00239   // publish motor status
00240   if (motor_status_publisher_ && motorStatusTimer.update() /* && current_time >= last_motor_status_time_ + control_period_ */) {
00241     hector_uav_msgs::MotorStatus motor_status = model_.getMotorStatus();
00242     motor_status.header.stamp = ros::Time(current_time.sec, current_time.nsec);
00243     motor_status_publisher_.publish(motor_status);
00244     last_motor_status_time_ = current_time;
00245   }
00246 
00247   // publish supply
00248   if (supply_publisher_ && current_time >= last_supply_time_ + 1.0) {
00249     supply_publisher_.publish(model_.getSupply());
00250     last_supply_time_ = current_time;
00251   }
00252 
00253   // set force and torque in gazebo
00254   link->AddRelativeForce(force);
00255   link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
00256 }
00257 
00259 // Reset the controller
00260 void GazeboQuadrotorPropulsion::Reset()
00261 {
00262   model_.reset();
00263   last_time_ = Time();
00264   last_trigger_time_ = Time();
00265   last_motor_status_time_ = Time();
00266   last_supply_time_ = Time();
00267 }
00268 
00270 // custom callback queue thread
00271 void GazeboQuadrotorPropulsion::QueueThread()
00272 {
00273   static const double timeout = 0.01;
00274 
00275   while (node_handle_->ok())
00276   {
00277     callback_queue_.callAvailable(ros::WallDuration(timeout));
00278   }
00279 }
00280 
00281 // Register this plugin with the simulator
00282 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorPropulsion)
00283 
00284 } // namespace gazebo


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