Merge branch 'add-velocity-limit' into rviz-plugin-with-velocities

spot_driver/src/spot_driver/ros_helpers.py

@@ -215,6 +215,7 @@ def GetEStopStateFromState(state, spot_wrapper):
         estop_msg.name = estop.name
         estop_msg.type = estop.type
         estop_msg.state = estop.state
+        estop_msg.state_description = estop.state_description
         estop_array_msg.estop_states.append(estop_msg)
 
     return estop_array_msg

spot_driver/src/spot_driver/spot_ros.py

@@ -11,7 +11,7 @@
 
 from bosdyn.api.spot import robot_command_pb2 as spot_command_pb2
 from bosdyn.api import geometry_pb2, trajectory_pb2
-from bosdyn.api.geometry_pb2 import Quaternion
+from bosdyn.api.geometry_pb2 import Quaternion, SE2VelocityLimit
 from bosdyn.client import math_helpers
 import actionlib
 import functools
@@ -31,7 +31,10 @@
 from spot_msgs.msg import MobilityParams
 from spot_msgs.msg import NavigateToAction, NavigateToResult, NavigateToFeedback
 from spot_msgs.msg import TrajectoryAction, TrajectoryResult, TrajectoryFeedback
-from spot_msgs.srv import ListGraph, ListGraphResponse, SetLocomotion, SetLocomotionResponse, ClearBehaviorFault, ClearBehaviorFaultResponse
+from spot_msgs.srv import ListGraph, ListGraphResponse
+from spot_msgs.srv import SetLocomotion, SetLocomotionResponse
+from spot_msgs.srv import ClearBehaviorFault, ClearBehaviorFaultResponse
+from spot_msgs.srv import SetVelocity, SetVelocityResponse
 
 from .ros_helpers import *
 from .spot_wrapper import SpotWrapper
@@ -277,7 +280,8 @@ def handle_estop_hard(self, req):
         return TriggerResponse(resp[0], resp[1])
 
     def handle_estop_soft(self, req):
-        """ROS service handler to soft-eStop the robot.  The robot will try to settle on the ground before cutting power to the motors"""
+        """ROS service handler to soft-eStop the robot.  The robot will try to settle on the ground before cutting
+        power to the motors """
         resp = self.spot_wrapper.assertEStop(False)
         return TriggerResponse(resp[0], resp[1])
 
@@ -286,7 +290,7 @@ def handle_estop_disengage(self, req):
         resp = self.spot_wrapper.disengageEStop()
         return TriggerResponse(resp[0], resp[1])
 
-    def handle_clear_bahavior_fault(self, req):
+    def handle_clear_behavior_fault(self, req):
         """ROS service handler for clearing behavior faults"""
         resp = self.spot_wrapper.clear_behavior_fault(req.id)
         return ClearBehaviorFaultResponse(resp[0], resp[1])
@@ -311,6 +315,27 @@ def handle_locomotion_mode(self, req):
         except Exception as e:
             return SetLocomotionResponse(False, 'Error:{}'.format(e))
 
+    def handle_max_vel(self, req):
+        """
+        Handle a max_velocity service call. This will modify the mobility params to have a limit on the maximum
+        velocity that the robot can move during motion commmands. This affects trajectory commands and velocity
+        commands
+
+        Args:
+            req: SetVelocityRequest containing requested maximum velocity
+
+        Returns: SetVelocityResponse
+        """
+        try:
+            mobility_params = self.spot_wrapper.get_mobility_params()
+            mobility_params.vel_limit.CopyFrom(SE2VelocityLimit(max_vel=math_helpers.SE2Velocity(req.velocity_limit.linear.x,
+                                                                                                 req.velocity_limit.linear.y,
+                                                                                                 req.velocity_limit.angular.z).to_proto()))
+            self.spot_wrapper.set_mobility_params(mobility_params)
+            return SetVelocityResponse(True, 'Success')
+        except Exception as e:
+            return SetVelocityResponse(False, 'Error:{}'.format(e))
+
     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':
@@ -552,10 +577,10 @@ def main(self):
             rospy.Service("estop/gentle", Trigger, self.handle_estop_soft)
             rospy.Service("estop/release", Trigger, self.handle_estop_disengage)
 
-
             rospy.Service("stair_mode", SetBool, self.handle_stair_mode)
             rospy.Service("locomotion_mode", SetLocomotion, self.handle_locomotion_mode)
-            rospy.Service("clear_behavior_fault", ClearBehaviorFault, self.handle_clear_bahavior_fault)
+            rospy.Service("max_velocity", SetVelocity, self.handle_max_vel)
+            rospy.Service("clear_behavior_fault", ClearBehaviorFault, self.handle_clear_behavior_fault)
 
             rospy.Service("list_graph", ListGraph, self.handle_list_graph)
 

spot_driver/src/spot_driver/spot_wrapper.py

@@ -361,6 +361,13 @@ def time_skew(self):
         """Return the time skew between local and spot time"""
         return self._robot.time_sync.endpoint.clock_skew
 
+    def resetMobilityParams(self):
+        """
+        Resets the mobility parameters used for motion commands to the default values provided by the bosdyn api.
+        Returns:
+        """
+        self._mobility_params = RobotCommandBuilder.mobility_params()
+
     def robotToLocalTime(self, timestamp):
         """Takes a timestamp and an estimated skew and return seconds and nano seconds in local time
 
@@ -523,8 +530,8 @@ def power_on(self):
         try:
             power.power_on(self._power_client)
             return True, "Success"
-        except:
-            return False, "Error"
+        except Exception as e:
+            return False, str(e)
 
     def set_mobility_params(self, mobility_params):
         """Set Params for mobility and movement
@@ -574,7 +581,7 @@ def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name=
             body_tform_goal = math_helpers.SE3Pose(x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading))
             vision_tform_goal = vision_tform_body * body_tform_goal
             response = self._robot_command(
-                            RobotCommandBuilder.trajectory_command(
+                            RobotCommandBuilder.synchro_se2_trajectory_point_command(
                                 goal_x=vision_tform_goal.x,
                                 goal_y=vision_tform_goal.y,
                                 goal_heading=vision_tform_goal.rot.to_yaw(),
@@ -588,7 +595,7 @@ def trajectory_cmd(self, goal_x, goal_y, goal_heading, cmd_duration, frame_name=
             body_tform_goal = math_helpers.SE3Pose(x=goal_x, y=goal_y, z=0, rot=math_helpers.Quat.from_yaw(goal_heading))
             odom_tform_goal = odom_tform_body * body_tform_goal
             response = self._robot_command(
-                            RobotCommandBuilder.trajectory_command(
+                            RobotCommandBuilder.synchro_se2_trajectory_point_command(
                                 goal_x=odom_tform_goal.x,
                                 goal_y=odom_tform_goal.y,
                                 goal_heading=odom_tform_goal.rot.to_yaw(),

spot_msgs/CMakeLists.txt

@@ -38,6 +38,7 @@ add_service_files(
   FILES
   ListGraph.srv
   SetLocomotion.srv
+  SetVelocity.srv
   ClearBehaviorFault.srv
 )
 

spot_msgs/srv/SetVelocity.srv

@@ -0,0 +1,6 @@
+# The api only takes into account x and y for linear velocity, and z for angular.
+# Other values are ignored.
+geometry_msgs/Twist velocity_limit
+---
+bool success
+string message