// 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 GazeboOdometryPlugin::OnUpdate(const common::UpdateInfo& _info) {
// 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_ != "world") {
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_;
}
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) {
nav_msgs::Odometry odometry;
odometry.header.frame_id = parent_frame_id_;
odometry.header.seq = odometry_sequence_++;
odometry.header.stamp.sec = (world_->GetSimTime()).sec + ros::Duration(unknown_delay_).sec;
odometry.header.stamp.nsec = (world_->GetSimTime()).nsec + ros::Duration(unknown_delay_).nsec;
odometry.child_frame_id = namespace_;
copyPosition(gazebo_pose.pos, &odometry.pose.pose.position);
odometry.pose.pose.orientation.w = gazebo_pose.rot.w;
odometry.pose.pose.orientation.x = gazebo_pose.rot.x;
odometry.pose.pose.orientation.y = gazebo_pose.rot.y;
odometry.pose.pose.orientation.z = gazebo_pose.rot.z;
odometry.twist.twist.linear.x = gazebo_linear_velocity.x;
odometry.twist.twist.linear.y = gazebo_linear_velocity.y;
odometry.twist.twist.linear.z = gazebo_linear_velocity.z;
odometry.twist.twist.angular.x = gazebo_angular_velocity.x;
odometry.twist.twist.angular.y = gazebo_angular_velocity.y;
odometry.twist.twist.angular.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) {
nav_msgs::OdometryPtr odometry(new nav_msgs::Odometry);
odometry->header = odometry_queue_.front().second.header;
odometry->child_frame_id = odometry_queue_.front().second.child_frame_id;
odometry->pose.pose = odometry_queue_.front().second.pose.pose;
odometry->twist.twist.linear = odometry_queue_.front().second.twist.twist.linear;
odometry->twist.twist.angular = odometry_queue_.front().second.twist.twist.angular;
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_);
geometry_msgs::Point& p = odometry->pose.pose.position;
p.x += pos_n[0];
p.y += pos_n[1];
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();
geometry_msgs::Quaternion& q_W_L = odometry->pose.pose.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.w = _q_W_L.w();
q_W_L.x = _q_W_L.x();
q_W_L.y = _q_W_L.y();
q_W_L.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_);
geometry_msgs::Vector3& linear_velocity = odometry->twist.twist.linear;
linear_velocity.x += linear_velocity_n[0];
linear_velocity.y += linear_velocity_n[1];
linear_velocity.z += linear_velocity_n[2];
// Calculate angular veocity 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_);
geometry_msgs::Vector3& angular_velocity = odometry->twist.twist.angular;
angular_velocity.x += angular_velocity_n[0];
angular_velocity.y += angular_velocity_n[1];
angular_velocity.z += angular_velocity_n[2];
odometry->pose.covariance = pose_covariance_matrix_;
odometry->twist.covariance = twist_covariance_matrix_;
// Publish all the topics, for which the topic name is specified.
if (pose_pub_.getNumSubscribers() > 0) {
geometry_msgs::PoseStampedPtr pose(new geometry_msgs::PoseStamped);
pose->header = odometry->header;
pose->pose = odometry->pose.pose;
pose_pub_.publish(pose);
}
if (pose_with_covariance_pub_.getNumSubscribers() > 0) {
geometry_msgs::PoseWithCovarianceStampedPtr pose_with_covariance(
new geometry_msgs::PoseWithCovarianceStamped);
pose_with_covariance->header = odometry->header;
pose_with_covariance->pose.pose = odometry->pose.pose;
pose_with_covariance->pose.covariance = odometry->pose.covariance;
pose_with_covariance_pub_.publish(pose_with_covariance);
}
if (position_pub_.getNumSubscribers() > 0) {
geometry_msgs::PointStampedPtr position(new geometry_msgs::PointStamped);
position->header = odometry->header;
position->point = p;
position_pub_.publish(position);
}
if (transform_pub_.getNumSubscribers() > 0) {
geometry_msgs::TransformStampedPtr transform(new geometry_msgs::TransformStamped);
transform->header = odometry->header;
geometry_msgs::Vector3 translation;
translation.x = p.x;
translation.y = p.y;
translation.z = p.z;
transform->transform.translation = translation;
transform->transform.rotation = q_W_L;
transform_pub_.publish(transform);
}
if (odometry_pub_.getNumSubscribers() > 0) {
odometry_pub_.publish(odometry);
}
tf::Quaternion tf_q_W_L(q_W_L.x, q_W_L.y, q_W_L.z, q_W_L.w);
tf::Vector3 tf_p(p.x, p.y, p.z);
tf_ = tf::Transform(tf_q_W_L, tf_p);
transform_broadcaster_.sendTransform(tf::StampedTransform(tf_, odometry->header.stamp, parent_frame_id_, namespace_));
// std::cout << "published odometry with timestamp " << odometry->header.stamp << "at time t" << world_->GetSimTime().Double()
// << "delay should be " << measurement_delay_ << "sim cycles" << std::endl;
}
++gazebo_sequence_;
}
