Log message for CMDERROR messages

CommonplaceRobotics · Mar 18, 2024 · b8897e6 · b8897e6
1 parent 9fbb5d0
commit b8897e6
Show file tree

Hide file tree

Showing 3 changed files with 7 additions and 1 deletion.
diff --git a/irc_ros_hardware/include/irc_ros_hardware/CRI/cri_messages.hpp b/irc_ros_hardware/include/irc_ros_hardware/CRI/cri_messages.hpp
@@ -38,6 +38,7 @@ enum class MessageType {
   EXECPAUSE,
   EXECEND,
   EXECERROR,
+  CMDERROR,
   UNKNOWN = 1000
 };
 

diff --git a/irc_ros_hardware/src/CRI/cri_messages.cpp b/irc_ros_hardware/src/CRI/cri_messages.cpp
@@ -36,6 +36,7 @@ MessageType CriMessage::GetMessageType(const std::string & msg)
     {cri_keywords::TYPE_EXECPAUSE, MessageType::EXECPAUSE},
     {cri_keywords::TYPE_EXECEND, MessageType::EXECEND},
     {cri_keywords::TYPE_EXECPAUSE, MessageType::EXECPAUSE},
+    {cri_keywords::TYPE_CMDERROR, MessageType::CMDERROR}
   };
 
   auto it = message_type_map.find(typeString);

diff --git a/irc_ros_hardware/src/irc_ros_cri.cpp b/irc_ros_hardware/src/irc_ros_cri.cpp
@@ -128,7 +128,6 @@ void IrcRosCri::MessageThreadFunction()
 
           break;
         }
-
         case cri_messages::MessageType::INFO: {
           cri_messages::Info info = cri_messages::Info(msg);
           RCLCPP_INFO(rclcpp::get_logger("iRC_ROS"), "INFO: %s", info.info.c_str());
@@ -139,6 +138,11 @@ void IrcRosCri::MessageThreadFunction()
           RCLCPP_INFO(rclcpp::get_logger("iRC_ROS"), "EXECACK received");
           break;
         }
+        case cri_messages::MessageType::CMDERROR: 
+        {   
+            RCLCPP_ERROR(rclcpp::get_logger("iRC_ROS"), "CMDERROR: %s", msg.c_str());
+            break;
+        }
       }
     }
   }