StateAndMode improvements

Signed-off-by: Arne Nordmann <arne.nordmann@de.bosch.com>

Author: norro

system_modes/include/system_modes/mode_impl.hpp

@@ -17,13 +17,17 @@
 #include <rclcpp/node_interfaces/node_parameters.hpp>
 #include <rclcpp/parameter_map.hpp>
 
+#include <lifecycle_msgs/msg/state.hpp>
+
 #include <map>
 #include <mutex>
 #include <vector>
 #include <string>
 #include <memory>
 #include <utility>
 
+using lifecycle_msgs::msg::State;
+
 namespace system_modes
 {
 
@@ -49,17 +53,22 @@ struct StateAndMode
     mode = newmode;
   }
 
-  bool operator!=(const StateAndMode & cmp) const
+  bool operator==(const StateAndMode & cmp) const
   {
-    return cmp.state == state &&                                     // same state
-           (cmp.mode.compare(mode) == 0 ||                           // same mode
+    if (cmp.state != state) {
+      return false;
+    } else if (cmp.state != State::PRIMARY_STATE_ACTIVE) {
+      return true;
+    }
+
+    return (cmp.mode.compare(mode) == 0 ||                           // same mode
            (cmp.mode.compare(DEFAULT_MODE) == 0 && mode.empty()) ||  // we consider empty and
            (mode.compare(DEFAULT_MODE) == 0 && cmp.mode.empty()));   // DEFAULT_MODE the same
   }
 
-  bool operator==(const StateAndMode & cmp) const
+  bool operator!=(const StateAndMode & cmp) const
   {
-    return !(*this != cmp);
+    return !(*this == cmp);
   }
 
   void from_string(const std::string & sam) {
@@ -73,9 +82,8 @@ struct StateAndMode
     }
   }
 
-  std::string as_string() const
-  {
-    if (mode.empty()) {
+  std::string as_string() const {
+    if (state != State::PRIMARY_STATE_ACTIVE || mode.empty()) {
       return state_label_(state);
     }
     return state_label_(state) + "." + mode;

system_modes/src/system_modes/mode_manager.cpp

@@ -50,6 +50,8 @@ using system_modes::msg::ModeEvent;
 using system_modes::srv::ChangeMode;
 using system_modes::srv::GetAvailableModes;
 
+using namespace std::chrono_literals;
+
 namespace system_modes
 {
 
@@ -327,7 +329,6 @@ ModeManager::on_get_available_modes(
   }
 }
 
-
 bool
 ModeManager::change_state(
   const std::string & node_name,

system_modes/test/test_state_and_mode_struct.cpp

@@ -60,15 +60,20 @@ TEST_F(TestStateAndMode, comparison) {
     EXPECT_NE(active, active_foo);
     EXPECT_NE(active_default, active_foo);
     EXPECT_NE(active_default, inactive);
+
+    active_foo.state = State::PRIMARY_STATE_INACTIVE;
+    EXPECT_EQ(inactive, active_foo);
   }
 }
 
 TEST_F(TestStateAndMode, string_getter) {
   {
+    EXPECT_EQ("inactive"s, inactive.as_string());
     EXPECT_EQ("active"s, active.as_string());
     EXPECT_EQ("active.__DEFAULT__"s, active_default.as_string());
     EXPECT_EQ("active.FOO"s, active_foo.as_string());
-    EXPECT_EQ("inactive"s, inactive.as_string());
+    active_foo.state = State::PRIMARY_STATE_INACTIVE;
+    EXPECT_EQ("inactive"s, active_foo.as_string());
   }
 }