Merge pull request #270 from HarvestX/add/tcp-if-mutex

Add mutex and lock_guard of tcp interface in DashboardCommander and MotionCommander
HarvestX · Oct 4, 2024 · 2932073 · 2932073
2 parents 3d1c97a + 6a10aac
commit 2932073
Show file tree

Hide file tree

Showing 6 changed files with 21 additions and 2 deletions.
diff --git a/mg400_interface/include/mg400_interface/commander/dashboard_commander.hpp b/mg400_interface/include/mg400_interface/commander/dashboard_commander.hpp
@@ -58,6 +58,7 @@ class DashboardCommander
   using User = mg400_msgs::msg::User;
 
   DashboardTcpInterfaceBase * tcp_if_;
+  mutable std::mutex mutex_tcp_if_;
   rclcpp::Clock::SharedPtr clock_;
   const std::chrono::nanoseconds TIMEOUT;
 

diff --git a/mg400_interface/include/mg400_interface/commander/motion_commander.hpp b/mg400_interface/include/mg400_interface/commander/motion_commander.hpp
@@ -44,6 +44,7 @@ class MotionCommander
   using User = mg400_msgs::msg::User;
 
   MotionTcpInterfaceBase * tcp_if_;
+  mutable std::mutex mutex_tcp_if_;
 
 public:
   MotionCommander() = delete;

diff --git a/mg400_interface/include/mg400_interface/tcp_interface/dashboard_tcp_interface.hpp b/mg400_interface/include/mg400_interface/tcp_interface/dashboard_tcp_interface.hpp
@@ -45,7 +45,6 @@ class DashboardTcpInterface : public DashboardTcpInterfaceBase
   const uint16_t PORT_ = 29999;
 
   std::atomic<bool> is_running_;
-  std::mutex mutex_;
   std::unique_ptr<std::thread> thread_;
   std::shared_ptr<TcpSocketHandler> tcp_socket_;
 

diff --git a/mg400_interface/include/mg400_interface/tcp_interface/motion_tcp_interface.hpp b/mg400_interface/include/mg400_interface/tcp_interface/motion_tcp_interface.hpp
@@ -45,7 +45,6 @@ class MotionTcpInterface : public MotionTcpInterfaceBase
 private:
   const uint16_t PORT_ = 30003;
 
-  std::mutex mutex_;
   std::atomic<bool> is_running_;
   std::unique_ptr<std::thread> thread_;
   std::shared_ptr<TcpSocketHandler> tcp_socket_;

diff --git a/mg400_interface/src/commander/dashboard_commander.cpp b/mg400_interface/src/commander/dashboard_commander.cpp
@@ -272,6 +272,7 @@ int DashboardCommander::modbusCreate(
   const std::string & ip, const int port,
   const int slave_id, const int isRTU)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf), "ModbusCreate(%s,%d,%d,%d)",
@@ -292,6 +293,7 @@ bool DashboardCommander::modbusClose(const std::string & index)
 std::vector<int> DashboardCommander::getInBits(
   const int index, const int addr, const int count)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf), "GetInBits(%d,%d,%d)",
@@ -305,6 +307,7 @@ std::vector<int> DashboardCommander::getInRegs(
   const int index, const int addr,
   const int count, const std::string & valType)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf), "GetInRegs(%d,%d,%d,{%s})",
@@ -318,6 +321,7 @@ std::vector<int> DashboardCommander::getInRegs(
 std::vector<int> DashboardCommander::getCoils(
   const int index, const int addr, const int count)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf), "GetCoils(%d,%d,%d)",
@@ -332,6 +336,7 @@ int DashboardCommander::setCoils(
   const int index, const int addr,
   const int count, const std::string & valTab)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf), "SetCoils(%d,%d,%d,{%s})",
@@ -346,6 +351,7 @@ std::vector<int> DashboardCommander::getHoldRegs(
   const int index, const int addr,
   const int count, const std::string & valType)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf), "GetHoldRegs(%d,%d,%d,%s)",
@@ -359,6 +365,7 @@ int DashboardCommander::setHoldRegs(
   const int index, const int addr,
   const int count, const std::string & valTab, const std::string & valType)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf), "SetHoldRegs(%d,%d,%d,{%s},%s)",
@@ -417,6 +424,7 @@ const rclcpp::Logger DashboardCommander::getLogger()
 std::string DashboardCommander::sendAndWaitResponse(
   const std::string & command) const
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   this->tcp_if_->sendCommand(command);
 
   const auto start = this->clock_->now();

diff --git a/mg400_interface/src/commander/motion_commander.cpp b/mg400_interface/src/commander/motion_commander.cpp
@@ -26,6 +26,7 @@ void MotionCommander::movJ(
   const si_m x, const si_m y, const si_m z,
   const si_rad rx, const si_rad ry, const si_rad rz)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),
@@ -39,6 +40,7 @@ void MotionCommander::movL(
   const si_m x, const si_m y, const si_m z,
   const si_rad rx, const si_rad ry, const si_rad rz)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),
@@ -52,6 +54,7 @@ void MotionCommander::jointMovJ(
   const si_rad j1, const si_rad j2, const si_rad j3,
   const si_rad j4, const si_rad j5, const si_rad j6)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),
@@ -81,6 +84,7 @@ void MotionCommander::movLIO(
   const DOIndex::_index_type & index,
   const DOStatus::_status_type & status)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),
@@ -109,6 +113,7 @@ void MotionCommander::movJIO(
   const DOIndex::_index_type & index,
   const DOStatus::_status_type & status)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),
@@ -126,6 +131,7 @@ void MotionCommander::arc(
   const si_m x2, const si_m y2, const si_m z2,
   const si_rad rx2, const si_rad ry2, const si_rad rz2)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),
@@ -145,6 +151,7 @@ void MotionCommander::moveJog(const MoveJog::SharedPtr & jog_mode)
 
 void MotionCommander::moveJog(const MoveJog::_jog_mode_type & jog_mode)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(buf, sizeof(buf), "MoveJog(%s)", jog_mode.c_str());
   this->tcp_if_->sendCommand(buf);
@@ -153,6 +160,7 @@ void MotionCommander::moveJog(const MoveJog::_jog_mode_type & jog_mode)
 
 void MotionCommander::sync()
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(buf, sizeof(buf), "Sync()");
   this->tcp_if_->sendCommand(buf);
@@ -173,6 +181,7 @@ void MotionCommander::relMovJUser(
   const si_rad rx, const si_rad ry, const si_rad rz,
   const User::_user_type & user)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),
@@ -197,6 +206,7 @@ void MotionCommander::relMovLUser(
   const si_rad rx, const si_rad ry, const si_rad rz,
   const User::_user_type & user)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),
@@ -210,6 +220,7 @@ void MotionCommander::relJointMovJ(
   const si_rad j1, const si_rad j2, const si_rad j3,
   const si_rad j4, const si_rad j5, const si_rad j6)
 {
+  std::lock_guard<std::mutex> lock_tcp_if_(this->mutex_tcp_if_);
   char buf[100];
   snprintf(
     buf, sizeof(buf),