Remove event based logic and go back to regular updates

Signed-off-by: Luca Della Vedova <[email protected]>

Author: luca-della-vedova

rmf_building_sim_gz_plugins/src/door.cpp

@@ -30,20 +30,18 @@ class GZ_SIM_VISIBLE DoorPlugin
   public ISystemPreUpdate
 {
 private:
+  // TODO(luca) make this a parameter of the door manager
+  static constexpr double PUBLISH_DT = 1.0;
   rclcpp::Node::SharedPtr _ros_node;
   rclcpp::Publisher<DoorState>::SharedPtr _door_state_pub;
   rclcpp::Subscription<DoorRequest>::SharedPtr _door_request_sub;
 
-  std::unordered_set<Entity> _sent_states;
-  bool _send_all_states = false;
-
-  // Records which doors were just requested an action, they will respond by always
-  // reporting their latest state regardless of whether there was a change or not.
-  std::unordered_set<Entity> _queried_doors;
-
   // Used to do open loop joint position control
   std::unordered_map<Entity, double> _last_cmd_vel;
 
+  // Saves the last timestamp a door state was sent
+  std::unordered_map<Entity, double> _last_state_pub;
+
   bool _first_iteration = true;
 
   Entity get_joint_entity(const EntityComponentManager& ecm,
@@ -144,12 +142,9 @@ class GZ_SIM_VISIBLE DoorPlugin
     }
   }
 
-  void publish_state(const UpdateInfo& info, const std::string& name,
+  void publish_state(const double t, const std::string& name,
     const DoorModeCmp& door_state)
   {
-    double t =
-      (std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).
-      count()) * 1e-9;
     DoorState msg;
     msg.door_name = name;
     msg.door_time.sec = t;
@@ -186,22 +181,9 @@ class GZ_SIM_VISIBLE DoorPlugin
       "Loading DoorManager");
 
     // Subscribe to door requests, publish door states
-    auto pub_options = rclcpp::PublisherOptions();
-    pub_options.event_callbacks.matched_callback =
-      [this](rclcpp::MatchedInfo& s)
-      {
-        if (s.current_count_change > 0)
-        {
-          // Trigger a status send for all doors
-          _send_all_states = true;
-          _sent_states.clear();
-          gzmsg << "Detected new door subscriber, triggering state publish" <<
-            std::endl;
-        }
-      };
-    const auto pub_qos = rclcpp::QoS(200).reliable();
+    const auto pub_qos = rclcpp::QoS(100).reliable();
     _door_state_pub = _ros_node->create_publisher<DoorState>(
-      "door_states", pub_qos, pub_options);
+      "door_states", pub_qos);
 
     _door_request_sub = _ros_node->create_subscription<DoorRequest>(
       "door_requests", rclcpp::SystemDefaultsQoS(),
@@ -223,7 +205,6 @@ class GZ_SIM_VISIBLE DoorPlugin
           DoorModeCmp::OPEN : DoorModeCmp::CLOSE;
           ecm.CreateComponent<components::DoorCmd>(entity,
           components::DoorCmd(door_command));
-          _queried_doors.insert(entity);
         }
         else
         {
@@ -250,20 +231,37 @@ class GZ_SIM_VISIBLE DoorPlugin
       });
   }
 
+  void initialize_pub_times(EntityComponentManager& ecm)
+  {
+    ecm.Each<components::Door>([&](const Entity& e,
+      const components::Door* door_comp) -> bool
+      {
+        if (door_comp->Data().ros_interface == false)
+          return true;
+        _last_state_pub[e] = ((double) std::rand()) /
+        ((double) RAND_MAX/PUBLISH_DT);
+        return true;
+      });
+  }
+
   void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override
   {
     rclcpp::spin_some(_ros_node);
     if (_first_iteration)
     {
       _first_iteration = false;
       initialize_components(ecm);
+      initialize_pub_times(ecm);
       return;
     }
 
     // Don't update if the simulation is paused
     if (info.paused)
       return;
 
+    const double t =
+      (std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).
+      count()) * 1e-9;
     std::unordered_set<Entity> finished_cmds;
     // Process commands
     ecm.Each<components::Door, components::DoorCmd,
@@ -283,14 +281,12 @@ class GZ_SIM_VISIBLE DoorPlugin
         command_door(entity, ecm, door, dt, door_cmd);
         // Publish state if there was a change
         const auto cur_mode = get_current_mode(entity, ecm, door);
-        if (cur_mode != door_state_comp->Data() ||
-        _queried_doors.find(entity) != _queried_doors.end())
+        if (cur_mode != door_state_comp->Data())
         {
           last_mode = cur_mode;
           if (door_comp->Data().ros_interface)
           {
-            publish_state(info, name, cur_mode);
-            _queried_doors.erase(entity);
+            publish_state(t, name, cur_mode);
           }
         }
         if (door_cmd == cur_mode)
@@ -301,27 +297,22 @@ class GZ_SIM_VISIBLE DoorPlugin
       });
 
     // Publish states
-    if (_send_all_states)
-    {
-      bool keep_sending = false;
-      ecm.Each<components::Door, components::DoorStateComp,
-        components::Name>([&](const Entity& e,
-        const components::Door* door_comp,
-        const components::DoorStateComp* door_state_comp,
-        const components::Name* name_comp) -> bool
+    ecm.Each<components::Door, components::DoorStateComp,
+      components::Name>([&](const Entity& e,
+      const components::Door* door_comp,
+      const components::DoorStateComp* door_state_comp,
+      const components::Name* name_comp) -> bool
+      {
+        if (door_comp->Data().ros_interface == false)
+          return true;
+        auto it = _last_state_pub.find(e);
+        if (it != _last_state_pub.end() && t - it->second >= PUBLISH_DT)
         {
-          if (_sent_states.find(e) != _sent_states.end())
-            return true;
-          if (door_comp->Data().ros_interface == false)
-            return true;
-          publish_state(info, name_comp->Data(), door_state_comp->Data());
-          _sent_states.insert(e);
-          // Mark to keep sending but stop doing so for now
-          keep_sending = true;
-          return false;
-        });
-      _send_all_states = keep_sending;
-    }
+          it->second = t;
+          publish_state(t, name_comp->Data(), door_state_comp->Data());
+        }
+        return true;
+      });
 
     for (const auto& entity: finished_cmds)
     {
@@ -337,4 +328,3 @@ GZ_ADD_PLUGIN(
   DoorPlugin::ISystemPreUpdate)
 
 GZ_ADD_PLUGIN_ALIAS(DoorPlugin, "door")
-

rmf_building_sim_gz_plugins/src/lift.cpp

@@ -41,6 +41,8 @@ class GZ_SIM_VISIBLE LiftPlugin
   public ISystemPreUpdate
 {
 private:
+  // TODO(luca) make this a parameter of the lift manager
+  static constexpr double PUBLISH_DT = 1.0;
   rclcpp::Node::SharedPtr _ros_node;
 
   rclcpp::Publisher<LiftState>::SharedPtr _lift_state_pub;
@@ -54,12 +56,12 @@ class GZ_SIM_VISIBLE LiftPlugin
   std::unordered_map<Entity, LiftCommand> _last_lift_command;
   std::unordered_map<Entity, LiftState> _last_states;
 
+  // Saves the last timestamp a door state was sent
+  std::unordered_map<Entity, double> _last_state_pub;
+
   bool _components_initialized = false;
   bool _aabb_read = false;
 
-  std::unordered_set<Entity> _sent_states;
-  bool _send_all_states = false;
-
   std::vector<Entity> get_payloads(EntityComponentManager& ecm,
     const Entity& lift_entity)
   {
@@ -397,23 +399,9 @@ class GZ_SIM_VISIBLE LiftPlugin
     std::string plugin_name("rmf_simulation_lift_manager");
     _ros_node = std::make_shared<rclcpp::Node>(plugin_name);
 
-    // initialize pub & sub
-    auto pub_options = rclcpp::PublisherOptions();
-    pub_options.event_callbacks.matched_callback =
-      [this](rclcpp::MatchedInfo& s)
-      {
-        if (s.current_count_change > 0)
-        {
-          // Trigger a status send for all lifts
-          _send_all_states = true;
-          _sent_states.clear();
-          gzmsg << "Detected new lift subscriber, triggering state publish" <<
-            std::endl;
-        }
-      };
-    const auto reliable_qos = rclcpp::QoS(200).reliable();
+    const auto reliable_qos = rclcpp::QoS(100).reliable();
     _lift_state_pub = _ros_node->create_publisher<LiftState>(
-      "lift_states", reliable_qos, pub_options);
+      "lift_states", reliable_qos);
 
     _lift_request_sub = _ros_node->create_subscription<LiftRequest>(
       "lift_requests", reliable_qos,
@@ -482,12 +470,24 @@ class GZ_SIM_VISIBLE LiftPlugin
       "Starting LiftManager");
   }
 
+  void initialize_pub_times(EntityComponentManager& ecm)
+  {
+    ecm.Each<components::Lift>([&](const Entity& e,
+      const components::Lift*) -> bool
+      {
+        _last_state_pub[e] = ((double) std::rand()) /
+        ((double) RAND_MAX/PUBLISH_DT);
+        return true;
+      });
+  }
+
   void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override
   {
     // Read from components that may not have been initialized in configure()
     if (!_components_initialized)
     {
       initialize_components(ecm);
+      initialize_pub_times(ecm);
       _components_initialized = true;
       return;
     }
@@ -506,10 +506,10 @@ class GZ_SIM_VISIBLE LiftPlugin
 
     std::unordered_set<Entity> finished_cmds;
 
-    // Command lifts
-    double dt =
+    const double dt =
       (std::chrono::duration_cast<std::chrono::nanoseconds>(info.dt).
       count()) * 1e-9;
+    // Command lifts
     ecm.Each<components::Lift,
       components::Pose,
       components::LiftCmd>([&](const Entity& entity,
@@ -568,6 +568,9 @@ class GZ_SIM_VISIBLE LiftPlugin
         return true;
       });
 
+    const double t =
+      (std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).
+      count()) * 1e-9;
     // Update states
     ecm.Each<components::Lift,
       components::Name>([&](const Entity& entity,
@@ -585,9 +588,6 @@ class GZ_SIM_VISIBLE LiftPlugin
         if (current_state != _last_states[entity])
         {
           _last_states[entity] = current_state;
-          double t =
-          (std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).
-          count()) * 1e-9;
           current_state.lift_name = name;
           current_state.lift_time.sec = t;
           current_state.lift_time.nanosec = (t - static_cast<int>(t)) * 1e9;
@@ -605,37 +605,30 @@ class GZ_SIM_VISIBLE LiftPlugin
     }
 
     // Publish state
-    if (_send_all_states)
-    {
-      bool keep_sending = false;
-      ecm.Each<components::Lift,
-        components::Name>([&](const Entity& entity,
-        const components::Lift* lift_comp,
-        const components::Name* name_comp) -> bool
+    ecm.Each<components::Lift,
+      components::Name>([&](const Entity& e,
+      const components::Lift* lift_comp,
+      const components::Name* name_comp) -> bool
+      {
+        auto it = _last_state_pub.find(e);
+        if (it != _last_state_pub.end() && t - it->second >= PUBLISH_DT)
         {
-          if (_sent_states.find(entity) != _sent_states.end())
-            return true;
+          it->second = t;
           const auto& name = name_comp->Data();
           const auto& lift = lift_comp->Data();
 
           const auto* lift_cmd_comp = ecm.Component<components::LiftCmd>(
-            entity);
+            e);
 
-          auto msg = get_current_state(entity, ecm, lift, lift_cmd_comp);
-          _last_states[entity] = msg;
-          double t =
-          (std::chrono::duration_cast<std::chrono::nanoseconds>(info.simTime).
-          count()) * 1e-9;
+          auto msg = get_current_state(e, ecm, lift, lift_cmd_comp);
+          _last_states[e] = msg;
           msg.lift_time.sec = t;
           msg.lift_time.nanosec = (t - static_cast<int>(t)) * 1e9;
           msg.lift_name = name;
           _lift_state_pub->publish(msg);
-          _sent_states.insert(entity);
-          keep_sending = true;
-          return false;
-        });
-      _send_all_states = keep_sending;
-    }
+        }
+        return true;
+      });
   }
 };