Added new Navigate action and added methods to call the Navigate action

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

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.");

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<mbf_msgs::GetPathAction> ActionServerGetPath;
 typedef boost::shared_ptr<ActionServerGetPath> ActionServerGetPathPtr;
 
-//! ExePath action server
-typedef actionlib::ActionServer<mbf_msgs::ExePathAction> ActionServerExePath;
-typedef boost::shared_ptr<ActionServerExePath> ActionServerExePathPtr;
+//! Navigation action server
+typedef actionlib::ActionServer<forklift_interfaces::NavigateAction> ActionServerNavigate;
+typedef boost::shared_ptr<ActionServerNavigate> ActionServerNavigatePtr;
 
 //! Recovery action server
 typedef actionlib::ActionServer<mbf_msgs::RecoveryAction> ActionServerRecovery;
@@ -95,8 +97,8 @@ typedef boost::shared_ptr<ActionServerRecovery> ActionServerRecoveryPtr;
 typedef actionlib::ActionServer<mbf_msgs::MoveBaseAction> ActionServerMoveBase;
 typedef boost::shared_ptr<ActionServerMoveBase> 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<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
  * @brief The AbstractNavigationServer is the abstract base class for all navigation servers in move_base_flex
  *        and bundles the @ref controller_execution "controller execution classes",the @ref planner_execution
  *        "planner execution classes" and the @ref recovery_execution "recovery execution classes". It provides
- *        the following action servers ActionServerGetPath -> 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<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
 
     virtual void cancelActionGetPath(ActionServerGetPath::GoalHandle goal_handle);
 
-    /**
-     * @brief ExePath action execution method. This method will be called if the action server receives a goal
+
+      /**
+     * @brief Navigate action execution method. This method will be called if the action server receives a goal
      * @param goal SimpleActionServer goal containing all necessary parameters for the action execution. See the action
      *        definitions in mbf_msgs.
      */
-    virtual void callActionExePath(ActionServerExePath::GoalHandle goal_handle);
+    virtual void callActionNavigate(ActionServerNavigate::GoalHandle goal_handle);
 
-    virtual void cancelActionExePath(ActionServerExePath::GoalHandle goal_handle);
+    virtual void cancelActionNavigate(ActionServerNavigate::GoalHandle goal_handle);
 
     /**
      * @brief Recovery action execution method. This method will be called if the action server receives a goal
@@ -311,8 +314,8 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
     //! shared pointer to the Recovery action server
     ActionServerRecoveryPtr action_server_recovery_ptr_;
 
-    //! shared pointer to the ExePath action server
-    ActionServerExePathPtr action_server_exe_path_ptr_;
+    //! shared pointer to the Navigation action server
+    ActionServerNavigatePtr action_server_navigate_ptr_;
 
     //! shared pointer to the GetPath action server
     ActionServerGetPathPtr action_server_get_path_ptr_;

mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h

@@ -45,19 +45,18 @@
 #include "mbf_abstract_nav/abstract_controller_execution.h"
 #include "mbf_abstract_nav/robot_information.h"
 #include <actionlib/server/action_server.h>
-#include <mbf_msgs/ExePathAction.h>
+#include <forklift_interfaces/NavigateAction.h>
 
 namespace mbf_abstract_nav{
 
 class ControllerAction :
-    public AbstractAction<mbf_msgs::ExePathAction, AbstractControllerExecution>
+    public AbstractAction<forklift_interfaces::NavigateAction, AbstractControllerExecution>
 {
  public:
 
   typedef boost::shared_ptr<ControllerAction> 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

mbf_abstract_nav/include/mbf_abstract_nav/move_base_action.h

@@ -45,7 +45,7 @@
 
 #include <mbf_msgs/MoveBaseAction.h>
 #include <mbf_msgs/GetPathAction.h>
-#include <mbf_msgs/ExePathAction.h>
+#include <forklift_interfaces/NavigateAction.h>
 #include <mbf_msgs/RecoveryAction.h>
 
 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
@@ -61,7 +61,7 @@ class MoveBaseAction
 
   //! Action clients for the MoveBase action
   typedef actionlib::SimpleActionClient<mbf_msgs::GetPathAction> ActionClientGetPath;
-  typedef actionlib::SimpleActionClient<mbf_msgs::ExePathAction> ActionClientExePath;
+  typedef actionlib::SimpleActionClient<forklift_interfaces::NavigateAction> ActionClientNavigate;
   typedef actionlib::SimpleActionClient<mbf_msgs::RecoveryAction> ActionClientRecovery;
 
   typedef actionlib::ActionServer<mbf_msgs::MoveBaseAction>::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,

mbf_abstract_nav/package.xml

@@ -17,6 +17,7 @@
     <build_depend>actionlib</build_depend>
     <build_depend>actionlib_msgs</build_depend>
     <build_depend>dynamic_reconfigure</build_depend>
+    <build_depend>forklift_interfaces</build_depend>
     <build_depend>std_msgs</build_depend>
     <build_depend>std_srvs</build_depend>
     <build_depend>nav_msgs</build_depend>
@@ -39,6 +40,7 @@
     <run_depend>mbf_msgs</run_depend>
     <run_depend>mbf_utility</run_depend>
     <run_depend>xmlrpcpp</run_depend>
+    <run_depend>forklift_interfaces</run_depend>
 
     <export>
       <rosdoc config="rosdoc.yaml" />

mbf_abstract_nav/src/abstract_navigation_server.cpp

@@ -60,7 +60,7 @@ AbstractNavigationServer::AbstractNavigationServer(const TFPtr &tf_listener_ptr)
       global_frame_(private_nh_.param<std::string>("global_frame", "map")),
       robot_frame_(private_nh_.param<std::string>("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();
 }