From f46e64c0243ca5260bc32141f6d00ea864a2ec75 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Fri, 30 Jul 2021 10:26:28 +0900 Subject: [PATCH 1/2] add timeout to preorientation --- src/auto_dock.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/auto_dock.cpp b/src/auto_dock.cpp index 21fd618..4177707 100644 --- a/src/auto_dock.cpp +++ b/src/auto_dock.cpp @@ -129,11 +129,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(10); 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. } } From 890c468e1e82a23f70104b4b5f023cbbe06756b1 Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 19 Oct 2021 16:40:59 +0900 Subject: [PATCH 2/2] [fetch_open_auto_dock] add duration_timeout_preorientation param --- include/fetch_auto_dock/auto_dock.h | 3 ++- src/auto_dock.cpp | 3 ++- 2 files changed, 4 insertions(+), 2 deletions(-) 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 4177707..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,7 +130,7 @@ 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(10); + ros::Time time_timeout = ros::Time::now() + ros::Duration(duration_timeout_preorientation_); while (!controller_.backup(0.0, -dock_yaw) && continueDocking(result) && ros::ok()