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 #include <geometry_msgs/WrenchStamped.h>
00036
00037 namespace gazebo {
00038
00039 using namespace common;
00040 using namespace math;
00041 using namespace hector_quadrotor_model;
00042
00043 GazeboQuadrotorAerodynamics::GazeboQuadrotorAerodynamics()
00044 : node_handle_(0)
00045 {
00046 }
00047
00048 GazeboQuadrotorAerodynamics::~GazeboQuadrotorAerodynamics()
00049 {
00050 event::Events::DisconnectWorldUpdateBegin(updateConnection);
00051 if (node_handle_) {
00052 node_handle_->shutdown();
00053 if (callback_queue_thread_.joinable())
00054 callback_queue_thread_.join();
00055 delete node_handle_;
00056 }
00057 }
00058
00060
00061 void GazeboQuadrotorAerodynamics::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00062 {
00063 world = _model->GetWorld();
00064 link = _model->GetLink();
00065
00066
00067 namespace_.clear();
00068 param_namespace_ = "quadrotor_aerodynamics";
00069 wind_topic_ = "/wind";
00070 wrench_topic_ = "aerodynamics/wrench";
00071 frame_id_ = link->GetName();
00072
00073
00074 if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->Get<std::string>();
00075 if (_sdf->HasElement("paramNamespace")) param_namespace_ = _sdf->GetElement("paramNamespace")->Get<std::string>();
00076 if (_sdf->HasElement("windTopicName")) wind_topic_ = _sdf->GetElement("windTopicName")->Get<std::string>();
00077 if (_sdf->HasElement("wrenchTopic")) wrench_topic_ = _sdf->GetElement("wrenchTopic")->Get<std::string>();
00078 if (_sdf->HasElement("frameId")) frame_id_= _sdf->GetElement("frameId")->Get<std::string>();
00079
00080
00081 if (!ros::isInitialized())
00082 {
00083 ROS_FATAL_STREAM("A ROS node for Gazebo has not been initialized, unable to load plugin. "
00084 << "Load the Gazebo system plugin 'libgazebo_ros_api_plugin.so' in the gazebo_ros package)");
00085 return;
00086 }
00087
00088 node_handle_ = new ros::NodeHandle(namespace_);
00089
00090
00091 if (!model_.configure(ros::NodeHandle(*node_handle_, param_namespace_))) {
00092 gzwarn << "[quadrotor_propulsion] Could not properly configure the aerodynamics plugin. Make sure you loaded the parameter file." << std::endl;
00093 return;
00094 }
00095
00096
00097 if (!wind_topic_.empty())
00098 {
00099 ros::SubscribeOptions ops;
00100 ops.callback_queue = &callback_queue_;
00101 ops.initByFullCallbackType<const geometry_msgs::Vector3 &>(
00102 wind_topic_, 1,
00103 boost::bind(&QuadrotorAerodynamics::setWind, &model_, _1)
00104 );
00105 wind_subscriber_ = node_handle_->subscribe(ops);
00106 }
00107
00108
00109 if (!wrench_topic_.empty())
00110 {
00111 ros::AdvertiseOptions ops;
00112 ops.callback_queue = &callback_queue_;
00113 ops.init<geometry_msgs::WrenchStamped>(wrench_topic_, 10);
00114 wrench_publisher_ = node_handle_->advertise(ops);
00115 }
00116
00117
00118
00119
00120
00121
00122 updateConnection = event::Events::ConnectWorldUpdateBegin(
00123 boost::bind(&GazeboQuadrotorAerodynamics::Update, this));
00124 }
00125
00127
00128 void GazeboQuadrotorAerodynamics::Update()
00129 {
00130
00131 Time current_time = world->GetSimTime();
00132 Time dt = current_time - last_time_;
00133 last_time_ = current_time;
00134 if (dt <= 0.0) return;
00135
00136
00137 callback_queue_.callAvailable();
00138
00139
00140 geometry_msgs::Quaternion orientation;
00141 fromQuaternion(link->GetWorldPose().rot, orientation);
00142 model_.setOrientation(orientation);
00143
00144 geometry_msgs::Twist twist;
00145 fromVector(link->GetWorldLinearVel(), twist.linear);
00146 fromVector(link->GetWorldAngularVel(), twist.angular);
00147 model_.setTwist(twist);
00148
00149
00150 model_.update(dt.Double());
00151
00152
00153 Vector3 force, torque;
00154 toVector(model_.getWrench().force, force);
00155 toVector(model_.getWrench().torque, torque);
00156
00157
00158 if (wrench_publisher_) {
00159 geometry_msgs::WrenchStamped wrench_msg;
00160 wrench_msg.header.stamp = ros::Time(current_time.sec, current_time.nsec);
00161 wrench_msg.header.frame_id = frame_id_;
00162 wrench_msg.wrench = model_.getWrench();
00163 wrench_publisher_.publish(wrench_msg);
00164 }
00165
00166
00167 link->AddRelativeForce(force);
00168 link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
00169 }
00170
00172
00173 void GazeboQuadrotorAerodynamics::Reset()
00174 {
00175 model_.reset();
00176 }
00177
00179
00180 void GazeboQuadrotorAerodynamics::QueueThread()
00181 {
00182 static const double timeout = 0.01;
00183
00184 while (node_handle_->ok())
00185 {
00186 callback_queue_.callAvailable(ros::WallDuration(timeout));
00187 }
00188 }
00189
00190
00191 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorAerodynamics)
00192
00193 }