From 045ed07168d646b6c3ae4d7bbbf8cad7c1aacf0c Mon Sep 17 00:00:00 2001 From: Luca Della Vedova Date: Wed, 8 May 2024 17:10:00 +0800 Subject: [PATCH] Remove event based logic and go back to regular updates Signed-off-by: Luca Della Vedova --- rmf_building_sim_gz_plugins/src/door.cpp | 94 +++++++++++------------- rmf_building_sim_gz_plugins/src/lift.cpp | 83 ++++++++++----------- 2 files changed, 80 insertions(+), 97 deletions(-) diff --git a/rmf_building_sim_gz_plugins/src/door.cpp b/rmf_building_sim_gz_plugins/src/door.cpp index 61918c1..74c7cfa 100644 --- a/rmf_building_sim_gz_plugins/src/door.cpp +++ b/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::SharedPtr _door_state_pub; rclcpp::Subscription::SharedPtr _door_request_sub; - std::unordered_set _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 _queried_doors; - // Used to do open loop joint position control std::unordered_map _last_cmd_vel; + // Saves the last timestamp a door state was sent + std::unordered_map _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(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( - "door_states", pub_qos, pub_options); + "door_states", pub_qos); _door_request_sub = _ros_node->create_subscription( "door_requests", rclcpp::SystemDefaultsQoS(), @@ -223,7 +205,6 @@ class GZ_SIM_VISIBLE DoorPlugin DoorModeCmp::OPEN : DoorModeCmp::CLOSE; ecm.CreateComponent(entity, components::DoorCmd(door_command)); - _queried_doors.insert(entity); } else { @@ -250,6 +231,19 @@ class GZ_SIM_VISIBLE DoorPlugin }); } + void initialize_pub_times(EntityComponentManager& ecm) + { + ecm.Each([&](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); @@ -257,6 +251,7 @@ class GZ_SIM_VISIBLE DoorPlugin { _first_iteration = false; initialize_components(ecm); + initialize_pub_times(ecm); return; } @@ -264,6 +259,9 @@ class GZ_SIM_VISIBLE DoorPlugin if (info.paused) return; + const double t = + (std::chrono::duration_cast(info.simTime). + count()) * 1e-9; std::unordered_set finished_cmds; // Process commands ecm.EachData() || - _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([&](const Entity& e, - const components::Door* door_comp, - const components::DoorStateComp* door_state_comp, - const components::Name* name_comp) -> bool + ecm.Each([&](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") - diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index df4cb8b..c22a8f9 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/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::SharedPtr _lift_state_pub; @@ -54,12 +56,12 @@ class GZ_SIM_VISIBLE LiftPlugin std::unordered_map _last_lift_command; std::unordered_map _last_states; + // Saves the last timestamp a door state was sent + std::unordered_map _last_state_pub; + bool _components_initialized = false; bool _aabb_read = false; - std::unordered_set _sent_states; - bool _send_all_states = false; - std::vector 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(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( - "lift_states", reliable_qos, pub_options); + "lift_states", reliable_qos); _lift_request_sub = _ros_node->create_subscription( "lift_requests", reliable_qos, @@ -482,12 +470,24 @@ class GZ_SIM_VISIBLE LiftPlugin "Starting LiftManager"); } + void initialize_pub_times(EntityComponentManager& ecm) + { + ecm.Each([&](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 finished_cmds; - // Command lifts - double dt = + const double dt = (std::chrono::duration_cast(info.dt). count()) * 1e-9; + // Command lifts ecm.Each([&](const Entity& entity, @@ -568,6 +568,9 @@ class GZ_SIM_VISIBLE LiftPlugin return true; }); + const double t = + (std::chrono::duration_cast(info.simTime). + count()) * 1e-9; // Update states ecm.Each([&](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(info.simTime). - count()) * 1e-9; current_state.lift_name = name; current_state.lift_time.sec = t; current_state.lift_time.nanosec = (t - static_cast(t)) * 1e9; @@ -605,37 +605,30 @@ class GZ_SIM_VISIBLE LiftPlugin } // Publish state - if (_send_all_states) - { - bool keep_sending = false; - ecm.Each([&](const Entity& entity, - const components::Lift* lift_comp, - const components::Name* name_comp) -> bool + ecm.Each([&](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( - entity); + e); - auto msg = get_current_state(entity, ecm, lift, lift_cmd_comp); - _last_states[entity] = msg; - double t = - (std::chrono::duration_cast(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(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; + }); } };