void pd::player::move_right()
{
m_stoped = false;
m_ticks_until_stop = SDL_GetTicks();
flipped(false);
pd::vec2 vec = linear_velocity();
if (vec.x < (airborne() ? max_airborne_velocity : max_velocity))
apply_force(movement_force, 0.0f);
}
void MovementSystem::step(float /*dt*/)
{
for (unsigned int id: _entities) {
Movement *mv = _em->get_component<Movement>(id);
PhysicsBodyComponent* rb = _em->get_component<PhysicsBodyComponent>(id);
b2Vec2 linear_velocity(mv->speed);
linear_velocity.x *= mv->speed_multiplier.x;
linear_velocity.y *= mv->speed_multiplier.y;
//todo don't do this, using a more sensible world scale should remove the
//need for this.
linear_velocity *= 10000.0f;
rb->body->SetLinearVelocity(linear_velocity);
}
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosIMU::UpdateChild()
{
common::Time cur_time = this->world_->GetSimTime();
// rate control
if (this->update_rate_ > 0 &&
(cur_time - this->last_time_).Double() < (1.0 / this->update_rate_))
return;
if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != ""))
{
ignition::math::Pose3d pose;
ignition::math::Quaterniond rot;
ignition::math::Vector3d pos;
// Get Pose/Orientation ///@todo: verify correctness
#if GAZEBO_MAJOR_VERSION >= 8
pose = this->link->WorldPose();
#else
pose = this->link->GetWorldPose().Ign();
#endif
// apply xyz offsets and get position and rotation components
pos = pose.Pos() + this->offset_.Pos();
rot = pose.Rot();
// apply rpy offsets
rot = this->offset_.Rot()*rot;
rot.Normalize();
// get Rates
ignition::math::Vector3d vpos = this->link->GetWorldLinearVel().Ign();
ignition::math::Vector3d veul = this->link->GetWorldAngularVel().Ign();
// differentiate to get accelerations
double tmp_dt = this->last_time_.Double() - cur_time.Double();
if (tmp_dt != 0)
{
this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
this->last_vpos_ = vpos;
this->last_veul_ = veul;
}
// copy data into pose message
this->imu_msg_.header.frame_id = this->frame_name_;
this->imu_msg_.header.stamp.sec = cur_time.sec;
this->imu_msg_.header.stamp.nsec = cur_time.nsec;
// orientation quaternion
// uncomment this if we are reporting orientation in the local frame
// not the case for our imu definition
// // apply fixed orientation offsets of initial pose
// rot = this->initial_pose_.Rot()*rot;
// rot.Normalize();
this->imu_msg_.orientation.x = rot.X();
this->imu_msg_.orientation.y = rot.Y();
this->imu_msg_.orientation.z = rot.Z();
this->imu_msg_.orientation.w = rot.W();
// pass euler angular rates
ignition::math::Vector3d linear_velocity(
veul.X() + this->GaussianKernel(0, this->gaussian_noise_),
veul.Y() + this->GaussianKernel(0, this->gaussian_noise_),
veul.Z() + this->GaussianKernel(0, this->gaussian_noise_));
// rotate into local frame
// @todo: deal with offsets!
linear_velocity = rot.RotateVector(linear_velocity);
this->imu_msg_.angular_velocity.x = linear_velocity.X();
this->imu_msg_.angular_velocity.y = linear_velocity.Y();
this->imu_msg_.angular_velocity.z = linear_velocity.Z();
// pass accelerations
ignition::math::Vector3d linear_acceleration(
apos_.X() + this->GaussianKernel(0, this->gaussian_noise_),
apos_.Y() + this->GaussianKernel(0, this->gaussian_noise_),
apos_.Z() + this->GaussianKernel(0, this->gaussian_noise_));
// rotate into local frame
// @todo: deal with offsets!
linear_acceleration = rot.RotateVector(linear_acceleration);
this->imu_msg_.linear_acceleration.x = linear_acceleration.X();
this->imu_msg_.linear_acceleration.y = linear_acceleration.Y();
this->imu_msg_.linear_acceleration.z = linear_acceleration.Z();
// fill in covariance matrix
/// @todo: let user set separate linear and angular covariance values.
/// @todo: apply appropriate rotations from frame_pose
double gn2 = this->gaussian_noise_*this->gaussian_noise_;
this->imu_msg_.orientation_covariance[0] = gn2;
this->imu_msg_.orientation_covariance[4] = gn2;
this->imu_msg_.orientation_covariance[8] = gn2;
this->imu_msg_.angular_velocity_covariance[0] = gn2;
this->imu_msg_.angular_velocity_covariance[4] = gn2;
this->imu_msg_.angular_velocity_covariance[8] = gn2;
this->imu_msg_.linear_acceleration_covariance[0] = gn2;
this->imu_msg_.linear_acceleration_covariance[4] = gn2;
this->imu_msg_.linear_acceleration_covariance[8] = gn2;
{
boost::mutex::scoped_lock lock(this->lock_);
// publish to ros
if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")
this->pub_Queue->push(this->imu_msg_, this->pub_);
}
// save last time stamp
this->last_time_ = cur_time;
}
}
////////////////////////////////////////////////////////////////////////////////
// Update the controller
void GazeboRosIMU::UpdateChild()
{
if ((this->pub_.getNumSubscribers() > 0 && this->topic_name_ != ""))
{
math::Pose pose;
math::Quaternion rot;
math::Vector3 pos;
// Get Pose/Orientation ///@todo: verify correctness
pose = this->link->GetWorldPose();
// apply xyz offsets and get position and rotation components
pos = pose.pos + this->offset_.pos;
rot = pose.rot;
// apply rpy offsets
rot = this->offset_.rot*rot;
rot.Normalize();
common::Time cur_time = this->world_->GetSimTime();
// get Rates
math::Vector3 vpos = this->link->GetWorldLinearVel();
math::Vector3 veul = this->link->GetWorldAngularVel();
// differentiate to get accelerations
double tmp_dt = this->last_time_.Double() - cur_time.Double();
if (tmp_dt != 0)
{
this->apos_ = (this->last_vpos_ - vpos) / tmp_dt;
this->aeul_ = (this->last_veul_ - veul) / tmp_dt;
this->last_vpos_ = vpos;
this->last_veul_ = veul;
}
// copy data into pose message
this->imu_msg_.header.frame_id = this->frame_name_;
this->imu_msg_.header.stamp.sec = cur_time.sec;
this->imu_msg_.header.stamp.nsec = cur_time.nsec;
// orientation quaternion
// uncomment this if we are reporting orientation in the local frame
// not the case for our imu definition
// // apply fixed orientation offsets of initial pose
// rot = this->initial_pose_.rot*rot;
// rot.Normalize();
this->imu_msg_.orientation.x = rot.x;
this->imu_msg_.orientation.y = rot.y;
this->imu_msg_.orientation.z = rot.z;
this->imu_msg_.orientation.w = rot.w;
// pass euler angular rates
math::Vector3 linear_velocity(
veul.x + this->GaussianKernel(0, this->gaussian_noise_),
veul.y + this->GaussianKernel(0, this->gaussian_noise_),
veul.z + this->GaussianKernel(0, this->gaussian_noise_));
// rotate into local frame
// @todo: deal with offsets!
linear_velocity = rot.RotateVector(linear_velocity);
this->imu_msg_.angular_velocity.x = linear_velocity.x;
this->imu_msg_.angular_velocity.y = linear_velocity.y;
this->imu_msg_.angular_velocity.z = linear_velocity.z;
// pass accelerations
math::Vector3 linear_acceleration(
apos_.x + this->GaussianKernel(0, this->gaussian_noise_),
apos_.y + this->GaussianKernel(0, this->gaussian_noise_),
apos_.z + this->GaussianKernel(0, this->gaussian_noise_));
// rotate into local frame
// @todo: deal with offsets!
linear_acceleration = rot.RotateVector(linear_acceleration);
this->imu_msg_.linear_acceleration.x = linear_acceleration.x;
this->imu_msg_.linear_acceleration.y = linear_acceleration.y;
this->imu_msg_.linear_acceleration.z = linear_acceleration.z;
// fill in covariance matrix
/// @todo: let user set separate linear and angular covariance values.
/// @todo: apply appropriate rotations from frame_pose
double gn2 = this->gaussian_noise_*this->gaussian_noise_;
this->imu_msg_.orientation_covariance[0] = gn2;
this->imu_msg_.orientation_covariance[4] = gn2;
this->imu_msg_.orientation_covariance[8] = gn2;
this->imu_msg_.angular_velocity_covariance[0] = gn2;
this->imu_msg_.angular_velocity_covariance[4] = gn2;
this->imu_msg_.angular_velocity_covariance[8] = gn2;
this->imu_msg_.linear_acceleration_covariance[0] = gn2;
this->imu_msg_.linear_acceleration_covariance[4] = gn2;
this->imu_msg_.linear_acceleration_covariance[8] = gn2;
{
boost::mutex::scoped_lock lock(this->lock_);
// publish to ros
if (this->pub_.getNumSubscribers() > 0 && this->topic_name_ != "")
this->pub_.publish(this->imu_msg_);
}
// save last time stamp
this->last_time_ = cur_time;
}
}
