// 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); }