void GazeboRosSkidSteerDrive::publishOdometry(double step_time) {
ros::Time current_time = ros::Time::now();
std::string odom_frame =
tf::resolve(tf_prefix_, odometry_frame_);
std::string base_footprint_frame =
tf::resolve(tf_prefix_, robot_base_frame_);
// TODO create some non-perfect odometry!
// getting data for base_footprint to odom transform
math::Pose pose = this->parent->GetWorldPose();
tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
tf::Transform base_footprint_to_odom(qt, vt);
if (this->broadcast_tf_) {
transform_broadcaster_->sendTransform(
tf::StampedTransform(base_footprint_to_odom, current_time,
odom_frame, base_footprint_frame));
}
// publish odom topic
odom_.pose.pose.position.x = pose.pos.x;
odom_.pose.pose.position.y = pose.pos.y;
odom_.pose.pose.orientation.x = pose.rot.x;
odom_.pose.pose.orientation.y = pose.rot.y;
odom_.pose.pose.orientation.z = pose.rot.z;
odom_.pose.pose.orientation.w = pose.rot.w;
odom_.pose.covariance[0] = 0.00001;
odom_.pose.covariance[7] = 0.00001;
odom_.pose.covariance[14] = 1000000000000.0;
odom_.pose.covariance[21] = 1000000000000.0;
odom_.pose.covariance[28] = 1000000000000.0;
odom_.pose.covariance[35] = 0.01;
// get velocity in /odom frame
math::Vector3 linear;
linear = this->parent->GetWorldLinearVel();
odom_.twist.twist.angular.z = this->parent->GetWorldAngularVel().z;
// convert velocity to child_frame_id (aka base_footprint)
float yaw = pose.rot.GetYaw();
odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y;
odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x;
odom_.header.stamp = current_time;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_footprint_frame;
odometry_publisher_.publish(odom_);
}
void GazeboRosCarDrive::publishOdometry ( double step_time ) {
ros::Time current_time = ros::Time::now();
std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ );
std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ );
// getting data form gazebo world
math::Pose pose_ = this->parent->GetWorldPose();
tf::Quaternion qt = tf::Quaternion ( pose_.rot.x, pose_.rot.y, pose_.rot.z, pose_.rot.w );
tf::Vector3 vt = tf::Vector3 ( pose_.pos.x, pose_.pos.y, pose_.pos.z );
odom_.pose.pose.position.x = vt.x();
odom_.pose.pose.position.y = vt.y();
odom_.pose.pose.position.z = vt.z();
odom_.pose.pose.orientation.x = qt.x();
odom_.pose.pose.orientation.y = qt.y();
odom_.pose.pose.orientation.z = qt.z();
odom_.pose.pose.orientation.w = qt.w();
math::Vector3 linear_vel_ = this->parent->GetWorldLinearVel();
math::Vector3 angular_vel_ = this->parent->GetWorldAngularVel();
odom_.twist.twist.angular.z = angular_vel_.z;
float yaw_ = pose_.rot.GetYaw();
odom_.twist.twist.linear.x = cosf ( yaw_ ) * linear_vel_.x + sinf ( yaw_ ) * linear_vel_.y;
odom_.twist.twist.linear.y = cosf ( yaw_ ) * linear_vel_.y - sinf ( yaw_ ) * linear_vel_.x;
tf::Transform base_footprint_to_odom ( qt, vt );
transform_broadcaster_->sendTransform (
tf::StampedTransform ( base_footprint_to_odom, current_time,
odom_frame, base_footprint_frame ) );
// set covariance
odom_.pose.covariance[0] = 0.00001;
odom_.pose.covariance[7] = 0.00001;
odom_.pose.covariance[14] = 1000000000000.0;
odom_.pose.covariance[21] = 1000000000000.0;
odom_.pose.covariance[28] = 1000000000000.0;
odom_.pose.covariance[35] = 0.001;
// set header
odom_.header.stamp = current_time;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_footprint_frame;
odometry_publisher_.publish ( odom_ );
}
// NEW: Update this to publish odometry topic
void DiffDrivePlugin6W::publish_odometry()
{
// get current time
ros::Time current_time_((world->GetSimTime()).sec, (world->GetSimTime()).nsec);
// getting data for base_footprint to odom transform
math::Pose pose = link->GetWorldPose();
math::Vector3 velocity = link->GetWorldLinearVel();
math::Vector3 angular_velocity = link->GetWorldAngularVel();
btQuaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
btVector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
tf::Transform base_footprint_to_odom(qt, vt);
transform_broadcaster_->sendTransform(tf::StampedTransform(base_footprint_to_odom,
current_time_,
"odom",
"base_footprint"));
// publish odom topic
odom_.pose.pose.position.x = pose.pos.x;
odom_.pose.pose.position.y = pose.pos.y;
odom_.pose.pose.orientation.x = pose.rot.x;
odom_.pose.pose.orientation.y = pose.rot.y;
odom_.pose.pose.orientation.z = pose.rot.z;
odom_.pose.pose.orientation.w = pose.rot.w;
odom_.twist.twist.linear.x = velocity.x;
odom_.twist.twist.linear.y = velocity.y;
odom_.twist.twist.angular.z = angular_velocity.z;
odom_.header.frame_id = tf::resolve(tf_prefix_, "odom");
odom_.child_frame_id = "base_footprint";
odom_.header.stamp = current_time_;
pub_.publish(odom_);
}
void GazeboRosDiffDrive::publishOdometry ( double step_time )
{
ros::Time current_time = ros::Time::now();
std::string odom_frame = gazebo_ros_->resolveTF ( odometry_frame_ );
std::string base_footprint_frame = gazebo_ros_->resolveTF ( robot_base_frame_ );
tf::Quaternion qt;
tf::Vector3 vt;
if ( odom_source_ == ENCODER ) {
// getting data form encoder integration
qt = tf::Quaternion ( odom_.pose.pose.orientation.x, odom_.pose.pose.orientation.y, odom_.pose.pose.orientation.z, odom_.pose.pose.orientation.w );
vt = tf::Vector3 ( odom_.pose.pose.position.x, odom_.pose.pose.position.y, odom_.pose.pose.position.z );
}
if ( odom_source_ == WORLD ) {
// getting data form gazebo world
math::Pose pose = parent->GetWorldPose();
qt = tf::Quaternion ( pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w );
vt = tf::Vector3 ( pose.pos.x, pose.pos.y, pose.pos.z );
odom_.pose.pose.position.x = vt.x();
odom_.pose.pose.position.y = vt.y();
odom_.pose.pose.position.z = vt.z();
odom_.pose.pose.orientation.x = qt.x();
odom_.pose.pose.orientation.y = qt.y();
odom_.pose.pose.orientation.z = qt.z();
odom_.pose.pose.orientation.w = qt.w();
// get velocity in /odom frame
math::Vector3 linear;
linear = parent->GetWorldLinearVel();
odom_.twist.twist.angular.z = parent->GetWorldAngularVel().z;
// convert velocity to child_frame_id (aka base_footprint)
float yaw = pose.rot.GetYaw();
odom_.twist.twist.linear.x = cosf ( yaw ) * linear.x + sinf ( yaw ) * linear.y;
odom_.twist.twist.linear.y = cosf ( yaw ) * linear.y - sinf ( yaw ) * linear.x;
}
tf::Transform base_footprint_to_odom ( qt, vt );
transform_broadcaster_->sendTransform (
tf::StampedTransform ( base_footprint_to_odom, current_time,
odom_frame, base_footprint_frame ) );
// set covariance
odom_.pose.covariance[0] = 0.00001;
odom_.pose.covariance[7] = 0.00001;
odom_.pose.covariance[14] = 1000000000000.0;
odom_.pose.covariance[21] = 1000000000000.0;
odom_.pose.covariance[28] = 1000000000000.0;
odom_.pose.covariance[35] = 0.001;
// set header
odom_.header.stamp = current_time;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_footprint_frame;
odometry_publisher_.publish ( odom_ );
}
void GazeboRosPlanarMove::publishOdometry(double step_time)
{
ros::Time current_time = ros::Time::now();
std::string odom_frame = tf::resolve(tf_prefix_, odometry_frame_);
std::string base_footprint_frame =
tf::resolve(tf_prefix_, robot_base_frame_);
// getting data for base_footprint to odom transform
math::Pose pose = this->parent_->GetWorldPose();
tf::Quaternion qt(pose.rot.x, pose.rot.y, pose.rot.z, pose.rot.w);
tf::Vector3 vt(pose.pos.x, pose.pos.y, pose.pos.z);
tf::Transform base_footprint_to_odom(qt, vt);
transform_broadcaster_->sendTransform(
tf::StampedTransform(base_footprint_to_odom, current_time, odom_frame,
base_footprint_frame));
// publish odom topic
odom_.pose.pose.position.x = pose.pos.x;
odom_.pose.pose.position.y = pose.pos.y;
odom_.pose.pose.orientation.x = pose.rot.x;
odom_.pose.pose.orientation.y = pose.rot.y;
odom_.pose.pose.orientation.z = pose.rot.z;
odom_.pose.pose.orientation.w = pose.rot.w;
odom_.pose.covariance[0] = 0.00001;
odom_.pose.covariance[7] = 0.00001;
odom_.pose.covariance[14] = 1000000000000.0;
odom_.pose.covariance[21] = 1000000000000.0;
odom_.pose.covariance[28] = 1000000000000.0;
odom_.pose.covariance[35] = 0.001;
// get velocity in /odom frame
math::Vector3 linear;
linear.x = (pose.pos.x - last_odom_pose_.pos.x) / step_time;
linear.y = (pose.pos.y - last_odom_pose_.pos.y) / step_time;
if (rot_ > M_PI / step_time)
{
// we cannot calculate the angular velocity correctly
odom_.twist.twist.angular.z = rot_;
}
else
{
float last_yaw = last_odom_pose_.rot.GetYaw();
float current_yaw = pose.rot.GetYaw();
while (current_yaw < last_yaw - M_PI) current_yaw += 2 * M_PI;
while (current_yaw > last_yaw + M_PI) current_yaw -= 2 * M_PI;
float angular_diff = current_yaw - last_yaw;
odom_.twist.twist.angular.z = angular_diff / step_time;
}
last_odom_pose_ = pose;
// convert velocity to child_frame_id (aka base_footprint)
float yaw = pose.rot.GetYaw();
odom_.twist.twist.linear.x = cosf(yaw) * linear.x + sinf(yaw) * linear.y;
odom_.twist.twist.linear.y = cosf(yaw) * linear.y - sinf(yaw) * linear.x;
odom_.header.stamp = current_time;
odom_.header.frame_id = odom_frame;
odom_.child_frame_id = base_footprint_frame;
odometry_pub_.publish(odom_);
}
