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_simple_controller.h>
00030 #include <gazebo/common/Events.hh>
00031 #include <gazebo/physics/physics.hh>
00032
00033 #include <cmath>
00034
00035 #include <geometry_msgs/Wrench.h>
00036
00037 namespace gazebo {
00038
00039 GazeboQuadrotorSimpleController::GazeboQuadrotorSimpleController()
00040 {
00041 }
00042
00044
00045 GazeboQuadrotorSimpleController::~GazeboQuadrotorSimpleController()
00046 {
00047 event::Events::DisconnectWorldUpdateBegin(updateConnection);
00048
00049 node_handle_->shutdown();
00050 delete node_handle_;
00051 }
00052
00054
00055 void GazeboQuadrotorSimpleController::Load(physics::ModelPtr _model, sdf::ElementPtr _sdf)
00056 {
00057 world = _model->GetWorld();
00058 link = _model->GetLink();
00059 link_name_ = link->GetName();
00060
00061
00062 namespace_.clear();
00063 velocity_topic_ = "cmd_vel";
00064 imu_topic_.clear();
00065 state_topic_.clear();
00066 wrench_topic_ = "wrench_out";
00067 max_force_ = -1;
00068
00069
00070 if (_sdf->HasElement("robotNamespace")) namespace_ = _sdf->GetElement("robotNamespace")->GetValueString();
00071 if (_sdf->HasElement("topicName")) velocity_topic_ = _sdf->GetElement("topicName")->GetValueString();
00072 if (_sdf->HasElement("imuTopic")) imu_topic_ = _sdf->GetElement("imuTopic")->GetValueString();
00073 if (_sdf->HasElement("stateTopic")) state_topic_ = _sdf->GetElement("stateTopic")->GetValueString();
00074 if (_sdf->HasElement("wrenchTopic")) wrench_topic_ = _sdf->GetElement("wrenchTopic")->GetValueString();
00075 if (_sdf->HasElement("maxForce")) max_force_ = _sdf->GetElement("maxForce")->GetValueDouble();
00076
00077 if (_sdf->HasElement("bodyName") && _sdf->GetElement("bodyName")->GetValue()) {
00078 link_name_ = _sdf->GetElement("bodyName")->GetValueString();
00079 link = _model->GetLink(link_name_);
00080 }
00081
00082 if (!link)
00083 {
00084 ROS_FATAL("gazebo_ros_baro plugin error: bodyName: %s does not exist\n", link_name_.c_str());
00085 return;
00086 }
00087
00088
00089 controllers_.roll.Load(_sdf, "rollpitch");
00090 controllers_.pitch.Load(_sdf, "rollpitch");
00091 controllers_.yaw.Load(_sdf, "yaw");
00092 controllers_.velocity_x.Load(_sdf, "velocityXY");
00093 controllers_.velocity_y.Load(_sdf, "velocityXY");
00094 controllers_.velocity_z.Load(_sdf, "velocityZ");
00095
00096
00097 inertia = link->GetInertial()->GetPrincipalMoments();
00098 mass = link->GetInertial()->GetMass();
00099
00100
00101 if (!ros::isInitialized())
00102 {
00103 int argc = 0;
00104 char **argv = NULL;
00105 ros::init(argc,argv,"gazebo",ros::init_options::NoSigintHandler|ros::init_options::AnonymousName);
00106 }
00107 node_handle_ = new ros::NodeHandle(namespace_);
00108
00109
00110 if (!velocity_topic_.empty())
00111 {
00112 ros::SubscribeOptions ops = ros::SubscribeOptions::create<geometry_msgs::Twist>(
00113 velocity_topic_, 1,
00114 boost::bind(&GazeboQuadrotorSimpleController::VelocityCallback, this, _1),
00115 ros::VoidPtr(), &callback_queue_);
00116 velocity_subscriber_ = node_handle_->subscribe(ops);
00117 }
00118
00119
00120 if (!imu_topic_.empty())
00121 {
00122 ros::SubscribeOptions ops = ros::SubscribeOptions::create<sensor_msgs::Imu>(
00123 imu_topic_, 1,
00124 boost::bind(&GazeboQuadrotorSimpleController::ImuCallback, this, _1),
00125 ros::VoidPtr(), &callback_queue_);
00126 imu_subscriber_ = node_handle_->subscribe(ops);
00127
00128 ROS_INFO_NAMED("quadrotor_simple_controller", "Using imu information on topic %s as source of orientation and angular velocity.", imu_topic_.c_str());
00129 }
00130
00131
00132 if (!state_topic_.empty())
00133 {
00134 ros::SubscribeOptions ops = ros::SubscribeOptions::create<nav_msgs::Odometry>(
00135 state_topic_, 1,
00136 boost::bind(&GazeboQuadrotorSimpleController::StateCallback, this, _1),
00137 ros::VoidPtr(), &callback_queue_);
00138 state_subscriber_ = node_handle_->subscribe(ops);
00139
00140 ROS_INFO_NAMED("quadrotor_simple_controller", "Using state information on topic %s as source of state information.", state_topic_.c_str());
00141 }
00142
00143
00144 if (!wrench_topic_.empty())
00145 {
00146 ros::AdvertiseOptions ops = ros::AdvertiseOptions::create<geometry_msgs::Wrench>(
00147 wrench_topic_, 10,
00148 ros::SubscriberStatusCallback(), ros::SubscriberStatusCallback(), ros::VoidConstPtr(), &callback_queue_
00149 );
00150 wrench_publisher_ = node_handle_->advertise(ops);
00151 }
00152
00153
00154
00155
00156 Reset();
00157
00158
00159
00160
00161 controlTimer.Load(world, _sdf);
00162 updateConnection = event::Events::ConnectWorldUpdateBegin(
00163 boost::bind(&GazeboQuadrotorSimpleController::Update, this));
00164 }
00165
00167
00168 void GazeboQuadrotorSimpleController::VelocityCallback(const geometry_msgs::TwistConstPtr& velocity)
00169 {
00170 velocity_command_ = *velocity;
00171 }
00172
00173 void GazeboQuadrotorSimpleController::ImuCallback(const sensor_msgs::ImuConstPtr& imu)
00174 {
00175 pose.rot.Set(imu->orientation.w, imu->orientation.x, imu->orientation.y, imu->orientation.z);
00176 euler = pose.rot.GetAsEuler();
00177 angular_velocity = pose.rot.RotateVector(math::Vector3(imu->angular_velocity.x, imu->angular_velocity.y, imu->angular_velocity.z));
00178 }
00179
00180 void GazeboQuadrotorSimpleController::StateCallback(const nav_msgs::OdometryConstPtr& state)
00181 {
00182 math::Vector3 velocity1(velocity);
00183
00184 if (imu_topic_.empty()) {
00185 pose.pos.Set(state->pose.pose.position.x, state->pose.pose.position.y, state->pose.pose.position.z);
00186 pose.rot.Set(state->pose.pose.orientation.w, state->pose.pose.orientation.x, state->pose.pose.orientation.y, state->pose.pose.orientation.z);
00187 euler = pose.rot.GetAsEuler();
00188 angular_velocity.Set(state->twist.twist.angular.x, state->twist.twist.angular.y, state->twist.twist.angular.z);
00189 }
00190
00191 velocity.Set(state->twist.twist.linear.x, state->twist.twist.linear.y, state->twist.twist.linear.z);
00192
00193
00194 double dt = !state_stamp.isZero() ? (state->header.stamp - state_stamp).toSec() : 0.0;
00195 state_stamp = state->header.stamp;
00196 if (dt > 0.0) {
00197 acceleration = (velocity - velocity1) / dt;
00198 } else {
00199 acceleration.Set();
00200 }
00201 }
00202
00204
00205 void GazeboQuadrotorSimpleController::Update()
00206 {
00207
00208 callback_queue_.callAvailable();
00209
00210 double dt;
00211 if (controlTimer.update(dt) && dt > 0.0) {
00212
00213 if (imu_topic_.empty()) {
00214 pose = link->GetWorldPose();
00215 angular_velocity = link->GetWorldAngularVel();
00216 euler = pose.rot.GetAsEuler();
00217 }
00218 if (state_topic_.empty()) {
00219 acceleration = (link->GetWorldLinearVel() - velocity) / dt;
00220 velocity = link->GetWorldLinearVel();
00221 }
00222
00223
00224
00225
00226
00227
00228
00229
00230
00231
00232 math::Vector3 gravity_body = pose.rot.RotateVector(world->GetPhysicsEngine()->GetGravity());
00233 double gravity = gravity_body.GetLength();
00234 double load_factor = gravity * gravity / world->GetPhysicsEngine()->GetGravity().Dot(gravity_body);
00235
00236
00237 math::Quaternion heading_quaternion(cos(euler.z/2),0,0,sin(euler.z/2));
00238 math::Vector3 velocity_xy = heading_quaternion.RotateVectorReverse(velocity);
00239 math::Vector3 acceleration_xy = heading_quaternion.RotateVectorReverse(acceleration);
00240 math::Vector3 angular_velocity_body = pose.rot.RotateVectorReverse(angular_velocity);
00241
00242
00243 force.Set(0.0, 0.0, 0.0);
00244 torque.Set(0.0, 0.0, 0.0);
00245 double pitch_command = controllers_.velocity_x.update(velocity_command_.linear.x, velocity_xy.x, acceleration_xy.x, dt) / gravity;
00246 double roll_command = -controllers_.velocity_y.update(velocity_command_.linear.y, velocity_xy.y, acceleration_xy.y, dt) / gravity;
00247 torque.x = inertia.x * controllers_.roll.update(roll_command, euler.x, angular_velocity_body.x, dt);
00248 torque.y = inertia.y * controllers_.pitch.update(pitch_command, euler.y, angular_velocity_body.y, dt);
00249
00250
00251 torque.z = inertia.z * controllers_.yaw.update(velocity_command_.angular.z, angular_velocity.z, 0, dt);
00252 force.z = mass * (controllers_.velocity_z.update(velocity_command_.linear.z, velocity.z, acceleration.z, dt) + load_factor * gravity);
00253 if (max_force_ > 0.0 && force.z > max_force_) force.z = max_force_;
00254 if (force.z < 0.0) force.z = 0.0;
00255
00256
00257
00258
00259
00260
00261
00262
00263
00264
00265
00266 if (wrench_publisher_) {
00267 geometry_msgs::Wrench wrench;
00268 wrench.force.x = force.x;
00269 wrench.force.y = force.y;
00270 wrench.force.z = force.z;
00271 wrench.torque.x = torque.x;
00272 wrench.torque.y = torque.y;
00273 wrench.torque.z = torque.z;
00274 wrench_publisher_.publish(wrench);
00275 }
00276 }
00277
00278
00279 link->AddRelativeForce(force);
00280 link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
00281 }
00282
00284
00285 void GazeboQuadrotorSimpleController::Reset()
00286 {
00287 controllers_.roll.reset();
00288 controllers_.pitch.reset();
00289 controllers_.yaw.reset();
00290 controllers_.velocity_x.reset();
00291 controllers_.velocity_y.reset();
00292 controllers_.velocity_z.reset();
00293
00294 force.Set();
00295 torque.Set();
00296
00297
00298 pose.Reset();
00299 velocity.Set();
00300 angular_velocity.Set();
00301 acceleration.Set();
00302 euler.Set();
00303 state_stamp = ros::Time();
00304 }
00305
00307
00308 GazeboQuadrotorSimpleController::PIDController::PIDController()
00309 {
00310 }
00311
00312 GazeboQuadrotorSimpleController::PIDController::~PIDController()
00313 {
00314 }
00315
00316 void GazeboQuadrotorSimpleController::PIDController::Load(sdf::ElementPtr _sdf, const std::string& prefix)
00317 {
00318 gain_p = 0.0;
00319 gain_d = 0.0;
00320 gain_i = 0.0;
00321 time_constant = 0.0;
00322 limit = -1.0;
00323
00324 if (!_sdf) return;
00325
00326 if (_sdf->HasElement(prefix + "ProportionalGain")) gain_p = _sdf->GetElement(prefix + "ProportionalGain")->GetValueDouble();
00327 if (_sdf->HasElement(prefix + "DifferentialGain")) gain_d = _sdf->GetElement(prefix + "DifferentialGain")->GetValueDouble();
00328 if (_sdf->HasElement(prefix + "IntegralGain")) gain_i = _sdf->GetElement(prefix + "IntegralGain")->GetValueDouble();
00329 if (_sdf->HasElement(prefix + "TimeConstant")) time_constant = _sdf->GetElement(prefix + "TimeConstant")->GetValueDouble();
00330 if (_sdf->HasElement(prefix + "Limit")) limit = _sdf->GetElement(prefix + "Limit")->GetValueDouble();
00331 }
00332
00333 double GazeboQuadrotorSimpleController::PIDController::update(double new_input, double x, double dx, double dt)
00334 {
00335
00336 if (limit > 0.0 && fabs(new_input) > limit) new_input = (new_input < 0 ? -1.0 : 1.0) * limit;
00337
00338
00339 if (dt + time_constant > 0.0) {
00340 dinput = (new_input - input) / (dt + time_constant);
00341 input = (dt * new_input + time_constant * input) / (dt + time_constant);
00342 }
00343
00344
00345 p = input - x;
00346 d = dinput - dx;
00347 i = i + dt * p;
00348
00349
00350 output = gain_p * p + gain_d * d + gain_i * i;
00351 return output;
00352 }
00353
00354 void GazeboQuadrotorSimpleController::PIDController::reset()
00355 {
00356 input = dinput = 0;
00357 p = i = d = output = 0;
00358 }
00359
00360
00361 GZ_REGISTER_MODEL_PLUGIN(GazeboQuadrotorSimpleController)
00362
00363 }