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

Verified

This commit was created on GitHub.com and signed with GitHub’s verified signature. The key has expired.
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.