45 #include "mbf_abstract_nav/MoveBaseFlexConfig.h" 51 const std::vector<std::string>& behaviors)
53 , robot_info_(robot_info)
55 , action_client_exe_path_(private_nh_,
"exe_path")
56 , action_client_get_path_(private_nh_,
"get_path")
57 , action_client_recovery_(private_nh_,
"recovery")
58 , oscillation_timeout_(0)
59 , oscillation_distance_(0)
60 , recovery_enabled_(true)
61 , behaviors_(behaviors)
63 , recovery_trigger_(NONE)
64 , dist_to_goal_(
std::numeric_limits<double>::infinity())
76 mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
78 if (config.planner_frequency > 0.0)
119 const mbf_msgs::MoveBaseGoal& goal = *goal_handle.
getGoal();
121 mbf_msgs::MoveBaseResult move_base_result;
142 move_base_result.message =
"Could not get the current robot pose!";
143 move_base_result.outcome = mbf_msgs::MoveBaseResult::TF_ERROR;
144 goal_handle.
setAborted(move_base_result, move_base_result.message);
155 "\"get_path\", \"exe_path\", \"recovery \"!");
156 move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR;
157 move_base_result.message =
"Could not connect to the move_base_flex actions!";
158 goal_handle.
setAborted(move_base_result, move_base_result.message);
174 const mbf_msgs::ExePathFeedbackConstPtr &feedback)
176 mbf_msgs::MoveBaseFeedback move_base_feedback;
177 move_base_feedback.outcome = feedback->outcome;
178 move_base_feedback.message = feedback->message;
179 move_base_feedback.angle_to_goal = feedback->angle_to_goal;
180 move_base_feedback.dist_to_goal = feedback->dist_to_goal;
181 move_base_feedback.current_pose = feedback->current_pose;
182 move_base_feedback.last_cmd_vel = feedback->last_cmd_vel;
202 ROS_INFO_NAMED(
"move_base",
"Recovered from robot oscillation: restart recovery behaviors");
209 std::stringstream oscillation_msgs;
220 mbf_msgs::MoveBaseResult move_base_result;
221 move_base_result.outcome = mbf_msgs::MoveBaseResult::OSCILLATION;
222 move_base_result.message = oscillation_msgs.str();
224 move_base_result.angle_to_goal = move_base_feedback.angle_to_goal;
225 move_base_result.dist_to_goal = move_base_feedback.dist_to_goal;
234 const mbf_msgs::GetPathResultConstPtr &result_ptr)
236 const mbf_msgs::GetPathResult &get_path_result = *result_ptr;
237 mbf_msgs::MoveBaseResult move_base_result;
245 ROS_FATAL_STREAM_NAMED(
"move_base",
"get_path PENDING state not implemented, this should not be reachable!");
250 <<
"move_base\" received a path from \"" 251 <<
"get_path\": " << state.
getText());
255 <<
"move_base\" sends the path to \"" 260 ROS_WARN_NAMED(
"move_base",
"Recovered from planner failure: restart recovery behaviors");
282 ROS_WARN_STREAM_NAMED(
"move_base",
"Abort the execution of the planner: " << get_path_result.message);
321 const mbf_msgs::ExePathResultConstPtr &result_ptr)
325 const mbf_msgs::ExePathResult& exe_path_result = *result_ptr;
326 mbf_msgs::MoveBaseResult move_base_result;
336 move_base_result.outcome = mbf_msgs::MoveBaseResult::SUCCESS;
337 move_base_result.message =
"Action \"move_base\" succeeded!";
346 switch (exe_path_result.outcome)
348 case mbf_msgs::ExePathResult::INVALID_PATH:
349 case mbf_msgs::ExePathResult::TF_ERROR:
350 case mbf_msgs::ExePathResult::NOT_INITIALIZED:
351 case mbf_msgs::ExePathResult::INVALID_PLUGIN:
352 case mbf_msgs::ExePathResult::INTERNAL_ERROR:
366 ROS_WARN_STREAM_NAMED(
"move_base",
"Abort the execution of the controller: " << exe_path_result.message);
438 const mbf_msgs::RecoveryResultConstPtr &result_ptr)
443 const mbf_msgs::RecoveryResult& recovery_result = *result_ptr;
444 mbf_msgs::MoveBaseResult move_base_result;
458 <<
", outcome: " << recovery_result.outcome);
464 "All recovery behaviors failed. Abort recovering and abort the move_base action");
484 "Try planning again and increment the current recovery behavior in the list.");
537 ROS_DEBUG_STREAM_NAMED(
"move_base",
"Replanning succeeded; sending a goal to \"exe_path\" with the new plan");
547 "Replanning failed with error code " << result->outcome <<
": " << result->message);
554 update_period.
sleep();
void publishFeedback(const Feedback &feedback)
const mbf_utility::RobotInformation & robot_info_
current robot state
std::vector< std::string > recovery_behaviors_
#define ROS_INFO_NAMED(name,...)
mbf_msgs::GetPathGoal get_path_goal_
double dist_to_goal_
current distance to goal (we will stop replanning if very close to avoid destabilizing the controller...
ResultConstPtr getResult() const
ros::Duration oscillation_timeout_
timeout after a oscillation is detected
#define ROS_DEBUG_STREAM_NAMED(name, args)
geometry_msgs::PoseStamped last_oscillation_pose_
#define ROS_ERROR_STREAM_NAMED(name, args)
boost::shared_ptr< const Goal > getGoal() const
#define ROS_WARN_NAMED(name,...)
void actionExePathDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::ExePathResultConstPtr &result)
boost::thread replanning_thread_
Replanning thread, running permanently.
bool waitForResult(const ros::Duration &timeout=ros::Duration(0, 0))
ros::Time last_oscillation_reset_
std::string toString() const
#define ROS_INFO_STREAM_NAMED(name, args)
MoveBaseAction(const std::string &name, const mbf_utility::RobotInformation &robot_info, const std::vector< std::string > &controllers)
void setCanceled(const Result &result=Result(), const std::string &text=std::string(""))
ActionClientGetPath action_client_get_path_
Action client used by the move_base action.
void setAccepted(const std::string &text=std::string(""))
bool waitForServer(const ros::Duration &timeout=ros::Duration(0, 0)) const
bool replanningActive() const
void reconfigure(mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level)
void setSucceeded(const Result &result=Result(), const std::string &text=std::string(""))
SimpleClientGoalState getState() const
const std::vector< std::string > & behaviors_
Duration & fromSec(double t)
#define ROS_FATAL_STREAM_NAMED(name, args)
mbf_msgs::RecoveryGoal recovery_goal_
void actionRecoveryDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::RecoveryResultConstPtr &result)
void actionGetPathDone(const actionlib::SimpleClientGoalState &state, const mbf_msgs::GetPathResultConstPtr &result)
MoveBaseActionState action_state_
#define ROS_DEBUG_STREAM(args)
void fillMoveBaseResult(const ResultType &result, mbf_msgs::MoveBaseResult &move_base_result)
mbf_msgs::ExePathGoal exe_path_goal_
double distance(const geometry_msgs::PoseStamped &pose1, const geometry_msgs::PoseStamped &pose2)
MoveBaseActionState recovery_trigger_
bool recovery_enabled_
true, if recovery behavior for the MoveBase action is enabled.
void setAborted(const Result &result=Result(), const std::string &text=std::string(""))
void sendGoal(const Goal &goal, SimpleDoneCallback done_cb=SimpleDoneCallback(), SimpleActiveCallback active_cb=SimpleActiveCallback(), SimpleFeedbackCallback feedback_cb=SimpleFeedbackCallback())
geometry_msgs::PoseStamped goal_pose_
current goal pose; used to compute remaining distance and angle
std::vector< std::string >::iterator current_recovery_behavior_
geometry_msgs::PoseStamped robot_pose_
current robot pose; updated with exe_path action feedback
std::string getText() const
ActionClientRecovery action_client_recovery_
Action client used by the move_base action.
void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback)
ros::Duration replanning_period_
Replanning period dynamically reconfigurable.
void actionExePathActive()
ActionClientExePath action_client_exe_path_
Action client used by the move_base action.
void start(GoalHandle &goal_handle)
double oscillation_distance_
minimal move distance to not detect an oscillation
#define ROS_WARN_STREAM_NAMED(name, args)