diff --git a/config/simulator.yaml b/config/simulator.yaml index 46934c08..45a259b7 100644 --- a/config/simulator.yaml +++ b/config/simulator.yaml @@ -9,6 +9,7 @@ simulator: save_rate: 1.0 save_history: 4096 headless: false + motor_timeout: 100 ref_heading: 90.0 # For the GPS sensor to work diff --git a/simulator/simulator.cpp b/simulator/simulator.cpp index e8f19e9c..897a1c99 100644 --- a/simulator/simulator.cpp +++ b/simulator/simulator.cpp @@ -18,6 +18,8 @@ namespace mrover { mIkTargetPub = create_publisher("arm_ik", 1); + mMotorTimeoutMs = get_parameter("motor_timeout").as_int(); + mIsHeadless = get_parameter("headless").as_bool(); mEnablePhysics = mIsHeadless; { diff --git a/simulator/simulator.hpp b/simulator/simulator.hpp index 65492a94..959d0d70 100644 --- a/simulator/simulator.hpp +++ b/simulator/simulator.hpp @@ -263,6 +263,8 @@ namespace mrover { R3d mOrientationDrift = R3d::Zero(); bool mIsHeadless{}; + + int64_t mMotorTimeoutMs{}; // Rendering diff --git a/simulator/simulator.physics.cpp b/simulator/simulator.physics.cpp index d902bff3..7e5c33de 100644 --- a/simulator/simulator.physics.cpp +++ b/simulator/simulator.physics.cpp @@ -13,8 +13,6 @@ namespace mrover { constexpr double TAU = 2 * std::numbers::pi; - constexpr int MOTOR_TIMEOUT_MS = 20; - auto btTransformToSe3(btTransform const& transform) -> SE3d { btVector3 const& p = transform.getOrigin(); btQuaternion const& q = transform.getRotation(); @@ -58,7 +56,7 @@ namespace mrover { // check if arm motor commands have expired // TODO: fix hard-coded names? for (auto const& name: {"arm_a_link", "arm_b_link", "arm_c_link", "arm_d_link", "arm_e_link"}) { - bool expired = std::chrono::duration_cast(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > MOTOR_TIMEOUT_MS; + bool expired = std::chrono::duration_cast(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > mMotorTimeoutMs; if (expired) { int linkIndex = rover.linkNameToMeta.at(name).index; auto* motor = std::bit_cast(rover.physics->getLink(linkIndex).m_userPtr);