int main(int argc, char** argv){
ros::init(argc, argv, "go_home");
// Create Runner object
Runner Robot;
// Docking position is:
//x=-0.65, y=0.02, a=-0.15
// Assign input arguments to goal object
Robot.setCurrentGoal(-0.65, 0.02, 180);
// Send the goal
Robot.sendGoal(Robot.current_goal);
ros::Duration(1.0).sleep();
// If successful, start docking
if (Robot.nav_state == actionlib::SimpleClientGoalState::SUCCEEDED) {
ROS_INFO("Beginning docking.");
Robot.dock();
}else {
ROS_INFO("Did not succeed, will not dock.");
}// end if (goal)
return 0;
}
void LapManager:: dockWhenClose(Runner &runner) {
// Check if we're close to dock
double delta_x = std::abs(runner.pos_x_ - dock_x_);
double delta_y = std::abs(runner.pos_y_ - dock_y_);
// Only start docking once we are close enough
if (delta_x < dock_proximity_ && delta_y < dock_proximity_) {
ROS_INFO("Going to try and dock");
runner.setActionGoal(dock_x_, dock_y_, dock_yaw_);
runner.sendActionGoal();
runner.dock();
}// end if
}// end dockWhenClose
