////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboQuadrotorAerodynamics::Update()
{
// Get simulator time
Time current_time = world->GetSimTime();
Time dt = current_time - last_time_;
last_time_ = current_time;
if (dt <= 0.0) return;
// Get new commands/state
callback_queue_.callAvailable();
// fill input vector u for drag model
geometry_msgs::Quaternion orientation;
fromQuaternion(link->GetWorldPose().rot, orientation);
model_.setOrientation(orientation);
geometry_msgs::Twist twist;
fromVector(link->GetWorldLinearVel(), twist.linear);
fromVector(link->GetWorldAngularVel(), twist.angular);
model_.setTwist(twist);
// update the model
model_.update(dt.Double());
// get wrench from model
Vector3 force, torque;
toVector(model_.getWrench().force, force);
toVector(model_.getWrench().torque, torque);
// set force and torque in gazebo
link->AddRelativeForce(force);
link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
}
// Update the controller
void AckermannPlugin::UpdateChild()
{
// TODO: Step should be in a parameter of this function
double wd, wsw, wsl;
double d1, d2, a;
double dr, da, r;
Time stepTime;
//myIface->Lock(1);
GetPositionCmd();
wd = **(wheelDiamP);
wsw = **(wheelSepWidthP);
wsl = **(wheelSepLengthP);
//stepTime = World::Instance()->GetPhysicsEngine()->GetStepTime();
stepTime = Simulator::Instance()->GetSimTime() - prevUpdateTime;
prevUpdateTime = Simulator::Instance()->GetSimTime();
// Distance traveled by each back wheel
d1 = stepTime.Double() * wd / 2 * joints[LEFT]->GetVelocity(0);
d2 = stepTime.Double() * wd / 2 * joints[RIGHT]->GetVelocity(0);
// Get steering angle
a = joints[STEER]->GetAngle(0).GetAsRadian();
// compute radius of curvature
// may be infinite. that's ok
r = wsl * tan(TAU/4 - a);
// compute displacement along arc
dr = (d1 + d2) / 2;
da = dr / r;
// Compute odometric pose
odomPose[0] += dr * cos(odomPose[2]);
odomPose[1] += dr * sin(odomPose[2]);
odomPose[2] += da;
// Compute odometric instantaneous velocity
odomVel[0] = dr / stepTime.Double();
odomVel[1] = 0.0;
odomVel[2] = da / stepTime.Double();
if (enableMotors)
{
joints[LEFT]->SetVelocity(0, wheelSpeed[LEFT] / (**(wheelDiamP) / 2.0));
joints[RIGHT]->SetVelocity(0, wheelSpeed[RIGHT] / (**(wheelDiamP) / 2.0));
// FIXME: come up with something nicer for doing position control
// than this simple proportional controller
joints[STEER]->SetVelocity(0, (steerAngle - a) / stepTime.Double());
joints[LEFT]->SetMaxForce(0, **(driveTorqueP));
joints[RIGHT]->SetMaxForce(0, **(driveTorqueP));
joints[STEER]->SetMaxForce(0, **(steerTorqueP));
}
write_position_data();
publish_odometry();
//myIface->Unlock();
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboQuadrotorPropulsion::Update()
{
// Get simulator time
Time current_time = world->GetSimTime();
Time dt = current_time - last_time_;
last_time_ = current_time;
if (dt <= 0.0) return;
// Send trigger
bool trigger = controlTimer.getUpdatePeriod() > 0.0 ? controlTimer.update() : false;
if (trigger && trigger_publisher_) {
rosgraph_msgs::Clock clock;
clock.clock = ros::Time(current_time.sec, current_time.nsec);
trigger_publisher_.publish(clock);
ROS_DEBUG_STREAM_NAMED("quadrotor_propulsion", "Sent a trigger message at t = " << current_time.Double() << " (dt = " << (current_time - last_trigger_time_).Double() << ")");
last_trigger_time_ = current_time;
}
// Get new commands/state
callback_queue_.callAvailable();
// Process input queue
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_);
// fill input vector u for propulsion model
geometry_msgs::Twist twist;
fromVector(link->GetRelativeLinearVel(), twist.linear);
fromVector(link->GetRelativeAngularVel(), twist.angular);
model_.setTwist(twist);
// update the model
model_.update(dt.Double());
// get wrench from model
Vector3 force, torque;
toVector(model_.getWrench().force, force);
toVector(model_.getWrench().torque, torque);
// publish wrench
if (wrench_publisher_) {
geometry_msgs::WrenchStamped wrench_msg;
wrench_msg.header.stamp = ros::Time(current_time.sec, current_time.nsec);
wrench_msg.header.frame_id = frame_id_;
wrench_msg.wrench = model_.getWrench();
wrench_publisher_.publish(wrench_msg);
}
// publish motor status
if (motor_status_publisher_ && motorStatusTimer.update() /* && current_time >= last_motor_status_time_ + control_period_ */) {
hector_uav_msgs::MotorStatus motor_status = model_.getMotorStatus();
motor_status.header.stamp = ros::Time(current_time.sec, current_time.nsec);
motor_status_publisher_.publish(motor_status);
last_motor_status_time_ = current_time;
}
// publish supply
if (supply_publisher_ && current_time >= last_supply_time_ + 1.0) {
supply_publisher_.publish(model_.getSupply());
last_supply_time_ = current_time;
}
// set force and torque in gazebo
link->AddRelativeForce(force);
link->AddRelativeTorque(torque - link->GetInertial()->GetCoG().Cross(force));
}
