From e66f15876dd8697bfe0660ee9aaf4f963363be24 Mon Sep 17 00:00:00 2001
From: Michal Staniaszek <m.staniaszek@gmail.com>
Date: Wed, 20 Oct 2021 10:21:24 +0100
Subject: [PATCH] add ros param to set the estop timeout, wrapper takes it as
 an arg

---
 spot_driver/launch/driver.launch            | 2 ++
 spot_driver/src/spot_driver/spot_ros.py     | 3 ++-
 spot_driver/src/spot_driver/spot_wrapper.py | 5 +++--
 3 files changed, 7 insertions(+), 3 deletions(-)

diff --git a/spot_driver/launch/driver.launch b/spot_driver/launch/driver.launch
index 8fc4425d..e19b091f 100644
--- a/spot_driver/launch/driver.launch
+++ b/spot_driver/launch/driver.launch
@@ -2,6 +2,7 @@
   <arg name="username" default="dummyusername" />
   <arg name="password" default="dummypassword" />
   <arg name="hostname" default="192.168.50.3" />
+  <arg name="estop_timeout" default="9.0"/>
 
   <include file="$(find spot_description)/launch/description.launch" />
   <include file="$(find spot_driver)/launch/control.launch" />
@@ -12,6 +13,7 @@
     <param name="username" value="$(arg username)" />
     <param name="password" value="$(arg password)" />
     <param name="hostname" value="$(arg hostname)" />
+    <param name="estop_timeout" value="$(arg estop_timeout)"/>
     <remap from="joint_states" to="/joint_states"/>
     <remap from="tf" to="/tf"/>
   </node>
diff --git a/spot_driver/src/spot_driver/spot_ros.py b/spot_driver/src/spot_driver/spot_ros.py
index a26b3f0e..bbfb9206 100644
--- a/spot_driver/src/spot_driver/spot_ros.py
+++ b/spot_driver/src/spot_driver/spot_ros.py
@@ -496,6 +496,7 @@ def main(self):
         self.password = rospy.get_param('~password', 'default_value')
         self.hostname = rospy.get_param('~hostname', 'default_value')
         self.motion_deadzone = rospy.get_param('~deadzone', 0.05)
+        self.estop_timeout = rospy.get_param('~estop_timeout', 9.0)
 
         self.camera_static_transform_broadcaster = tf2_ros.StaticTransformBroadcaster()
         # Static transform broadcaster is super simple and just a latched publisher. Every time we add a new static
@@ -519,7 +520,7 @@ def main(self):
         self.logger = logging.getLogger('rosout')
 
         rospy.loginfo("Starting ROS driver for Spot")
-        self.spot_wrapper = SpotWrapper(self.username, self.password, self.hostname, self.logger, self.rates, self.callbacks)
+        self.spot_wrapper = SpotWrapper(self.username, self.password, self.hostname, self.logger, self.estop_timeout, self.rates, self.callbacks)
 
         if self.spot_wrapper.is_valid:
             # Images #
diff --git a/spot_driver/src/spot_driver/spot_wrapper.py b/spot_driver/src/spot_driver/spot_wrapper.py
index 70243108..a9f66e77 100644
--- a/spot_driver/src/spot_driver/spot_wrapper.py
+++ b/spot_driver/src/spot_driver/spot_wrapper.py
@@ -206,13 +206,14 @@ def _start_query(self):
 
 class SpotWrapper():
     """Generic wrapper class to encompass release 1.1.4 API features as well as maintaining leases automatically"""
-    def __init__(self, username, password, hostname, logger, rates = {}, callbacks = {}):
+    def __init__(self, username, password, hostname, logger, estop_timeout=9.0, rates = {}, callbacks = {}):
         self._username = username
         self._password = password
         self._hostname = hostname
         self._logger = logger
         self._rates = rates
         self._callbacks = callbacks
+        self._estop_timeout = estop_timeout
         self._keep_alive = True
         self._valid = True
 
@@ -422,7 +423,7 @@ def updateTasks(self):
 
     def resetEStop(self):
         """Get keepalive for eStop"""
-        self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', 9.0)
+        self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros', self._estop_timeout)
         self._estop_endpoint.force_simple_setup()  # Set this endpoint as the robot's sole estop.
         self._estop_keepalive = EstopKeepAlive(self._estop_endpoint)