From e4a502f11ae6d2701321e69e8b242dfe29d46f4a Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Thu, 14 Nov 2019 18:27:02 +0900 Subject: [PATCH 1/7] Added new Navigate action and added methods to call the Navigate action --- .../abstract_navigation_server.h | 19 ++++++++++++++++ .../src/abstract_navigation_server.cpp | 22 +++++++++++++++++++ mbf_msgs/CMakeLists.txt | 1 + mbf_msgs/action/Navigate.action | 5 +++++ 4 files changed, 47 insertions(+) create mode 100644 mbf_msgs/action/Navigate.action 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..e7d8dfea 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 @@ -87,6 +87,10 @@ typedef boost::shared_ptr ActionServerGetPathPtr; typedef actionlib::ActionServer ActionServerExePath; typedef boost::shared_ptr ActionServerExePathPtr; +//! Navigation action server +typedef actionlib::ActionServer ActionServerNavigation; +typedef boost::shared_ptr ActionServerNavigationPtr; + //! Recovery action server typedef actionlib::ActionServer ActionServerRecovery; typedef boost::shared_ptr ActionServerRecoveryPtr; @@ -97,6 +101,8 @@ 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_exe_path = "navigation"; //! GetPath action topic name const std::string name_action_get_path = "get_path"; //! Recovery action topic name @@ -241,6 +247,15 @@ typedef boost::shared_ptr("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_navigate_(name_action_navigation, 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()) @@ -110,6 +111,13 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionMoveBase, this, _1), false)); + action_server_navigation_ptr_ = ActionServerNavigationPtr( + new ActionServerNavigation( + private_nh_, + name_action_navigation, + boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionNavigation, this, _1), + boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionNavigation, 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 @@ -244,6 +252,17 @@ void AbstractNavigationServer::cancelActionExePath(ActionServerExePath::GoalHand controller_action_.cancel(goal_handle); } +void AbstractNavigationServer::callActionNavigation(ActionServerNavigation::GoalHandle goal_handle) +{ + //TODO: Handle Navigate action call +} + +void AbstractNavigationServer::cancelActionNavigation(ActionServerNavigation::GoalHandle goal_handle) +{ + ROS_INFO_STREAM_NAMED("navigation", "Cancel action \"navigation\""); + controller_action_navigate_.cancel(goal_handle); +} + void AbstractNavigationServer::callActionRecovery(ActionServerRecovery::GoalHandle goal_handle) { const mbf_msgs::RecoveryGoal &goal = *(goal_handle.getGoal().get()); @@ -341,6 +360,7 @@ void AbstractNavigationServer::startActionServers() { action_server_get_path_ptr_->start(); action_server_exe_path_ptr_->start(); + action_server_navigation_ptr_->start(); action_server_recovery_ptr_->start(); action_server_move_base_ptr_->start(); } @@ -372,6 +392,7 @@ void AbstractNavigationServer::reconfigure( } planner_action_.reconfigureAll(config, level); controller_action_.reconfigureAll(config, level); + controller_action_navigate_.reconfigureAll(config, level); recovery_action_.reconfigureAll(config, level); move_base_action_.reconfigure(config, level); @@ -381,6 +402,7 @@ void AbstractNavigationServer::reconfigure( void AbstractNavigationServer::stop(){ planner_action_.cancelAll(); controller_action_.cancelAll(); + controller_action_navigate_.cancelAll(); recovery_action_.cancelAll(); move_base_action_.cancel(); } diff --git a/mbf_msgs/CMakeLists.txt b/mbf_msgs/CMakeLists.txt index dd7659ea..8da47dc3 100644 --- a/mbf_msgs/CMakeLists.txt +++ b/mbf_msgs/CMakeLists.txt @@ -29,6 +29,7 @@ add_action_files( ExePath.action Recovery.action MoveBase.action + Navigate.action ) generate_messages( diff --git a/mbf_msgs/action/Navigate.action b/mbf_msgs/action/Navigate.action new file mode 100644 index 00000000..34875847 --- /dev/null +++ b/mbf_msgs/action/Navigate.action @@ -0,0 +1,5 @@ +NavigatePath path # Path to follow +--- +NavigationResult result # Final result of navigation +--- +NavigationFeedback feedback # Continuous feedback regarding progress From 957f42b6a7b6ad35691c752478ce47c31a60334b Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Fri, 15 Nov 2019 18:58:22 +0900 Subject: [PATCH 2/7] Added funtion to call controller from Navigate action and replace Exepath --- .../src/abstract_navigation_server.cpp | 59 ++++++++++++++++--- 1 file changed, 52 insertions(+), 7 deletions(-) diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp index af0e7aac..5b69ab0d 100644 --- a/mbf_abstract_nav/src/abstract_navigation_server.cpp +++ b/mbf_abstract_nav/src/abstract_navigation_server.cpp @@ -60,8 +60,8 @@ 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_navigate_(name_action_navigation, robot_info_), + //controller_action_(name_action_exe_path, robot_info_), + controller_action_(name_action_navigation, 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()) @@ -254,13 +254,60 @@ void AbstractNavigationServer::cancelActionExePath(ActionServerExePath::GoalHand void AbstractNavigationServer::callActionNavigation(ActionServerNavigation::GoalHandle goal_handle) { - //TODO: Handle Navigate action call + const mbf_msgs::NavigationGoal &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; + } + else + { + mbf_msgs::NavigationResult result; + result.outcome = mbf_msgs::NavigationResult::INVALID_PLUGIN; + result.message = "No plugins loaded at all!"; + ROS_WARN_STREAM_NAMED("navigation", result.message); + goal_handle.setRejected(result, result.message); + return; + } + + if(!controller_plugin_manager_.hasPlugin(controller_name)) + { + mbf_msgs::NavigationResult result; + result.outcome = mbf_msgs::NavigationResult::INVALID_PLUGIN; + result.message = "No plugin loaded with the given name \"" + goal.controller + "\"!"; + ROS_WARN_STREAM_NAMED("navigation", result.message); + goal_handle.setRejected(result, result.message); + return; + } + + mbf_abstract_core::AbstractController::Ptr controller_plugin = controller_plugin_manager_.getPlugin(controller_name); + ROS_INFO_STREAM_NAMED("navigation", "Start action \"navigation\" using controller \"" << controller_name + << "\" of type \"" << controller_plugin_manager_.getType(controller_name) << "\""); + + if(controller_plugin) + { + mbf_abstract_nav::AbstractControllerExecution::Ptr controller_execution + = newControllerExecution(controller_name, controller_plugin); + + // starts another controller action + controller_action_.start(goal_handle, controller_execution); + } + else + { + mbf_msgs::ExePathResult result; + result.outcome = mbf_msgs::NavigationResult::FAILURE; + result.message = "Failure: \"controller_plugin\" pointer should not be a null pointer!"; + ROS_FATAL_STREAM_NAMED("navigation", result.message); + goal_handle.setRejected(result, result.message); + } + } void AbstractNavigationServer::cancelActionNavigation(ActionServerNavigation::GoalHandle goal_handle) { ROS_INFO_STREAM_NAMED("navigation", "Cancel action \"navigation\""); - controller_action_navigate_.cancel(goal_handle); + controller_action_.cancel(goal_handle); } void AbstractNavigationServer::callActionRecovery(ActionServerRecovery::GoalHandle goal_handle) @@ -359,7 +406,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_exe_path_ptr_->start(); action_server_navigation_ptr_->start(); action_server_recovery_ptr_->start(); action_server_move_base_ptr_->start(); @@ -392,7 +439,6 @@ void AbstractNavigationServer::reconfigure( } planner_action_.reconfigureAll(config, level); controller_action_.reconfigureAll(config, level); - controller_action_navigate_.reconfigureAll(config, level); recovery_action_.reconfigureAll(config, level); move_base_action_.reconfigure(config, level); @@ -402,7 +448,6 @@ void AbstractNavigationServer::reconfigure( void AbstractNavigationServer::stop(){ planner_action_.cancelAll(); controller_action_.cancelAll(); - controller_action_navigate_.cancelAll(); recovery_action_.cancelAll(); move_base_action_.cancel(); } From ef3bfe06ec8deb6c96384f502bacad8acf77560a Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Wed, 20 Nov 2019 18:33:21 +0900 Subject: [PATCH 3/7] Modified abstract_navigation_server to accept the custom navigate action instead of exepath --- mbf_abstract_nav/CMakeLists.txt | 2 + .../abstract_navigation_server.h | 36 ++---- .../mbf_abstract_nav/controller_action.h | 19 ++- .../mbf_abstract_nav/move_base_action.h | 18 +-- mbf_abstract_nav/package.xml | 2 + .../src/abstract_navigation_server.cpp | 109 ++++-------------- mbf_abstract_nav/src/controller_action.cpp | 74 +++++++----- mbf_abstract_nav/src/move_base_action.cpp | 106 +++++++++-------- mbf_msgs/CMakeLists.txt | 4 +- mbf_msgs/action/Navigate.action | 5 - mbf_msgs/package.xml | 2 + 11 files changed, 156 insertions(+), 221 deletions(-) delete mode 100644 mbf_msgs/action/Navigate.action 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_navigation_server.h b/mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h index e7d8dfea..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,13 +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 ActionServerNavigation; -typedef boost::shared_ptr ActionServerNavigationPtr; +typedef actionlib::ActionServer ActionServerNavigate; +typedef boost::shared_ptr ActionServerNavigatePtr; //! Recovery action server typedef actionlib::ActionServer ActionServerRecovery; @@ -99,10 +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_exe_path = "navigation"; +const std::string name_action_navigate = "navigate"; //! GetPath action topic name const std::string name_action_get_path = "get_path"; //! Recovery action topic name @@ -117,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 @@ -238,23 +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 5b69ab0d..dbba01cb 100644 --- a/mbf_abstract_nav/src/abstract_navigation_server.cpp +++ b/mbf_abstract_nav/src/abstract_navigation_server.cpp @@ -60,8 +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_navigation, 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()) @@ -87,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_, @@ -111,12 +102,12 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr) boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionMoveBase, this, _1), false)); - action_server_navigation_ptr_ = ActionServerNavigationPtr( - new ActionServerNavigation( + action_server_navigate_ptr_ = ActionServerNavigatePtr( + new ActionServerNavigate( private_nh_, - name_action_navigation, - boost::bind(&mbf_abstract_nav::AbstractNavigationServer::callActionNavigation, this, _1), - boost::bind(&mbf_abstract_nav::AbstractNavigationServer::cancelActionNavigation, this, _1), + 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 @@ -194,95 +185,38 @@ void AbstractNavigationServer::cancelActionGetPath(ActionServerGetPath::GoalHand planner_action_.cancel(goal_handle); } -void AbstractNavigationServer::callActionExePath(ActionServerExePath::GoalHandle goal_handle) -{ - const mbf_msgs::ExePathGoal &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; - } - 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); - 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); - 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 - << "\" of type \"" << controller_plugin_manager_.getType(controller_name) << "\""); - - - if(controller_plugin) - { - mbf_abstract_nav::AbstractControllerExecution::Ptr controller_execution - = newControllerExecution(controller_name, controller_plugin); - - // starts another controller action - controller_action_.start(goal_handle, controller_execution); - } - 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); - } -} - -void AbstractNavigationServer::cancelActionExePath(ActionServerExePath::GoalHandle goal_handle) -{ - ROS_INFO_STREAM_NAMED("exe_path", "Cancel action \"exe_path\""); - controller_action_.cancel(goal_handle); -} -void AbstractNavigationServer::callActionNavigation(ActionServerNavigation::GoalHandle goal_handle) +void AbstractNavigationServer::callActionNavigate(ActionServerNavigate::GoalHandle goal_handle) { - const mbf_msgs::NavigationGoal &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::NavigationResult result; - result.outcome = mbf_msgs::NavigationResult::INVALID_PLUGIN; + forklift_interfaces::NavigateResult result; + result.outcome = forklift_interfaces::NavigateResult::INVALID_PLUGIN; result.message = "No plugins loaded at all!"; - ROS_WARN_STREAM_NAMED("navigation", result.message); + ROS_WARN_STREAM_NAMED("navigate", result.message); goal_handle.setRejected(result, result.message); return; } if(!controller_plugin_manager_.hasPlugin(controller_name)) { - mbf_msgs::NavigationResult result; - result.outcome = mbf_msgs::NavigationResult::INVALID_PLUGIN; - result.message = "No plugin loaded with the given name \"" + goal.controller + "\"!"; - ROS_WARN_STREAM_NAMED("navigation", result.message); + forklift_interfaces::NavigateResult result; + result.outcome = forklift_interfaces::NavigateResult::INVALID_PLUGIN; + result.message = "No plugin loaded with the given name \"" + controller_name + "\"!"; + ROS_WARN_STREAM_NAMED("navigate", result.message); goal_handle.setRejected(result, result.message); return; } mbf_abstract_core::AbstractController::Ptr controller_plugin = controller_plugin_manager_.getPlugin(controller_name); - ROS_INFO_STREAM_NAMED("navigation", "Start action \"navigation\" 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) @@ -295,8 +229,8 @@ void AbstractNavigationServer::callActionNavigation(ActionServerNavigation::Goal } else { - mbf_msgs::ExePathResult result; - result.outcome = mbf_msgs::NavigationResult::FAILURE; + forklift_interfaces::NavigateResult result; + result.outcome = forklift_interfaces::NavigateResult::FAILURE; result.message = "Failure: \"controller_plugin\" pointer should not be a null pointer!"; ROS_FATAL_STREAM_NAMED("navigation", result.message); goal_handle.setRejected(result, result.message); @@ -304,7 +238,7 @@ void AbstractNavigationServer::callActionNavigation(ActionServerNavigation::Goal } -void AbstractNavigationServer::cancelActionNavigation(ActionServerNavigation::GoalHandle goal_handle) +void AbstractNavigationServer::cancelActionNavigate(ActionServerNavigate::GoalHandle goal_handle) { ROS_INFO_STREAM_NAMED("navigation", "Cancel action \"navigation\""); controller_action_.cancel(goal_handle); @@ -406,8 +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_navigation_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..4a7df70f 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,11 +75,16 @@ 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); + std::vector goal_path; + for(std::size_t it = 0; itpath.checkpoints.size(); it++) + { + goal_path[it] = goal_handle.getGoal()->path.checkpoints[it].pose; + } + execution_ptr->setNewPlan(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); + goal_pose_ = goal_handle.getGoal()->path.checkpoints.back().pose; + forklift_interfaces::NavigateResult result; + fillNavigateResult(forklift_interfaces::NavigateResult::CANCELED, "Goal preempted by a new plan", result); concurrency_slots_[slot].goal_handle.setCanceled(result, result.message); concurrency_slots_[slot].goal_handle = goal_handle; concurrency_slots_[slot].goal_handle.setAccepted(); @@ -116,19 +120,24 @@ 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(std::size_t it = 0; itpath.checkpoints.size(); it++) + { + goal_path[it] = goal_handle.getGoal()->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); + fillNavigateResult(forklift_interfaces::NavigateResult::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."); controller_active = false; @@ -139,7 +148,7 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution << 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,7 +168,7 @@ 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_mtx_.unlock(); @@ -185,12 +194,13 @@ 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); + fillNavigateResult(forklift_interfaces::NavigateResult::CANCELED, "Controller canceled", result); goal_handle.setCanceled(result, result.message); controller_active = false; break; @@ -217,42 +227,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); + fillNavigateResult(execution.getOutcome(), execution.getMessage(), result); goal_handle.setAborted(result, result.message); 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); + fillNavigateResult(forklift_interfaces::NavigateResult::PAT_EXCEEDED, execution.getMessage(), result); goal_handle.setAborted(result, result.message); 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); + fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started without a path", result); goal_handle.setAborted(result, result.message); 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); + fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started with an empty plan", result); goal_handle.setAborted(result, result.message); 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); + fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started with an invalid plan", result); goal_handle.setAborted(result, result.message); 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,25 +280,25 @@ 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); + fillNavigateResult(forklift_interfaces::NavigateResult::OSCILLATION, "Oscillation detected!", result); goal_handle.setAborted(result, result.message); 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); + fillNavigateResult(forklift_interfaces::NavigateResult::SUCCESS, "Controller succeeded; arrived at goal!", result); goal_handle.setSucceeded(result, result.message); 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); + fillNavigateResult(forklift_interfaces::NavigateResult::INTERNAL_ERROR, "Internal error: Unknown error thrown by the plugin!", result); goal_handle.setAborted(result, result.message); break; @@ -296,7 +306,7 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution 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); + fillNavigateResult(forklift_interfaces::NavigateResult::INTERNAL_ERROR, ss.str(), result); ROS_FATAL_STREAM_NAMED(name_, result.message); goal_handle.setAborted(result, result.message); controller_active = false; @@ -325,18 +335,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; + forklift_interfaces::NavigateFeedback feedback; feedback.outcome = outcome; feedback.message = 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,9 +354,9 @@ 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; diff --git a/mbf_abstract_nav/src/move_base_action.cpp b/mbf_abstract_nav/src/move_base_action.cpp index 26a7fab9..f70d0441 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_.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,15 +353,15 @@ 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; @@ -368,7 +372,7 @@ void MoveBaseAction::actionExePathDone( 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_) { @@ -383,11 +387,8 @@ void MoveBaseAction::actionExePathDone( case actionlib::SimpleClientGoalState::ABORTED: switch (result.outcome) { - 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,7 +398,7 @@ void MoveBaseAction::actionExePathDone( if (attemptRecovery()) { - recovery_trigger_ = EXE_PATH; + recovery_trigger_ = NAVIGATE; } else { @@ -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 8da47dc3..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( @@ -29,11 +30,11 @@ add_action_files( ExePath.action Recovery.action MoveBase.action - Navigate.action ) generate_messages( DEPENDENCIES + forklift_interfaces actionlib_msgs geometry_msgs nav_msgs @@ -44,6 +45,7 @@ catkin_package( CATKIN_DEPENDS actionlib_msgs geometry_msgs + forklift_interfaces message_runtime nav_msgs std_msgs diff --git a/mbf_msgs/action/Navigate.action b/mbf_msgs/action/Navigate.action deleted file mode 100644 index 34875847..00000000 --- a/mbf_msgs/action/Navigate.action +++ /dev/null @@ -1,5 +0,0 @@ -NavigatePath path # Path to follow ---- -NavigationResult result # Final result of navigation ---- -NavigationFeedback feedback # Continuous feedback regarding progress 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 From 68545ad4f9195be635bf4a7482fb3d8070f23a71 Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Fri, 29 Nov 2019 12:33:14 +0900 Subject: [PATCH 4/7] Fixed segfault issues related to concurrency and iterators after testing --- .../mbf_abstract_nav/abstract_action.h | 6 ++++-- mbf_abstract_nav/src/controller_action.cpp | 20 +++++++++---------- 2 files changed, 13 insertions(+), 13 deletions(-) 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..07cf1154 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,7 @@ class AbstractAction ) { uint8_t slot = goal_handle.getGoal()->concurrency_slot; - + ROS_INFO_STREAM("In parent action"); if(goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING) { goal_handle.setCanceled(); @@ -112,9 +112,11 @@ class AbstractAction virtual void runAndCleanUp(GoalHandle &goal_handle, typename Execution::Ptr execution_ptr){ uint8_t slot = goal_handle.getGoal()->concurrency_slot; - + ROS_INFO_STREAM("In run and cleanup"); execution_ptr->preRun(); + ROS_INFO_STREAM("Finished prerun and running"); run_(goal_handle, *execution_ptr); + ROS_INFO_STREAM("Finished run"); ROS_DEBUG_STREAM_NAMED(name_, "Finished action \"" << name_ << "\" run method, waiting for execution thread to finish."); execution_ptr->join(); ROS_DEBUG_STREAM_NAMED(name_, "Execution thread for action \"" << name_ << "\" stopped, cleaning up execution leftovers."); diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp index 4a7df70f..d31c1788 100644 --- a/mbf_abstract_nav/src/controller_action.cpp +++ b/mbf_abstract_nav/src/controller_action.cpp @@ -75,14 +75,15 @@ 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; + const forklift_interfaces::NavigateGoal &goal = *(goal_handle.getGoal().get()); std::vector goal_path; - for(std::size_t it = 0; itpath.checkpoints.size(); it++) + for(std::size_t it = 0; itpath.checkpoints[it].pose; + goal_path.push_back(goal.path.checkpoints[it].pose); } execution_ptr->setNewPlan(goal_path); // Update also goal pose, so the feedback remains consistent - goal_pose_ = goal_handle.getGoal()->path.checkpoints.back().pose; + 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.message); @@ -104,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 @@ -125,15 +125,14 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution typename AbstractControllerExecution::ControllerState state_moving_input; bool controller_active = true; - goal_mtx_.lock(); const forklift_interfaces::NavigateGoal &goal = *(goal_handle.getGoal().get()); - std::vector goal_path; - for(std::size_t it = 0; itpath.checkpoints.size(); it++) - { - goal_path[it] = goal_handle.getGoal()->path.checkpoints[it].pose; - } + 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()) { @@ -142,7 +141,6 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution ROS_ERROR_STREAM_NAMED(name_, result.message << " Canceling the action call."); controller_active = false; } - goal_pose_ = plan.back(); ROS_DEBUG_STREAM_NAMED(name_, "Called action \"" << name_ << "\" with plan:" << std::endl From f3d314aa3c062bbf44641de84e705f38d5cb242b Mon Sep 17 00:00:00 2001 From: Ayush Sharma Date: Fri, 29 Nov 2019 12:53:35 +0900 Subject: [PATCH 5/7] Remove some unneeded log info statements used during testing --- mbf_abstract_nav/include/mbf_abstract_nav/abstract_action.h | 4 ---- 1 file changed, 4 deletions(-) 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 07cf1154..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; - ROS_INFO_STREAM("In parent action"); if(goal_handle.getGoalStatus().status == actionlib_msgs::GoalStatus::RECALLING) { goal_handle.setCanceled(); @@ -112,11 +111,8 @@ class AbstractAction virtual void runAndCleanUp(GoalHandle &goal_handle, typename Execution::Ptr execution_ptr){ uint8_t slot = goal_handle.getGoal()->concurrency_slot; - ROS_INFO_STREAM("In run and cleanup"); execution_ptr->preRun(); - ROS_INFO_STREAM("Finished prerun and running"); run_(goal_handle, *execution_ptr); - ROS_INFO_STREAM("Finished run"); ROS_DEBUG_STREAM_NAMED(name_, "Finished action \"" << name_ << "\" run method, waiting for execution thread to finish."); execution_ptr->join(); ROS_DEBUG_STREAM_NAMED(name_, "Execution thread for action \"" << name_ << "\" stopped, cleaning up execution leftovers."); From 5a339865d3fd7643816ecc26ada9dd269e76851f Mon Sep 17 00:00:00 2001 From: dennyboby123 Date: Wed, 4 Dec 2019 18:25:40 +0900 Subject: [PATCH 6/7] fixed code to work with old message type --- .../src/abstract_navigation_server.cpp | 24 +++++------ mbf_abstract_nav/src/controller_action.cpp | 40 +++++++++---------- mbf_abstract_nav/src/move_base_action.cpp | 12 +++--- 3 files changed, 38 insertions(+), 38 deletions(-) diff --git a/mbf_abstract_nav/src/abstract_navigation_server.cpp b/mbf_abstract_nav/src/abstract_navigation_server.cpp index dbba01cb..adbc5e2c 100644 --- a/mbf_abstract_nav/src/abstract_navigation_server.cpp +++ b/mbf_abstract_nav/src/abstract_navigation_server.cpp @@ -198,20 +198,20 @@ void AbstractNavigationServer::callActionNavigate(ActionServerNavigate::GoalHand else { forklift_interfaces::NavigateResult result; - result.outcome = forklift_interfaces::NavigateResult::INVALID_PLUGIN; - result.message = "No plugins loaded at all!"; - ROS_WARN_STREAM_NAMED("navigate", result.message); - goal_handle.setRejected(result, result.message); + 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)) { forklift_interfaces::NavigateResult result; - result.outcome = forklift_interfaces::NavigateResult::INVALID_PLUGIN; - result.message = "No plugin loaded with the given name \"" + controller_name + "\"!"; - ROS_WARN_STREAM_NAMED("navigate", result.message); - goal_handle.setRejected(result, result.message); + 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; } @@ -230,10 +230,10 @@ void AbstractNavigationServer::callActionNavigate(ActionServerNavigate::GoalHand else { forklift_interfaces::NavigateResult result; - result.outcome = forklift_interfaces::NavigateResult::FAILURE; - result.message = "Failure: \"controller_plugin\" pointer should not be a null pointer!"; - ROS_FATAL_STREAM_NAMED("navigation", result.message); - goal_handle.setRejected(result, result.message); + 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); } } diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp index d31c1788..65a7db54 100644 --- a/mbf_abstract_nav/src/controller_action.cpp +++ b/mbf_abstract_nav/src/controller_action.cpp @@ -86,7 +86,7 @@ void ControllerAction::start( 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.message); + concurrency_slots_[slot].goal_handle.setCanceled(result, result.remarks); concurrency_slots_[slot].goal_handle = goal_handle; concurrency_slots_[slot].goal_handle.setAccepted(); } @@ -137,8 +137,8 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution if (plan.empty()) { fillNavigateResult(forklift_interfaces::NavigateResult::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."); + goal_handle.setAborted(result, result.remarks); + ROS_ERROR_STREAM_NAMED(name_, result.remarks << " Canceling the action call."); controller_active = false; } goal_pose_ = plan.back(); @@ -168,9 +168,9 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution controller_active = false; 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; } @@ -199,7 +199,7 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution case AbstractControllerExecution::CANCELED: ROS_INFO_STREAM("Action \"exe_path\" canceled"); fillNavigateResult(forklift_interfaces::NavigateResult::CANCELED, "Controller canceled", result); - goal_handle.setCanceled(result, result.message); + goal_handle.setCanceled(result, result.remarks); controller_active = false; break; @@ -226,35 +226,35 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution ROS_WARN_STREAM_NAMED(name_, "The controller has been aborted after it exceeded the maximum number of retries!"); controller_active = false; fillNavigateResult(execution.getOutcome(), execution.getMessage(), result); - goal_handle.setAborted(result, result.message); + 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; fillNavigateResult(forklift_interfaces::NavigateResult::PAT_EXCEEDED, execution.getMessage(), result); - goal_handle.setAborted(result, result.message); + 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; fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started without a path", result); - goal_handle.setAborted(result, result.message); + 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; fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started with an empty plan", result); - goal_handle.setAborted(result, result.message); + 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; fillNavigateResult(forklift_interfaces::NavigateResult::INVALID_PATH, "Controller started with an invalid plan", result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; case AbstractControllerExecution::NO_LOCAL_CMD: @@ -279,7 +279,7 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution execution.stop(); controller_active = false; fillNavigateResult(forklift_interfaces::NavigateResult::OSCILLATION, "Oscillation detected!", result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; } } @@ -290,14 +290,14 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution ROS_DEBUG_STREAM_NAMED(name_, "Controller succeeded; arrived at goal"); controller_active = false; fillNavigateResult(forklift_interfaces::NavigateResult::SUCCESS, "Controller succeeded; arrived at goal!", result); - goal_handle.setSucceeded(result, result.message); + 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; fillNavigateResult(forklift_interfaces::NavigateResult::INTERNAL_ERROR, "Internal error: Unknown error thrown by the plugin!", result); - goal_handle.setAborted(result, result.message); + goal_handle.setAborted(result, result.remarks); break; default: @@ -305,8 +305,8 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution ss << "Internal error: Unknown state in a move base flex controller execution with the number: " << static_cast(state_moving_input); fillNavigateResult(forklift_interfaces::NavigateResult::INTERNAL_ERROR, ss.str(), result); - ROS_FATAL_STREAM_NAMED(name_, result.message); - goal_handle.setAborted(result, result.message); + ROS_FATAL_STREAM_NAMED(name_, result.remarks); + goal_handle.setAborted(result, result.remarks); controller_active = false; } goal_mtx_.unlock(); @@ -339,8 +339,8 @@ void ControllerAction::publishNavigateFeedback( const geometry_msgs::TwistStamped& current_twist) { forklift_interfaces::NavigateFeedback feedback; - feedback.outcome = outcome; - feedback.message = message; + feedback.status = outcome; + feedback.remarks = message; feedback.velocity = current_twist; if (feedback.velocity.header.stamp.isZero()) @@ -356,8 +356,8 @@ void ControllerAction::fillNavigateResult( uint32_t outcome, const std::string &message, 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 f70d0441..2ebe6998 100644 --- a/mbf_abstract_nav/src/move_base_action.cpp +++ b/mbf_abstract_nav/src/move_base_action.cpp @@ -186,8 +186,8 @@ void MoveBaseAction::actionNavigateActive() 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; @@ -366,8 +366,8 @@ void MoveBaseAction::actionNavigateDone( 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; @@ -385,7 +385,7 @@ void MoveBaseAction::actionNavigateDone( break; case actionlib::SimpleClientGoalState::ABORTED: - switch (result.outcome) + switch (result.status) { case forklift_interfaces::NavigateResult::INVALID_PATH: case forklift_interfaces::NavigateResult::INVALID_PLUGIN: @@ -402,7 +402,7 @@ void MoveBaseAction::actionNavigateDone( } 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; From f557d893b5d55ed91694492c45049c955b58275e Mon Sep 17 00:00:00 2001 From: dennyboby123 Date: Wed, 4 Dec 2019 22:45:32 +0900 Subject: [PATCH 7/7] fixed comments --- mbf_abstract_nav/src/controller_action.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/mbf_abstract_nav/src/controller_action.cpp b/mbf_abstract_nav/src/controller_action.cpp index 65a7db54..19e14fb5 100644 --- a/mbf_abstract_nav/src/controller_action.cpp +++ b/mbf_abstract_nav/src/controller_action.cpp @@ -197,7 +197,7 @@ void ControllerAction::run(GoalHandle &goal_handle, AbstractControllerExecution break; case AbstractControllerExecution::CANCELED: - ROS_INFO_STREAM("Action \"exe_path\" canceled"); + ROS_INFO_STREAM("Action \"navigate_path\" canceled"); fillNavigateResult(forklift_interfaces::NavigateResult::CANCELED, "Controller canceled", result); goal_handle.setCanceled(result, result.remarks); controller_active = false;