From 0a409cbb17de61ec69edb650f2535fdde9b83210 Mon Sep 17 00:00:00 2001 From: Grey Date: Tue, 24 Sep 2024 09:35:30 +0800 Subject: [PATCH] Remove undefined behavior from the startup conditions (#132) * Remove undefined behavior from the startup conditions Signed-off-by: Michael X. Grey * Tweak style Signed-off-by: Michael X. Grey * Tweak style again Signed-off-by: Michael X. Grey --------- Signed-off-by: Michael X. Grey --- rmf_building_sim_gz_plugins/src/lift.cpp | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index 0822ca1..ee5d3a9 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -145,6 +145,7 @@ class LiftPlugin } } lift_command.destination_floor = initial_floor; + lift_command.door_state = DoorModeCmp::CLOSE; _last_lift_command[entity] = lift_command; std::vector joint_position = {target_elevation}; @@ -338,6 +339,7 @@ class LiftPlugin { continue; } + if (door_state_comp->Data() != cmd) return false; } @@ -455,8 +457,12 @@ class LiftPlugin cur_lift_cmd.request_type != msg->request_type || cur_lift_cmd.session_id != msg->session_id) { - gzwarn << "Discarding request: [" << msg->lift_name << - "] is busy at the moment" << std::endl; + gzwarn << "Discarding request to go to [" + << msg->destination_floor + << "] for [" << msg->session_id << "]. Lift [" + << msg->lift_name << "] is busy going to [" + << cur_lift_cmd.destination_floor << "] for [" + << cur_lift_cmd.session_id << "]" << std::endl; return; } }