Nav feature refactor

CMakeLists.txt

@@ -13,7 +13,6 @@ find_package(catkin REQUIRED COMPONENTS
   message_generation
   cmake_modules
   moveit_ros_planning_interface
-  move_base_msgs
   geometry_msgs
   std_msgs
   tf2
@@ -33,6 +32,7 @@ find_package(class_loader REQUIRED)
 
 add_message_files(FILES
   CustomFeedback.msg
+  NavigationFeedback.msg
 )
 
 add_service_files(FILES
@@ -48,8 +48,9 @@ add_service_files(FILES
   RobotGetTarget.srv
   RobotGetNamedTargets.srv
   RobotGetConfig.srv
-  RobotNavigationGoal.srv
+  NavigationGoal.srv
   RobotGripperControlPosition.srv
+  CancelNavigationGoal.srv
 )
 
 generate_messages(DEPENDENCIES
@@ -85,6 +86,7 @@ add_executable(temoto_robot_manager
   src/robot_features.cpp
   src/robot_common_procedures.cpp
   src/custom_plugin_helper.cpp
+  src/navigation_plugin_helper.cpp
 )
 add_dependencies(temoto_robot_manager ${catkin_EXPORTED_TARGETS} ${${PROJECT_NAME}_EXPORTED_TARGETS})
 target_link_libraries(temoto_robot_manager 

include/temoto_robot_manager/custom_datastructures.h

@@ -1,54 +1,22 @@
 #ifndef TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H
 #define TEMOTO_ROBOT_MANAGER__CUSTOM_DATASTRUCTURES_H
 
-#include <string>
-#include <vector>
+#include "temoto_robot_manager/rm_datastructures.h"
 
 namespace temoto_robot_manager
 {
 
 struct RmCustomRequest
 {
-  struct Header
-  {
-    std::string frame_id;
-    uint64_t timestamp;
-    uint64_t sequence_id;
-  };
-
-  struct Position
-  {
-    double x;
-    double y;
-    double z;
-  };
-
-  struct Orientation
-  {
-    double x;
-    double y;
-    double z;
-    double w;
-  };
-
-  struct Pose
-  {
-    Position position;
-    Orientation orientation;
-  }; 
-
-  struct PoseStamped
-  {
-    Header header;
-    Pose pose;
-  };
-
+  Header header;
+  Position position;  
+  Orientation orientation;
+  Pose pose;
+  PoseStamped poseStamped;
   std::string data_str;
   std::vector<std::string> data_str_array;
-
   double data_num;
   std::vector<double> data_num_array;
-
   PoseStamped data_pose;
   std::vector<PoseStamped> data_pose_array;
 };

include/temoto_robot_manager/navigation_datastructures.h

@@ -0,0 +1,23 @@
+#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_DATASTRUCTURES_H
+#define TEMOTO_ROBOT_MANAGER__NAVIGATION_DATASTRUCTURES_H
+
+#include "temoto_robot_manager/rm_datastructures.h"
+
+namespace temoto_robot_manager
+{
+
+struct RmNavigationGoal
+{
+  PoseStamped goal_pose;
+};
+
+struct RmNavigationFeedback
+{
+  uint8_t status;
+  double progress;
+  PoseStamped base_position;
+};
+
+} // temoto_robot_manager
+
+#endif

include/temoto_robot_manager/navigation_plugin_base.h

@@ -0,0 +1,23 @@
+#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_BASE_H
+#define TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_BASE_H
+
+#include "temoto_robot_manager/navigation_datastructures.h"
+#include <optional>
+
+namespace temoto_robot_manager
+{
+
+class NavigationPluginBase
+{
+public:
+  virtual bool initialize(const std::string& robot_ns) = 0;
+  virtual bool sendGoal(RmNavigationGoal goal) = 0;
+  virtual std::optional<RmNavigationFeedback> getFeedback() = 0;
+  virtual bool cancelGoal() = 0;
+  virtual bool deinitialize() = 0;
+  virtual ~NavigationPluginBase(){};
+};
+
+} // temoto_robot_manager
+
+#endif

include/temoto_robot_manager/navigation_plugin_helper.h

@@ -0,0 +1,69 @@
+#ifndef TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_HELPER_H
+#define TEMOTO_ROBOT_MANAGER__NAVIGATION_PLUGIN_HELPER_H
+
+#include "class_loader/class_loader.hpp"
+#include "temoto_robot_manager/navigation_plugin_base.h"
+#include <memory>
+#include <thread>
+
+namespace temoto_robot_manager
+{
+
+struct RmNavigationFeedbackWrap : RmNavigationFeedback
+{
+  std::string robot_name;
+  std::string request_id;
+};
+
+struct RmNavigationRequestWrap : RmNavigationGoal
+{
+  std::string robot_name;
+  std::string request_id;
+};
+
+class NavigationPluginHelper;
+
+typedef std::shared_ptr<NavigationPluginHelper> NavigationPluginHelperPtr;
+typedef std::function<void(const RmNavigationFeedbackWrap&)> NavigationFeatureUpdateCb;
+
+class NavigationPluginHelper
+{
+public:
+  enum class State
+  {
+    NOT_LOADED,
+    UNINITIALIZED,
+    INITIALIZED,
+    PROCESSING,
+    FINISHED,
+    STOPPING,
+    ERROR
+  };
+
+  NavigationPluginHelper(const std::string& plugin_path, const std::string& robot_ns, NavigationFeatureUpdateCb update_cb);
+  ~NavigationPluginHelper();
+  void initialize();
+  void sendGoal(const RmNavigationRequestWrap& request);
+  void sendUpdate() const;
+  void cancelGoal();
+  void deinitialize();
+  State getState() const;
+
+private:
+  void setState(State state);
+
+  std::shared_ptr<NavigationPluginBase> plugin;
+  std::shared_ptr<class_loader::ClassLoader> class_loader;
+  std::thread exec_thread_;
+  std::string plugin_path_;
+  std::string robot_ns_;
+  bool is_thread_running_ = false;
+
+  State state_;
+  mutable std::mutex mutex_state_;
+
+  std::optional<RmNavigationRequestWrap> current_request_;
+  NavigationFeatureUpdateCb update_cb_;
+};
+} // temoto_robot_manager namespace
+#endif

include/temoto_robot_manager/rm_datastructures.h

@@ -0,0 +1,46 @@
+#ifndef TEMOTO_ROBOT_MANAGER__RM_DATASTRUCTURES_H
+#define TEMOTO_ROBOT_MANAGER__RM_DATASTRUCTURES_H
+
+#include <string>
+#include <vector>
+
+namespace temoto_robot_manager
+{
+
+struct Header
+{
+  std::string frame_id;
+  uint64_t timestamp;
+  uint64_t sequence_id;
+};
+
+struct Position
+{
+  double x;
+  double y;
+  double z;
+};
+
+struct Orientation
+{
+  double x;
+  double y;
+  double z;
+  double w;
+};
+
+struct Pose
+{
+  Position position;
+  Orientation orientation;
+}; 
+
+struct PoseStamped
+{
+  Header header;
+  Pose pose;
+};
+
+} // temoto_robot_manager
+
+#endif

include/temoto_robot_manager/robot.h

@@ -24,9 +24,9 @@
 #include "temoto_robot_manager/robot_common_procedures.h"
 #include "temoto_robot_manager/GripperControl.h"
 #include "temoto_robot_manager/custom_plugin_helper.h"
+#include "temoto_robot_manager/navigation_plugin_helper.h"
 #include <moveit/move_group_interface/move_group_interface.h>
 #include <moveit/planning_interface/planning_interface.h>
-#include <move_base_msgs/MoveBaseAction.h>
 #include <geometry_msgs/PoseWithCovarianceStamped.h>
 #include <string>
 #include <map>
@@ -43,7 +43,8 @@ class Robot
   Robot(RobotConfigPtr config_
   , const std::string& resource_id
   , temoto_resource_registrar::ResourceRegistrarRos1& resource_registrar
-  , CustomFeatureUpdateCb custom_feature_update_cb);
+  , CustomFeatureUpdateCb custom_feature_update_cb
+  , NavigationFeatureUpdateCb navigation_feature_update_cb);
 
   virtual ~Robot();
   void load();
@@ -60,7 +61,8 @@ class Robot
   std::vector<double> getCurrentJointValues(const std::string& planning_group_name);
   std::vector<std::string> getNamedTargetPoses(const std::string& planning_group_name);
 
-  void goalNavigation(const geometry_msgs::PoseStamped& target_pose);
+  void goalNavigation(const RmNavigationRequestWrap& request);
+  void cancelNavigationGoal();
   void controlGripper(const std::string& robot_name, const float position);
 
   void invokeCustomFeature(const std::string& custom_feature_name, const RmCustomRequestWrap& request);
@@ -135,10 +137,15 @@ class Robot
   std::map<std::string, std::unique_ptr<moveit::planning_interface::MoveGroupInterface>> planning_groups_;
 
   // Navigation related
-  typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;
   ros::Subscriber localized_pose_sub_;
   geometry_msgs::PoseWithCovarianceStamped current_pose_navigation_;
-
+
+  NavigationPluginHelperPtr navigation_feature_plugin_;
+  mutable std::mutex navigation_feature_plugin_mutex_;
+  NavigationFeatureUpdateCb navigation_feature_update_cb_;
+  std::thread navigation_feature_feedback_thread_;
+  bool navigation_feature_feedback_thread_running_;
+
   // Custom related
   std::map<std::string, CustomPluginHelperPtr> custom_feature_plugins_;
   mutable std::mutex custom_feature_plugins_mutex_;

include/temoto_robot_manager/robot_features.h

@@ -240,12 +240,18 @@ class FeatureNavigation : public FeatureWithDriver
     return pose_topic_;
   }
 
+  const std::string& getControllerInterface() const
+  {
+    return controller_interface_;
+  }
+
 private:
   std::string global_planner_;
   std::string local_planner_;
   std::string odom_topic_;
   std::string cmd_vel_topic_;
   std::string pose_topic_;
+  std::string controller_interface_;
 };
 
 class FeatureGripper : public FeatureWithDriver

include/temoto_robot_manager/robot_manager.h

@@ -23,6 +23,7 @@
 #include "temoto_core/ConfigSync.h"
 #include "temoto_process_manager/process_manager_services.hpp"
 #include "temoto_robot_manager/custom_datastructures.h"
+// #include "temoto_robot_manager/navigation_datastructures.h"
 #include "temoto_robot_manager/robot_manager_services.h"
 #include "temoto_robot_manager/robot.h"
 #include "temoto_robot_manager/robot_config.h"
@@ -89,7 +90,7 @@ class RobotManager : public temoto_core::BaseSubsystem
 
   bool getManipulationNamedTargetsCb(RobotGetNamedTargets::Request& req, RobotGetNamedTargets::Response& res);
 
-  bool goalNavigationCb(RobotNavigationGoal::Request& req, RobotNavigationGoal::Response& res);
+  bool goalNavigationCb(NavigationGoal::Request& req, NavigationGoal::Response& res);
 
   bool gripperControlPositionCb(RobotGripperControlPosition::Request& req, RobotGripperControlPosition::Response& res);
 
@@ -109,6 +110,10 @@ class RobotManager : public temoto_core::BaseSubsystem
 
   void customFeatureUpdateCb(const RmCustomFeedbackWrap& feedback);
 
+  void navigationFeatureUpdateCb(const RmNavigationFeedbackWrap& feedback);
+
+  bool cancelNavigationGoalCb(CancelNavigationGoal::Request& req, CancelNavigationGoal::Response& res);
+
   RobotConfigs parseRobotConfigs(const YAML::Node& config); 
 
   RobotConfigPtr findRobot(const std::string& robot_name, const RobotConfigs& robot_infos);
@@ -142,6 +147,7 @@ class RobotManager : public temoto_core::BaseSubsystem
   ros::ServiceServer server_navigation_goal_;
   ros::ServiceServer server_gripper_control_position_;
   ros::ServiceServer server_get_robot_config_;
+  ros::ServiceServer server_cancel_navigation_goal_;
 
   ros::ServiceClient client_plan_;
   ros::ServiceClient client_exec_;
@@ -152,6 +158,13 @@ class RobotManager : public temoto_core::BaseSubsystem
   ros::ServiceClient client_set_mode_;
   ros::ServiceClient client_navigation_goal_;
   ros::ServiceClient client_gripper_control_position_;
+  ros::ServiceClient client_cancel_navigation_goal_;
+
+  std::map<std::string, NavigationGoal> ongoing_navigation_requests_;
+  std::mutex mutex_ongoing_navigation_requests_;
+
+  ros::Publisher pub_navigation_feature_feedback_;
+  std::mutex mutex_pub_navigation_feature_feedback_;
 
   /*
    * CUSTOM FEATURE