00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #include <hector_path_follower/hector_path_follower.h>
00033
00034
00035 namespace pose_follower {
00036 HectorPathFollower::HectorPathFollower(): tf_(NULL) {}
00037
00038 void HectorPathFollower::initialize(tf::TransformListener* tf){
00039 tf_ = tf;
00040 current_waypoint_ = 0;
00041 goal_reached_time_ = ros::Time::now();
00042 ros::NodeHandle node_private("~/");
00043
00044 node_private.param("k_trans", K_trans_, 2.0);
00045 node_private.param("k_rot", K_rot_, 2.0);
00046
00047 node_private.param("tolerance_trans", tolerance_trans_, 0.1);
00048 node_private.param("tolerance_rot", tolerance_rot_, 0.2);
00049 node_private.param("tolerance_timeout", tolerance_timeout_, 0.5);
00050
00051 node_private.param("holonomic", holonomic_, false);
00052
00053 node_private.param("samples", samples_, 10);
00054
00055 node_private.param("max_vel_lin", max_vel_lin_, 0.9);
00056 node_private.param("max_vel_th", max_vel_th_, 1.4);
00057
00058 node_private.param("min_vel_lin", min_vel_lin_, 0.1);
00059 node_private.param("min_vel_th", min_vel_th_, 0.0);
00060 node_private.param("min_in_place_vel_th", min_in_place_vel_th_, 0.0);
00061 node_private.param("in_place_trans_vel", in_place_trans_vel_, 0.0);
00062
00063 node_private.param("trans_stopped_velocity", trans_stopped_velocity_, 1e-4);
00064 node_private.param("rot_stopped_velocity", rot_stopped_velocity_, 1e-4);
00065
00066 node_private.param("robot_base_frame", p_robot_base_frame_, std::string("base_link"));
00067 node_private.param("global_frame", p_global_frame_, std::string("map"));
00068
00069
00070
00071
00072 ROS_DEBUG("Initialized");
00073 }
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087 double HectorPathFollower::headingDiff(double x, double y, double pt_x, double pt_y, double heading)
00088 {
00089 double v1_x = x - pt_x;
00090 double v1_y = y - pt_y;
00091 double v2_x = cos(heading);
00092 double v2_y = sin(heading);
00093
00094 double perp_dot = v1_x * v2_y - v1_y * v2_x;
00095 double dot = v1_x * v2_x + v1_y * v2_y;
00096
00097
00098 double vector_angle = atan2(perp_dot, dot);
00099
00100 return -1.0 * vector_angle;
00101 }
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118 bool HectorPathFollower::computeVelocityCommands(geometry_msgs::Twist& cmd_vel){
00119
00120 if (global_plan_.size() == 0)
00121 return false;
00122
00123
00124 tf::Stamped<tf::Pose> robot_pose;
00125 if(!this->getRobotPose(robot_pose)){
00126 ROS_ERROR("Can't get robot pose");
00127 geometry_msgs::Twist empty_twist;
00128 cmd_vel = empty_twist;
00129 return false;
00130 }
00131
00132
00133
00134
00135 tf::Stamped<tf::Pose> target_pose;
00136 tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
00137
00138 ROS_DEBUG("HectorPathFollower: current robot pose %f %f ==> %f", robot_pose.getOrigin().x(), robot_pose.getOrigin().y(), tf::getYaw(robot_pose.getRotation()));
00139 ROS_DEBUG("HectorPathFollower: target robot pose %f %f ==> %f", target_pose.getOrigin().x(), target_pose.getOrigin().y(), tf::getYaw(target_pose.getRotation()));
00140
00141
00142 geometry_msgs::Twist diff = diff2D(target_pose, robot_pose);
00143 ROS_DEBUG("HectorPathFollower: diff %f %f ==> %f", diff.linear.x, diff.linear.y, diff.angular.z);
00144
00145 geometry_msgs::Twist limit_vel = limitTwist(diff);
00146
00147 geometry_msgs::Twist test_vel = limit_vel;
00148 bool legal_traj = true;
00149
00150 double scaling_factor = 1.0;
00151 double ds = scaling_factor / samples_;
00152
00153
00154 if(!legal_traj){
00155 for(int i = 0; i < samples_; ++i){
00156 test_vel.linear.x = limit_vel.linear.x * scaling_factor;
00157 test_vel.linear.y = limit_vel.linear.y * scaling_factor;
00158 test_vel.angular.z = limit_vel.angular.z * scaling_factor;
00159 test_vel = limitTwist(test_vel);
00160
00161 legal_traj = true;
00162 break;
00163
00164 scaling_factor -= ds;
00165 }
00166 }
00167
00168 if(!legal_traj){
00169 ROS_ERROR("Not legal (%.2f, %.2f, %.2f)", limit_vel.linear.x, limit_vel.linear.y, limit_vel.angular.z);
00170 geometry_msgs::Twist empty_twist;
00171 cmd_vel = empty_twist;
00172 return false;
00173 }
00174
00175
00176 cmd_vel = test_vel;
00177
00178 bool in_goal_position = false;
00179 while(fabs(diff.linear.x) <= tolerance_trans_ &&
00180 fabs(diff.linear.y) <= tolerance_trans_ &&
00181 fabs(diff.angular.z) <= tolerance_rot_)
00182 {
00183 if(current_waypoint_ < global_plan_.size() - 1)
00184 {
00185 current_waypoint_++;
00186 tf::poseStampedMsgToTF(global_plan_[current_waypoint_], target_pose);
00187 diff = diff2D(target_pose, robot_pose);
00188 }
00189 else
00190 {
00191 ROS_INFO("Reached goal: %d", current_waypoint_);
00192 in_goal_position = true;
00193 break;
00194 }
00195 }
00196
00197
00198 if(!in_goal_position)
00199 goal_reached_time_ = ros::Time::now();
00200
00201
00202 if(goal_reached_time_ + ros::Duration(tolerance_timeout_) < ros::Time::now()){
00203 geometry_msgs::Twist empty_twist;
00204 cmd_vel = empty_twist;
00205 }
00206
00207 return true;
00208 }
00209
00210 bool HectorPathFollower::setPlan(const std::vector<geometry_msgs::PoseStamped>& global_plan){
00211 current_waypoint_ = 0;
00212 goal_reached_time_ = ros::Time::now();
00213 if(!transformGlobalPlan(*tf_, global_plan, p_global_frame_, global_plan_)){
00214 ROS_ERROR("Could not transform the global plan to the frame of the controller");
00215 return false;
00216 }
00217 return true;
00218 }
00219
00220 bool HectorPathFollower::isGoalReached(){
00221
00222
00223
00224
00225
00226
00227 return false;
00228 }
00229
00230 geometry_msgs::Twist HectorPathFollower::diff2D(const tf::Pose& pose1, const tf::Pose& pose2)
00231 {
00232 geometry_msgs::Twist res;
00233 tf::Pose diff = pose2.inverse() * pose1;
00234 res.linear.x = diff.getOrigin().x();
00235 res.linear.y = diff.getOrigin().y();
00236 res.angular.z = tf::getYaw(diff.getRotation());
00237
00238 if(holonomic_ || (fabs(res.linear.x) <= tolerance_trans_ && fabs(res.linear.y) <= tolerance_trans_))
00239 return res;
00240
00241
00242
00243
00244
00245 double yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
00246 pose2.getOrigin().x(), pose2.getOrigin().y(), tf::getYaw(pose2.getRotation()));
00247
00248
00249 double neg_yaw_diff = headingDiff(pose1.getOrigin().x(), pose1.getOrigin().y(),
00250 pose2.getOrigin().x(), pose2.getOrigin().y(), M_PI + tf::getYaw(pose2.getRotation()));
00251
00252
00253 if(fabs(neg_yaw_diff) < fabs(yaw_diff)){
00254 ROS_DEBUG("Negative is better: %.2f", neg_yaw_diff);
00255 yaw_diff = neg_yaw_diff;
00256 }
00257
00258
00259 tf::Quaternion rot_diff = tf::createQuaternionFromYaw(yaw_diff);
00260 tf::Quaternion rot = pose2.getRotation() * rot_diff;
00261 tf::Pose new_pose = pose1;
00262 new_pose.setRotation(rot);
00263
00264 diff = pose2.inverse() * new_pose;
00265 res.linear.x = diff.getOrigin().x();
00266 res.linear.y = diff.getOrigin().y();
00267 res.angular.z = tf::getYaw(diff.getRotation());
00268 return res;
00269 }
00270
00271
00272 geometry_msgs::Twist HectorPathFollower::limitTwist(const geometry_msgs::Twist& twist)
00273 {
00274 geometry_msgs::Twist res = twist;
00275 res.linear.x *= K_trans_;
00276 if(!holonomic_)
00277 res.linear.y = 0.0;
00278 else
00279 res.linear.y *= K_trans_;
00280 res.angular.z *= K_rot_;
00281
00282
00283 double lin_overshoot = sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y) / max_vel_lin_;
00284 double lin_undershoot = min_vel_lin_ / sqrt(res.linear.x * res.linear.x + res.linear.y * res.linear.y);
00285 if (lin_overshoot > 1.0)
00286 {
00287 res.linear.x /= lin_overshoot;
00288 res.linear.y /= lin_overshoot;
00289 }
00290
00291
00292 if(lin_undershoot > 1.0)
00293 {
00294 res.linear.x *= lin_undershoot;
00295 res.linear.y *= lin_undershoot;
00296 }
00297
00298 if (fabs(res.angular.z) > max_vel_th_) res.angular.z = max_vel_th_ * sign(res.angular.z);
00299 if (fabs(res.angular.z) < min_vel_th_) res.angular.z = min_vel_th_ * sign(res.angular.z);
00300
00301
00302 if(sqrt(twist.linear.x * twist.linear.x + twist.linear.y * twist.linear.y) < in_place_trans_vel_){
00303 if (fabs(res.angular.z) < min_in_place_vel_th_) res.angular.z = min_in_place_vel_th_ * sign(res.angular.z);
00304 res.linear.x = 0.0;
00305 res.linear.y = 0.0;
00306 }
00307
00308 ROS_DEBUG("Angular command %f", res.angular.z);
00309 return res;
00310 }
00311
00312 bool HectorPathFollower::transformGlobalPlan(const tf::TransformListener& tf, const std::vector<geometry_msgs::PoseStamped>& global_plan,
00313 const std::string& global_frame,
00314 std::vector<geometry_msgs::PoseStamped>& transformed_plan){
00315 const geometry_msgs::PoseStamped& plan_pose = global_plan[0];
00316
00317 transformed_plan.clear();
00318
00319 try{
00320 if (!global_plan.size() > 0)
00321 {
00322 ROS_ERROR("Received plan with zero length");
00323 return false;
00324 }
00325
00326 tf::StampedTransform transform;
00327 tf.lookupTransform(global_frame, ros::Time(),
00328 plan_pose.header.frame_id, plan_pose.header.stamp,
00329 plan_pose.header.frame_id, transform);
00330
00331 tf::Stamped<tf::Pose> tf_pose;
00332 geometry_msgs::PoseStamped newer_pose;
00333
00334 for(unsigned int i = 0; i < global_plan.size(); ++i){
00335 const geometry_msgs::PoseStamped& pose = global_plan[i];
00336 poseStampedMsgToTF(pose, tf_pose);
00337 tf_pose.setData(transform * tf_pose);
00338 tf_pose.stamp_ = transform.stamp_;
00339 tf_pose.frame_id_ = global_frame;
00340 poseStampedTFToMsg(tf_pose, newer_pose);
00341
00342 transformed_plan.push_back(newer_pose);
00343 }
00344 }
00345 catch(tf::LookupException& ex) {
00346 ROS_ERROR("No Transform available Error: %s\n", ex.what());
00347 return false;
00348 }
00349 catch(tf::ConnectivityException& ex) {
00350 ROS_ERROR("Connectivity Error: %s\n", ex.what());
00351 return false;
00352 }
00353 catch(tf::ExtrapolationException& ex) {
00354 ROS_ERROR("Extrapolation Error: %s\n", ex.what());
00355 if (global_plan.size() > 0)
00356 ROS_ERROR("Global Frame: %s Plan Frame size %d: %s\n", global_frame.c_str(), (unsigned int)global_plan.size(), global_plan[0].header.frame_id.c_str());
00357
00358 return false;
00359 }
00360
00361 return true;
00362 }
00363
00364
00365 bool HectorPathFollower::getRobotPose(tf::Stamped<tf::Pose>& global_pose) const {
00366
00367 global_pose.setIdentity();
00368 tf::Stamped<tf::Pose> robot_pose;
00369 robot_pose.setIdentity();
00370 robot_pose.frame_id_ = p_robot_base_frame_;
00371 robot_pose.stamp_ = ros::Time(0);
00372 ros::Time current_time = ros::Time::now();
00373
00374
00375 try{
00376 tf_->transformPose(p_global_frame_, robot_pose, global_pose);
00377 }
00378 catch(tf::LookupException& ex) {
00379 ROS_ERROR_THROTTLE(1.0, "No Transform available Error looking up robot pose: %s\n", ex.what());
00380 return false;
00381 }
00382 catch(tf::ConnectivityException& ex) {
00383 ROS_ERROR_THROTTLE(1.0, "Connectivity Error looking up robot pose: %s\n", ex.what());
00384 return false;
00385 }
00386 catch(tf::ExtrapolationException& ex) {
00387 ROS_ERROR_THROTTLE(1.0, "Extrapolation Error looking up robot pose: %s\n", ex.what());
00388 return false;
00389 }
00390
00391
00392
00393
00394
00395
00396
00397
00398
00399
00400
00401 return true;
00402 }
00403 };