/**
* @brief Preempt callback for the server, cancels the current running goal and all associated movement actions.
*/
void preemptCb(){
boost::unique_lock<boost::mutex> lock(move_client_lock_);
move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now());
ROS_WARN("Current exploration task cancelled");
if(as_.isActive()){
as_.setPreempted();
}
}
// move platform server receives a new goal
void moveGoalCB()
{
set_terminal_state_ = true;
move_goal_.nav_goal = as_.acceptNewGoal()->nav_goal;
ROS_INFO_STREAM_NAMED(logger_name_, "Received Goal #" <<move_goal_.nav_goal.header.seq);
if (as_.isPreemptRequested() ||!ros::ok())
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested on goal #" << move_goal_.nav_goal.header.seq);
if (planning_)
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
if (controlling_)
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.result_state = 0;
move_result_.error_string = "Preempt Requested!!!";
as_.setPreempted(move_result_);
return;
}
// Convert move goal to plan goal
pose_goal_.pose_goal = move_goal_.nav_goal;
if (planner_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - planner is down");
planning_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Planner is down";
as_.setAborted(move_result_);
}
else
{
ac_planner_.sendGoal(pose_goal_, boost::bind(&MovePlatformAction::planningDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_planner::planAction>::SimpleFeedbackCallback());
planning_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " sent to planner");
}
return;
}
void movePreemptCB()
{
ROS_WARN_STREAM_NAMED(logger_name_, "Preempt Requested");
if (planning_)
{
ROS_DEBUG_NAMED(logger_name_, "Planning - cancelling all plan goals");
ac_planner_.cancelGoalsAtAndBeforeTime(ros::Time::now());
}
if (controlling_)
{
ROS_DEBUG_NAMED(logger_name_, "Controlling - cancelling all ctrl goals");
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
}
move_result_.result_state = 0;
move_result_.error_string = "Move Platform Preempt Request!";
as_.setPreempted(move_result_);
set_terminal_state_ = false;
return;
}
void planningDoneCB(const actionlib::SimpleClientGoalState& state, const oea_planner::planResultConstPtr &result)
{
planning_ = false;
ROS_DEBUG_STREAM_NAMED(logger_name_, "Plan Action finished: " << state.toString());
move_result_.result_state = result->result_state;
if (move_result_.result_state) //if plan OK
{
planned_path_goal_.plan_goal = result->planned_path; // goal for the controller is result of the planner
if (ctrl_state_sub.getNumPublishers()==0)
{
ROS_WARN_STREAM_NAMED(logger_name_, "Goal #" << move_goal_.nav_goal.header.seq << " not sent - Controller is down");
controlling_ = false;
move_result_.result_state = 0;
move_result_.error_string = "Controller is down!!!";
as_.setAborted(move_result_);
}
else
{
ac_control_.sendGoal(planned_path_goal_, boost::bind(&MovePlatformAction::ControlDoneCB, this, _1, _2),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleActiveCallback(),
actionlib::SimpleActionClient<oea_controller::controlPlatformAction>::SimpleFeedbackCallback()); //boost::bind(&MovePlatformAction::ControlFeedbackCB, this,_1));
controlling_ = true;
ROS_DEBUG_STREAM_NAMED(logger_name_,"Goal #" << move_goal_.nav_goal.header.seq << " sent to Controller");
}
}
else //if plan NOT OK
{
ac_control_.cancelGoalsAtAndBeforeTime(ros::Time::now());
move_result_.error_string = "Planning Failed: " + result->error_string;
ROS_WARN_STREAM_NAMED(logger_name_, "Aborting because " << move_result_.error_string);
as_.setAborted(move_result_);
}
return;
}
void executeCb(const frontier_exploration::ExploreTaskGoalConstPtr &goal)
{
success_ = false;
moving_ = false;
explore_costmap_ros_->resetLayers();
//create costmap services
ros::ServiceClient updateBoundaryPolygon = private_nh_.serviceClient<frontier_exploration::UpdateBoundaryPolygon>("explore_costmap/explore_boundary/update_boundary_polygon");
ros::ServiceClient getNextFrontier = private_nh_.serviceClient<frontier_exploration::GetNextFrontier>("explore_costmap/explore_boundary/get_next_frontier");
//wait for move_base and costmap services
if(!move_client_.waitForServer() || !updateBoundaryPolygon.waitForExistence() || !getNextFrontier.waitForExistence()){
as_.setAborted();
return;
}
//set region boundary on costmap
if(ros::ok() && as_.isActive()){
frontier_exploration::UpdateBoundaryPolygon srv;
srv.request.explore_boundary = goal->explore_boundary;
if(updateBoundaryPolygon.call(srv)){
ROS_INFO("Region boundary set");
}else{
ROS_ERROR("Failed to set region boundary");
as_.setAborted();
return;
}
}
//loop until all frontiers are explored
ros::Rate rate(frequency_);
while(ros::ok() && as_.isActive()){
frontier_exploration::GetNextFrontier srv;
//placeholder for next goal to be sent to move base
geometry_msgs::PoseStamped goal_pose;
//get current robot pose in frame of exploration boundary
tf::Stamped<tf::Pose> robot_pose;
explore_costmap_ros_->getRobotPose(robot_pose);
//provide current robot pose to the frontier search service request
tf::poseStampedTFToMsg(robot_pose,srv.request.start_pose);
//evaluate if robot is within exploration boundary using robot_pose in boundary frame
geometry_msgs::PoseStamped eval_pose = srv.request.start_pose;
if(eval_pose.header.frame_id != goal->explore_boundary.header.frame_id){
tf_listener_.transformPose(goal->explore_boundary.header.frame_id, srv.request.start_pose, eval_pose);
}
//check if robot is not within exploration boundary and needs to return to center of search area
if(goal->explore_boundary.polygon.points.size() > 0 && !pointInPolygon(eval_pose.pose.position,goal->explore_boundary.polygon)){
//check if robot has explored at least one frontier, and promote debug message to warning
if(success_){
ROS_WARN("Robot left exploration boundary, returning to center");
}else{
ROS_DEBUG("Robot not initially in exploration boundary, traveling to center");
}
//get current robot position in frame of exploration center
geometry_msgs::PointStamped eval_point;
eval_point.header = eval_pose.header;
eval_point.point = eval_pose.pose.position;
if(eval_point.header.frame_id != goal->explore_center.header.frame_id){
geometry_msgs::PointStamped temp = eval_point;
tf_listener_.transformPoint(goal->explore_center.header.frame_id, temp, eval_point);
}
//set goal pose to exploration center
goal_pose.header = goal->explore_center.header;
goal_pose.pose.position = goal->explore_center.point;
goal_pose.pose.orientation = tf::createQuaternionMsgFromYaw( yawOfVector(eval_point.point, goal->explore_center.point) );
}else if(getNextFrontier.call(srv)){ //if in boundary, try to find next frontier to search
ROS_DEBUG("Found frontier to explore");
success_ = true;
goal_pose = feedback_.next_frontier = srv.response.next_frontier;
retry_ = 5;
}else{ //if no frontier found, check if search is successful
ROS_DEBUG("Couldn't find a frontier");
//search is succesful
if(retry_ == 0 && success_){
ROS_WARN("Finished exploring room");
as_.setSucceeded();
boost::unique_lock<boost::mutex> lock(move_client_lock_);
move_client_.cancelGoalsAtAndBeforeTime(ros::Time::now());
return;
}else if(retry_ == 0 || !ros::ok()){ //search is not successful
ROS_ERROR("Failed exploration");
as_.setAborted();
return;
}
ROS_DEBUG("Retrying...");
retry_--;
//try to find frontier again, without moving robot
continue;
}
//if above conditional does not escape this loop step, search has a valid goal_pose
//check if new goal is close to old goal, hence no need to resend
if(!moving_ || !pointsNearby(move_client_goal_.target_pose.pose.position,goal_pose.pose.position,goal_aliasing_*0.5)){
ROS_DEBUG("New exploration goal");
move_client_goal_.target_pose = goal_pose;
boost::unique_lock<boost::mutex> lock(move_client_lock_);
if(as_.isActive()){
move_client_.sendGoal(move_client_goal_, boost::bind(&FrontierExplorationServer::doneMovingCb, this, _1, _2),0,boost::bind(&FrontierExplorationServer::feedbackMovingCb, this, _1));
moving_ = true;
}
lock.unlock();
}
//check if continuous goal updating is enabled
if(frequency_ > 0){
//sleep for specified frequency and then continue searching
rate.sleep();
}else{
//wait for movement to finish before continuing
while(ros::ok() && as_.isActive() && moving_){
ros::WallDuration(0.1).sleep();
}
}
}
//goal should never be active at this point
ROS_ASSERT(!as_.isActive());
}