Fully working on the Gripper example.

Author: destogl

gpio_controllers/CMakeLists.txt

@@ -105,6 +105,17 @@ if(BUILD_TESTING)
     ros2_control_test_assets::ros2_control_test_assets
   )
 
+  ament_add_gmock(
+    test_load_gpio_tool_controller
+    test/test_load_gpio_tool_controller.cpp
+  )
+  target_include_directories(test_load_gpio_tool_controller PRIVATE include)
+  target_link_libraries(test_load_gpio_tool_controller
+    gpio_tool_controllers
+    controller_manager::controller_manager
+    ros2_control_test_assets::ros2_control_test_assets
+  )
+
   # ament_add_gmock(test_gpio_tool_controller
   #   test/gpio_tool_controller/test_gpio_tool_controller.cpp
   #   test/gpio_tool_controller/test_gpio_tool_controller_open.cpp

gpio_controllers/doc/userdoc.rst

@@ -57,3 +57,26 @@ The controller expects at least one GPIO interface and the corresponding command
               - ana.2
 
 With the above-defined controller configuration, the controller will accept commands for all gpios' interfaces and will only publish the state of Gpio2.
+
+
+
+### gpio_tool_controller
+
+The controller enables control of tools connected to the robot or components of a robot via general-purpose inputs and outputs (GPIO), digital or analog.
+Concretely the controller is made for controlling the custom-made industrial grippers or tools, or components like lifts or breaks.
+The IOs might even be virtual, like controlling the state machine of a robot that is implemented on a PLC.
+
+The controller provides high-level interface for engaging or disengaging a tool or a component, as well as for reconfiguring it.
+The reconfiguration is possible only in *disengaged* state.
+In the example of gripper this would be *open* state.
+
+
+
+The controller implements state machine for the transition between states and
+
+
+1. Tool Actions
+
+1. Tool state machine
+
+2. Transitions

gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp

@@ -132,33 +132,28 @@ class GpioToolController : public controller_interface::ControllerInterface
   {
     std::vector<std::string> possible_states;
 
-    std::unordered_map<std::string, std::unordered_map<std::string, double>> set_before_commands;
-    std::unordered_map<std::string, size_t> set_before_commands_start_index;
-    std::unordered_map<std::string, std::unordered_map<std::string, double>> set_before_states;
-    std::unordered_map<std::string, size_t> set_before_states_start_index;
+    std::unordered_map<std::string, std::unordered_map<std::string, std::pair<double, size_t>>> set_before_commands;
+    std::unordered_map<std::string, std::unordered_map<std::string, std::pair<double, size_t>>> set_before_states;
 
-    std::unordered_map<std::string, std::unordered_map<std::string, double>> commands;
-    std::unordered_map<std::string, size_t> commands_start_index;
+    std::unordered_map<std::string, std::unordered_map<std::string, std::pair<double, size_t>>> commands;
 
-    std::unordered_map<std::string, std::unordered_map<std::string, double>> states;
-    std::unordered_map<std::string, size_t> states_start_index;
+    std::unordered_map<std::string, std::unordered_map<std::string, std::pair<double, size_t>>> states;
     std::unordered_map<std::string, std::vector<double>> states_joint_states;
-    std::unordered_map<std::string, std::unordered_map<std::string, double>> set_after_commands;
-    std::unordered_map<std::string, size_t> set_after_commands_start_index;
-    std::unordered_map<std::string, std::unordered_map<std::string, double>> set_after_states;
-    std::unordered_map<std::string, size_t> set_after_states_start_index;
+    std::unordered_map<std::string, std::unordered_map<std::string, std::pair<double, size_t>>> set_after_commands;
+    std::unordered_map<std::string, std::unordered_map<std::string, std::pair<double, size_t>>> set_after_states;
   };
 
   ToolTransitionIOs disengaged_gpios_;
   ToolTransitionIOs engaged_gpios_;
   ToolTransitionIOs reconfigure_gpios_;
 
+  // Not needed to be atomic or protected as used only in the RT loop
   rclcpp::Time state_change_start_;
   std::string current_configuration_;
   std::string current_state_;
 
-  std::unordered_set<std::string> command_if_ios;
-  std::unordered_set<std::string> state_if_ios;
+  std::unordered_set<std::string> command_if_ios_;
+  std::unordered_set<std::string> state_if_ios_;
 
   using EngagingSrvType = example_interfaces::srv::Trigger;
   using ResetSrvType = example_interfaces::srv::Trigger;
@@ -218,12 +213,13 @@ class GpioToolController : public controller_interface::ControllerInterface
     const rclcpp::Time & current_time, const ToolTransitionIOs & ios,
     std::vector<double> & joint_states, const size_t joint_states_start_index,
     const std::string & output_prefix, const uint8_t next_transition,
-    std::string & found_state_name);
+    std::string & found_state_name, const bool warning_output = false);
 
   /**
    * @brief Prepares the command and state IOs.
+   * \returns true if successful, false otherwise. Check the output if error has happend.
    */
-  void prepare_command_and_state_ios();
+  bool prepare_command_and_state_ios();
 
   /**
    * @brief Prepares the publishers and services.
@@ -239,15 +235,14 @@ class GpioToolController : public controller_interface::ControllerInterface
   /**
    * @brief Checks the tools state.
    */
-  void check_tool_state(const rclcpp::Time & current_time);
+  void check_tool_state(const rclcpp::Time & current_time, const bool warning_output = false);
 
   bool set_commands(
-    const std::unordered_map<std::string, double> & commands,
-    const size_t start_index,
-    const std::string & transition_name,
+    const std::unordered_map<std::string, std::pair<double, size_t>> & commands,
+    const std::string & output_prefix,
     const uint8_t next_transition);
   bool check_states(
-    const rclcpp::Time & current_time, const std::unordered_map<std::string, double> & states, const size_t start_index, const std::string & transition_name, const uint8_t next_transition);
+    const rclcpp::Time & current_time, const std::unordered_map<std::string, std::pair<double, size_t>> & states, const std::string & output_prefix, const uint8_t next_transition, const bool warning_output = false);
 
   std::vector<std::string> configurations_list_;
   std::vector<gpio_tool_controller::Params::ConfigurationSetup::MapConfigurations> config_map_;