Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029 #include <hector_quadrotor_gazebo_plugins/gazebo_quadrotor_aerodynamics.h>
00030 #include <hector_quadrotor_model/helpers.h>
00031
00032 #include <gazebo/common/Events.hh>
00033 #include <gazebo/physics/physics.hh>
00034
00035 namespace gazebo {
00036
00037 using namespace common;
00038 using namespace math;
00039 using namespace hector_quadrotor_model;
00040
00041 GazeboQuadrotorAerodynamics::GazeboQuadrotorAerodynamics()
00042 {
00043 }
00044
00045 GazeboQuadrotorAerodynamics::~GazeboQuadrotorAerodynamics()
00046 {
00047 event::Events::DisconnectWorldUpdateBegin(updateConnection);
00048 node_handle_->shutdown();
00049 callback_queue_thread_.join();
00050 delete node_handle_;
00051 }
00052
00054
00055 void GazeboQuadrotorAerodynamics::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00056 {
00057 world = _model->GetWorld();
00058 link = _model->GetLink();
00059
00060
00061 namespace_.clear();
00062 param_namespace_ = "quadrotor_aerodynamics";
00063 wind_topic_ = "wind";
00064
00065
00066 if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->GetValueString();
00067 if (_sdf->HasElement("paramNamespace")) param_namespace_ = _sdf->GetElement("paramNamespace")->GetValueString();
00068 if (_sdf->HasElement("windTopicName")) wind_topic_ = _sdf->GetElement("windTopicName")->GetValueString();
00069
00070
00071 model_.configure(param_namespace_);
00072
00073
00074 if (!ros::isInitialized())
00075 {
00076 int argc = 0;
00077 char **argv = NULL;
00078 ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00079 }
00080 node_handle_ = new ros::NodeHandle(namespace_);
00081
00082
00083 if (!wind_topic_.empty())
00084 {
00085 ros::SubscribeOptions ops;
00086 ops.callback_queue = &callback_queue_;
00087 ops.initByFullCallbackType<const geometry_msgs::Vector3 &>(
00088 wind_topic_, 1,
00089 boost::bind(&QuadrotorAerodynamics::setWind, &model_, _1)
00090 );
00091 wind_subscriber_ = node_handle_->subscribe(ops);
00092 }
00093
00094 callback_queue_thread_ = boost::thread( boost::bind( &GazeboQuadrotorAerodynamics::QueueThread,this ) );
00095
00096
00097
00098
00099 updateConnection = event::Events::ConnectWorldUpdateBegin(
00100 boost::bind(&GazeboQuadrotorAerodynamics::Update, this));
00101 }
00102
00104
00105 void GazeboQuadrotorAerodynamics::Update()
00106 {
00107
00108 Time current_time = world->GetSimTime();
00109 Time dt = current_time - last_time_;
00110 last_time_ = current_time;
00111 if (dt <= 0.0) return;
00112
00113
00114
00115
00116
00117 geometry_msgs::Twist twist;
00118 fromVector(link->GetRelativeLinearVel(), twist.linear);
00119 fromVector(link->GetRelativeAngularVel(), twist.angular);
00120 model_.setTwist(twist);
00121
00122
00123 model_.update(dt.Double());
00124
00125
00126 Vector3 force, torque;
00127 toVector(model_.getWrench().force, force);
00128 toVector(model_.getWrench().torque, torque);
00129
00130
00131 link->AddRelativeForce(force);
00132 link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
00133 }
00134
00136
00137 void GazeboQuadrotorAerodynamics::Reset()
00138 {
00139 model_.reset();
00140 }
00141
00143
00144 void GazeboQuadrotorAerodynamics::QueueThread()
00145 {
00146 static const double timeout = 0.01;
00147
00148 while (node_handle_->ok())
00149 {
00150 callback_queue_.callAvailable(ros::WallDuration(timeout));
00151 }
00152 }
00153
00154
00155 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorAerodynamics)
00156
00157 }