gpio_controller fix

gpio_controllers/include/gpio_controllers/gpio_tool_controller.hpp

@@ -18,6 +18,7 @@
 #include <atomic>
 #include <functional>
 #include <memory>
+#include <mutex>
 #include <set>
 #include <string>
 #include <vector>
@@ -177,7 +178,9 @@ class GpioToolController : public controller_interface::ControllerInterface
   std::atomic<uint8_t> current_tool_transition_{GPIOToolTransition::IDLE};
   std::atomic<bool> reset_halted_{false};
   std::atomic<bool> transition_time_updated_{false};
-  std::atomic<std::shared_ptr<std::string>> target_configuration_;
+  // for complex type like shared_ptr<T>, a mutex is necessary instead of std::atomic
+  std::shared_ptr<std::string> target_configuration_;
+  std::mutex target_configuration_mutex_;
 
   using ToolJointStatePublisher = realtime_tools::RealtimePublisher<sensor_msgs::msg::JointState>;
   rclcpp::Publisher<sensor_msgs::msg::JointState>::SharedPtr t_js_publisher_;

gpio_controllers/src/gpio_tool_controller.cpp

@@ -55,7 +55,10 @@ controller_interface::CallbackReturn GpioToolController::on_init()
 {
   current_tool_action_.store(ToolAction::IDLE);
   current_tool_transition_.store(GPIOToolTransition::IDLE);
-  target_configuration_.store(std::make_shared<std::string>(""));
+  {
+    std::lock_guard<std::mutex> lock(target_configuration_mutex_);
+    target_configuration_ = std::make_shared<std::string>("");
+  }
   current_state_ = "";
   current_configuration_ = "";
 
@@ -329,8 +332,13 @@ controller_interface::return_type GpioToolController::update(
     }
     case ToolAction::RECONFIGURING:
     {
+      std::shared_ptr<const std::string> target_config_ptr;
+      {
+        std::lock_guard<std::mutex> lock(target_configuration_mutex_);
+        target_config_ptr = target_configuration_;
+      }
       handle_tool_state_transition(
-        time, reconfigure_gpios_, *(target_configuration_.load()), joint_states_values_, params_.engaged_joints.size(), current_configuration_);
+        time, reconfigure_gpios_, *(target_config_ptr), joint_states_values_, params_.engaged_joints.size(), current_configuration_);
       break;
     }
     case ToolAction::CANCELING:
@@ -871,7 +879,10 @@ GpioToolController::EngagingSrvType::Response GpioToolController::process_reconf
   {
     current_tool_action_.store(ToolAction::RECONFIGURING);
     current_tool_transition_.store(GPIOToolTransition::SET_BEFORE_COMMAND);
-    target_configuration_.store(std::make_shared<std::string>(config_name));
+    {
+      std::lock_guard<std::mutex> lock(target_configuration_mutex_);
+      target_configuration_ = std::make_shared<std::string>(config_name);
+    }
     response.message = "Tool reconfiguration to '" + config_name + "' has started.";
     RCLCPP_INFO(get_node()->get_logger(), "%s", response.message.c_str());
   }

io_gripper_controller/package.xml

@@ -29,6 +29,7 @@
   <depend>sensor_msgs</depend>
   <depend>std_msgs</depend>
   <depend>std_srvs</depend>
+  <depend>gpio_tool_controller</depend>
 
   <test_depend>ament_cmake_gmock</test_depend>
   <test_depend>ament_lint_auto</test_depend>