00001
00002
00003
00004
00005
00006
00007
00008
00009
00010 #include <math.h>
00011 #include <art_msgs/ArtVehicle.h>
00012 #include <art/epsilon.h>
00013 #include <art_map/euclidean_distance.h>
00014 #include <art_map/coordinates.h>
00015
00016 #include <art_nav/estimate.h>
00017
00022 namespace Estimate
00023 {
00038 void control_pose(const nav_msgs::Odometry &odom,
00039 ros::Time est_time,
00040 nav_msgs::Odometry &est)
00041 {
00042
00043
00044 est = odom;
00045 est.header.stamp = est_time;
00046
00047 if (odom.header.stamp == ros::Time() || est_time == ros::Time())
00048 {
00049 ROS_WARN_STREAM("invalid estimate time stamp, odom: "
00050 << odom.header.stamp
00051 << ", est_time: " << est_time);
00052 return;
00053 }
00054
00055 double dt = est_time.toSec() - odom.header.stamp.toSec();
00056 if (dt < 0.0 || dt > 1.0)
00057 {
00058 ROS_WARN("bogus delta time for estimate: %.3f", dt);
00059 return;
00060 }
00061
00062 #if 1 // seems to be working now...
00063
00064
00065
00066
00067 double odom_yaw = tf::getYaw(odom.pose.pose.orientation);
00068 double odom_yawrate = odom.twist.twist.angular.z;
00069 double est_dist = odom.twist.twist.linear.x * dt;
00070 double est_yaw = Coordinates::normalize(odom_yaw
00071 + odom_yawrate * dt);
00072 if (fabs(odom_yawrate) < Epsilon::yaw)
00073 {
00074
00075 est.pose.pose.position.x = (odom.pose.pose.position.x
00076 + est_dist * cos(odom_yaw));
00077 est.pose.pose.position.y = (odom.pose.pose.position.y
00078 + est_dist * sin(odom_yaw));
00079
00080 ROS_DEBUG("estimated straight path distance = %.3f", est_dist);
00081 }
00082 else
00083 {
00084
00085
00086
00087 double est_radius = est_dist / odom_yawrate;
00088 est.pose.pose.position.x = (odom.pose.pose.position.x
00089 - est_radius * sin(odom_yaw)
00090 + est_radius * sin(est_yaw));
00091 est.pose.pose.position.y = (odom.pose.pose.position.y
00092 + est_radius * cos(odom_yaw)
00093 - est_radius * cos(est_yaw));
00094
00095 ROS_DEBUG("estimated path distance = %.3f, radius = %.3f",
00096 est_dist, est_radius);
00097 }
00098
00099 ROS_DEBUG("estimated control pose = (%.3f, %.3f, %.3f)",
00100 est.pose.pose.position.x,
00101 est.pose.pose.position.y,
00102 est_yaw);
00103
00104
00105
00106
00107 est.pose.pose.orientation = tf::createQuaternionMsgFromYaw(est_yaw);
00108
00109 #if 0 // for detailed debugging only, extremely verbose:
00110 ROS_INFO_STREAM("last reported Odometry: " << odom);
00111 ROS_INFO_STREAM("estimated Odometry: " << est);
00112 #endif // for detailed debugging only
00113 #endif
00114 }
00115
00116 void front_bumper_pose(const nav_msgs::Odometry &odom,
00117 nav_msgs::Odometry &est)
00118 {
00119 Polar bump_rel(0,art_msgs::ArtVehicle::front_bumper_px);
00120 MapXY bump_abs =
00121 Coordinates::Polar_to_MapXY(bump_rel, MapPose(odom.pose.pose));
00122 est = odom;
00123 est.pose.pose.position.x = bump_abs.x;
00124 est.pose.pose.position.y = bump_abs.y;
00125 }
00126
00127 void front_axle_pose(const nav_msgs::Odometry &odom,
00128 nav_msgs::Odometry &est)
00129 {
00130 Polar axle_rel(0,art_msgs::ArtVehicle::wheelbase);
00131 MapXY axle_abs = Coordinates::Polar_to_MapXY(axle_rel,
00132 MapPose(odom.pose.pose));
00133 est = odom;
00134 est.pose.pose.position.x = axle_abs.x;
00135 est.pose.pose.position.y = axle_abs.y;
00136 }
00137
00138 }