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++ 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 ee5d3a9..aef5d65 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -8,9 +8,11 @@ #include #include #include +#include #include -#include #include +#include +#include #include #include #include @@ -106,11 +108,10 @@ class LiftPlugin { 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; @@ -203,7 +204,7 @@ class LiftPlugin } 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 +285,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()) @@ -365,6 +371,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) @@ -372,8 +383,10 @@ 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}; + + ecm.SetComponentData(joint_entity, {target_vel}); + ecm.RemoveComponent(joint_entity); + _last_cmd_vel[joint_entity] = target_vel; } return target_vel; @@ -533,21 +546,41 @@ class LiftPlugin 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); @@ -558,8 +591,10 @@ class LiftPlugin 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,18 +603,18 @@ 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()); + // 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 b8d7573..ab1046f 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: @@ -211,9 +213,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 +238,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 +246,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]; @@ -454,6 +453,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) { @@ -523,6 +532,7 @@ GZ_ADD_PLUGIN( SlotcarPlugin, System, SlotcarPlugin::ISystemConfigure, + SlotcarPlugin::ISystemConfigurePriority, SlotcarPlugin::ISystemPreUpdate) GZ_ADD_PLUGIN_ALIAS(SlotcarPlugin, "slotcar")