diff --git a/mbf_abstract_nav/CMakeLists.txt b/mbf_abstract_nav/CMakeLists.txt index d747b1e4..6a39b3d0 100644 --- a/mbf_abstract_nav/CMakeLists.txt +++ b/mbf_abstract_nav/CMakeLists.txt @@ -6,6 +6,7 @@ find_package(catkin REQUIRED actionlib actionlib_msgs dynamic_reconfigure + forklift_interfaces geometry_msgs mbf_msgs mbf_abstract_core @@ -36,6 +37,7 @@ catkin_package( CATKIN_DEPENDS actionlib actionlib_msgs + forklift_interfaces dynamic_reconfigure geometry_msgs mbf_msgs diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action.h index fe70f2f2..1d0fabfc 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_action.h @@ -71,7 +71,6 @@ class AbstractAction ) { uint8_t slot = goal_handle.getGoal()->concurrency_slot; - if(goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING) { goal_handle.setCanceled(); @@ -112,7 +111,6 @@ class AbstractAction virtual void runAndCleanUp(GoalHandle &goal_handle, typename Execution::Ptr execution_ptr){ uint8_t slot = goal_handle.getGoal()->concurrency_slot; - execution_ptr->preRun(); run_(goal_handle, *execution_ptr); ROS_DEBUG_STREAM_NAMED(name_, "Finished action \"" << name_ << "\" run method, waiting for execution thread to finish."); diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h index 50e9dd6a..c2f29415 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h @@ -66,6 +66,8 @@ #include "mbf_abstract_nav/MoveBaseFlexConfig.h" + + namespace mbf_abstract_nav { /** @@ -83,9 +85,9 @@ namespace mbf_abstract_nav typedef actionlib::ActionServer ActionServerGetPath; typedef boost::shared_ptr ActionServerGetPathPtr; -//! ExePath action server -typedef actionlib::ActionServer ActionServerExePath; -typedef boost::shared_ptr ActionServerExePathPtr; +//! Navigation action server +typedef actionlib::ActionServer ActionServerNavigate; +typedef boost::shared_ptr ActionServerNavigatePtr; //! Recovery action server typedef actionlib::ActionServer ActionServerRecovery; @@ -95,8 +97,8 @@ typedef boost::shared_ptr ActionServerRecoveryPtr; typedef actionlib::ActionServer ActionServerMoveBase; typedef boost::shared_ptr ActionServerMoveBasePtr; -//! ExePath action topic name -const std::string name_action_exe_path = "exe_path"; +//! Navigation action topic name +const std::string name_action_navigate = "navigate"; //! GetPath action topic name const std::string name_action_get_path = "get_path"; //! Recovery action topic name @@ -111,7 +113,7 @@ typedef boost::shared_ptr callActionGetPath(), ActionServerExePath -> callActionExePath(), + * the following action servers ActionServerGetPath -> callActionGetPath(), ActionServerNavigate -> callActionNavigate(), * ActionServerRecovery -> callActionRecovery() and ActionServerMoveBase -> callActionMoveBase(). * * @ingroup abstract_server navigation_server @@ -232,14 +234,15 @@ typedef boost::shared_ptr -#include +#include namespace mbf_abstract_nav{ class ControllerAction : - public AbstractAction + public AbstractAction { public: typedef boost::shared_ptr Ptr; - ControllerAction(const std::string &name, - const RobotInformation &robot_info); + ControllerAction(const std::string &name,const RobotInformation &robot_info); /** * @brief Start controller action. @@ -73,20 +72,20 @@ class ControllerAction : void run(GoalHandle &goal_handle, AbstractControllerExecution &execution); protected: - void publishExePathFeedback( + void publishNavigateFeedback( GoalHandle& goal_handle, uint32_t outcome, const std::string &message, const geometry_msgs::TwistStamped& current_twist); /** - * @brief Utility method to fill the ExePath action result in a single line - * @param outcome ExePath action outcome - * @param message ExePath action message + * @brief Utility method to fill the Navigate action result in a single line + * @param outcome Navigate action outcome + * @param message Navigate action message * @param result The action result to fill */ - void fillExePathResult( + void fillNavigateResult( uint32_t outcome, const std::string &message, - mbf_msgs::ExePathResult &result); + forklift_interfaces::NavigateResult &result); boost::mutex goal_mtx_; ///< lock goal handle for updating it while running geometry_msgs::PoseStamped robot_pose_; ///< Current robot pose diff --git a/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h b/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h index 24185537..e3469b5c 100644 --- a/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h +++ b/mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h @@ -45,7 +45,7 @@ #include #include -#include +#include #include #include "mbf_abstract_nav/MoveBaseFlexConfig.h" @@ -61,7 +61,7 @@ class MoveBaseAction //! Action clients for the MoveBase action typedef actionlib::SimpleActionClient ActionClientGetPath; - typedef actionlib::SimpleActionClient ActionClientExePath; + typedef actionlib::SimpleActionClient ActionClientNavigate; typedef actionlib::SimpleActionClient ActionClientRecovery; typedef actionlib::ActionServer::GoalHandle GoalHandle; @@ -79,17 +79,17 @@ class MoveBaseAction protected: - void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback); + void actionNavigateFeedback(const forklift_interfaces::NavigateFeedbackConstPtr &feedback); void actionGetPathDone( const actionlib::SimpleClientGoalState &state, const mbf_msgs::GetPathResultConstPtr &result); - void actionExePathActive(); + void actionNavigateActive(); - void actionExePathDone( + void actionNavigateDone( const actionlib::SimpleClientGoalState &state, - const mbf_msgs::ExePathResultConstPtr &result); + const forklift_interfaces::NavigateResultConstPtr &result); void actionGetPathReplanningDone( const actionlib::SimpleClientGoalState &state, @@ -101,7 +101,7 @@ class MoveBaseAction bool attemptRecovery(); - mbf_msgs::ExePathGoal exe_path_goal_; + forklift_interfaces::NavigateGoal navigate_goal_; mbf_msgs::GetPathGoal get_path_goal_; mbf_msgs::RecoveryGoal recovery_goal_; @@ -123,7 +123,7 @@ class MoveBaseAction ros::NodeHandle private_nh_; //! Action client used by the move_base action - ActionClientExePath action_client_exe_path_; + ActionClientNavigate action_client_navigate_; //! Action client used by the move_base action ActionClientGetPath action_client_get_path_; @@ -149,7 +149,7 @@ class MoveBaseAction { NONE, GET_PATH, - EXE_PATH, + NAVIGATE, RECOVERY, OSCILLATING, SUCCEEDED, diff --git a/mbf_abstract_nav/package.xml b/mbf_abstract_nav/package.xml index 33d28df3..e8546082 100644 --- a/mbf_abstract_nav/package.xml +++ b/mbf_abstract_nav/package.xml @@ -17,6 +17,7 @@ actionlib actionlib_msgs dynamic_reconfigure + forklift_interfaces std_msgs std_srvs nav_msgs @@ -39,6 +40,7 @@ mbf_msgs mbf_utility xmlrpcpp + forklift_interfaces diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp index 51b69a2a..adbc5e2c 100644 --- a/mbf_abstract_nav/src/abstract_navigation_server.cpp +++ b/mbf_abstract_nav/src/abstract_navigation_server.cpp @@ -60,7 +60,7 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) global_frame_(private_nh_.param("global_frame", "map")), robot_frame_(private_nh_.param("robot_frame", "base_link")), robot_info_(*tf_listener_ptr, global_frame_, robot_frame_, tf_timeout_), - controller_action_(name_action_exe_path, robot_info_), + controller_action_(name_action_navigate, robot_info_), planner_action_(name_action_get_path, robot_info_), recovery_action_(name_action_recovery, robot_info_), move_base_action_(name_action_move_base, robot_info_, recovery_plugin_manager_.getLoadedNames()) @@ -86,14 +86,6 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionGetPath, this, _1), false)); - action_server_exe_path_ptr_ = ActionServerExePathPtr( - new ActionServerExePath( - private_nh_, - name_action_exe_path, - boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionExePath, this, _1), - boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionExePath, this, _1), - false)); - action_server_recovery_ptr_ = ActionServerRecoveryPtr( new ActionServerRecovery( private_nh_, @@ -110,6 +102,13 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionMoveBase, this, _1), false)); + action_server_navigate_ptr_ = ActionServerNavigatePtr( + new ActionServerNavigate( + private_nh_, + name_action_navigate, + boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionNavigate, this, _1), + boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionNavigate, this, _1), + false)); // XXX note that we don't start a dynamic reconfigure server, to avoid colliding with the one possibly created by // the base class. If none, it should call startDynamicReconfigureServer method to start the one defined here for // providing just the abstract server parameters @@ -186,40 +185,40 @@ void AbstractNavigationServer::cancelActionGetPath(ActionServerGetPath::GoalHand planner_action_.cancel(goal_handle); } -void AbstractNavigationServer::callActionExePath(ActionServerExePath::GoalHandle goal_handle) + +void AbstractNavigationServer::callActionNavigate(ActionServerNavigate::GoalHandle goal_handle) { - const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get()); + const forklift_interfaces::NavigateGoal &goal = *(goal_handle.getGoal().get()); std::string controller_name; if(!controller_plugin_manager_.getLoadedNames().empty()) { - controller_name = goal.controller.empty() ? controller_plugin_manager_.getLoadedNames().front() : goal.controller; + controller_name = controller_plugin_manager_.getLoadedNames().front(); } else { - mbf_msgs::ExePathResult result; - result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN; - result.message = "No plugins loaded at all!"; - ROS_WARN_STREAM_NAMED("exe_path", result.message); - goal_handle.setRejected(result, result.message); + forklift_interfaces::NavigateResult result; + result.status = forklift_interfaces::NavigateResult::INVALID_PLUGIN; + result.remarks = "No plugins loaded at all!"; + ROS_WARN_STREAM_NAMED("navigate", result.remarks); + goal_handle.setRejected(result, result.remarks); return; } if(!controller_plugin_manager_.hasPlugin(controller_name)) { - mbf_msgs::ExePathResult result; - result.outcome = mbf_msgs::ExePathResult::INVALID_PLUGIN; - result.message = "No plugin loaded with the given name \"" + goal.controller + "\"!"; - ROS_WARN_STREAM_NAMED("exe_path", result.message); - goal_handle.setRejected(result, result.message); + forklift_interfaces::NavigateResult result; + result.status = forklift_interfaces::NavigateResult::INVALID_PLUGIN; + result.remarks = "No plugin loaded with the given name \"" + controller_name + "\"!"; + ROS_WARN_STREAM_NAMED("navigate", result.remarks); + goal_handle.setRejected(result, result.remarks); return; } mbf_abstract_core::AbstractController::Ptr controller_plugin = controller_plugin_manager_.getPlugin(controller_name); - ROS_INFO_STREAM_NAMED("exe_path", "Start action \"exe_path\" using controller \"" << controller_name + ROS_INFO_STREAM_NAMED("navigate", "Start action \"navigate\" using controller \"" << controller_name << "\" of type \"" << controller_plugin_manager_.getType(controller_name) << "\""); - - + if(controller_plugin) { mbf_abstract_nav::AbstractControllerExecution::Ptr controller_execution @@ -230,17 +229,18 @@ void AbstractNavigationServer::callActionExePath(ActionServerExePath::GoalHandle } else { - mbf_msgs::ExePathResult result; - result.outcome = mbf_msgs::ExePathResult::INTERNAL_ERROR; - result.message = "Internal Error: \"controller_plugin\" pointer should not be a null pointer!"; - ROS_FATAL_STREAM_NAMED("exe_path", result.message); - goal_handle.setRejected(result, result.message); + forklift_interfaces::NavigateResult result; + result.status = forklift_interfaces::NavigateResult::FAILURE; + result.remarks = "Failure: \"controller_plugin\" pointer should not be a null pointer!"; + ROS_FATAL_STREAM_NAMED("navigation", result.remarks); + goal_handle.setRejected(result, result.remarks); } + } -void AbstractNavigationServer::cancelActionExePath(ActionServerExePath::GoalHandle goal_handle) +void AbstractNavigationServer::cancelActionNavigate(ActionServerNavigate::GoalHandle goal_handle) { - ROS_INFO_STREAM_NAMED("exe_path", "Cancel action \"exe_path\""); + ROS_INFO_STREAM_NAMED("navigation", "Cancel action \"navigation\""); controller_action_.cancel(goal_handle); } @@ -340,7 +340,7 @@ mbf_abstract_nav::AbstractRecoveryExecution::Ptr AbstractNavigationServer::newRe void AbstractNavigationServer::startActionServers() { action_server_get_path_ptr_->start(); - action_server_exe_path_ptr_->start(); + action_server_navigate_ptr_->start(); action_server_recovery_ptr_->start(); action_server_move_base_ptr_->start(); } diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp index 8d2df808..19e14fb5 100644 --- a/mbf_abstract_nav/src/controller_action.cpp +++ b/mbf_abstract_nav/src/controller_action.cpp @@ -60,7 +60,6 @@ void ControllerAction::start( goal_handle.setCanceled(); return; } - uint8_t slot = goal_handle.getGoal()->concurrency_slot; bool update_plan = false; @@ -76,12 +75,18 @@ void ControllerAction::start( // Goal requests to run the same controller on the same concurrency slot: // we update the goal handle and pass the new plan to the execution without stopping it execution_ptr = slot_it->second.execution; - execution_ptr->setNewPlan(goal_handle.getGoal()->path.poses); + const forklift_interfaces::NavigateGoal &goal = *(goal_handle.getGoal().get()); + std::vector goal_path; + for(std::size_t it = 0; itsetNewPlan(goal_path); // Update also goal pose, so the feedback remains consistent - goal_pose_ = goal_handle.getGoal()->path.poses.back(); - mbf_msgs::ExePathResult result; - fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Goal preempted by a new plan", result); - concurrency_slots_[slot].goal_handle.setCanceled(result, result.message); + goal_pose_ = goal_path.back(); + forklift_interfaces::NavigateResult result; + fillNavigateResult(forklift_interfaces::NavigateResult::CANCELED, "Goal preempted by a new plan", result); + concurrency_slots_[slot].goal_handle.setCanceled(result, result.remarks); concurrency_slots_[slot].goal_handle = goal_handle; concurrency_slots_[slot].goal_handle.setAccepted(); } @@ -100,7 +105,6 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution // Note that we always use the goal handle stored on the concurrency slots map, as it can change when replanning uint8_t slot = goal_handle.getGoal()->concurrency_slot; goal_mtx_.unlock(); - ROS_DEBUG_STREAM_NAMED(name_, "Start action " << name_); // ensure we don't provide values from previous execution on case of error before filling both poses @@ -116,30 +120,33 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution double oscillation_distance; private_nh.param("oscillation_distance", oscillation_distance, 0.03); - mbf_msgs::ExePathResult result; - mbf_msgs::ExePathFeedback feedback; + forklift_interfaces::NavigateResult result; + forklift_interfaces::NavigateFeedback feedback; typename AbstractControllerExecution::ControllerState state_moving_input; bool controller_active = true; - goal_mtx_.lock(); - const mbf_msgs::ExePathGoal &goal = *(goal_handle.getGoal().get()); - - const std::vector &plan = goal.path.poses; + const forklift_interfaces::NavigateGoal &goal = *(goal_handle.getGoal().get()); + std::vector goal_path; + for (int it = 0; it < goal.path.checkpoints.size(); it++) + { + goal_path.push_back(goal.path.checkpoints[it].pose); + } + + const std::vector &plan = goal_path; if (plan.empty()) { - fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan!", result); - goal_handle.setAborted(result, result.message); - ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call."); + fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started with an empty plan!", result); + goal_handle.setAborted(result, result.remarks); + ROS_ERROR_STREAM_NAMED(name_, result.remarks << " Canceling the action call."); controller_active = false; } - goal_pose_ = plan.back(); ROS_DEBUG_STREAM_NAMED(name_, "Called action \"" << name_ << "\" with plan:" << std::endl << "frame: \"" << goal.path.header.frame_id << "\" " << std::endl << "stamp: " << goal.path.header.stamp << std::endl - << "poses: " << goal.path.poses.size() << std::endl + << "poses: " << goal.path.checkpoints.size() << std::endl << "goal: (" << goal_pose_.pose.position.x << ", " << goal_pose_.pose.position.y << ", " << goal_pose_.pose.position.z << ")"); @@ -159,11 +166,11 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution if (!robot_info_.getRobotPose(robot_pose_)) { controller_active = false; - fillExePathResult(mbf_msgs::ExePathResult::TF_ERROR, "Could not get the robot pose!", result); + fillNavigateResult(forklift_interfaces::NavigateResult::TF_ERROR, "Could not get the robot pose!", result); goal_mtx_.lock(); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); goal_mtx_.unlock(); - ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call."); + ROS_ERROR_STREAM_NAMED(name_, result.remarks << " Canceling the action call."); break; } @@ -185,13 +192,14 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution case AbstractControllerExecution::STOPPED: ROS_WARN_STREAM_NAMED(name_, "The controller has been stopped!"); + //TODO Check if paused feedback to be sent controller_active = false; break; case AbstractControllerExecution::CANCELED: - ROS_INFO_STREAM("Action \"exe_path\" canceled"); - fillExePathResult(mbf_msgs::ExePathResult::CANCELED, "Controller canceled", result); - goal_handle.setCanceled(result, result.message); + ROS_INFO_STREAM("Action \"navigate_path\" canceled"); + fillNavigateResult(forklift_interfaces::NavigateResult::CANCELED, "Controller canceled", result); + goal_handle.setCanceled(result, result.remarks); controller_active = false; break; @@ -217,42 +225,42 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution case AbstractControllerExecution::MAX_RETRIES: ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the maximum number of retries!"); controller_active = false; - fillExePathResult(execution.getOutcome(), execution.getMessage(), result); - goal_handle.setAborted(result, result.message); + fillNavigateResult(execution.getOutcome(), execution.getMessage(), result); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::PAT_EXCEEDED: ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the patience time"); controller_active = false; - fillExePathResult(mbf_msgs::ExePathResult::PAT_EXCEEDED, execution.getMessage(), result); - goal_handle.setAborted(result, result.message); + fillNavigateResult(forklift_interfaces::NavigateResult::PAT_EXCEEDED, execution.getMessage(), result); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::NO_PLAN: ROS_WARN_STREAM_NAMED(name_, "The controller has been started without a plan!"); controller_active = false; - fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started without a path", result); - goal_handle.setAborted(result, result.message); + fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started without a path", result); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::EMPTY_PLAN: ROS_WARN_STREAM_NAMED(name_, "The controller has received an empty plan"); controller_active = false; - fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an empty plan", result); - goal_handle.setAborted(result, result.message); + fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started with an empty plan", result); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::INVALID_PLAN: ROS_WARN_STREAM_NAMED(name_, "The controller has received an invalid plan"); controller_active = false; - fillExePathResult(mbf_msgs::ExePathResult::INVALID_PATH, "Controller started with an invalid plan", result); - goal_handle.setAborted(result, result.message); + fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started with an invalid plan", result); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::NO_LOCAL_CMD: ROS_WARN_STREAM_THROTTLE_NAMED(3, name_, "No velocity command received from controller! " << execution.getMessage()); - publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd()); + publishNavigateFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd()); break; case AbstractControllerExecution::GOT_LOCAL_CMD: @@ -270,35 +278,35 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution << (ros::Time::now() - last_oscillation_reset).toSec() << "s"); execution.stop(); controller_active = false; - fillExePathResult(mbf_msgs::ExePathResult::OSCILLATION, "Oscillation detected!", result); - goal_handle.setAborted(result, result.message); + fillNavigateResult(forklift_interfaces::NavigateResult::OSCILLATION, "Oscillation detected!", result); + goal_handle.setAborted(result, result.remarks); break; } } - publishExePathFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd()); + publishNavigateFeedback(goal_handle, execution.getOutcome(), execution.getMessage(), execution.getVelocityCmd()); break; case AbstractControllerExecution::ARRIVED_GOAL: ROS_DEBUG_STREAM_NAMED(name_, "Controller succeeded; arrived at goal"); controller_active = false; - fillExePathResult(mbf_msgs::ExePathResult::SUCCESS, "Controller succeeded; arrived at goal!", result); - goal_handle.setSucceeded(result, result.message); + fillNavigateResult(forklift_interfaces::NavigateResult::SUCCESS, "Controller succeeded; arrived at goal!", result); + goal_handle.setSucceeded(result, result.remarks); break; case AbstractControllerExecution::INTERNAL_ERROR: ROS_FATAL_STREAM_NAMED(name_, "Internal error: Unknown error thrown by the plugin: " << execution.getMessage()); controller_active = false; - fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, "Internal error: Unknown error thrown by the plugin!", result); - goal_handle.setAborted(result, result.message); + fillNavigateResult(forklift_interfaces::NavigateResult::INTERNAL_ERROR, "Internal error: Unknown error thrown by the plugin!", result); + goal_handle.setAborted(result, result.remarks); break; default: std::stringstream ss; ss << "Internal error: Unknown state in a move base flex controller execution with the number: " << static_cast(state_moving_input); - fillExePathResult(mbf_msgs::ExePathResult::INTERNAL_ERROR, ss.str(), result); - ROS_FATAL_STREAM_NAMED(name_, result.message); - goal_handle.setAborted(result, result.message); + fillNavigateResult(forklift_interfaces::NavigateResult::INTERNAL_ERROR, ss.str(), result); + ROS_FATAL_STREAM_NAMED(name_, result.remarks); + goal_handle.setAborted(result, result.remarks); controller_active = false; } goal_mtx_.unlock(); @@ -325,18 +333,18 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution } } -void ControllerAction::publishExePathFeedback( +void ControllerAction::publishNavigateFeedback( GoalHandle& goal_handle, uint32_t outcome, const std::string &message, const geometry_msgs::TwistStamped& current_twist) { - mbf_msgs::ExePathFeedback feedback; - feedback.outcome = outcome; - feedback.message = message; + forklift_interfaces::NavigateFeedback feedback; + feedback.status = outcome; + feedback.remarks = message; - feedback.last_cmd_vel = current_twist; - if (feedback.last_cmd_vel.header.stamp.isZero()) - feedback.last_cmd_vel.header.stamp = ros::Time::now(); + feedback.velocity = current_twist; + if (feedback.velocity.header.stamp.isZero()) + feedback.velocity.header.stamp = ros::Time::now(); feedback.current_pose = robot_pose_; feedback.dist_to_goal = static_cast(mbf_utility::distance(robot_pose_, goal_pose_)); @@ -344,12 +352,12 @@ void ControllerAction::publishExePathFeedback( goal_handle.publishFeedback(feedback); } -void ControllerAction::fillExePathResult( +void ControllerAction::fillNavigateResult( uint32_t outcome, const std::string &message, - mbf_msgs::ExePathResult &result) + forklift_interfaces::NavigateResult &result) { - result.outcome = outcome; - result.message = message; + result.status = outcome; + result.remarks = message; result.final_pose = robot_pose_; result.dist_to_goal = static_cast(mbf_utility::distance(robot_pose_, goal_pose_)); result.angle_to_goal = static_cast(mbf_utility::angle(robot_pose_, goal_pose_)); diff --git a/mbf_abstract_nav/src/move_base_action.cpp b/mbf_abstract_nav/src/move_base_action.cpp index 26a7fab9..2ebe6998 100644 --- a/mbf_abstract_nav/src/move_base_action.cpp +++ b/mbf_abstract_nav/src/move_base_action.cpp @@ -50,7 +50,7 @@ MoveBaseAction::MoveBaseAction(const std::string &name, const RobotInformation &robot_info, const std::vector &behaviors) : name_(name), robot_info_(robot_info), private_nh_("~"), - action_client_exe_path_(private_nh_, "exe_path"), + action_client_navigate_(private_nh_, "navigate"), action_client_get_path_(private_nh_, "get_path"), action_client_recovery_(private_nh_, "recovery"), oscillation_timeout_(0), @@ -77,11 +77,11 @@ void MoveBaseAction::reconfigure( if (!replanning_) { replanning_ = true; - if (action_state_ == EXE_PATH && + if (action_state_ == NAVIGATE && action_client_get_path_.getState() != actionlib::SimpleClientGoalState::PENDING && action_client_get_path_.getState() != actionlib::SimpleClientGoalState::ACTIVE) { - // exe_path is running and user has enabled replanning + // navigate is running and user has enabled replanning ROS_INFO_STREAM_NAMED("move_base", "Planner frequency set to " << config.planner_frequency << ": start replanning, using the \"get_path\" action!"); action_client_get_path_.sendGoal( @@ -108,9 +108,9 @@ void MoveBaseAction::cancel() action_client_get_path_.cancelGoal(); } - if(!action_client_exe_path_.getState().isDone()) + if(!action_client_navigate_.getState().isDone()) { - action_client_exe_path_.cancelGoal(); + action_client_navigate_.cancelGoal(); } if(!action_client_recovery_.getState().isDone()) @@ -136,7 +136,7 @@ void MoveBaseAction::start(GoalHandle &goal_handle) get_path_goal_.target_pose = goal.target_pose; get_path_goal_.use_start_pose = false; // use the robot pose get_path_goal_.planner = goal.planner; - exe_path_goal_.controller = goal.controller; + navigate_goal_.controller = goal.controller; ros::Duration connection_timeout(1.0); @@ -149,7 +149,7 @@ void MoveBaseAction::start(GoalHandle &goal_handle) current_recovery_behavior_ = recovery_behaviors_.begin(); geometry_msgs::PoseStamped robot_pose; - // get the current robot pose only at the beginning, as exe_path will keep updating it as we move + // get the current robot pose only at the beginning, as navigate will keep updating it as we move if (!robot_info_.getRobotPose(robot_pose)) { ROS_ERROR_STREAM_NAMED("move_base", "Could not get the current robot pose!"); @@ -161,11 +161,11 @@ void MoveBaseAction::start(GoalHandle &goal_handle) // wait for server connections if (!action_client_get_path_.waitForServer(connection_timeout) || - !action_client_exe_path_.waitForServer(connection_timeout) || + !action_client_navigate_.waitForServer(connection_timeout) || !action_client_recovery_.waitForServer(connection_timeout)) { ROS_ERROR_STREAM_NAMED("move_base", "Could not connect to one or more of move_base_flex actions:" - "\"get_path\" , \"exe_path\", \"recovery \"!"); + "\"get_path\" , \"navigate\", \"recovery \"!"); move_base_result.outcome = mbf_msgs::MoveBaseResult::INTERNAL_ERROR; move_base_result.message = "Could not connect to the move_base_flex actions!"; goal_handle.setAborted(move_base_result, move_base_result.message); @@ -178,24 +178,24 @@ void MoveBaseAction::start(GoalHandle &goal_handle) boost::bind(&MoveBaseAction::actionGetPathDone, this, _1, _2)); } -void MoveBaseAction::actionExePathActive() +void MoveBaseAction::actionNavigateActive() { - ROS_DEBUG_STREAM_NAMED("move_base", "The \"exe_path\" action is active."); + ROS_DEBUG_STREAM_NAMED("move_base", "The \"navigate\" action is active."); } -void MoveBaseAction::actionExePathFeedback( - const mbf_msgs::ExePathFeedbackConstPtr &feedback) +void MoveBaseAction::actionNavigateFeedback( + const forklift_interfaces::NavigateFeedbackConstPtr &feedback) { - move_base_feedback_.outcome = feedback->outcome; - move_base_feedback_.message = feedback->message; + move_base_feedback_.outcome = feedback->status; + move_base_feedback_.message = feedback->remarks; move_base_feedback_.angle_to_goal = feedback->angle_to_goal; move_base_feedback_.dist_to_goal = feedback->dist_to_goal; move_base_feedback_.current_pose = feedback->current_pose; - move_base_feedback_.last_cmd_vel = feedback->last_cmd_vel; + move_base_feedback_.last_cmd_vel = feedback->velocity; robot_pose_ = feedback->current_pose; goal_handle_.publishFeedback(move_base_feedback_); - // we create a navigation-level oscillation detection using exe_path action's feedback, + // we create a navigation-level oscillation detection using navigate action's feedback, // as the later doesn't handle oscillations created by quickly failing repeated plans // if oscillation detection is enabled by osciallation_timeout != 0 @@ -219,8 +219,8 @@ void MoveBaseAction::actionExePathFeedback( { std::stringstream oscillation_msgs; oscillation_msgs << "Robot is oscillating for " << (ros::Time::now() - last_oscillation_reset_).toSec() << "s!"; - ROS_WARN_STREAM_NAMED("exe_path", oscillation_msgs.str()); - action_client_exe_path_.cancelGoal(); + ROS_WARN_STREAM_NAMED("navigate", oscillation_msgs.str()); + action_client_navigate_.cancelGoal(); if (attemptRecovery()) { @@ -260,10 +260,14 @@ void MoveBaseAction::actionGetPathDone( << "move_base\" received a path from \"" << "get_path\": " << state.getText()); - exe_path_goal_.path = result.path; + navigate_goal_.path.header = result.path.header; //TODO handle conversion from nav_msgs/Path to NavigatePath/Path + for(std::size_t it=0; it guard(replanning_mtx_); replanning_rate_.reset(); replanning_rate_.sleep(); - if (!replanning_ || action_state_ != EXE_PATH || + if (!replanning_ || action_state_ != NAVIGATE || action_client_get_path_.getState() == actionlib::SimpleClientGoalState::PENDING || action_client_get_path_.getState() == actionlib::SimpleClientGoalState::ACTIVE) return; // another chance to stop replannings after waiting for replanning period @@ -349,26 +353,26 @@ void MoveBaseAction::actionGetPathDone( ); } -void MoveBaseAction::actionExePathDone( +void MoveBaseAction::actionNavigateDone( const actionlib::SimpleClientGoalState &state, - const mbf_msgs::ExePathResultConstPtr &result_ptr) + const forklift_interfaces::NavigateResultConstPtr &result_ptr) { action_state_ = FAILED; - ROS_DEBUG_STREAM_NAMED("move_base", "Action \"exe_path\" finished."); + ROS_DEBUG_STREAM_NAMED("move_base", "Action \"navigate\" finished."); - const mbf_msgs::ExePathResult& result = *(result_ptr.get()); + const forklift_interfaces::NavigateResult& result = *(result_ptr.get()); const mbf_msgs::MoveBaseGoal& goal = *(goal_handle_.getGoal().get()); mbf_msgs::MoveBaseResult move_base_result; // copy result from get_path action - move_base_result.outcome = result.outcome; - move_base_result.message = result.message; + move_base_result.outcome = result.status; + move_base_result.message = result.remarks; move_base_result.dist_to_goal = result.dist_to_goal; move_base_result.angle_to_goal = result.angle_to_goal; move_base_result.final_pose = result.final_pose; - ROS_DEBUG_STREAM_NAMED("exe_path", "Current state:" << state.toString()); + ROS_DEBUG_STREAM_NAMED("navigate", "Current state:" << state.toString()); switch (state.state_) { @@ -381,13 +385,10 @@ void MoveBaseAction::actionExePathDone( break; case actionlib::SimpleClientGoalState::ABORTED: - switch (result.outcome) + switch (result.status) { - case mbf_msgs::ExePathResult::INVALID_PATH: - case mbf_msgs::ExePathResult::TF_ERROR: - case mbf_msgs::ExePathResult::NOT_INITIALIZED: - case mbf_msgs::ExePathResult::INVALID_PLUGIN: - case mbf_msgs::ExePathResult::INTERNAL_ERROR: + case forklift_interfaces::NavigateResult::INVALID_PATH: + case forklift_interfaces::NavigateResult::INVALID_PLUGIN: // none of these errors is recoverable goal_handle_.setAborted(move_base_result, state.getText()); break; @@ -397,11 +398,11 @@ void MoveBaseAction::actionExePathDone( if (attemptRecovery()) { - recovery_trigger_ = EXE_PATH; + recovery_trigger_ = NAVIGATE; } else { - ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << result.message); + ROS_WARN_STREAM_NAMED("move_base", "Abort the execution of the controller: " << result.remarks); goal_handle_.setAborted(move_base_result, state.getText()); } break; @@ -411,19 +412,19 @@ void MoveBaseAction::actionExePathDone( case actionlib::SimpleClientGoalState::PREEMPTED: // action was preempted successfully! ROS_DEBUG_STREAM_NAMED("move_base", "The action \"" - << "exe_path" << "\" was preempted successfully!"); + << "navigate" << "\" was preempted successfully!"); // TODO break; case actionlib::SimpleClientGoalState::RECALLED: ROS_DEBUG_STREAM_NAMED("move_base", "The action \"" - << "exe_path" << "\" was recalled!"); + << "navigate" << "\" was recalled!"); // TODO break; case actionlib::SimpleClientGoalState::REJECTED: ROS_DEBUG_STREAM_NAMED("move_base", "The action \"" - << "exe_path" << "\" was rejected!"); + << "navigate" << "\" was rejected!"); // TODO break; @@ -568,26 +569,31 @@ void MoveBaseAction::actionGetPathReplanningDone( const actionlib::SimpleClientGoalState &state, const mbf_msgs::GetPathResultConstPtr &result) { - if (!replanning_ || action_state_ != EXE_PATH) + if (!replanning_ || action_state_ != NAVIGATE) return; // replan only while following a path and if replanning is enabled (can be disabled by dynamic reconfigure) if(state == actionlib::SimpleClientGoalState::SUCCEEDED) { - ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"exe_path\" with the new plan"); - exe_path_goal_.path = result->path; - mbf_msgs::ExePathGoal goal(exe_path_goal_); - action_client_exe_path_.sendGoal( + ROS_DEBUG_STREAM_NAMED("move_base", "Replanning succeeded; sending a goal to \"navigate\" with the new plan"); + + navigate_goal_.path.header = result->path.header; //TODO handle conversion from nav_msgs/Path to NavigatePath/Path + for(std::size_t it=0; itpath.poses.size(); it++) + { + navigate_goal_.path.checkpoints[it].pose = result->path.poses[it]; + } + forklift_interfaces::NavigateGoal goal(navigate_goal_); + action_client_navigate_.sendGoal( goal, - boost::bind(&MoveBaseAction::actionExePathDone, this, _1, _2), - boost::bind(&MoveBaseAction::actionExePathActive, this), - boost::bind(&MoveBaseAction::actionExePathFeedback, this, _1)); + boost::bind(&MoveBaseAction::actionNavigateDone, this, _1, _2), + boost::bind(&MoveBaseAction::actionNavigateActive, this), + boost::bind(&MoveBaseAction::actionNavigateFeedback, this, _1)); } replanning_mtx_.lock(); replanning_rate_.sleep(); replanning_mtx_.unlock(); - if (!replanning_ || action_state_ != EXE_PATH) + if (!replanning_ || action_state_ != NAVIGATE) return; // another chance to stop replannings after waiting for replanning period ROS_DEBUG_STREAM_NAMED("move_base", "Next replanning cycle, using the \"get_path\" action!"); diff --git a/mbf_msgs/CMakeLists.txt b/mbf_msgs/CMakeLists.txt index dd7659ea..854c5e1d 100644 --- a/mbf_msgs/CMakeLists.txt +++ b/mbf_msgs/CMakeLists.txt @@ -10,6 +10,7 @@ find_package(catkin REQUIRED message_runtime nav_msgs std_msgs + forklift_interfaces ) add_service_files( @@ -33,6 +34,7 @@ add_action_files( generate_messages( DEPENDENCIES + forklift_interfaces actionlib_msgs geometry_msgs nav_msgs @@ -43,6 +45,7 @@ catkin_package( CATKIN_DEPENDS actionlib_msgs geometry_msgs + forklift_interfaces message_runtime nav_msgs std_msgs diff --git a/mbf_msgs/package.xml b/mbf_msgs/package.xml index f4417271..1477dea9 100644 --- a/mbf_msgs/package.xml +++ b/mbf_msgs/package.xml @@ -19,11 +19,13 @@ genmsg message_runtime message_generation + forklift_interfaces std_msgs nav_msgs geometry_msgs actionlib_msgs message_runtime + forklift_interfaces