State machine

mbf_abstract_core/include/mbf_abstract_core/abstract_controller.h

@@ -45,6 +45,7 @@
 #include <boost/shared_ptr.hpp>
 #include <geometry_msgs/TwistStamped.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <forklift_interfaces/Checkpoint.h>
 
 namespace mbf_abstract_core{
 
@@ -106,6 +107,19 @@ namespace mbf_abstract_core{
        */
       virtual bool setPlan(const std::vector<geometry_msgs::PoseStamped> &plan) = 0;
 
+      /**
+       * @brief Set the plan that the local planner is following
+       * @param plan The plan to pass to the local planner
+       * @return True if the plan was updated successfully, false otherwise
+       */
+      virtual bool setPlan(const std::vector<forklift_interfaces::Checkpoint> &plan) { };
+
+      /**
+       * @brief Requests the planner to give feedback for the current plan
+       * @return Returns the id of last checkpoint covered and the checkpoint targeting
+       */
+      virtual std::pair<uint32_t, uint32_t> getFeedback() { };
+
       /**
        * @brief Requests the planner to cancel, e.g. if it takes too much time.
        * @return True if a cancel has been successfully requested, false if not implemented.

mbf_abstract_nav/CMakeLists.txt

@@ -5,15 +5,18 @@ find_package(catkin REQUIRED
   COMPONENTS
   actionlib
   actionlib_msgs
+  forklift_interfaces
   dynamic_reconfigure
   geometry_msgs
+  aifl_msg
   mbf_msgs
   mbf_abstract_core
   mbf_utility
   nav_msgs
   roscpp
   std_msgs
   std_srvs
+  rr_path_follower
   tf
   xmlrpcpp
   )
@@ -38,13 +41,16 @@ catkin_package(
       actionlib_msgs
       dynamic_reconfigure
       geometry_msgs
+      forklift_interfaces
+      aifl_msg
       mbf_msgs
       mbf_abstract_core
       mbf_utility
       nav_msgs
       roscpp
       std_msgs
       std_srvs
+      rr_path_follower
       tf
       xmlrpcpp
   DEPENDS Boost
@@ -62,7 +68,7 @@ add_library(${MBF_ABSTRACT_SERVER_LIB}
   src/controller_action.cpp
   src/planner_action.cpp
   src/recovery_action.cpp
-  src/move_base_action.cpp
+  src/navigate_action.cpp
   src/abstract_execution_base.cpp
   src/abstract_navigation_server.cpp
   src/abstract_planner_execution.cpp

mbf_abstract_nav/include/mbf_abstract_nav/abstract_controller_execution.h

@@ -48,6 +48,7 @@
 
 #include <tf/transform_listener.h>
 #include <geometry_msgs/PoseStamped.h>
+#include <forklift_interfaces/Checkpoint.h>
 #include <geometry_msgs/Twist.h>
 
 #include <mbf_utility/navigation_utility.h>
@@ -112,6 +113,17 @@ namespace mbf_abstract_nav
      */
     void setNewPlan(const std::vector<geometry_msgs::PoseStamped> &plan);
 
+    /**
+     * @brief Sets a new plan to the controller execution
+     * @param plan A vector of stamped poses.
+     */
+    void setNewPlan(const std::vector<forklift_interfaces::Checkpoint> &plan);
+
+    /**
+     * @brief Requests the planner to give feedback for the current plan
+     * @return Returns the id of last checkpoint covered and the checkpoint targeting
+     */
+    virtual std::pair<uint32_t, uint32_t> getFeedback();
     /**
      * @brief Cancel the planner execution. This calls the cancel method of the planner plugin. This could be useful if the
      * computation takes too much time.
@@ -294,13 +306,13 @@ namespace mbf_abstract_nav
      * @brief Gets the new available plan. This method is thread safe.
      * @return The plan
      */
-    std::vector<geometry_msgs::PoseStamped> getNewPlan();
+    std::vector<forklift_interfaces::Checkpoint> getNewPlan();
 
     //! the last calculated velocity command
     geometry_msgs::TwistStamped vel_cmd_stamped_;
 
     //! the last set plan which is currently processed by the controller
-    std::vector<geometry_msgs::PoseStamped> plan_;
+    std::vector<forklift_interfaces::Checkpoint> plan_;
 
     //! the duration which corresponds with the controller frequency.
     boost::chrono::microseconds calling_duration_;

mbf_abstract_nav/include/mbf_abstract_nav/abstract_execution_base.h

@@ -43,6 +43,7 @@
 #include <boost/thread.hpp>
 #include <boost/chrono/duration.hpp>
 #include <boost/chrono/thread_clock.hpp>
+#include <forklift_interfaces/Checkpoint.h>
 
 namespace mbf_abstract_nav
 {
@@ -92,6 +93,12 @@ class AbstractExecutionBase
    */
   virtual void postRun() { };
 
+    /**
+   * @brief Requests the planner to give feedback for the current plan
+   * @return Returns the id of last checkpoint covered and the checkpoint targeting
+   */
+  virtual std::pair<uint32_t, uint32_t> getFeedback() { };
+
 protected:
   virtual void run() = 0;
 

mbf_abstract_nav/include/mbf_abstract_nav/abstract_navigation_server.h

@@ -62,7 +62,9 @@
 #include "mbf_abstract_nav/planner_action.h"
 #include "mbf_abstract_nav/controller_action.h"
 #include "mbf_abstract_nav/recovery_action.h"
-#include "mbf_abstract_nav/move_base_action.h"
+#include "mbf_abstract_nav/navigate_action.h"
+
+
 
 #include "mbf_abstract_nav/MoveBaseFlexConfig.h"
 
@@ -91,18 +93,20 @@ typedef boost::shared_ptr<ActionServerExePath> ActionServerExePathPtr;
 typedef actionlib::ActionServer<mbf_msgs::RecoveryAction> ActionServerRecovery;
 typedef boost::shared_ptr<ActionServerRecovery> ActionServerRecoveryPtr;
 
-//! MoveBase action server
-typedef actionlib::ActionServer<mbf_msgs::MoveBaseAction> ActionServerMoveBase;
-typedef boost::shared_ptr<ActionServerMoveBase> ActionServerMoveBasePtr;
+//! Navigate action server
+typedef actionlib::ActionServer<forklift_interfaces::NavigateAction> ActionServerNavigate;
+typedef boost::shared_ptr<ActionServerNavigate> ActionServerNavigatePtr;
+
 
 //! ExePath action topic name
 const std::string name_action_exe_path = "exe_path";
 //! GetPath action topic name
 const std::string name_action_get_path = "get_path";
 //! Recovery action topic name
 const std::string name_action_recovery = "recovery";
-//! MoveBase action topic name
-const std::string name_action_move_base = "move_base";
+//! Navigate action topic name
+const std::string name_action_navigate = "navigate";
+
 
 
 typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBaseFlexConfig> > DynamicReconfigureServer;
@@ -255,9 +259,9 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
      * @param goal SimpleActionServer goal containing all necessary parameters for the action execution. See the action
      *        definitions in mbf_msgs.
      */
-    virtual void callActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle);
+    virtual void callActionNavigate(ActionServerNavigate::GoalHandle goal_handle);
 
-    virtual void cancelActionMoveBase(ActionServerMoveBase::GoalHandle goal_handle);
+    virtual void cancelActionNavigate(ActionServerNavigate::GoalHandle goal_handle);
 
     /**
      * @brief starts all action server.
@@ -318,7 +322,8 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
     ActionServerGetPathPtr action_server_get_path_ptr_;
 
     //! shared pointer to the MoveBase action server
-    ActionServerMoveBasePtr action_server_move_base_ptr_;
+    ActionServerNavigatePtr action_server_navigate_ptr_;
+
 
     //! dynamic reconfigure server
     DynamicReconfigureServer dsrv_;
@@ -376,7 +381,7 @@ typedef boost::shared_ptr<dynamic_reconfigure::Server<mbf_abstract_nav::MoveBase
     ControllerAction controller_action_;
     PlannerAction planner_action_;
     RecoveryAction recovery_action_;
-    MoveBaseAction move_base_action_;
+    NavigateAction navigate_action_;
   };
 
 } /* namespace mbf_abstract_nav */

mbf_abstract_nav/include/mbf_abstract_nav/controller_action.h

@@ -76,7 +76,8 @@ class ControllerAction :
   void publishExePathFeedback(
           GoalHandle& goal_handle,
           uint32_t outcome, const std::string &message,
-          const geometry_msgs::TwistStamped& current_twist);
+          const geometry_msgs::TwistStamped& current_twist,
+          const std::pair<uint32_t, uint32_t>& checkpoint_feedback);
 
   /**
    * @brief Utility method to fill the ExePath action result in a single line

mbf_abstract_nav/include/mbf_abstract_nav/navigate_action.h

@@ -0,0 +1,179 @@
+/*
+ *  Copyright 2018, Magazino GmbH, Sebastian Pütz, Jorge Santos Simón
+ *
+ *  Redistribution and use in source and binary forms, with or without
+ *  modification, are permitted provided that the following conditions
+ *  are met:
+ *
+ *  1. Redistributions of source code must retain the above copyright
+ *     notice, this list of conditions and the following disclaimer.
+ *
+ *  2. Redistributions in binary form must reproduce the above
+ *     copyright notice, this list of conditions and the following
+ *     disclaimer in the documentation and/or other materials provided
+ *     with the distribution.
+ *
+ *  3. Neither the name of the copyright holder nor the names of its
+ *     contributors may be used to endorse or promote products derived
+ *     from this software without specific prior written permission.
+ *
+ *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ *  COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ *  POSSIBILITY OF SUCH DAMAGE.
+ *
+ *  move_base_action.h
+ *
+ *  authors:
+ *    Sebastian Pütz <spuetz@uni-osnabrueck.de>
+ *    Jorge Santos Simón <santos@magazino.eu>
+ *
+ */
+#ifndef MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_
+#define MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_
+
+#include <mutex>
+
+#include <actionlib/server/action_server.h>
+#include <actionlib/client/simple_action_client.h>
+
+#include <mbf_msgs/GetPathAction.h>
+#include <mbf_msgs/ExePathAction.h>
+#include <mbf_msgs/RecoveryAction.h>
+#include <forklift_interfaces/NavigateAction.h>
+#include <forklift_interfaces/Checkpoint.h>
+#include <aifl_msg/SpinTurnAction.h>
+
+#include "mbf_abstract_nav/MoveBaseFlexConfig.h"
+#include "mbf_abstract_nav/robot_information.h"
+
+
+namespace mbf_abstract_nav
+{
+
+class NavigateAction
+{
+ public:
+
+  //! Action clients for the Navigate action
+  typedef actionlib::SimpleActionClient<mbf_msgs::ExePathAction> ActionClientExePath;
+  typedef actionlib::SimpleActionClient<aifl_msg::SpinTurnAction> ActionClientSpinTurn;
+
+
+  typedef actionlib::ActionServer<forklift_interfaces::NavigateAction>::GoalHandle GoalHandle;
+
+  NavigateAction(const std::string &name, const RobotInformation &robot_info);
+
+  ~NavigateAction();
+
+  void start(GoalHandle &goal_handle);
+
+  void cancel();
+
+  void reconfigure(
+      mbf_abstract_nav::MoveBaseFlexConfig &config, uint32_t level);
+
+ protected:
+
+  void actionExePathFeedback(const mbf_msgs::ExePathFeedbackConstPtr &feedback);
+
+  bool isSmoothTurnPossible(const forklift_interfaces::Checkpoint& previous, 
+    const forklift_interfaces::Checkpoint& current, const forklift_interfaces::Checkpoint& next);
+
+  bool getSplitPath(
+      const forklift_interfaces::NavigatePath &plan,
+      std::vector<forklift_interfaces::NavigatePath> &result);
+
+  void actionExePathActive();
+
+  void actionSpinTurnActive();
+
+  void startNavigate();
+
+  void runNavigate();
+
+  void actionExePathDone(
+      const actionlib::SimpleClientGoalState &state,
+      const mbf_msgs::ExePathResultConstPtr &result);
+
+  void actionGetPathReplanningDone(
+      const actionlib::SimpleClientGoalState &state,
+      const mbf_msgs::GetPathResultConstPtr &result);
+
+  void actionSpinTurnFeedback(const aifl_msg::SpinTurnFeedbackConstPtr &feedback);
+
+  void actionSpinTurnDone(
+      const actionlib::SimpleClientGoalState &state,
+      const aifl_msg::SpinTurnResultConstPtr &result);
+
+  double getSpinAngle(geometry_msgs::Quaternion orientation);
+
+
+
+  mbf_msgs::ExePathGoal exe_path_goal_;
+  forklift_interfaces::NavigatePath get_path_goal_;
+  aifl_msg::SpinTurnGoal spin_turn_goal_;
+
+
+  geometry_msgs::PoseStamped last_oscillation_pose_;
+  ros::Time last_oscillation_reset_;
+
+  ros::Duration oscillation_timeout_;
+
+  std::vector<forklift_interfaces::NavigatePath> path_segments_;
+
+  double oscillation_distance_;
+
+  GoalHandle goal_handle_;
+
+  std::string name_;
+
+  RobotInformation robot_info_;
+
+  geometry_msgs::PoseStamped robot_pose_;
+
+  ros::NodeHandle private_nh_;
+
+  //! Action client used by the navigate action
+  ActionClientExePath action_client_exe_path_;
+
+  //! Action client used by the navigate action
+  ActionClientSpinTurn action_client_spin_turn_;
+
+  bool replanning_;
+  ros::Rate replanning_rate_;
+  boost::mutex replanning_mtx_;
+
+
+  forklift_interfaces::NavigateFeedback navigate_feedback_;
+
+
+  enum NavigateActionState
+  {
+    ACTIVE,
+    IDLE,
+    SPLIT_PATH,
+    NAVIGATE,
+    EXE_PATH,
+    SPIN_TURN,
+    OSCILLATING,
+    SUCCEEDED,
+    CANCELED,
+    FAILED
+  };
+
+  NavigateActionState action_state_;
+  std::mutex action_mutex_;
+};
+
+} /* mbf_abstract_nav */
+
+#endif //MBF_ABSTRACT_NAV__MOVE_BASE_ACTION_H_