Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add timeout to pre orientation during docking #9

Open
wants to merge 2 commits into
base: ros1
Choose a base branch
from
Open

Add timeout to pre orientation during docking #9

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
f46e64c
add timeout to preorientation
sktometometo Jul 30, 2021
890c468
[fetch_open_auto_dock] add duration_timeout_preorientation param
sktometometo Oct 19, 2021
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
3 changes: 2 additions & 1 deletion include/fetch_auto_dock/auto_dock.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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
#endif // FETCH_AUTO_DOCK_H
6 changes: 6 additions & 0 deletions src/auto_dock.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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<fetch_driver_msgs::RobotState>("robot_state",
Expand Down Expand Up @@ -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.
}
}
Expand Down