From 43bd620adfc3b3d1d0df3a1abcba21d28e29843f Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 19 Oct 2021 15:59:55 +0900 Subject: [PATCH 1/2] [fetch_open_auto_dock] add duration_timeout_undock 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..98f71cd 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_undock_; // Duration for dock/undock action timeout }; -#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..ace68fd 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_undock", duration_timeout_undock_, 5.0); // Subscribe to robot state state_ = nh_.subscribe("robot_state", @@ -429,7 +430,7 @@ void AutoDocking::undockCallback(const fetch_auto_dock_msgs::UndockGoalConstPtr& controller_.stop(); // Timeout for undocking - ros::Time timeout = ros::Time::now() + ros::Duration(5.0); + ros::Time timeout = ros::Time::now() + ros::Duration(duration_timeout_undock_); // Control ros::Rate r(50.0); From 8d027bf3c21275ae02634787428e642541c8b79e Mon Sep 17 00:00:00 2001 From: Koki Shinjo Date: Tue, 19 Oct 2021 16:41:37 +0900 Subject: [PATCH 2/2] [fetch_open_auto_dock] fix comment --- include/fetch_auto_dock/auto_dock.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/include/fetch_auto_dock/auto_dock.h b/include/fetch_auto_dock/auto_dock.h index 98f71cd..6a16e49 100644 --- a/include/fetch_auto_dock/auto_dock.h +++ b/include/fetch_auto_dock/auto_dock.h @@ -163,7 +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_undock_; // Duration for dock/undock action timeout + double duration_timeout_undock_; // Duration for undock action timeout }; #endif // FETCH_AUTO_DOCK_H