// calculates odometry from current measurement values
// and publishes it via an odometry topic and the tf broadcaster
void NodeClass::UpdateOdometry()
{
double vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad;
double dummy1, dummy2;
double dt;
ros::Time current_time;
// if drive chain already initialized process joint data
if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
{
// Get resulting Pltf Velocities from Ctrl-Class (result of forward kinematics)
// !Careful! Controller internally calculates with mm instead of m
// ToDo: change internal calculation to SI-Units
// ToDo: last values are not used anymore --> remove from interface
ucar_ctrl_.GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1,
vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2);
// convert variables to SI-Units
vel_x_rob_ms = vel_x_rob_ms/1000.0;
vel_y_rob_ms = vel_y_rob_ms/1000.0;
delta_x_rob_m = delta_x_rob_m/1000.0;
delta_y_rob_m = delta_y_rob_m/1000.0;
}
else
{
// otherwise set data (velocity and pose-delta) to zero
vel_x_rob_ms = 0.0;
vel_y_rob_ms = 0.0;
delta_x_rob_m = 0.0;
delta_y_rob_m = 0.0;
}
// calc odometry (from startup)
// get time since last odometry-measurement
current_time = ros::Time::now();
dt = current_time.toSec() - last_time_.toSec();
last_time_ = current_time;
// calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom
x_rob_m_ = x_rob_m_ + (vel_x_rob_ms * cos(theta_rob_rad_) - vel_y_rob_ms * sin(theta_rob_rad_)) * dt;
y_rob_m_ = y_rob_m_ + (vel_x_rob_ms * sin(theta_rob_rad_) + vel_y_rob_ms * cos(theta_rob_rad_)) * dt;
theta_rob_rad_ = rot_rob_rads * dt;
// format data for compatibility with tf-package and standard odometry msg
// generate quaternion for rotation
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_);
// compose and publish transform for tf package
geometry_msgs::TransformStamped odom_tf;
// compose header
odom_tf.header.stamp = current_time;
odom_tf.header.frame_id = "/odom";
odom_tf.child_frame_id = "/base_footprint";
// compose data container
odom_tf.transform.translation.x = x_rob_m_;
odom_tf.transform.translation.y = y_rob_m_;
odom_tf.transform.translation.z = 0.0;
odom_tf.transform.rotation = odom_quat;
// compose and publish odometry message as topic
nav_msgs::Odometry odom_top;
// compose header
odom_top.header.stamp = current_time;
odom_top.header.frame_id = "/odom";
odom_top.child_frame_id = "/base_footprint";
// compose pose of robot
odom_top.pose.pose.position.x = x_rob_m_;
odom_top.pose.pose.position.y = y_rob_m_;
odom_top.pose.pose.position.z = 0.0;
odom_top.pose.pose.orientation = odom_quat;
// compose twist of robot
odom_top.twist.twist.linear.x = vel_x_rob_ms;
odom_top.twist.twist.linear.y = vel_y_rob_ms;
odom_top.twist.twist.linear.z = 0.0;
odom_top.twist.twist.angular.x = 0.0;
odom_top.twist.twist.angular.y = 0.0;
odom_top.twist.twist.angular.z = rot_rob_rads;
// publish data
// publish the transform
tf_broadcast_odometry_.sendTransform(odom_tf);
// publish odometry msg
topic_pub_odometry_.publish(odom_top);
}
// calculates odometry from current measurement values
// and publishes it via an odometry topic and the tf broadcaster
void NodeClass::UpdateOdometry()
{
double vel_x_rob_ms, vel_y_rob_ms, vel_rob_ms, rot_rob_rads, delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad;
double dummy1, dummy2;
double dt;
ros::Time current_time;
// if drive chain already initialized process joint data
//if (drive_chain_diagnostic_ != diagnostic_status_lookup_.OK)
if (is_initialized_bool_)
{
// Get resulting Pltf Velocities from Ctrl-Class (result of forward kinematics)
// !Careful! Controller internally calculates with mm instead of m
// ToDo: change internal calculation to SI-Units
// ToDo: last values are not used anymore --> remove from interface
ucar_ctrl_->GetActualPltfVelocity(delta_x_rob_m, delta_y_rob_m, delta_theta_rob_rad, dummy1,
vel_x_rob_ms, vel_y_rob_ms, rot_rob_rads, dummy2);
// convert variables to SI-Units
vel_x_rob_ms = vel_x_rob_ms/1000.0;
vel_y_rob_ms = vel_y_rob_ms/1000.0;
delta_x_rob_m = delta_x_rob_m/1000.0;
delta_y_rob_m = delta_y_rob_m/1000.0;
ROS_DEBUG("Odmonetry delta is: x=%f, y=%f, th=%f", delta_x_rob_m, delta_y_rob_m, rot_rob_rads);
}
else
{
// otherwise set data (velocity and pose-delta) to zero
vel_x_rob_ms = 0.0;
vel_y_rob_ms = 0.0;
delta_x_rob_m = 0.0;
delta_y_rob_m = 0.0;
}
// calc odometry (from startup)
// get time since last odometry-measurement
current_time = ros::Time::now();
dt = current_time.toSec() - last_time_.toSec();
last_time_ = current_time;
vel_rob_ms = sqrt(vel_x_rob_ms*vel_x_rob_ms + vel_y_rob_ms*vel_y_rob_ms);
// calculation from ROS odom publisher tutorial http://www.ros.org/wiki/navigation/Tutorials/RobotSetup/Odom, using now midpoint integration
x_rob_m_ = x_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * cos(theta_rob_rad_) - (vel_y_rob_ms+vel_y_rob_last_)/2.0 * sin(theta_rob_rad_)) * dt;
y_rob_m_ = y_rob_m_ + ((vel_x_rob_ms+vel_x_rob_last_)/2.0 * sin(theta_rob_rad_) + (vel_y_rob_ms+vel_y_rob_last_)/2.0 * cos(theta_rob_rad_)) * dt;
theta_rob_rad_ = theta_rob_rad_ + rot_rob_rads * dt;
//theta_rob_rad_ = theta_rob_rad_ + (rot_rob_rads+vel_theta_rob_last_)/2.0 * dt;
vel_x_rob_last_ = vel_x_rob_ms;
vel_y_rob_last_ = vel_y_rob_ms;
vel_theta_rob_last_ = rot_rob_rads;
// format data for compatibility with tf-package and standard odometry msg
// generate quaternion for rotation
geometry_msgs::Quaternion odom_quat = tf::createQuaternionMsgFromYaw(theta_rob_rad_);
// compose and publish transform for tf package
geometry_msgs::TransformStamped odom_tf;
// compose header
odom_tf.header.stamp = joint_state_odom_stamp_;
odom_tf.header.frame_id = "/wheelodom";
odom_tf.child_frame_id = "/base_footprint";
// compose data container
odom_tf.transform.translation.x = x_rob_m_;
odom_tf.transform.translation.y = y_rob_m_;
odom_tf.transform.translation.z = 0.0;
odom_tf.transform.rotation = odom_quat;
// compose and publish odometry message as topic
nav_msgs::Odometry odom_top;
// compose header
odom_top.header.stamp = joint_state_odom_stamp_;
odom_top.header.frame_id = "/wheelodom";
odom_top.child_frame_id = "/base_footprint";
// compose pose of robot
odom_top.pose.pose.position.x = x_rob_m_;
odom_top.pose.pose.position.y = y_rob_m_;
odom_top.pose.pose.position.z = 0.0;
odom_top.pose.pose.orientation = odom_quat;
for(int i = 0; i < 6; i++)
odom_top.pose.covariance[i*6+i] = 0.1;
// compose twist of robot
odom_top.twist.twist.linear.x = vel_x_rob_ms;
odom_top.twist.twist.linear.y = vel_y_rob_ms;
odom_top.twist.twist.linear.z = 0.0;
odom_top.twist.twist.angular.x = 0.0;
odom_top.twist.twist.angular.y = 0.0;
odom_top.twist.twist.angular.z = rot_rob_rads;
for(int i = 0; i < 6; i++)
odom_top.twist.covariance[6*i+i] = 0.1;
if (fabs(vel_x_rob_ms) > 0.005 or fabs(vel_y_rob_ms) > 0.005 or fabs(rot_rob_rads) > 0.005)
{
is_moving_ = true;
}
else
{
is_moving_ = false;
}
// publish the transform (for debugging, conflicts with robot-pose-ekf)
// tf_broadcast_odometry_.sendTransform(odom_tf);
// publish odometry msg
topic_pub_odometry_.publish(odom_top);
}
