Skip to content

Commit

Permalink
Merge branch 'master' into roll_over
Browse files Browse the repository at this point in the history
heuristicus authored Oct 28, 2022
2 parents 335aa46 + 99349c5 commit 55d2966
Showing 7 changed files with 107 additions and 7 deletions.
16 changes: 16 additions & 0 deletions spot_driver/src/spot_driver/ros_helpers.py
Original file line number Diff line number Diff line change
@@ -20,6 +20,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 DockState

from bosdyn.api import image_pb2
from bosdyn.client.math_helpers import SE3Pose
@@ -443,6 +444,21 @@ def GetPowerStatesFromState(state, spot_wrapper):
power_state_msg.locomotion_estimated_runtime = rospy.Time(state.power_state.locomotion_estimated_runtime.seconds, state.power_state.locomotion_estimated_runtime.nanos)
return power_state_msg

def GetDockStatesFromState(state):
"""Maps dock state data from robot state proto to ROS DockState message
Args:
state: Robot State proto
Returns:
DockState message
"""
dock_state_msg = DockState()
dock_state_msg.status = state.status
dock_state_msg.dock_type = state.dock_type
dock_state_msg.dock_id = state.dock_id
dock_state_msg.power_status = state.power_status
return dock_state_msg

def getBehaviorFaults(behavior_faults, spot_wrapper):
"""Helper function to strip out behavior faults into a list
22 changes: 21 additions & 1 deletion spot_driver/src/spot_driver/spot_ros.py
Original file line number Diff line number Diff line change
@@ -42,6 +42,7 @@
from spot_msgs.srv import SetObstacleParams, SetObstacleParamsResponse
from spot_msgs.srv import ClearBehaviorFault, ClearBehaviorFaultResponse
from spot_msgs.srv import SetVelocity, SetVelocityResponse
from spot_msgs.srv import Dock, DockResponse, GetDockState, GetDockStateResponse
from spot_msgs.srv import PosedStand, PosedStandResponse
from spot_msgs.srv import SetSwingHeight, SetSwingHeightResponse
from spot_msgs.srv import ArmJointMovement, ArmJointMovementResponse, ArmJointMovementRequest
@@ -762,6 +763,21 @@ def handle_roll_over_left(self, req):
resp = self.spot_wrapper.battery_change_pose(2)
return TriggerResponse(resp[0], resp[1])

def handle_dock(self, req):
"""Dock the robot"""
resp = self.spot_wrapper.dock(req.dock_id)
return DockResponse(resp[0], resp[1])

def handle_undock(self, req):
"""Undock the robot"""
resp = self.spot_wrapper.undock()
return TriggerResponse(resp[0], resp[1])

def handle_get_docking_state(self, req):
"""Get docking state of robot"""
resp = self.spot_wrapper.get_docking_state()
return GetDockStateResponse(GetDockStatesFromState(resp))

def _send_trajectory_command(self, pose, duration, precise=True):
"""
Send a trajectory command to the robot
@@ -1229,7 +1245,10 @@ def main(self):

rospy.Service("roll_over_right", Trigger, self.handle_roll_over_right)
rospy.Service("roll_over_left", Trigger, self.handle_roll_over_left)

# Docking
rospy.Service("dock", Dock, self.handle_dock)
rospy.Service("undock", Trigger, self.handle_undock)
rospy.Service("docking_state", GetDockState, self.handle_get_docking_state)
# Arm Services #########################################
rospy.Service("arm_stow", Trigger, self.handle_arm_stow)
rospy.Service("arm_unstow", Trigger, self.handle_arm_unstow)
@@ -1247,6 +1266,7 @@ def main(self):
auto_start = False)
self.navigate_as.start()


self.trajectory_server = actionlib.SimpleActionServer("trajectory", TrajectoryAction,
execute_cb=self.handle_trajectory,
auto_start=False)
45 changes: 39 additions & 6 deletions spot_driver/src/spot_driver/spot_wrapper.py
Original file line number Diff line number Diff line change
@@ -14,6 +14,8 @@
from bosdyn.client.power import safe_power_off, PowerClient, power_on
from bosdyn.client.lease import LeaseClient, LeaseKeepAlive
from bosdyn.client.image import ImageClient, build_image_request
from bosdyn.client.docking import DockingClient, blocking_dock_robot, blocking_undock
from bosdyn.api import image_pb2
from bosdyn.api import estop_pb2, image_pb2
from bosdyn.api.graph_nav import graph_nav_pb2
from bosdyn.api.graph_nav import map_pb2
@@ -22,6 +24,7 @@
from bosdyn.client import power
from bosdyn.client import frame_helpers
from bosdyn.client import math_helpers
from bosdyn.client import robot_command
from bosdyn.client.exceptions import InternalServerError

from . import graph_nav_util
@@ -219,13 +222,11 @@ def _start_query(self):

self._spot_wrapper._is_moving = is_moving

# We must check if any command currently has a non-None value for its id. If we don't do this, this stand
# command can cause other commands to be interrupted before they get to start
if (self._spot_wrapper.is_standing and not self._spot_wrapper.is_moving
and self._spot_wrapper._last_trajectory_command is not None
and self._spot_wrapper._last_stand_command is not None
and self._spot_wrapper._last_velocity_command_time is not None
):
and self._spot_wrapper._last_trajectory_command is not None
and self._spot_wrapper._last_stand_command is not None
and self._spot_wrapper._last_velocity_command_time is not None
and self._spot_wrapper._last_docking_command is not None):
self._spot_wrapper.stand(False)

class AsyncEStopMonitor(AsyncPeriodicQuery):
@@ -281,6 +282,7 @@ def __init__(self, username, password, hostname, logger, estop_timeout=9.0, rate
self._last_trajectory_command = None
self._last_trajectory_command_precise = None
self._last_velocity_command_time = None
self._last_docking_command = None

self._front_image_requests = []
for source in front_image_sources:
@@ -342,6 +344,7 @@ def __init__(self, username, password, hostname, logger, estop_timeout=9.0, rate
self._lease_wallet = self._lease_client.lease_wallet
self._image_client = self._robot.ensure_client(ImageClient.default_service_name)
self._estop_client = self._robot.ensure_client(EstopClient.default_service_name)
self._docking_client = self._robot.ensure_client(DockingClient.default_service_name)
initialised = True
except Exception as e:
sleep_secs = 15
@@ -1442,3 +1445,33 @@ def _match_edge(self, current_edges, waypoint1, waypoint2):
# This edge matches the pair of waypoints! Add it the edge list and continue.
return map_pb2.Edge.Id(from_waypoint=waypoint1, to_waypoint=waypoint2)
return None

def dock(self, dock_id):
"""Dock the robot to the docking station with fiducial ID [dock_id]."""
try:
# Make sure we're powered on and standing
self._robot.power_on()
self.stand()
# Dock the robot
self.last_docking_command = dock_id
blocking_dock_robot(self._robot, dock_id)
self.last_docking_command = None
return True, "Success"
except Exception as e:
return False, str(e)

def undock(self, timeout=20):
"""Power motors on and undock the robot from the station."""
try:
# Maker sure we're powered on
self._robot.power_on()
# Undock the robot
blocking_undock(self._robot ,timeout)
return True, "Success"
except Exception as e:
return False, str(e)

def get_docking_state(self, **kwargs):
"""Get docking state of robot."""
state = self._docking_client.get_docking_state(**kwargs)
return state
3 changes: 3 additions & 0 deletions spot_msgs/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -32,6 +32,7 @@ add_message_files(
LeaseResource.msg
PowerState.msg
SystemFaultState.msg
DockState.msg
ObstacleParams.msg
TerrainParams.msg
TerrainState.msg
@@ -52,6 +53,8 @@ add_service_files(
SetObstacleParams.srv
SetTerrainParams.srv
PosedStand.srv
Dock.srv
GetDockState.srv
)

add_action_files(
22 changes: 22 additions & 0 deletions spot_msgs/msg/DockState.msg
Original file line number Diff line number Diff line change
@@ -0,0 +1,22 @@
# Status
uint8 DOCK_STATUS_UNKNOWN = 0
uint8 DOCK_STATUS_DOCKED = 1
uint8 DOCK_STATUS_DOCKING = 2
uint8 DOCK_STATUS_UNDOCKED = 3
uint8 DOCK_STATUS_UNDOCKING = 4

# DockType
uint8 DOCK_TYPE_UNKNOWN = 0
uint8 DOCK_TYPE_CONTACT_PROTOTYPE = 2
uint8 DOCK_TYPE_SPOT_DOCK = 3

# LinkStatus
uint8 LINK_STATUS_UNKNOWN = 0
uint8 LINK_STATUS_CONNECTED = 1
uint8 LINK_STATUS_ERROR = 2
uint8 LINK_STATUS_DETECTING = 3

uint8 status
uint8 dock_type
uint32 dock_id
uint8 power_status
4 changes: 4 additions & 0 deletions spot_msgs/srv/Dock.srv
Original file line number Diff line number Diff line change
@@ -0,0 +1,4 @@
uint32 dock_id # dock fiducials id
---
bool success # indicate successful run of triggered service
string message # informational, e.g. for error messages
2 changes: 2 additions & 0 deletions spot_msgs/srv/GetDockState.srv
Original file line number Diff line number Diff line change
@@ -0,0 +1,2 @@
---
DockState dock_state

0 comments on commit 55d2966

Please sign in to comment.