feat: improved out and in states

src/pdf_beamtime/CMakeLists.txt

@@ -26,7 +26,7 @@ set(dependencies
 include_directories(include)  # This makes sure the libraries are visible
 
 add_executable(pdf_beamtime_server 
-  src/pdf_beamtime_server_tests.cpp 
+  src/pdf_beamtime_server.cpp 
   src/inner_state_machine.cpp )
 
 ament_target_dependencies(pdf_beamtime_server ${dependencies})

src/pdf_beamtime/include/pdf_beamtime/inner_state_machine.hpp

@@ -20,11 +20,9 @@ class InnerStateMachine
   /// @brief true if this outer state machine works on this particular state, false otherwise
   bool state_active_ = false;
 
-  // moveit::planning_interface::MoveGroupInterface move_group_interface_;
-
   std::vector<std::string> external_state_names_ =
-  {"HOME", "PICKUP_APPROACH", "PICKUP", "GRASP_SUCCESS", "PICKUP_RETREAT",
-    "PLACE_APPROACH", "PLACE", "RELEASE_SUCCESS", "PLACE_RETREAT"};
+  {"HOME", "PICKUP_APPROACH", "PICKUP", "GRASP_SUCCESS", "GRASP_FAILURE", "PICKUP_RETREAT",
+    "PLACE_APPROACH", "PLACE", "RELEASE_SUCCESS", "RELEASE_FAILURE", "PLACE_RETREAT"};
 
   std::vector<std::string> internal_state_names =
   {"RESTING", "MOVING", "PAUSED", "ABORT", "HALT", "STOP"};
@@ -35,29 +33,26 @@ class InnerStateMachine
   /// @brief move the robot to the passed joint angles
   /// @param mgi move_group_interface_
   /// @return next outer state
-  State move_robot(moveit::planning_interface::MoveGroupInterface & mgi);
+  moveit::core::MoveItErrorCode move_robot(
+    moveit::planning_interface::MoveGroupInterface & mgi,
+    std::vector<double> joint_goal);
 
   /// @brief state change if paused command was issues
-  void pause();
-  State resume(moveit::planning_interface::MoveGroupInterface & mgi);
+  void pause(moveit::planning_interface::MoveGroupInterface & mgi);
+  // State resume(moveit::planning_interface::MoveGroupInterface & mgi);
   /// @brief TODO: rewind the state back to RESTING
   void rewind();
-  State abort();
-  State halt();
+  void abort();
+  void halt();
   void stop();
 
-  State open_gripper();
-
-  State close_gripper();
+  void set_internal_state(Internal_State state);
+  Internal_State get_internal_state();
 
-  /// @brief sets the joint target in a variable
-  /// @param joint_goal joint target
-  void set_joint_goal(std::vector<double> joint_goal);
+  void set_external_state(State state);
 
-  /// @brief setters and getter for the active state
-  void set_active_true();
-  void set_active_false();
-  bool is_active();
+  moveit::core::MoveItErrorCode open_gripper();
+  moveit::core::MoveItErrorCode close_gripper();
 
   ~InnerStateMachine();
 };

src/pdf_beamtime/include/pdf_beamtime/pdf_beamtime_server.hpp

@@ -59,12 +59,18 @@ class PdfBeamtimeServer
   std::shared_ptr<rclcpp::ParameterEventHandler> param_subscriber_;
   std::shared_ptr<rclcpp::ParameterCallbackHandle> cb_handle_;
 
-  const int num_of_states = 9;
-  InnerStateMachine * state_holder_[9];
+  InnerStateMachine * inner_state_machine_;
 
-  std::vector<std::string> state_names_ =
-  {"HOME", "PICKUP_APPROACH", "PICKUP", "GRASP_SUCCESS", "PICKUP_RETREAT",
-    "PLACE_APPROACH", "PLACE", "RELEASE_SUCCESS", "PLACE_RETREAT"};
+  std::vector<double, std::allocator<double>> goal_home_;
+
+  int paused_ = 0;
+
+  std::vector<std::string> external_state_names_ =
+  {"HOME", "PICKUP_APPROACH", "PICKUP", "GRASP_SUCCESS", "GRASP_FAILURE", "PICKUP_RETREAT",
+    "PLACE_APPROACH", "PLACE", "RELEASE_SUCCESS", "RELEASE_FAILURE", "PLACE_RETREAT"};
+
+  std::vector<std::string> internal_state_names =
+  {"RESTING", "MOVING", "PAUSED", "ABORT", "HALT", "STOP"};
 
   /// @brief current state of the robot
   State current_state_;
@@ -118,14 +124,16 @@ class PdfBeamtimeServer
   float get_action_completion_percentage();
 
   /// @brief Performs the transitions for each State
-  bool run_fsm(
+  moveit::core::MoveItErrorCode run_fsm(
     std::shared_ptr<const pdf_beamtime_interfaces::action::PickPlaceControlMsg_Goal> goal);
 
   /// @brief Set the current state to HOME and move robot to home position
-  bool reset_fsm(std::vector<double> joint_goal);
+  bool reset_fsm();
 
-  /// @brief use move_group_interface to set joint targets
-  bool set_joint_goal(std::vector<double> joint_goal);
+  void handle_pause();
+  void handle_stop();
+  void handle_abort();
+  void handle_resume();
 };
 
 #endif  // PDF_BEAMTIME__PDF_BEAMTIME_SERVER_HPP_

src/pdf_beamtime/include/pdf_beamtime/state_enum.hpp

@@ -1,8 +1,8 @@
 #pragma once
 
 /// @brief States of the robot transitions
-enum class State {HOME, PICKUP_APPROACH, PICKUP, GRASP_SUCCESS, PICKUP_RETREAT,
-  PLACE_APPROACH, PLACE, RELEASE_SUCCESS, PLACE_RETREAT};
+enum class State {HOME, PICKUP_APPROACH, PICKUP, GRASP_SUCCESS, GRASP_FAILURE, PICKUP_RETREAT,
+  PLACE_APPROACH, PLACE, RELEASE_SUCCESS, RELEASE_FAILURE, PLACE_RETREAT};
 
 /// @brief Internal states of the state machine
 enum class Internal_State {RESTING, MOVING, PAUSED, ABORT, HALT, STOP};

src/pdf_beamtime/src/inner_state_machine.cpp

@@ -5,164 +5,131 @@ InnerStateMachine::InnerStateMachine(
 : node_(node), external_state_enum_(external_state_enum)
 {
   internal_state_enum_ = Internal_State::RESTING;
-
 }
 
 InnerStateMachine::~InnerStateMachine()
 {
 }
 
-void InnerStateMachine::set_joint_goal(std::vector<double> joint_goal)
+moveit::core::MoveItErrorCode InnerStateMachine::move_robot(
+  moveit::planning_interface::MoveGroupInterface & mgi, std::vector<double> joint_goal)
 {
+  moveit::core::MoveItErrorCode return_error_code = moveit::core::MoveItErrorCode::FAILURE;
   joint_goal_ = joint_goal;
-}
-
-State InnerStateMachine::move_robot(
-  moveit::planning_interface::MoveGroupInterface & mgi)
-{
-
-  if (internal_state_enum_ == Internal_State::RESTING) {
-    RCLCPP_INFO(
-      node_->get_logger(), "[%s] Robot's current internal state is %s ",
-      external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
-      internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
 
-    mgi.setJointValueTarget(joint_goal_);
+  switch (internal_state_enum_) {
+    case Internal_State::RESTING: {
+        mgi.setJointValueTarget(joint_goal_);
+        // Create a plan to that target pose
+        auto const [planing_success, plan] = [&mgi] {
+            moveit::planning_interface::MoveGroupInterface::Plan msg;
+            auto const ok = static_cast<bool>(mgi.plan(msg));
+            return std::make_pair(ok, msg);
+          }();
+        if (planing_success) {
+          // Change inner state to Moving if the robot is ready to move
+          set_internal_state(Internal_State::MOVING);
+          auto exec_results = mgi.execute(plan);
+          return_error_code = exec_results;
+        } else {
+          return_error_code = moveit::core::MoveItErrorCode::FAILURE;
+        }
+      }
+      break;
 
-    // Create a plan to that target pose
-    auto const [success, plan] = [&mgi] {
-        moveit::planning_interface::MoveGroupInterface::Plan msg;
-        auto const ok = static_cast<bool>(mgi.plan(msg));
-        return std::make_pair(ok, msg);
-      }();
+    case Internal_State::PAUSED:
+      // Handle resume here
+      break;
 
-    if (success) {
-      // Change inner state to Moving if the robot is ready to move
-      internal_state_enum_ = Internal_State::MOVING;
-      RCLCPP_INFO(
-        node_->get_logger(), "[%s] Robot's current internal state is %s ",
-        external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
-        internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
-      auto exec_results = mgi.execute(plan);
-
-      if (exec_results == moveit::core::MoveItErrorCode::SUCCESS) {
-        // Change inner state to Resting upon successfully moving the robot
-        internal_state_enum_ = Internal_State::RESTING;
-        return static_cast<State>(static_cast<int>(external_state_enum_) + 1);
-      } else {
-        // Change inner state to Stop if moving failed or Paused
-        internal_state_enum_ = Internal_State::STOP;
-        return external_state_enum_;
-      }
-    } else {
+    default:
       RCLCPP_ERROR(
-        node_->get_logger(), "Inner State %s : Planning failed",
+        node_->get_logger(), "Robot's current internal state is %s ",
         internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
-      return external_state_enum_;
-
-    }
-  } else {
-    RCLCPP_ERROR(
-      node_->get_logger(), "Robot's current internal state is %s ",
-      internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
-    if (internal_state_enum_ == Internal_State::STOP) {
-      // Handle the rewind here
-    }
-    return external_state_enum_;
+      return_error_code = moveit::core::MoveItErrorCode::FAILURE;
+      break;
   }
 
+  return return_error_code;
 }
 
-State InnerStateMachine::close_gripper()
+moveit::core::MoveItErrorCode InnerStateMachine::close_gripper()
 {
-  return static_cast<State>(static_cast<int>(external_state_enum_) + 1);
+  return moveit::core::MoveItErrorCode::SUCCESS;
 }
 
-State InnerStateMachine::open_gripper()
+moveit::core::MoveItErrorCode InnerStateMachine::open_gripper()
 {
-  return static_cast<State>(static_cast<int>(external_state_enum_) + 1);
+  return moveit::core::MoveItErrorCode::SUCCESS;
 }
 
-
-void InnerStateMachine::pause()
+void InnerStateMachine::pause(moveit::planning_interface::MoveGroupInterface & mgi)
 {
-  if (internal_state_enum_ == Internal_State::RESTING) {
-    RCLCPP_INFO(
-      node_->get_logger(), "[%s] Paused while at internal state %s ",
-      external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
-      internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
-    internal_state_enum_ = Internal_State::PAUSED;
-  }
+  switch (internal_state_enum_) {
+    case Internal_State::RESTING:
+      RCLCPP_INFO(
+        node_->get_logger(), "[%s] Paused while at internal state %s ",
+        external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
+        internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
+      set_internal_state(Internal_State::PAUSED);
+      break;
 
-  if (internal_state_enum_ == Internal_State::MOVING) {
-    RCLCPP_INFO(
-      node_->get_logger(), "[%s] Paused while at internal state %s ",
-      external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
-      internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
-    internal_state_enum_ = Internal_State::PAUSED;
+    case Internal_State::MOVING:
+      mgi.stop();
+      RCLCPP_INFO(
+        node_->get_logger(), "[%s] Paused while at internal state %s ",
+        external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
+        internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
+      set_internal_state(Internal_State::PAUSED);
+      break;
+    default:
+      break;
   }
 }
 
-State InnerStateMachine::abort()
+void InnerStateMachine::abort()
 {
-  if (internal_state_enum_ == Internal_State::PAUSED) {
-    internal_state_enum_ = Internal_State::ABORT;
-    RCLCPP_INFO(
-      node_->get_logger(), "[%s] Abort while at internal state %s ",
-      external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
-      internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
-  }
-  return State::HOME;
-
+  set_internal_state(Internal_State::ABORT);
 }
 
-State InnerStateMachine::halt()
+void InnerStateMachine::halt()
 {
-  if (internal_state_enum_ == Internal_State::PAUSED) {
-    internal_state_enum_ = Internal_State::HALT;
-    RCLCPP_INFO(
-      node_->get_logger(), "[%s] Halt while at internal state %s ",
-      external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
-      internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
-  }
-  return State::HOME;
+  set_internal_state(Internal_State::HALT);
 }
 
 void InnerStateMachine::stop()
 {
-  if (internal_state_enum_ == Internal_State::PAUSED) {
-    RCLCPP_INFO(
-      node_->get_logger(), "[%s] Stop while at internal state %s ",
-      external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
-      internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
-    internal_state_enum_ = Internal_State::STOP;
-  }
+  set_internal_state(Internal_State::STOP);
 
 }
 
-State InnerStateMachine::resume(moveit::planning_interface::MoveGroupInterface & mgi)
+void InnerStateMachine::rewind()
 {
   if (internal_state_enum_ == Internal_State::PAUSED) {
-    internal_state_enum_ = Internal_State::MOVING;
-    RCLCPP_INFO(
-      node_->get_logger(), "[%s] Resume while at internal state %s ",
-      external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
-      internal_state_names[static_cast<int>(internal_state_enum_)].c_str());
+    set_internal_state(Internal_State::RESTING);
   }
 
-  return move_robot(mgi);
 }
 
-void InnerStateMachine::rewind()
+void InnerStateMachine::set_internal_state(Internal_State state)
 {
-  if (internal_state_enum_ == Internal_State::PAUSED) {
-    internal_state_enum_ = Internal_State::RESTING;
-  }
-
+  RCLCPP_INFO(
+    node_->get_logger(), "[%s]: Internal state changed from %s to %s ",
+    external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
+    internal_state_names[static_cast<int>(internal_state_enum_)].c_str(),
+    internal_state_names[static_cast<int>(state)].c_str());
+  internal_state_enum_ = state;
 }
 
-void InnerStateMachine::set_active_true() {state_active_ = true;}
-
-void InnerStateMachine::set_active_false() {state_active_ = false;}
+void InnerStateMachine::set_external_state(State state)
+{
+  RCLCPP_INFO(
+    node_->get_logger(), "[%s]: Internal state changed to %s ",
+    external_state_names_[static_cast<int>(external_state_enum_)].c_str(),
+    external_state_names_[static_cast<int>(state)].c_str());
+  external_state_enum_ = state;
+}
 
-bool InnerStateMachine::is_active() {return state_active_;}
+Internal_State InnerStateMachine::get_internal_state()
+{
+  return internal_state_enum_;
+}