diff --git a/rmf_building_sim_gz_plugins/src/lift.cpp b/rmf_building_sim_gz_plugins/src/lift.cpp index 3075103..042b2a2 100644 --- a/rmf_building_sim_gz_plugins/src/lift.cpp +++ b/rmf_building_sim_gz_plugins/src/lift.cpp @@ -9,7 +9,6 @@ #include #include #include -#include #include #include #include @@ -106,9 +105,6 @@ class GZ_SIM_VISIBLE LiftPlugin Model(entity).JointByName(ecm, lift.cabin_joint); enableComponent(ecm, entity); enableComponent(ecm, cabin_joint_entity); - enableComponent(ecm, cabin_joint_entity); - ecm.CreateComponent(cabin_joint_entity, - components::JointVelocityCmd({0.0})); LiftCommand lift_command; lift_command.request_type = LiftRequest::REQUEST_AGV_MODE; @@ -352,8 +348,9 @@ class GZ_SIM_VISIBLE 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}; + std::vector joint_position = {cur_elevation + target_vel * dt}; + ecm.CreateComponent(joint_entity, + components::JointPositionReset{joint_position}); _last_cmd_vel[joint_entity] = target_vel; } return target_vel;