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