convert trajectory command to an actionserver

spot_driver/scripts/spot_ros.py

@@ -12,6 +12,8 @@
 
 from bosdyn.api.geometry_pb2 import Quaternion
 from bosdyn.client import math_helpers
+import actionlib
+import functools
 import bosdyn.geometry
 
 from spot_msgs.msg import Metrics
@@ -23,8 +25,7 @@
 from spot_msgs.msg import BehaviorFault, BehaviorFaultState
 from spot_msgs.msg import SystemFault, SystemFaultState
 from spot_msgs.msg import BatteryState, BatteryStateArray
-from spot_msgs.msg import Feedback
-from spot_msgs.srv import Trajectory, TrajectoryResponse
+from spot_msgs.msg import Feedback, TrajectoryAction, TrajectoryResult, TrajectoryFeedback
 
 from ros_helpers import *
 from spot_wrapper import SpotWrapper
@@ -270,10 +271,11 @@ def handle_estop_soft(self, req):
         return TriggerResponse(resp[0], resp[1])
 
     def handle_trajectory(self, req):
-        """
-        """
+        """ROS actionserver execution handler to handle receiving a request to move to a location"""
         if req.target_pose.header.frame_id != 'body':
-            return TrajectoryResponse(False, 'frame_id of target_pose must be \'body\'')
+            self.trajectory_server.set_aborted(TrajectoryResult(False, 'frame_id of target_pose must be \'body\''))
+            return
+        cmd_duration = rospy.Duration(req.duration.data.secs, req.duration.data.nsecs)
         resp = self.spot_wrapper.trajectory_cmd(
                         goal_x=req.target_pose.pose.position.x,
                         goal_y=req.target_pose.pose.position.y,
@@ -283,9 +285,39 @@ def handle_trajectory(self, req):
                             y=req.target_pose.pose.orientation.y,
                             z=req.target_pose.pose.orientation.z
                             ).to_yaw(),
-                        cmd_duration=req.duration
+                        cmd_duration=cmd_duration.to_sec()
                         )
-        return TrajectoryResponse(resp[0], resp[1])
+
+        def timeout_cb(trajectory_server, _):
+            trajectory_server.publish_feedback(TrajectoryFeedback("Failed to reach goal, timed out"))
+            trajectory_server.set_aborted(TrajectoryResult(False, "Failed to reach goal, timed out"))
+
+        # Abort the actionserver if cmd_duration is exceeded - the driver stops but does not provide feedback to
+        # indicate this so we monitor it ourselves
+        cmd_timeout = rospy.Timer(cmd_duration, functools.partial(timeout_cb, self.trajectory_server), oneshot=True)
+
+        # The trajectory command is non-blocking but we need to keep this function up in order to interrupt if a
+        # preempt is requested and to return success if/when the robot reaches the goal. Also check the is_active to
+        # monitor whether the timeout_cb has already aborted the command
+        rate = rospy.Rate(10)
+        while not rospy.is_shutdown() and not self.trajectory_server.is_preempt_requested() and not self.spot_wrapper.at_goal and self.trajectory_server.is_active():
+            self.trajectory_server.publish_feedback(TrajectoryFeedback("Moving to goal"))
+            rate.sleep()
+
+        # If still active after exiting the loop, the command did not time out
+        if self.trajectory_server.is_active():
+            cmd_timeout.shutdown()
+            if self.trajectory_server.is_preempt_requested():
+                self.trajectory_server.publish_feedback(TrajectoryFeedback("Preempted"))
+                self.trajectory_server.set_preempted()
+                self.spot_wrapper.stop()
+
+            if self.spot_wrapper.at_goal:
+                self.trajectory_server.publish_feedback(TrajectoryFeedback("Reached goal"))
+                self.trajectory_server.set_succeeded(TrajectoryResult(resp[0], resp[1]))
+            else:
+                self.trajectory_server.publish_feedback(TrajectoryFeedback("Failed to reach goal"))
+                self.trajectory_server.set_aborted(TrajectoryResult(False, "Failed to reach goal"))
 
     def cmdVelCallback(self, data):
         """Callback for cmd_vel command"""
@@ -384,8 +416,6 @@ def main(self):
             rospy.Subscriber('cmd_vel', Twist, self.cmdVelCallback, queue_size = 1)
             rospy.Subscriber('body_pose', Pose, self.bodyPoseCallback, queue_size = 1)
 
-            rospy.Service("trajectory", Trajectory, self.handle_trajectory)
-
             rospy.Service("claim", Trigger, self.handle_claim)
             rospy.Service("release", Trigger, self.handle_release)
             rospy.Service("stop", Trigger, self.handle_stop)
@@ -398,6 +428,11 @@ def main(self):
             rospy.Service("estop/hard", Trigger, self.handle_estop_hard)
             rospy.Service("estop/gentle", Trigger, self.handle_estop_soft)
 
+            self.trajectory_server = actionlib.SimpleActionServer("trajectory", TrajectoryAction,
+                                                                  execute_cb=self.handle_trajectory,
+                                                                  auto_start=False)
+            self.trajectory_server.start()
+
             rospy.on_shutdown(self.shutdown)
 
             self.auto_claim = rospy.get_param('~auto_claim', False)

spot_driver/scripts/spot_wrapper.py

@@ -15,6 +15,7 @@
 from bosdyn.client import power
 from bosdyn.client import frame_helpers
 from bosdyn.client import math_helpers
+from bosdyn.client.exceptions import InternalServerError
 
 import bosdyn.api.robot_state_pb2 as robot_state_proto
 from bosdyn.api import basic_command_pb2
@@ -174,6 +175,11 @@ def _start_query(self):
                 if (response.feedback.mobility_feedback.se2_trajectory_feedback.status ==
                     basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_GOING_TO_GOAL):
                     is_moving = True
+                elif (response.feedback.mobility_feedback.se2_trajectory_feedback.status ==
+                    basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL):
+                    self._spot_wrapper._at_goal = True
+                    # Clear the command once at the goal
+                    self._spot_wrapper._last_trajectory_command = None
                 else:
                     self._spot_wrapper._last_trajectory_command = None
             except (ResponseError, RpcError) as e:
@@ -201,6 +207,7 @@ def __init__(self, username, password, hostname, logger, rates = {}, callbacks =
         self._is_standing = False
         self._is_sitting = True
         self._is_moving = False
+        self._at_goal = False
         self._last_stand_command = None
         self._last_sit_command = None
         self._last_trajectory_command = None
@@ -322,6 +329,10 @@ def is_moving(self):
         """Return boolean of walking state"""
         return self._is_moving
 
+    @property
+    def at_goal(self):
+        return self._at_goal
+
     @property
     def time_skew(self):
         """Return the time skew between local and spot time"""
@@ -508,6 +519,8 @@ def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name=
             cmd_duration: Time-to-live for the command in seconds.
             frame_name: frame_name to be used to calc the target position. 'odom' or 'vision'
         """
+        self._at_goal = False
+        self._logger.info("got command duration of {}".format(cmd_duration))
         end_time=time.time() + cmd_duration
         if frame_name == 'vision':
             vision_tform_body = frame_helpers.get_vision_tform_body(

spot_msgs/CMakeLists.txt

@@ -6,6 +6,7 @@ find_package(catkin REQUIRED COMPONENTS
   nav_msgs
   sensor_msgs
   std_msgs
+  actionlib_msgs
   message_generation
 )
 
@@ -31,15 +32,16 @@ add_message_files(
   SystemFaultState.msg
 )
 
-add_service_files(
+add_action_files(
   FILES
-  Trajectory.srv
+  Trajectory.action
 )
 
 generate_messages(
   DEPENDENCIES
   std_msgs
   geometry_msgs
+  actionlib_msgs
 )
 
 catkin_package(

spot_msgs/srv/Trajectory.srv → spot_msgs/action/Trajectory.action

@@ -1,5 +1,7 @@
 geometry_msgs/PoseStamped target_pose
-float64 duration
+std_msgs/Duration duration
 ---
 bool success
 string message
+---
+string feedback