diff --git a/include/fetch_auto_dock/auto_dock.h b/include/fetch_auto_dock/auto_dock.h index f953739..ef1c4f2 100644 --- a/include/fetch_auto_dock/auto_dock.h +++ b/include/fetch_auto_dock/auto_dock.h @@ -163,6 +163,7 @@ class AutoDocking ros::Time deadline_docking_; // Time when the docking times out. ros::Time deadline_not_charging_; // Time when robot gives up on the charge state and retries docking. bool charging_timeout_set_; // Flag to indicate if the deadline_not_charging has been set. + double duration_timeout_preorientation_; // Duration for preorientation timeout when docking }; -#endif // FETCH_AUTO_DOCK_H \ No newline at end of file +#endif // FETCH_AUTO_DOCK_H diff --git a/src/auto_dock.cpp b/src/auto_dock.cpp index 21fd618..ac03220 100644 --- a/src/auto_dock.cpp +++ b/src/auto_dock.cpp @@ -44,6 +44,7 @@ AutoDocking::AutoDocking() : pnh.param("num_of_retries", NUM_OF_RETRIES_, 5); pnh.param("dock_connector_clearance_distance", DOCK_CONNECTOR_CLEARANCE_DISTANCE_, 0.2); pnh.param("docked_distance_threshold", DOCKED_DISTANCE_THRESHOLD_, 0.34); + pnh.param("duration_timeout_preorientation", duration_timeout_preorientation_, 10.0); // Subscribe to robot state state_ = nh_.subscribe("robot_state", @@ -129,11 +130,16 @@ void AutoDocking::dockCallback(const fetch_auto_dock_msgs::DockGoalConstPtr& goa backup_limit_ *= 0.9; // Preorient the robot towards the dock. + ros::Time time_timeout = ros::Time::now() + ros::Duration(duration_timeout_preorientation_); while (!controller_.backup(0.0, -dock_yaw) && continueDocking(result) && ros::ok() ) { + if ( ros::Time::now() > time_timeout ) { + ROS_ERROR_STREAM_NAMED("auto_dock", "preorientation timeout"); + cancel_docking_ = true; + } r.sleep(); // Sleep the rate control object. } }