Fix door ros interface parameters

Signed-off-by: Luca Della Vedova <lucadv@intrinsic.ai>
open-rmf · Mar 15, 2024 · c2ecedd · c2ecedd
1 parent f489947
commit c2ecedd
Show file tree

Hide file tree

Showing 2 changed files with 28 additions and 26 deletions.
diff --git a/rmf_building_sim_gz_plugins/src/door.cpp b/rmf_building_sim_gz_plugins/src/door.cpp
@@ -6,7 +6,7 @@
 #include <gz/sim/components/Joint.hh>
 #include <gz/sim/components/JointAxis.hh>
 #include <gz/sim/components/JointPosition.hh>
-#include <gz/sim/components/JointPositionReset.hh>
+#include <gz/sim/components/JointVelocityCmd.hh>
 #include <gz/sim/components/Name.hh>
 
 #include <rclcpp/rclcpp.hpp>
@@ -137,8 +137,8 @@ class GZ_SIM_VISIBLE DoorPlugin
         auto target_vel = calculate_target_velocity(target_pos, cur_pos,
             _last_cmd_vel[joint_entity],
             dt, door.params);
-        ecm.CreateComponent<components::JointPositionReset>(joint_entity, components::JointPositionReset(
-            {cur_pos + target_vel * dt}));
+        ecm.CreateComponent<components::JointVelocityCmd>(joint_entity, components::JointVelocityCmd(
+            {target_vel}));
         _last_cmd_vel[joint_entity] = target_vel;
       }
     }
@@ -211,8 +211,14 @@ class GZ_SIM_VISIBLE DoorPlugin
         // TODO(luca) cache this to avoid expensive iteration over all entities?
         auto entity = ecm.EntityByComponents(components::Name(
           msg->door_name));
-        if (entity != kNullEntity)
+        const auto*  door = ecm.Component<components::Door>(entity);
+        if (entity != kNullEntity || door == nullptr)
         {
+          if (door->Data().ros_interface == false)
+          {
+            gzmsg << "Ignoring door " << msg->door_name << " because it doesn't have a ros interface" << std::endl;
+            return;
+          }
           auto door_command = msg->requested_mode.value == msg->requested_mode.MODE_OPEN ?
           DoorModeCmp::OPEN : DoorModeCmp::CLOSE;
           ecm.CreateComponent<components::DoorCmd>(entity,
@@ -277,11 +283,15 @@ 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() ||
+        _queried_doors.find(entity) != _queried_doors.end())
         {
           last_mode = cur_mode;
-          publish_state(info, name, cur_mode);
-          _queried_doors.erase(entity);
+          if (door_comp->Data().ros_interface)
+          {
+            publish_state(info, name, cur_mode);
+            _queried_doors.erase(entity);
+          }
         }
         if (door_cmd == cur_mode)
         {

diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp
@@ -106,7 +106,6 @@ class GZ_SIM_VISIBLE LiftPlugin
         Model(entity).JointByName(ecm, lift.cabin_joint);
         enableComponent<components::AxisAlignedBox>(ecm, entity);
         enableComponent<components::JointPosition>(ecm, cabin_joint_entity);
-        enableComponent<components::JointVelocityCmd>(ecm, cabin_joint_entity);
         ecm.CreateComponent<components::JointVelocityCmd>(cabin_joint_entity,
         components::JointVelocityCmd({0.0}));
 
@@ -140,6 +139,8 @@ class GZ_SIM_VISIBLE LiftPlugin
         std::vector<double> joint_position = {target_elevation};
         ecm.CreateComponent<components::JointPositionReset>(entity,
         components::JointPositionReset{joint_position});
+        ecm.CreateComponent<components::LiftCmd>(entity,
+        components::LiftCmd{lift_command});
         return true;
       });
   }
@@ -421,7 +422,7 @@ class GZ_SIM_VISIBLE LiftPlugin
         // Find entity with the name and create a DoorCmd component
         auto entity = entity_by_name(ecm, msg->lift_name);
         const auto* lift_comp = ecm.Component<components::Lift>(entity);
-        if (entity != kNullEntity || lift_comp == nullptr)
+        if (entity != kNullEntity && lift_comp != nullptr)
         {
           const auto& available_floors = lift_comp->Data().floors;
           if (available_floors.find(msg->destination_floor) ==
@@ -505,32 +506,21 @@ class GZ_SIM_VISIBLE LiftPlugin
 
     std::unordered_set<Entity> finished_cmds;
 
-    // Update state
+    // Command lifts
     double dt =
       (std::chrono::duration_cast<std::chrono::nanoseconds>(info.dt).
       count()) * 1e-9;
     ecm.Each<components::Lift,
-      components::Pose>([&](const Entity& entity,
+      components::Pose,
+      components::LiftCmd>([&](const Entity& entity,
       const components::Lift* lift_comp,
-      const components::Pose* pose_comp) -> bool
+      const components::Pose* pose_comp,
+      const components::LiftCmd* lift_cmd_comp) -> bool
       {
         const auto& lift = lift_comp->Data();
         const auto& pose = pose_comp->Data();
 
-        const auto* lift_cmd_comp = ecm.Component<components::LiftCmd>(entity);
-        LiftCommand lift_cmd;
-        if (lift_cmd_comp != nullptr)
-        {
-          lift_cmd = lift_cmd_comp->Data();
-        }
-        else if (_last_lift_command.find(entity) != _last_lift_command.end())
-        {
-          lift_cmd = _last_lift_command[entity];
-        }
-        else
-        {
-          return true;
-        }
+        const auto& lift_cmd = lift_cmd_comp->Data();
 
         const auto& destination_floor = lift_cmd.destination_floor;
         const double target_elevation = lift.floors.at(
@@ -610,7 +600,9 @@ class GZ_SIM_VISIBLE LiftPlugin
 
     // Clear finished commands
     for (const auto& e : finished_cmds)
+    {
       enableComponent<components::LiftCmd>(ecm, e, false);
+    }
 
     // Publish state
     if (_send_all_states)