void GazeboControllerInterface::OnUpdate(const common::UpdateInfo& /*_info*/) {
if (kPrintOnUpdates) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
if (!pubs_and_subs_created_) {
CreatePubsAndSubs();
pubs_and_subs_created_ = true;
}
if (!received_first_reference_) {
return;
}
common::Time now = world_->GetSimTime();
gz_sensor_msgs::Actuators turning_velocities_msg;
for (int i = 0; i < input_reference_.size(); i++) {
turning_velocities_msg.add_angular_velocities((double)input_reference_[i]);
}
turning_velocities_msg.mutable_header()->mutable_stamp()->set_sec(now.sec);
turning_velocities_msg.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
// Frame ID is not used for this particular message
turning_velocities_msg.mutable_header()->set_frame_id("");
motor_velocity_reference_pub_->Publish(turning_velocities_msg);
}
void GazeboFwDynamicsPlugin::OnUpdate(const common::UpdateInfo& _info) {
if (kPrintOnUpdates) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
if (!pubs_and_subs_created_) {
CreatePubsAndSubs();
pubs_and_subs_created_ = true;
}
UpdateForcesAndMoments();
}
void GazeboPressurePlugin::OnUpdate(const common::UpdateInfo& _info) {
if (kPrintOnUpdates) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
if (!pubs_and_subs_created_) {
CreatePubsAndSubs();
pubs_and_subs_created_ = true;
}
common::Time current_time = world_->SimTime();
// Get the current geometric height.
double height_geometric_m = ref_alt_ + model_->WorldPose().Pos().Z();
// Compute the geopotential height.
double height_geopotential_m = kEarthRadiusMeters * height_geometric_m /
(kEarthRadiusMeters + height_geometric_m);
// Compute the temperature at the current altitude.
double temperature_at_altitude_kelvin =
kSeaLevelTempKelvin - kTempLapseKelvinPerMeter * height_geopotential_m;
// Compute the current air pressure.
double pressure_at_altitude_pascal =
kPressureOneAtmospherePascals * exp(kAirConstantDimensionless *
log(kSeaLevelTempKelvin / temperature_at_altitude_kelvin));
// Add noise to pressure measurement.
if(pressure_var_ > 0.0) {
pressure_at_altitude_pascal += pressure_n_[0](random_generator_);
}
// Fill the pressure message.
pressure_message_.mutable_header()->mutable_stamp()->set_sec(
current_time.sec);
pressure_message_.mutable_header()->mutable_stamp()->set_nsec(
current_time.nsec);
pressure_message_.set_fluid_pressure(pressure_at_altitude_pascal);
// Publish the pressure message.
pressure_pub_->Publish(pressure_message_);
}
// This gets called by the world update start event.
void GazeboOdometryPlugin::OnUpdate(const common::UpdateInfo& _info) {
if (kPrintOnUpdates) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
if (!pubs_and_subs_created_) {
CreatePubsAndSubs();
pubs_and_subs_created_ = true;
}
// C denotes child frame, P parent frame, and W world frame.
// Further C_pose_W_P denotes pose of P wrt. W expressed in C.
math::Pose W_pose_W_C = link_->GetWorldCoGPose();
math::Vector3 C_linear_velocity_W_C = link_->GetRelativeLinearVel();
math::Vector3 C_angular_velocity_W_C = link_->GetRelativeAngularVel();
math::Vector3 gazebo_linear_velocity = C_linear_velocity_W_C;
math::Vector3 gazebo_angular_velocity = C_angular_velocity_W_C;
math::Pose gazebo_pose = W_pose_W_C;
if (parent_frame_id_ != kDefaultParentFrameId) {
math::Pose W_pose_W_P = parent_link_->GetWorldPose();
math::Vector3 P_linear_velocity_W_P = parent_link_->GetRelativeLinearVel();
math::Vector3 P_angular_velocity_W_P =
parent_link_->GetRelativeAngularVel();
math::Pose C_pose_P_C_ = W_pose_W_C - W_pose_W_P;
math::Vector3 C_linear_velocity_P_C;
// \prescript{}{C}{\dot{r}}_{PC} = -R_{CP}
// \cdot \prescript{}{P}{\omega}_{WP} \cross \prescript{}{P}{r}_{PC}
// + \prescript{}{C}{v}_{WC}
// - R_{CP} \cdot \prescript{}{P}{v}_{WP}
C_linear_velocity_P_C =
-C_pose_P_C_.rot.GetInverse() *
P_angular_velocity_W_P.Cross(C_pose_P_C_.pos) +
C_linear_velocity_W_C -
C_pose_P_C_.rot.GetInverse() * P_linear_velocity_W_P;
// \prescript{}{C}{\omega}_{PC} = \prescript{}{C}{\omega}_{WC}
// - R_{CP} \cdot \prescript{}{P}{\omega}_{WP}
gazebo_angular_velocity =
C_angular_velocity_W_C -
C_pose_P_C_.rot.GetInverse() * P_angular_velocity_W_P;
gazebo_linear_velocity = C_linear_velocity_P_C;
gazebo_pose = C_pose_P_C_;
}
// This flag could be set to false in the following code...
bool publish_odometry = true;
// First, determine whether we should publish a odometry.
if (covariance_image_.data != NULL) {
// We have an image.
// Image is always centered around the origin:
int width = covariance_image_.cols;
int height = covariance_image_.rows;
int x = static_cast<int>(
std::floor(gazebo_pose.pos.x / covariance_image_scale_)) +
width / 2;
int y = static_cast<int>(
std::floor(gazebo_pose.pos.y / covariance_image_scale_)) +
height / 2;
if (x >= 0 && x < width && y >= 0 && y < height) {
uint8_t pixel_value = covariance_image_.at<uint8_t>(y, x);
if (pixel_value == 0) {
publish_odometry = false;
// TODO: covariance scaling, according to the intensity values could be
// implemented here.
}
}
}
if (gazebo_sequence_ % measurement_divisor_ == 0) {
gz_geometry_msgs::Odometry odometry;
odometry.mutable_header()->set_frame_id(parent_frame_id_);
odometry.mutable_header()->mutable_stamp()->set_sec(
(world_->GetSimTime()).sec + static_cast<int32_t>(unknown_delay_));
odometry.mutable_header()->mutable_stamp()->set_nsec(
(world_->GetSimTime()).nsec + static_cast<int32_t>(unknown_delay_));
odometry.set_child_frame_id(child_frame_id_);
odometry.mutable_pose()->mutable_pose()->mutable_position()->set_x(
gazebo_pose.pos.x);
odometry.mutable_pose()->mutable_pose()->mutable_position()->set_y(
gazebo_pose.pos.y);
odometry.mutable_pose()->mutable_pose()->mutable_position()->set_z(
gazebo_pose.pos.z);
odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_x(
gazebo_pose.rot.x);
odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_y(
gazebo_pose.rot.y);
odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_z(
gazebo_pose.rot.z);
odometry.mutable_pose()->mutable_pose()->mutable_orientation()->set_w(
gazebo_pose.rot.w);
odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_x(
gazebo_linear_velocity.x);
odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_y(
gazebo_linear_velocity.y);
odometry.mutable_twist()->mutable_twist()->mutable_linear()->set_z(
gazebo_linear_velocity.z);
odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_x(
gazebo_angular_velocity.x);
odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_y(
gazebo_angular_velocity.y);
odometry.mutable_twist()->mutable_twist()->mutable_angular()->set_z(
gazebo_angular_velocity.z);
if (publish_odometry)
odometry_queue_.push_back(
std::make_pair(gazebo_sequence_ + measurement_delay_, odometry));
}
// Is it time to publish the front element?
if (gazebo_sequence_ == odometry_queue_.front().first) {
// Copy the odometry message that is on the queue
gz_geometry_msgs::Odometry odometry_msg(odometry_queue_.front().second);
// Now that we have copied the first element from the queue, remove it.
odometry_queue_.pop_front();
// Calculate position distortions.
Eigen::Vector3d pos_n;
pos_n << position_n_[0](random_generator_) +
position_u_[0](random_generator_),
position_n_[1](random_generator_) + position_u_[1](random_generator_),
position_n_[2](random_generator_) + position_u_[2](random_generator_);
gazebo::msgs::Vector3d* p =
odometry_msg.mutable_pose()->mutable_pose()->mutable_position();
p->set_x(p->x() + pos_n[0]);
p->set_y(p->y() + pos_n[1]);
p->set_z(p->z() + pos_n[2]);
// Calculate attitude distortions.
Eigen::Vector3d theta;
theta << attitude_n_[0](random_generator_) +
attitude_u_[0](random_generator_),
attitude_n_[1](random_generator_) + attitude_u_[1](random_generator_),
attitude_n_[2](random_generator_) + attitude_u_[2](random_generator_);
Eigen::Quaterniond q_n = QuaternionFromSmallAngle(theta);
q_n.normalize();
gazebo::msgs::Quaternion* q_W_L =
odometry_msg.mutable_pose()->mutable_pose()->mutable_orientation();
Eigen::Quaterniond _q_W_L(q_W_L->w(), q_W_L->x(), q_W_L->y(), q_W_L->z());
_q_W_L = _q_W_L * q_n;
q_W_L->set_w(_q_W_L.w());
q_W_L->set_x(_q_W_L.x());
q_W_L->set_y(_q_W_L.y());
q_W_L->set_z(_q_W_L.z());
// Calculate linear velocity distortions.
Eigen::Vector3d linear_velocity_n;
linear_velocity_n << linear_velocity_n_[0](random_generator_) +
linear_velocity_u_[0](random_generator_),
linear_velocity_n_[1](random_generator_) +
linear_velocity_u_[1](random_generator_),
linear_velocity_n_[2](random_generator_) +
linear_velocity_u_[2](random_generator_);
gazebo::msgs::Vector3d* linear_velocity =
odometry_msg.mutable_twist()->mutable_twist()->mutable_linear();
linear_velocity->set_x(linear_velocity->x() + linear_velocity_n[0]);
linear_velocity->set_y(linear_velocity->y() + linear_velocity_n[1]);
linear_velocity->set_z(linear_velocity->z() + linear_velocity_n[2]);
// Calculate angular velocity distortions.
Eigen::Vector3d angular_velocity_n;
angular_velocity_n << angular_velocity_n_[0](random_generator_) +
angular_velocity_u_[0](random_generator_),
angular_velocity_n_[1](random_generator_) +
angular_velocity_u_[1](random_generator_),
angular_velocity_n_[2](random_generator_) +
angular_velocity_u_[2](random_generator_);
gazebo::msgs::Vector3d* angular_velocity =
odometry_msg.mutable_twist()->mutable_twist()->mutable_angular();
angular_velocity->set_x(angular_velocity->x() + angular_velocity_n[0]);
angular_velocity->set_y(angular_velocity->y() + angular_velocity_n[1]);
angular_velocity->set_z(angular_velocity->z() + angular_velocity_n[2]);
odometry_msg.mutable_pose()->mutable_covariance()->Clear();
for (int i = 0; i < pose_covariance_matrix_.size(); i++) {
odometry_msg.mutable_pose()->mutable_covariance()->Add(
pose_covariance_matrix_[i]);
}
odometry_msg.mutable_twist()->mutable_covariance()->Clear();
for (int i = 0; i < twist_covariance_matrix_.size(); i++) {
odometry_msg.mutable_twist()->mutable_covariance()->Add(
twist_covariance_matrix_[i]);
}
// Publish all the topics, for which the topic name is specified.
if (pose_pub_->HasConnections()) {
pose_pub_->Publish(odometry_msg.pose().pose());
}
if (pose_with_covariance_stamped_pub_->HasConnections()) {
gz_geometry_msgs::PoseWithCovarianceStamped
pose_with_covariance_stamped_msg;
pose_with_covariance_stamped_msg.mutable_header()->CopyFrom(
odometry_msg.header());
pose_with_covariance_stamped_msg.mutable_pose_with_covariance()->CopyFrom(
odometry_msg.pose());
pose_with_covariance_stamped_pub_->Publish(
pose_with_covariance_stamped_msg);
}
if (position_stamped_pub_->HasConnections()) {
gz_geometry_msgs::Vector3dStamped position_stamped_msg;
position_stamped_msg.mutable_header()->CopyFrom(odometry_msg.header());
position_stamped_msg.mutable_position()->CopyFrom(
odometry_msg.pose().pose().position());
position_stamped_pub_->Publish(position_stamped_msg);
}
if (transform_stamped_pub_->HasConnections()) {
gz_geometry_msgs::TransformStamped transform_stamped_msg;
transform_stamped_msg.mutable_header()->CopyFrom(odometry_msg.header());
transform_stamped_msg.mutable_transform()->mutable_translation()->set_x(
p->x());
transform_stamped_msg.mutable_transform()->mutable_translation()->set_y(
p->y());
transform_stamped_msg.mutable_transform()->mutable_translation()->set_z(
p->z());
transform_stamped_msg.mutable_transform()->mutable_rotation()->CopyFrom(
*q_W_L);
transform_stamped_pub_->Publish(transform_stamped_msg);
}
if (odometry_pub_->HasConnections()) {
// DEBUG
odometry_pub_->Publish(odometry_msg);
}
//==============================================//
//========= BROADCAST TRANSFORM MSG ============//
//==============================================//
gz_geometry_msgs::TransformStampedWithFrameIds
transform_stamped_with_frame_ids_msg;
transform_stamped_with_frame_ids_msg.mutable_header()->CopyFrom(
odometry_msg.header());
transform_stamped_with_frame_ids_msg.mutable_transform()
->mutable_translation()
->set_x(p->x());
transform_stamped_with_frame_ids_msg.mutable_transform()
->mutable_translation()
->set_y(p->y());
transform_stamped_with_frame_ids_msg.mutable_transform()
->mutable_translation()
->set_z(p->z());
transform_stamped_with_frame_ids_msg.mutable_transform()
->mutable_rotation()
->CopyFrom(*q_W_L);
transform_stamped_with_frame_ids_msg.set_parent_frame_id(parent_frame_id_);
transform_stamped_with_frame_ids_msg.set_child_frame_id(child_frame_id_);
broadcast_transform_pub_->Publish(transform_stamped_with_frame_ids_msg);
} // if (gazebo_sequence_ == odometry_queue_.front().first) {
++gazebo_sequence_;
}
// This gets called by the world update start event.
void GazeboWindPlugin::OnUpdate(const common::UpdateInfo& _info) {
if (kPrintOnUpdates) {
gzdbg << __FUNCTION__ << "() called." << std::endl;
}
if (!pubs_and_subs_created_) {
CreatePubsAndSubs();
pubs_and_subs_created_ = true;
}
// Get the current simulation time.
common::Time now = world_->GetSimTime();
math::Vector3 wind_velocity(0.0, 0.0, 0.0);
// Choose user-specified method for calculating wind velocity.
if (!use_custom_static_wind_field_) {
// Calculate the wind force.
double wind_strength = wind_force_mean_;
math::Vector3 wind = wind_strength * wind_direction_;
// Apply a force from the constant wind to the link.
link_->AddForceAtRelativePosition(wind, xyz_offset_);
math::Vector3 wind_gust(0.0, 0.0, 0.0);
// Calculate the wind gust force.
if (now >= wind_gust_start_ && now < wind_gust_end_) {
double wind_gust_strength = wind_gust_force_mean_;
wind_gust = wind_gust_strength * wind_gust_direction_;
// Apply a force from the wind gust to the link.
link_->AddForceAtRelativePosition(wind_gust, xyz_offset_);
}
wrench_stamped_msg_.mutable_header()->set_frame_id(frame_id_);
wrench_stamped_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec);
wrench_stamped_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_x(wind.x +
wind_gust.x);
wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_y(wind.y +
wind_gust.y);
wrench_stamped_msg_.mutable_wrench()->mutable_force()->set_z(wind.z +
wind_gust.z);
// No torque due to wind, set x,y and z to 0.
wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_x(0);
wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_y(0);
wrench_stamped_msg_.mutable_wrench()->mutable_torque()->set_z(0);
wind_force_pub_->Publish(wrench_stamped_msg_);
// Calculate the wind speed.
wind_velocity = wind_speed_mean_ * wind_direction_;
} else {
// Get the current position of the aircraft in world coordinates.
math::Vector3 link_position = link_->GetWorldPose().pos;
// Calculate the x, y index of the grid points with x, y-coordinate
// just smaller than or equal to aircraft x, y position.
std::size_t x_inf = floor((link_position.x - min_x_) / res_x_);
std::size_t y_inf = floor((link_position.y - min_y_) / res_y_);
// In case aircraft is on one of the boundary surfaces at max_x or max_y,
// decrease x_inf, y_inf by one to have x_sup, y_sup on max_x, max_y.
if (x_inf == n_x_ - 1u) {
x_inf = n_x_ - 2u;
}
if (y_inf == n_y_ - 1u) {
y_inf = n_y_ - 2u;
}
// Calculate the x, y index of the grid points with x, y-coordinate just
// greater than the aircraft x, y position.
std::size_t x_sup = x_inf + 1u;
std::size_t y_sup = y_inf + 1u;
// Save in an array the x, y index of each of the eight grid points
// enclosing the aircraft.
constexpr unsigned int n_vertices = 8;
std::size_t idx_x[n_vertices] = {x_inf, x_inf, x_sup, x_sup, x_inf, x_inf, x_sup, x_sup};
std::size_t idx_y[n_vertices] = {y_inf, y_inf, y_inf, y_inf, y_sup, y_sup, y_sup, y_sup};
// Find the vertical factor of the aircraft in each of the four surrounding
// grid columns, and their minimal/maximal value.
constexpr unsigned int n_columns = 4;
float vertical_factors_columns[n_columns];
for (std::size_t i = 0u; i < n_columns; ++i) {
vertical_factors_columns[i] = (
link_position.z - bottom_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_]) /
(top_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_] - bottom_z_[idx_x[2u * i] + idx_y[2u * i] * n_x_]);
}
// Find maximal and minimal value amongst vertical factors.
float vertical_factors_min = std::min(std::min(std::min(
vertical_factors_columns[0], vertical_factors_columns[1]),
vertical_factors_columns[2]), vertical_factors_columns[3]);
float vertical_factors_max = std::max(std::max(std::max(
vertical_factors_columns[0], vertical_factors_columns[1]),
vertical_factors_columns[2]), vertical_factors_columns[3]);
// Check if aircraft is out of wind field or not, and act accordingly.
if (x_inf >= 0u && y_inf >= 0u && vertical_factors_max >= 0u &&
x_sup <= (n_x_ - 1u) && y_sup <= (n_y_ - 1u) && vertical_factors_min <= 1u) {
// Find indices in z-direction for each of the vertices. If link is not
// within the range of one of the columns, set to lowest or highest two.
std::size_t idx_z[n_vertices] = {0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u,
0u, static_cast<int>(vertical_spacing_factors_.size()) - 1u};
for (std::size_t i = 0u; i < n_columns; ++i) {
if (vertical_factors_columns[i] < 0u) {
// Link z-position below lowest grid point of that column.
idx_z[2u * i + 1u] = 1u;
} else if (vertical_factors_columns[i] >= 1u) {
// Link z-position above highest grid point of that column.
idx_z[2u * i] = vertical_spacing_factors_.size() - 2u;
} else {
// Link z-position between two grid points in that column.
for (std::size_t j = 0u; j < vertical_spacing_factors_.size() - 1u; ++j) {
if (vertical_spacing_factors_[j] <= vertical_factors_columns[i] &&
vertical_spacing_factors_[j + 1u] > vertical_factors_columns[i]) {
idx_z[2u * i] = j;
idx_z[2u * i + 1u] = j + 1u;
break;
}
}
}
}
// Extract the wind velocities corresponding to each vertex.
math::Vector3 wind_at_vertices[n_vertices];
for (std::size_t i = 0u; i < n_vertices; ++i) {
wind_at_vertices[i].x = u_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
wind_at_vertices[i].y = v_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
wind_at_vertices[i].z = w_[idx_x[i] + idx_y[i] * n_x_ + idx_z[i] * n_x_ * n_y_];
}
// Extract the relevant coordinate of every point needed for trilinear
// interpolation (first z-direction, then x-direction, then y-direction).
constexpr unsigned int n_points_interp_z = 8;
constexpr unsigned int n_points_interp_x = 4;
constexpr unsigned int n_points_interp_y = 2;
double interpolation_points[n_points_interp_x + n_points_interp_y + n_points_interp_z];
for (std::size_t i = 0u; i < n_points_interp_x + n_points_interp_y + n_points_interp_z; ++i) {
if (i < n_points_interp_z) {
interpolation_points[i] = (
top_z_[idx_x[i] + idx_y[i] * n_x_] - bottom_z_[idx_x[i] + idx_y[i] * n_x_])
* vertical_spacing_factors_[idx_z[i]] + bottom_z_[idx_x[i] + idx_y[i] * n_x_];
} else if (i >= n_points_interp_z && i < n_points_interp_x + n_points_interp_z) {
interpolation_points[i] = min_x_ + res_x_ * idx_x[2u * (i - n_points_interp_z)];
} else {
interpolation_points[i] = min_y_ + res_y_ * idx_y[4u * (i - n_points_interp_z - n_points_interp_x)];
}
}
// Interpolate wind velocity at aircraft position.
wind_velocity = TrilinearInterpolation(
link_position, wind_at_vertices, interpolation_points);
} else {
// Set the wind velocity to the default constant value specified by user.
wind_velocity = wind_speed_mean_ * wind_direction_;
}
}
wind_speed_msg_.mutable_header()->set_frame_id(frame_id_);
wind_speed_msg_.mutable_header()->mutable_stamp()->set_sec(now.sec);
wind_speed_msg_.mutable_header()->mutable_stamp()->set_nsec(now.nsec);
wind_speed_msg_.mutable_velocity()->set_x(wind_velocity.x);
wind_speed_msg_.mutable_velocity()->set_y(wind_velocity.y);
wind_speed_msg_.mutable_velocity()->set_z(wind_velocity.z);
wind_speed_pub_->Publish(wind_speed_msg_);
}
