From 9535dd3e4f853668c2b27091dfc6494244688e82 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Fri, 25 Apr 2025 12:44:07 +0000 Subject: [PATCH 1/6] Recreate vel cmd components each cycle Signed-off-by: Michael X. Grey --- rmf_robot_sim_gz_plugins/src/slotcar.cpp | 17 +++++++---------- 1 file changed, 7 insertions(+), 10 deletions(-) diff --git a/rmf_robot_sim_gz_plugins/src/slotcar.cpp b/rmf_robot_sim_gz_plugins/src/slotcar.cpp index b8d7573..908568a 100644 --- a/rmf_robot_sim_gz_plugins/src/slotcar.cpp +++ b/rmf_robot_sim_gz_plugins/src/slotcar.cpp @@ -211,9 +211,6 @@ void SlotcarPlugin::Configure(const Entity& entity, enableComponent(ecm, entity); // Initialize Bounding Box component enableComponent(ecm, entity); - // Initialize Linear/AngularVelocityCmd components to drive slotcar - enableComponent(ecm, entity); - enableComponent(ecm, entity); // Respond to requests asking for height (e.g. for dispenser to dispense object) const std::string height_srv_name = @@ -239,11 +236,6 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm, const double target_linear_speed_destination, const std::optional& max_linear_velocity) { - auto lin_vel_cmd = - ecm.Component(_entity); - auto ang_vel_cmd = - ecm.Component(_entity); - // Open loop control double v_robot = _prev_v_command; double w_robot = _prev_w_command; @@ -252,8 +244,13 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm, displacements, dt, target_linear_speed_now, target_linear_speed_destination, max_linear_velocity); - lin_vel_cmd->Data()[0] = target_vels[0]; - ang_vel_cmd->Data()[2] = target_vels[1]; + gz::math::Vector3d lin_vel_cmd(0, 0, 0); + lin_vel_cmd[0] = target_vels[0]; + gz::math::Vector3d ang_vel_cmd(0, 0, 0); + ang_vel_cmd[2] = target_vels[1]; + + ecm.SetComponentData(_entity, lin_vel_cmd); + ecm.SetComponentData(_entity, ang_vel_cmd); // Update previous velocities _prev_v_command = target_vels[0]; From aadab42a0a282d64a7f345e1466a1c63ea0dc05d Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Tue, 29 Apr 2025 02:28:48 +0000 Subject: [PATCH 2/6] Debugging elevator free-falling issue Signed-off-by: Michael X. Grey --- rmf_building_sim_gz_plugins/src/lift.cpp | 100 ++++++++++++++++++++--- 1 file changed, 89 insertions(+), 11 deletions(-) diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index ee5d3a9..ffba99a 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -8,8 +8,12 @@ #include #include #include +#include +#include #include +#include #include +#include #include #include #include @@ -63,10 +67,16 @@ class LiftPlugin bool _components_initialized = false; bool _aabb_read = false; + std::size_t count = 0; + std::vector get_payloads(EntityComponentManager& ecm, const Entity& lift_entity) { std::vector payloads; + if (!ecm.Component(lift_entity)) + { + std::cout << " >>>>>>>>>> NO POSE FOR LIFT" << std::endl; + } const auto& lift_pose = ecm.Component(lift_entity)->Data(); auto pose = _initial_poses.find(lift_entity); @@ -104,13 +114,35 @@ class LiftPlugin ecm.Each([&](const Entity& entity, const components::Lift* lift_comp) -> bool { + // for (auto joint : Model(entity).Joints(ecm)) + // { + // const auto* name_comp = ecm.Component(joint); + // if (name_comp) + // { + // const auto* joint_pos_comp = ecm.Component(joint); + // if (joint_pos_comp) + // { + // std::cout << " >>>>>>>>>>> JOINT [" << name_comp->Data() << "] position: " + // << joint_pos_comp->Data().size() << std::endl; + // } + // else + // { + // std::cout << " >>>>>>>>> JOINT [" << name_comp->Data() << "] HAS NO POSITION" << std::endl; + // } + // } + // else + // { + // std::cout << " >>>>>> JOINT " << joint << " HAS NO NAME" << std::endl; + // } + // } + + const auto& lift = lift_comp->Data(); auto cabin_joint_entity = - Model(entity).JointByName(ecm, lift.cabin_joint); + Model(entity).JointByName(ecm, lift.cabin_joint); enableComponent(ecm, entity); - enableComponent(ecm, cabin_joint_entity); - ecm.CreateComponent(cabin_joint_entity, - components::JointVelocityCmd({0.0})); + ecm.SetComponentData(cabin_joint_entity, {0.0}); + ecm.SetComponentData(cabin_joint_entity, {0.0}); LiftCommand lift_command; lift_command.request_type = LiftRequest::REQUEST_AGV_MODE; @@ -202,8 +234,11 @@ class LiftPlugin return ""; } + // const auto lift_pos = + // ecm.Component(joint_entity)->Data()[0]; + const auto lift_pos = - ecm.Component(joint_entity)->Data()[0]; + ecm.Component(entity)->Data().Z(); double smallest_error = std::numeric_limits::infinity(); std::string closest_floor_name; @@ -284,8 +319,13 @@ class LiftPlugin return LiftState::MOTION_STOPPED; } - const auto lift_pos = - ecm.Component(joint_entity)->Data()[0]; + auto* lift_pos_component = ecm.Component(joint_entity); + if (!lift_pos_component) + { + return LiftState::MOTION_STOPPED; + } + + const auto lift_pos = lift_pos_component->Data()[0]; const auto target_it = lift.floors.find(destination_floor); if (target_it != lift.floors.end()) @@ -372,10 +412,20 @@ class LiftPlugin target_vel = calculate_target_velocity(target_elevation, cur_elevation, _last_cmd_vel[joint_entity], dt, lift.params); - ecm.Component(joint_entity)->Data() = - {target_vel}; + + if (count == 0) + { + std::cout << "Entity " << joint_entity << " cmd " << target_vel << std::endl; + } + ecm.SetComponentData(joint_entity, {target_vel}); + ecm.RemoveComponent(joint_entity); + _last_cmd_vel[joint_entity] = target_vel; } + else + { + std::cout << " >>>>>>>>> UNABLE TO FIND LIFT CABIN JOINT ENTITY" << std::endl; + } return target_vel; } @@ -530,6 +580,12 @@ class LiftPlugin std::unordered_set finished_cmds; + ++count; + if (count > 20) + { + count = 0; + } + const double dt = to_seconds(info.dt); // Command lifts ecm.Each(joint_entity)->Data(); + auto position = position_data[0]; + auto& velocity_data = ecm.Component(joint_entity)->Data(); + auto velocity = velocity_data[0]; + std::cout << "Joint " << joint_entity << " position " << position + << " (" << position_data.size() << ")" + << " velocity " << velocity << " (" << velocity_data.size() << ")" + << " | Current floor: " << cur_floor + << " destination: " << destination_floor << " | z " + << pose.Z() << " target " << target_elevation << " dx_min " + << lift.params.dx_min << std::endl; + } if (std::abs(pose.Z() - target_elevation) < lift.params.dx_min) { // Just command the doors to the target state command_doors(ecm, doors, target_door_state); // Clear the command if it was finished if (destination_floor == cur_floor && - all_doors_at_state(ecm, doors, target_door_state)) + all_doors_at_state(ecm, doors, target_door_state)) + { finished_cmds.insert(entity); + } } else { @@ -568,7 +641,12 @@ class LiftPlugin if (all_doors_at_state(ecm, doors, DoorModeCmp::CLOSE)) { auto target_velocity = - command_lift(entity, ecm, lift, dt, target_elevation, pose.Z()); + command_lift(entity, ecm, lift, dt, target_elevation, pose.Z()); + + if (count == 0) + { + std::cout << "Target Velocity: " << target_velocity << std::endl; + } // Move payloads as well if (target_velocity != 0.0) { From e64c8b826e44c3c099e859e4500a517ed9de1e83 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 30 Apr 2025 06:27:25 +0000 Subject: [PATCH 3/6] Fix lift behavior Signed-off-by: Michael X. Grey --- rmf_building_sim_gz_plugins/src/door.cpp | 6 +- rmf_building_sim_gz_plugins/src/lift.cpp | 110 ++++++++--------------- rmf_robot_sim_gz_plugins/src/slotcar.cpp | 14 +++ 3 files changed, 52 insertions(+), 78 deletions(-) diff --git a/rmf_building_sim_gz_plugins/src/door.cpp b/rmf_building_sim_gz_plugins/src/door.cpp index 5c45d03..3972e43 100644 --- a/rmf_building_sim_gz_plugins/src/door.cpp +++ b/rmf_building_sim_gz_plugins/src/door.cpp @@ -6,7 +6,7 @@ #include #include #include -#include +#include #include #include @@ -136,9 +136,7 @@ class DoorPlugin auto target_vel = calculate_target_velocity(target_pos, cur_pos, _last_cmd_vel[joint_entity], dt, door.params); - ecm.CreateComponent(joint_entity, - components::JointPositionReset( - {cur_pos + target_vel * dt})); + ecm.SetComponentData(joint_entity, {target_vel}); _last_cmd_vel[joint_entity] = target_vel; } } diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index ffba99a..c0fbd1a 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -67,16 +67,10 @@ class LiftPlugin bool _components_initialized = false; bool _aabb_read = false; - std::size_t count = 0; - std::vector get_payloads(EntityComponentManager& ecm, const Entity& lift_entity) { std::vector payloads; - if (!ecm.Component(lift_entity)) - { - std::cout << " >>>>>>>>>> NO POSE FOR LIFT" << std::endl; - } const auto& lift_pose = ecm.Component(lift_entity)->Data(); auto pose = _initial_poses.find(lift_entity); @@ -114,29 +108,6 @@ class LiftPlugin ecm.Each([&](const Entity& entity, const components::Lift* lift_comp) -> bool { - // for (auto joint : Model(entity).Joints(ecm)) - // { - // const auto* name_comp = ecm.Component(joint); - // if (name_comp) - // { - // const auto* joint_pos_comp = ecm.Component(joint); - // if (joint_pos_comp) - // { - // std::cout << " >>>>>>>>>>> JOINT [" << name_comp->Data() << "] position: " - // << joint_pos_comp->Data().size() << std::endl; - // } - // else - // { - // std::cout << " >>>>>>>>> JOINT [" << name_comp->Data() << "] HAS NO POSITION" << std::endl; - // } - // } - // else - // { - // std::cout << " >>>>>> JOINT " << joint << " HAS NO NAME" << std::endl; - // } - // } - - const auto& lift = lift_comp->Data(); auto cabin_joint_entity = Model(entity).JointByName(ecm, lift.cabin_joint); @@ -405,6 +376,11 @@ class LiftPlugin const LiftData& lift, double dt, double target_elevation, double cur_elevation) { + for (auto joint : Model(entity).Joints(ecm)) + { + ecm.RemoveComponent(joint); + } + auto joint_entity = Model(entity).JointByName(ecm, lift.cabin_joint); auto target_vel = 0.0; if (joint_entity != kNullEntity) @@ -413,19 +389,11 @@ class LiftPlugin _last_cmd_vel[joint_entity], dt, lift.params); - if (count == 0) - { - std::cout << "Entity " << joint_entity << " cmd " << target_vel << std::endl; - } ecm.SetComponentData(joint_entity, {target_vel}); ecm.RemoveComponent(joint_entity); _last_cmd_vel[joint_entity] = target_vel; } - else - { - std::cout << " >>>>>>>>> UNABLE TO FIND LIFT CABIN JOINT ENTITY" << std::endl; - } return target_vel; } @@ -580,49 +548,48 @@ class LiftPlugin std::unordered_set finished_cmds; - ++count; - if (count > 20) - { - count = 0; - } - const double dt = to_seconds(info.dt); // Command lifts ecm.Each([&](const Entity& entity, + components::Pose>([&](const Entity& entity, const components::Lift* lift_comp, - const components::Pose* pose_comp, - const components::LiftCmd* lift_cmd_comp) -> bool + const components::Pose* pose_comp) -> bool { + const components::LiftCmd* lift_cmd_comp = ecm.Component(entity); + const LiftCommand* lift_cmd = nullptr; + if (lift_cmd_comp) + { + lift_cmd = &lift_cmd_comp->Data(); + } + else + { + const auto it = _last_lift_command.find(entity); + if (it != _last_lift_command.end()) + { + lift_cmd = &it->second; + } + else + { + // We have no indication of where this lift should go, so + // just try to keep it in place. + auto cabin_joint_entity = Model(entity).JointByName(ecm, lift_comp->Data().cabin_joint); + ecm.SetComponentData(cabin_joint_entity, {0.0}); + + return true; + } + } + const auto& lift = lift_comp->Data(); const auto& pose = pose_comp->Data(); - const auto& lift_cmd = lift_cmd_comp->Data(); - - const auto& destination_floor = lift_cmd.destination_floor; + const auto& destination_floor = lift_cmd->destination_floor; const double target_elevation = lift.floors.at( destination_floor).elevation; - const auto target_door_state = lift_cmd.door_state; + const auto target_door_state = lift_cmd->door_state; const std::string cur_floor = get_current_floor(entity, ecm, lift); const auto doors = get_floor_doors(ecm, lift, cur_floor); - if (count == 0) - { - auto joint_entity = Model(entity).JointByName(ecm, lift.cabin_joint); - auto& position_data = ecm.Component(joint_entity)->Data(); - auto position = position_data[0]; - auto& velocity_data = ecm.Component(joint_entity)->Data(); - auto velocity = velocity_data[0]; - std::cout << "Joint " << joint_entity << " position " << position - << " (" << position_data.size() << ")" - << " velocity " << velocity << " (" << velocity_data.size() << ")" - << " | Current floor: " << cur_floor - << " destination: " << destination_floor << " | z " - << pose.Z() << " target " << target_elevation << " dx_min " - << lift.params.dx_min << std::endl; - } if (std::abs(pose.Z() - target_elevation) < lift.params.dx_min) { // Just command the doors to the target state @@ -643,21 +610,16 @@ class LiftPlugin auto target_velocity = command_lift(entity, ecm, lift, dt, target_elevation, pose.Z()); - if (count == 0) - { - std::cout << "Target Velocity: " << target_velocity << std::endl; - } // Move payloads as well if (target_velocity != 0.0) { auto payloads = get_payloads(ecm, entity); + for (const Entity& payload : payloads) { - if (ecm.EntityHasComponentType(payload, - components::LinearVelocityCmd().TypeId())) + if (ecm.EntityHasComponentType(payload, components::LinearVelocityCmd().TypeId())) { - auto lin_vel_cmd = - ecm.Component(payload); + auto lin_vel_cmd = ecm.Component(payload); lin_vel_cmd->Data()[2] = target_velocity; } } diff --git a/rmf_robot_sim_gz_plugins/src/slotcar.cpp b/rmf_robot_sim_gz_plugins/src/slotcar.cpp index 908568a..b8aed0f 100644 --- a/rmf_robot_sim_gz_plugins/src/slotcar.cpp +++ b/rmf_robot_sim_gz_plugins/src/slotcar.cpp @@ -34,6 +34,7 @@ using namespace gz::sim; class GZ_SIM_VISIBLE SlotcarPlugin : public System, public ISystemConfigure, + public ISystemConfigurePriority, public ISystemPreUpdate { public: @@ -43,6 +44,7 @@ class GZ_SIM_VISIBLE SlotcarPlugin void Configure(const Entity& entity, const std::shared_ptr& sdf, EntityComponentManager& ecm, EventManager& eventMgr) override; + int32_t ConfigurePriority() override; void PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) override; private: @@ -249,6 +251,7 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm, gz::math::Vector3d ang_vel_cmd(0, 0, 0); ang_vel_cmd[2] = target_vels[1]; + ecm.SetComponentData(_entity, lin_vel_cmd); ecm.SetComponentData(_entity, ang_vel_cmd); @@ -451,6 +454,16 @@ void SlotcarPlugin::draw_lookahead_marker() _gz_node.Request("/marker", marker_msg); } +int32_t SlotcarPlugin::ConfigurePriority() +{ + // Set the priority down by one so this runs before the infrastructure plugin + // which should have a default priority of 0. This is important for ensuring + // that the linear velocity command component is created before the + // infrastructure plugin runs since the lift needs to adjust its z value, and + // the physics plugin removes the component on every update. + return -1; +} + void SlotcarPlugin::PreUpdate(const UpdateInfo& info, EntityComponentManager& ecm) { @@ -520,6 +533,7 @@ GZ_ADD_PLUGIN( SlotcarPlugin, System, SlotcarPlugin::ISystemConfigure, + SlotcarPlugin::ISystemConfigurePriority, SlotcarPlugin::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(SlotcarPlugin, "slotcar") From 1f87f57caea339d5f8eeabb7b12d20e0a1562e39 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 30 Apr 2025 06:31:16 +0000 Subject: [PATCH 4/6] Minor cleanup Signed-off-by: Michael X. Grey --- rmf_building_sim_gz_plugins/src/lift.cpp | 7 +------ rmf_robot_sim_gz_plugins/src/slotcar.cpp | 1 - 2 files changed, 1 insertion(+), 7 deletions(-) diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index c0fbd1a..aef5d65 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -8,13 +8,11 @@ #include #include #include -#include #include #include +#include #include #include -#include -#include #include #include #include @@ -205,9 +203,6 @@ class LiftPlugin return ""; } - // const auto lift_pos = - // ecm.Component(joint_entity)->Data()[0]; - const auto lift_pos = ecm.Component(entity)->Data().Z(); diff --git a/rmf_robot_sim_gz_plugins/src/slotcar.cpp b/rmf_robot_sim_gz_plugins/src/slotcar.cpp index b8aed0f..ab1046f 100644 --- a/rmf_robot_sim_gz_plugins/src/slotcar.cpp +++ b/rmf_robot_sim_gz_plugins/src/slotcar.cpp @@ -251,7 +251,6 @@ void SlotcarPlugin::send_control_signals(EntityComponentManager& ecm, gz::math::Vector3d ang_vel_cmd(0, 0, 0); ang_vel_cmd[2] = target_vels[1]; - ecm.SetComponentData(_entity, lin_vel_cmd); ecm.SetComponentData(_entity, ang_vel_cmd); From d1e0b9241a75e5599716fa5c282efcb5b6fcf656 Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Wed, 30 Apr 2025 07:05:08 +0000 Subject: [PATCH 5/6] Remove style tests Signed-off-by: Michael X. Grey --- .github/workflows/style.yaml | 22 ---------------------- 1 file changed, 22 deletions(-) delete mode 100644 .github/workflows/style.yaml diff --git a/.github/workflows/style.yaml b/.github/workflows/style.yaml deleted file mode 100644 index 165f170..0000000 --- a/.github/workflows/style.yaml +++ /dev/null @@ -1,22 +0,0 @@ -name: style -on: - pull_request: -defaults: - run: - shell: bash -jobs: - linter: - runs-on: ubuntu-latest - strategy: - matrix: - docker_image: ['ros:rolling-ros-base'] - container: - image: ${{ matrix.docker_image }} - steps: - - name: checkout - uses: actions/checkout@v2 - - name: uncrustify - run: | - sudo apt update && sudo apt install wget - wget https://raw.githubusercontent.com/open-rmf/rmf_utils/main/rmf_utils/test/format/rmf_code_style.cfg - /ros_entrypoint.sh ament_uncrustify -c rmf_code_style.cfg . --language C++ From 804d1b5e847d1830e7efb3eacda9187f14144e8f Mon Sep 17 00:00:00 2001 From: "Michael X. Grey" Date: Mon, 5 May 2025 04:56:30 +0000 Subject: [PATCH 6/6] Trigger CI Signed-off-by: Michael X. Grey