Merge pull request heuristicus#55 from ori-drs/feature/settable-estop…

…-timout

Add ros param to set the estop timeout, wrapper takes it as an arg

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>

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 #

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)