Skip to content

Commit

Permalink
Remove event based logic and go back to regular updates
Browse files Browse the repository at this point in the history 
Signed-off-by: Luca Della Vedova <[email protected]>
  • Loading branch information
@luca-della-vedova
luca-della-vedova committed May 8, 2024
1 parent 8f25bfc commit 045ed07
Show file tree
Hide file tree
Showing 2 changed files with 80 additions and 97 deletions.
94 changes: 42 additions & 52 deletions rmf_building_sim_gz_plugins/src/door.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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,
Expand Down Expand Up @@ -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;
Expand Down Expand Up @@ -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(),
Expand All @@ -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
{
Expand All @@ -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,
Expand All @@ -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)
Expand All @@ -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)
{
Expand All @@ -337,4 +328,3 @@ GZ_ADD_PLUGIN(
DoorPlugin::ISystemPreUpdate)

GZ_ADD_PLUGIN_ALIAS(DoorPlugin, "door")

83 changes: 38 additions & 45 deletions rmf_building_sim_gz_plugins/src/lift.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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;
Expand All @@ -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)
{
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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;
}
Expand All @@ -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,
Expand Down Expand Up @@ -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,
Expand All @@ -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;
Expand All @@ -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;
});
}
};

Expand Down

0 comments on commit 045ed07

Please sign in to comment.