changed motor timeout to ros param

umrover · Nov 24, 2024 · d3c4b2d · d3c4b2d
1 parent 899b524
commit d3c4b2d
Show file tree

Hide file tree

Showing 4 changed files with 6 additions and 3 deletions.
diff --git 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
@@ -18,6 +18,8 @@ namespace mrover {
 
             mIkTargetPub = create_publisher<msg::IK>("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
@@ -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
@@ -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<std::chrono::milliseconds>(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > MOTOR_TIMEOUT_MS;
+                bool expired = std::chrono::duration_cast<std::chrono::milliseconds>(Clock::now() - rover.linkNameToMeta.at(name).lastUpdate).count() > mMotorTimeoutMs;
                 if (expired) {
                     int linkIndex = rover.linkNameToMeta.at(name).index;
                     auto* motor = std::bit_cast<btMultiBodyJointMotor*>(rover.physics->getLink(linkIndex).m_userPtr);