class SpotROS():
"""Parent class for using the wrapper. Defines all callbacks and keeps the wrapper alive"""
def __init__(self):
self.spot_wrapper = None
self.callbacks = {}
"""Dictionary listing what callback to use for what data task"""
self.callbacks["robot_state"] = self.RobotStateCB
self.callbacks["metrics"] = self.MetricsCB
self.callbacks["lease"] = self.LeaseCB
self.callbacks["front_image"] = self.FrontImageCB
self.callbacks["side_image"] = self.SideImageCB
self.callbacks["rear_image"] = self.RearImageCB
def RobotStateCB(self, results):
"""Callback for when the Spot Wrapper gets new robot state data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
state = self.spot_wrapper.robot_state
if state:
## joint states ##
joint_state = GetJointStatesFromState(state, self.spot_wrapper)
self.joint_state_pub.publish(joint_state)
## TF ##
tf_msg = GetTFFromState(state, self.spot_wrapper, self.mode_parent_odom_tf)
if len(tf_msg.transforms) > 0:
self.tf_pub.publish(tf_msg)
# Odom Twist #
twist_odom_msg = GetOdomTwistFromState(state, self.spot_wrapper)
self.odom_twist_pub.publish(twist_odom_msg)
# Odom #
if self.mode_parent_odom_tf == 'vision':
odom_msg = GetOdomFromState(state, self.spot_wrapper, use_vision=True)
else:
odom_msg = GetOdomFromState(state, self.spot_wrapper, use_vision=False)
self.odom_pub.publish(odom_msg)
# Feet #
foot_array_msg = GetFeetFromState(state, self.spot_wrapper)
self.feet_pub.publish(foot_array_msg)
# EStop #
estop_array_msg = GetEStopStateFromState(state, self.spot_wrapper)
self.estop_pub.publish(estop_array_msg)
# WIFI #
wifi_msg = GetWifiFromState(state, self.spot_wrapper)
self.wifi_pub.publish(wifi_msg)
# Battery States #
battery_states_array_msg = GetBatteryStatesFromState(state, self.spot_wrapper)
self.battery_pub.publish(battery_states_array_msg)
# Power State #
power_state_msg = GetPowerStatesFromState(state, self.spot_wrapper)
self.power_pub.publish(power_state_msg)
# System Faults #
system_fault_state_msg = GetSystemFaultsFromState(state, self.spot_wrapper)
self.system_faults_pub.publish(system_fault_state_msg)
# Behavior Faults #
behavior_fault_state_msg = getBehaviorFaultsFromState(state, self.spot_wrapper)
self.behavior_faults_pub.publish(behavior_fault_state_msg)
def MetricsCB(self, results):
"""Callback for when the Spot Wrapper gets new metrics data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
metrics = self.spot_wrapper.metrics
if metrics:
metrics_msg = Metrics()
local_time = self.spot_wrapper.robotToLocalTime(metrics.timestamp)
metrics_msg.header.stamp = rospy.Time(local_time.seconds, local_time.nanos)
for metric in metrics.metrics:
if metric.label == "distance":
metrics_msg.distance = metric.float_value
if metric.label == "gait cycles":
metrics_msg.gait_cycles = metric.int_value
if metric.label == "time moving":
metrics_msg.time_moving = rospy.Time(metric.duration.seconds, metric.duration.nanos)
if metric.label == "electric power":
metrics_msg.electric_power = rospy.Time(metric.duration.seconds, metric.duration.nanos)
self.metrics_pub.publish(metrics_msg)
def LeaseCB(self, results):
"""Callback for when the Spot Wrapper gets new lease data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
lease_array_msg = LeaseArray()
lease_list = self.spot_wrapper.lease
if lease_list:
for resource in lease_list:
new_resource = LeaseResource()
new_resource.resource = resource.resource
new_resource.lease.resource = resource.lease.resource
new_resource.lease.epoch = resource.lease.epoch
for seq in resource.lease.sequence:
new_resource.lease.sequence.append(seq)
new_resource.lease_owner.client_name = resource.lease_owner.client_name
new_resource.lease_owner.user_name = resource.lease_owner.user_name
lease_array_msg.resources.append(new_resource)
self.lease_pub.publish(lease_array_msg)
def FrontImageCB(self, results):
"""Callback for when the Spot Wrapper gets new front image data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
data = self.spot_wrapper.front_images
if data:
image_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(data[0], self.spot_wrapper, self.mode_parent_odom_tf)
self.frontleft_image_pub.publish(image_msg0)
self.frontleft_image_info_pub.publish(camera_info_msg0)
self.tf_pub.publish(camera_tf_msg0)
image_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(data[1], self.spot_wrapper, self.mode_parent_odom_tf)
self.frontright_image_pub.publish(image_msg1)
self.frontright_image_info_pub.publish(camera_info_msg1)
self.tf_pub.publish(camera_tf_msg1)
image_msg2, camera_info_msg2, camera_tf_msg2 = getImageMsg(data[2], self.spot_wrapper, self.mode_parent_odom_tf)
self.frontleft_depth_pub.publish(image_msg2)
self.frontleft_depth_info_pub.publish(camera_info_msg2)
self.tf_pub.publish(camera_tf_msg2)
image_msg3, camera_info_msg3, camera_tf_msg3 = getImageMsg(data[3], self.spot_wrapper, self.mode_parent_odom_tf)
self.frontright_depth_pub.publish(image_msg3)
self.frontright_depth_info_pub.publish(camera_info_msg3)
self.tf_pub.publish(camera_tf_msg3)
def SideImageCB(self, results):
"""Callback for when the Spot Wrapper gets new side image data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
data = self.spot_wrapper.side_images
if data:
image_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(data[0], self.spot_wrapper, self.mode_parent_odom_tf)
self.left_image_pub.publish(image_msg0)
self.left_image_info_pub.publish(camera_info_msg0)
self.tf_pub.publish(camera_tf_msg0)
image_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(data[1], self.spot_wrapper, self.mode_parent_odom_tf)
self.right_image_pub.publish(image_msg1)
self.right_image_info_pub.publish(camera_info_msg1)
self.tf_pub.publish(camera_tf_msg1)
image_msg2, camera_info_msg2, camera_tf_msg2 = getImageMsg(data[2], self.spot_wrapper, self.mode_parent_odom_tf)
self.left_depth_pub.publish(image_msg2)
self.left_depth_info_pub.publish(camera_info_msg2)
self.tf_pub.publish(camera_tf_msg2)
image_msg3, camera_info_msg3, camera_tf_msg3 = getImageMsg(data[3], self.spot_wrapper, self.mode_parent_odom_tf)
self.right_depth_pub.publish(image_msg3)
self.right_depth_info_pub.publish(camera_info_msg3)
self.tf_pub.publish(camera_tf_msg3)
def RearImageCB(self, results):
"""Callback for when the Spot Wrapper gets new rear image data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
data = self.spot_wrapper.rear_images
if data:
mage_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(data[0], self.spot_wrapper, self.mode_parent_odom_tf)
self.back_image_pub.publish(mage_msg0)
self.back_image_info_pub.publish(camera_info_msg0)
self.tf_pub.publish(camera_tf_msg0)
mage_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(data[1], self.spot_wrapper, self.mode_parent_odom_tf)
self.back_depth_pub.publish(mage_msg1)
self.back_depth_info_pub.publish(camera_info_msg1)
self.tf_pub.publish(camera_tf_msg1)
def handle_claim(self, req):
"""ROS service handler for the claim service"""
resp = self.spot_wrapper.claim()
return TriggerResponse(resp[0], resp[1])
def handle_release(self, req):
"""ROS service handler for the release service"""
resp = self.spot_wrapper.release()
return TriggerResponse(resp[0], resp[1])
def handle_stop(self, req):
"""ROS service handler for the stop service"""
resp = self.spot_wrapper.stop()
return TriggerResponse(resp[0], resp[1])
def handle_self_right(self, req):
"""ROS service handler for the self-right service"""
resp = self.spot_wrapper.self_right()
return TriggerResponse(resp[0], resp[1])
def handle_sit(self, req):
"""ROS service handler for the sit service"""
resp = self.spot_wrapper.sit()
return TriggerResponse(resp[0], resp[1])
def handle_stand(self, req):
"""ROS service handler for the stand service"""
resp = self.spot_wrapper.stand()
return TriggerResponse(resp[0], resp[1])
def handle_power_on(self, req):
"""ROS service handler for the power-on service"""
resp = self.spot_wrapper.power_on()
return TriggerResponse(resp[0], resp[1])
def handle_safe_power_off(self, req):
"""ROS service handler for the safe-power-off service"""
resp = self.spot_wrapper.safe_power_off()
return TriggerResponse(resp[0], resp[1])
def handle_estop_hard(self, req):
"""ROS service handler to hard-eStop the robot. The robot will immediately cut power to the motors"""
resp = self.spot_wrapper.assertEStop(True)
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"""
resp = self.spot_wrapper.assertEStop(False)
return TriggerResponse(resp[0], resp[1])
def handle_stair_mode(self, req):
"""ROS service handler to set a stair mode to the robot."""
try:
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params.stair_hint = req.data
self.spot_wrapper.set_mobility_params( mobility_params )
return SetBoolResponse(True, 'Success')
except Exception as e:
return SetBoolResponse(False, 'Error:{}'.format(e))
def handle_locomotion_mode(self, req):
"""ROS service handler to set locomotion mode"""
try:
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params.locomotion_hint = req.locomotion_mode
self.spot_wrapper.set_mobility_params( mobility_params )
return SetLocomotionResponse(True, 'Success')
except Exception as e:
return SetLocomotionResponse(False, 'Error:{}'.format(e))
def cmdVelCallback(self, data):
"""Callback for cmd_vel command"""
self.spot_wrapper.velocity_cmd(data.linear.x, data.linear.y, data.angular.z)
def bodyPoseCallback(self, data):
"""Callback for cmd_vel command"""
q = Quaternion()
q.x = data.orientation.x
q.y = data.orientation.y
q.z = data.orientation.z
q.w = data.orientation.w
position = geometry_pb2.Vec3(z=data.position.z)
pose = geometry_pb2.SE3Pose(position=position, rotation=q)
point = trajectory_pb2.SE3TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE3Trajectory(points=[point])
body_control = spot_command_pb2.BodyControlParams(base_offset_rt_footprint=traj)
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params.body_control.CopyFrom(body_control)
self.spot_wrapper.set_mobility_params(mobility_params)
def handle_list_graph(self, upload_path):
"""ROS service handler for listing graph_nav waypoint_ids"""
resp = self.spot_wrapper.list_graph(upload_path)
return ListGraphResponse(resp)
def handle_navigate_to_feedback(self):
"""Thread function to send navigate_to feedback"""
while not rospy.is_shutdown() and self.run_navigate_to:
localization_state = self.spot_wrapper._graph_nav_client.get_localization_state()
if localization_state.localization.waypoint_id:
self.navigate_as.publish_feedback(NavigateToFeedback(localization_state.localization.waypoint_id))
rospy.Rate(10).sleep()
def handle_navigate_to(self, msg):
"""ROS service handler to run mission of the robot. The robot will replay a mission"""
# create thread to periodically publish feedback
feedback_thraed = threading.Thread(target = self.handle_navigate_to_feedback, args = ())
self.run_navigate_to = True
feedback_thraed.start()
# run navigate_to
resp = self.spot_wrapper.navigate_to(upload_path = msg.upload_path,
navigate_to = msg.navigate_to,
initial_localization_fiducial = msg.initial_localization_fiducial,
initial_localization_waypoint = msg.initial_localization_waypoint)
self.run_navigate_to = False
feedback_thraed.join()
# check status
if resp[0]:
self.navigate_as.set_succeeded(NavigateToResult(resp[0], resp[1]))
else:
self.navigate_as.set_aborted(NavigateToResult(resp[0], resp[1]))
def shutdown(self):
rospy.loginfo("Shutting down ROS driver for Spot")
self.spot_wrapper.sit()
rospy.Rate(0.25).sleep()
self.spot_wrapper.disconnect()
def main(self):
"""Main function for the SpotROS class. Gets config from ROS and initializes the wrapper. Holds lease from wrapper and updates all async tasks at the ROS rate"""
rospy.init_node('spot_ros', anonymous=True)
rate = rospy.Rate(50)
self.rates = rospy.get_param('~rates', {})
self.username = rospy.get_param('~username', 'default_value')
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)
# Spot has 2 types of odometries: 'odom' and 'vision'
# The former one is kinematic odometry and the second one is a combined odometry of vision and kinematics
# These params enables to change which odometry frame is a parent of body frame and to change tf names of each odometry frames.
self.mode_parent_odom_tf = rospy.get_param('~mode_parent_odom_tf', 'odom') # 'vision' or 'odom'
self.tf_name_kinematic_odom = rospy.get_param('~tf_name_kinematic_odom', 'odom')
self.tf_name_raw_kinematic = 'odom'
self.tf_name_vision_odom = rospy.get_param('~tf_name_vision_odom', 'vision')
self.tf_name_raw_vision = 'vision'
if self.mode_parent_odom_tf != self.tf_name_raw_kinematic and self.mode_parent_odom_tf != self.tf_name_raw_vision:
rospy.logerr('rosparam \'~mode_parent_odom_tf\' should be \'vision\' or \'vision\'.')
return
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)
if self.spot_wrapper.is_valid:
# Images #
self.back_image_pub = rospy.Publisher('camera/back/image', Image, queue_size=10)
self.frontleft_image_pub = rospy.Publisher('camera/frontleft/image', Image, queue_size=10)
self.frontright_image_pub = rospy.Publisher('camera/frontright/image', Image, queue_size=10)
self.left_image_pub = rospy.Publisher('camera/left/image', Image, queue_size=10)
self.right_image_pub = rospy.Publisher('camera/right/image', Image, queue_size=10)
# Depth #
self.back_depth_pub = rospy.Publisher('depth/back/image', Image, queue_size=10)
self.frontleft_depth_pub = rospy.Publisher('depth/frontleft/image', Image, queue_size=10)
self.frontright_depth_pub = rospy.Publisher('depth/frontright/image', Image, queue_size=10)
self.left_depth_pub = rospy.Publisher('depth/left/image', Image, queue_size=10)
self.right_depth_pub = rospy.Publisher('depth/right/image', Image, queue_size=10)
# Image Camera Info #
self.back_image_info_pub = rospy.Publisher('camera/back/camera_info', CameraInfo, queue_size=10)
self.frontleft_image_info_pub = rospy.Publisher('camera/frontleft/camera_info', CameraInfo, queue_size=10)
self.frontright_image_info_pub = rospy.Publisher('camera/frontright/camera_info', CameraInfo, queue_size=10)
self.left_image_info_pub = rospy.Publisher('camera/left/camera_info', CameraInfo, queue_size=10)
self.right_image_info_pub = rospy.Publisher('camera/right/camera_info', CameraInfo, queue_size=10)
# Depth Camera Info #
self.back_depth_info_pub = rospy.Publisher('depth/back/camera_info', CameraInfo, queue_size=10)
self.frontleft_depth_info_pub = rospy.Publisher('depth/frontleft/camera_info', CameraInfo, queue_size=10)
self.frontright_depth_info_pub = rospy.Publisher('depth/frontright/camera_info', CameraInfo, queue_size=10)
self.left_depth_info_pub = rospy.Publisher('depth/left/camera_info', CameraInfo, queue_size=10)
self.right_depth_info_pub = rospy.Publisher('depth/right/camera_info', CameraInfo, queue_size=10)
# Status Publishers #
self.joint_state_pub = rospy.Publisher('joint_states', JointState, queue_size=10)
"""Defining a TF publisher manually because of conflicts between Python3 and tf"""
self.tf_pub = rospy.Publisher('tf', TFMessage, queue_size=10)
self.metrics_pub = rospy.Publisher('status/metrics', Metrics, queue_size=10)
self.lease_pub = rospy.Publisher('status/leases', LeaseArray, queue_size=10)
self.odom_twist_pub = rospy.Publisher('odometry/twist', TwistWithCovarianceStamped, queue_size=10)
self.odom_pub = rospy.Publisher('odometry', Odometry, queue_size=10)
self.feet_pub = rospy.Publisher('status/feet', FootStateArray, queue_size=10)
self.estop_pub = rospy.Publisher('status/estop', EStopStateArray, queue_size=10)
self.wifi_pub = rospy.Publisher('status/wifi', WiFiState, queue_size=10)
self.power_pub = rospy.Publisher('status/power_state', PowerState, queue_size=10)
self.battery_pub = rospy.Publisher('status/battery_states', BatteryStateArray, queue_size=10)
self.behavior_faults_pub = rospy.Publisher('status/behavior_faults', BehaviorFaultState, queue_size=10)
self.system_faults_pub = rospy.Publisher('status/system_faults', SystemFaultState, queue_size=10)
self.feedback_pub = rospy.Publisher('status/feedback', Feedback, queue_size=10)
self.mobility_params_pub = rospy.Publisher('status/mobility_params', MobilityParams, queue_size=10)
rospy.Subscriber('cmd_vel', Twist, self.cmdVelCallback, queue_size = 1)
rospy.Subscriber('body_pose', Pose, self.bodyPoseCallback, queue_size = 1)
rospy.Service("claim", Trigger, self.handle_claim)
rospy.Service("release", Trigger, self.handle_release)
rospy.Service("stop", Trigger, self.handle_stop)
rospy.Service("self_right", Trigger, self.handle_self_right)
rospy.Service("sit", Trigger, self.handle_sit)
rospy.Service("stand", Trigger, self.handle_stand)
rospy.Service("power_on", Trigger, self.handle_power_on)
rospy.Service("power_off", Trigger, self.handle_safe_power_off)
rospy.Service("estop/hard", Trigger, self.handle_estop_hard)
rospy.Service("estop/gentle", Trigger, self.handle_estop_soft)
rospy.Service("stair_mode", SetBool, self.handle_stair_mode)
rospy.Service("locomotion_mode", SetLocomotion, self.handle_locomotion_mode)
rospy.Service("list_graph", ListGraph, self.handle_list_graph)
self.navigate_as = actionlib.SimpleActionServer('navigate_to', NavigateToAction,
execute_cb = self.handle_navigate_to,
auto_start = False)
self.navigate_as.start()
rospy.on_shutdown(self.shutdown)
self.auto_claim = rospy.get_param('~auto_claim', False)
self.auto_power_on = rospy.get_param('~auto_power_on', False)
self.auto_stand = rospy.get_param('~auto_stand', False)
if self.auto_claim:
self.spot_wrapper.claim()
if self.auto_power_on:
self.spot_wrapper.power_on()
if self.auto_stand:
self.spot_wrapper.stand()
while not rospy.is_shutdown():
self.spot_wrapper.updateTasks()
feedback_msg = Feedback()
feedback_msg.standing = self.spot_wrapper.is_standing
feedback_msg.sitting = self.spot_wrapper.is_sitting
feedback_msg.moving = self.spot_wrapper.is_moving
id = self.spot_wrapper.id
try:
feedback_msg.serial_number = id.serial_number
feedback_msg.species = id.species
feedback_msg.version = id.version
feedback_msg.nickname = id.nickname
feedback_msg.computer_serial_number = id.computer_serial_number
except:
pass
self.feedback_pub.publish(feedback_msg)
mobility_params_msg = MobilityParams()
try:
mobility_params = self.spot_wrapper.get_mobility_params()
mobility_params_msg.body_control.position.x = \
mobility_params.body_control.base_offset_rt_footprint.points[0].pose.position.x
mobility_params_msg.body_control.position.y = \
mobility_params.body_control.base_offset_rt_footprint.points[0].pose.position.y
mobility_params_msg.body_control.position.z = \
mobility_params.body_control.base_offset_rt_footprint.points[0].pose.position.z
mobility_params_msg.body_control.orientation.x = \
mobility_params.body_control.base_offset_rt_footprint.points[0].pose.rotation.x
mobility_params_msg.body_control.orientation.y = \
mobility_params.body_control.base_offset_rt_footprint.points[0].pose.rotation.y
mobility_params_msg.body_control.orientation.z = \
mobility_params.body_control.base_offset_rt_footprint.points[0].pose.rotation.z
mobility_params_msg.body_control.orientation.w = \
mobility_params.body_control.base_offset_rt_footprint.points[0].pose.rotation.w
mobility_params_msg.locomotion_hint = mobility_params.locomotion_hint
mobility_params_msg.stair_hint = mobility_params.stair_hint
except Exception as e:
print('Error:{}'.format(e))
pass
self.mobility_params_pub.publish(mobility_params_msg)
rate.sleep()
class SpotROS():
"""Parent class for using the wrapper. Defines all callbacks and keeps the wrapper alive"""
def __init__(self):
self.spot_wrapper = None
self.callbacks = {}
"""Dictionary listing what callback to use for what data task"""
self.callbacks["robot_state"] = self.RobotStateCB
self.callbacks["metrics"] = self.MetricsCB
self.callbacks["lease"] = self.LeaseCB
self.callbacks["front_image"] = self.FrontImageCB
self.callbacks["side_image"] = self.SideImageCB
self.callbacks["rear_image"] = self.RearImageCB
def RobotStateCB(self, results):
"""Callback for when the Spot Wrapper gets new robot state data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
state = self.spot_wrapper.robot_state
if state:
## joint states ##
joint_state = GetJointStatesFromState(state, self.spot_wrapper)
self.joint_state_pub.publish(joint_state)
## TF ##
tf_msg = GetTFFromState(state, self.spot_wrapper)
if len(tf_msg.transforms) > 0:
self.tf_pub.publish(tf_msg)
# Odom Twist #
twist_odom_msg = GetOdomTwistFromState(state, self.spot_wrapper)
self.odom_twist_pub.publish(twist_odom_msg)
# Feet #
foot_array_msg = GetFeetFromState(state, self.spot_wrapper)
self.feet_pub.publish(foot_array_msg)
# EStop #
estop_array_msg = GetEStopStateFromState(state, self.spot_wrapper)
self.estop_pub.publish(estop_array_msg)
# WIFI #
wifi_msg = GetWifiFromState(state, self.spot_wrapper)
self.wifi_pub.publish(wifi_msg)
# Battery States #
battery_states_array_msg = GetBatteryStatesFromState(
state, self.spot_wrapper)
self.battery_pub.publish(battery_states_array_msg)
# Power State #
power_state_msg = GetPowerStatesFromState(state, self.spot_wrapper)
self.power_pub.publish(power_state_msg)
# System Faults #
system_fault_state_msg = GetSystemFaultsFromState(
state, self.spot_wrapper)
self.system_faults_pub.publish(system_fault_state_msg)
# Behavior Faults #
behavior_fault_state_msg = getBehaviorFaultsFromState(
state, self.spot_wrapper)
self.behavior_faults_pub.publish(behavior_fault_state_msg)
def MetricsCB(self, results):
"""Callback for when the Spot Wrapper gets new metrics data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
metrics = self.spot_wrapper.metrics
if metrics:
metrics_msg = Metrics()
local_time = self.spot_wrapper.robotToLocalTime(metrics.timestamp)
metrics_msg.header.stamp = rospy.Time(local_time.seconds,
local_time.nanos)
for metric in metrics.metrics:
if metric.label == "distance":
metrics_msg.distance = metric.float_value
if metric.label == "gait cycles":
metrics_msg.gait_cycles = metric.int_value
if metric.label == "time moving":
metrics_msg.time_moving = rospy.Time(
metric.duration.seconds, metric.duration.nanos)
if metric.label == "electric power":
metrics_msg.electric_power = rospy.Time(
metric.duration.seconds, metric.duration.nanos)
self.metrics_pub.publish(metrics_msg)
def LeaseCB(self, results):
"""Callback for when the Spot Wrapper gets new lease data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
lease_array_msg = LeaseArray()
lease_list = self.spot_wrapper.lease
if lease_list:
for resource in lease_list:
new_resource = LeaseResource()
new_resource.resource = resource.resource
new_resource.lease.resource = resource.lease.resource
new_resource.lease.epoch = resource.lease.epoch
for seq in resource.lease.sequence:
new_resource.lease.sequence.append(seq)
new_resource.lease_owner.client_name = resource.lease_owner.client_name
new_resource.lease_owner.user_name = resource.lease_owner.user_name
lease_array_msg.resources.append(new_resource)
self.lease_pub.publish(lease_array_msg)
def FrontImageCB(self, results):
"""Callback for when the Spot Wrapper gets new front image data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
data = self.spot_wrapper.front_images
if data:
image_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(
data[0], self.spot_wrapper)
self.frontleft_image_pub.publish(image_msg0)
self.frontleft_image_info_pub.publish(camera_info_msg0)
self.tf_pub.publish(camera_tf_msg0)
image_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(
data[1], self.spot_wrapper)
self.frontright_image_pub.publish(image_msg1)
self.frontright_image_info_pub.publish(camera_info_msg1)
self.tf_pub.publish(camera_tf_msg1)
image_msg2, camera_info_msg2, camera_tf_msg2 = getImageMsg(
data[2], self.spot_wrapper)
self.frontleft_depth_pub.publish(image_msg2)
self.frontleft_depth_info_pub.publish(camera_info_msg2)
self.tf_pub.publish(camera_tf_msg2)
image_msg3, camera_info_msg3, camera_tf_msg3 = getImageMsg(
data[3], self.spot_wrapper)
self.frontright_depth_pub.publish(image_msg3)
self.frontright_depth_info_pub.publish(camera_info_msg3)
self.tf_pub.publish(camera_tf_msg3)
def SideImageCB(self, results):
"""Callback for when the Spot Wrapper gets new side image data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
data = self.spot_wrapper.side_images
if data:
image_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(
data[0], self.spot_wrapper)
self.left_image_pub.publish(image_msg0)
self.left_image_info_pub.publish(camera_info_msg0)
self.tf_pub.publish(camera_tf_msg0)
image_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(
data[1], self.spot_wrapper)
self.right_image_pub.publish(image_msg1)
self.right_image_info_pub.publish(camera_info_msg1)
self.tf_pub.publish(camera_tf_msg1)
image_msg2, camera_info_msg2, camera_tf_msg2 = getImageMsg(
data[2], self.spot_wrapper)
self.left_depth_pub.publish(image_msg2)
self.left_depth_info_pub.publish(camera_info_msg2)
self.tf_pub.publish(camera_tf_msg2)
image_msg3, camera_info_msg3, camera_tf_msg3 = getImageMsg(
data[3], self.spot_wrapper)
self.right_depth_pub.publish(image_msg3)
self.right_depth_info_pub.publish(camera_info_msg3)
self.tf_pub.publish(camera_tf_msg3)
def RearImageCB(self, results):
"""Callback for when the Spot Wrapper gets new rear image data.
Args:
results: FutureWrapper object of AsyncPeriodicQuery callback
"""
data = self.spot_wrapper.rear_images
if data:
mage_msg0, camera_info_msg0, camera_tf_msg0 = getImageMsg(
data[0], self.spot_wrapper)
self.back_image_pub.publish(mage_msg0)
self.back_image_info_pub.publish(camera_info_msg0)
self.tf_pub.publish(camera_tf_msg0)
mage_msg1, camera_info_msg1, camera_tf_msg1 = getImageMsg(
data[1], self.spot_wrapper)
self.back_depth_pub.publish(mage_msg1)
self.back_depth_info_pub.publish(camera_info_msg1)
self.tf_pub.publish(camera_tf_msg1)
def handle_claim(self, req):
"""ROS service handler for the claim service"""
resp = self.spot_wrapper.claim()
return TriggerResponse(resp[0], resp[1])
def handle_release(self, req):
"""ROS service handler for the release service"""
resp = self.spot_wrapper.release()
return TriggerResponse(resp[0], resp[1])
def handle_stop(self, req):
"""ROS service handler for the stop service"""
resp = self.spot_wrapper.stop()
return TriggerResponse(resp[0], resp[1])
def handle_self_right(self, req):
"""ROS service handler for the self-right service"""
resp = self.spot_wrapper.self_right()
return TriggerResponse(resp[0], resp[1])
def handle_sit(self, req):
"""ROS service handler for the sit service"""
resp = self.spot_wrapper.sit()
return TriggerResponse(resp[0], resp[1])
def handle_stand(self, req):
"""ROS service handler for the stand service"""
resp = self.spot_wrapper.stand()
return TriggerResponse(resp[0], resp[1])
def handle_power_on(self, req):
"""ROS service handler for the power-on service"""
resp = self.spot_wrapper.power_on()
return TriggerResponse(resp[0], resp[1])
def handle_safe_power_off(self, req):
"""ROS service handler for the safe-power-off service"""
resp = self.spot_wrapper.safe_power_off()
return TriggerResponse(resp[0], resp[1])
def handle_estop_hard(self, req):
"""ROS service handler to hard-eStop the robot. The robot will immediately cut power to the motors"""
resp = self.spot_wrapper.assertEStop(True)
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"""
resp = self.spot_wrapper.assertEStop(False)
return TriggerResponse(resp[0], resp[1])
def cmdVelCallback(self, data):
"""Callback for cmd_vel command"""
self.spot_wrapper.velocity_cmd(data.linear.x, data.linear.y,
data.angular.z)
def bodyPoseCallback(self, data):
"""Callback for cmd_vel command"""
q = Quaternion()
q.x = data.orientation.x
q.y = data.orientation.y
q.z = data.orientation.z
q.w = data.orientation.w
euler_zxy = q.to_euler_zxy()
self.spot_wrapper.set_mobility_params(data.position.z, euler_zxy)
def shutdown(self):
rospy.loginfo("Shutting down ROS driver for Spot")
self.spot_wrapper.sit()
rospy.Rate(0.25).sleep()
self.spot_wrapper.disconnect()
def main(self):
"""Main function for the SpotROS class. Gets config from ROS and initializes the wrapper. Holds lease from wrapper and updates all async tasks at the ROS rate"""
rospy.init_node('spot_ros', anonymous=True)
rate = rospy.Rate(50)
self.rates = rospy.get_param('~rates', {})
self.username = rospy.get_param('~username', 'default_value')
self.password = rospy.get_param('~password', 'default_value')
self.app_token = rospy.get_param('~app_token', 'default_value')
self.hostname = rospy.get_param('~hostname', 'default_value')
self.motion_deadzone = rospy.get_param('~deadzone', 0.05)
self.logger = logging.getLogger('rosout')
rospy.loginfo("Starting ROS driver for Spot")
self.spot_wrapper = SpotWrapper(self.username, self.password,
self.app_token, self.hostname,
self.logger, self.rates,
self.callbacks)
if self.spot_wrapper.is_valid:
# Images #
self.back_image_pub = rospy.Publisher('camera/back/image',
Image,
queue_size=10)
self.frontleft_image_pub = rospy.Publisher(
'camera/frontleft/image', Image, queue_size=10)
self.frontright_image_pub = rospy.Publisher(
'camera/frontright/image', Image, queue_size=10)
self.left_image_pub = rospy.Publisher('camera/left/image',
Image,
queue_size=10)
self.right_image_pub = rospy.Publisher('camera/right/image',
Image,
queue_size=10)
# Depth #
self.back_depth_pub = rospy.Publisher('depth/back/image',
Image,
queue_size=10)
self.frontleft_depth_pub = rospy.Publisher('depth/frontleft/image',
Image,
queue_size=10)
self.frontright_depth_pub = rospy.Publisher(
'depth/frontright/image', Image, queue_size=10)
self.left_depth_pub = rospy.Publisher('depth/left/image',
Image,
queue_size=10)
self.right_depth_pub = rospy.Publisher('depth/right/image',
Image,
queue_size=10)
# Image Camera Info #
self.back_image_info_pub = rospy.Publisher(
'camera/back/camera_info', CameraInfo, queue_size=10)
self.frontleft_image_info_pub = rospy.Publisher(
'camera/frontleft/camera_info', CameraInfo, queue_size=10)
self.frontright_image_info_pub = rospy.Publisher(
'camera/frontright/camera_info', CameraInfo, queue_size=10)
self.left_image_info_pub = rospy.Publisher(
'camera/left/camera_info', CameraInfo, queue_size=10)
self.right_image_info_pub = rospy.Publisher(
'camera/right/camera_info', CameraInfo, queue_size=10)
# Depth Camera Info #
self.back_depth_info_pub = rospy.Publisher(
'depth/back/camera_info', CameraInfo, queue_size=10)
self.frontleft_depth_info_pub = rospy.Publisher(
'depth/frontleft/camera_info', CameraInfo, queue_size=10)
self.frontright_depth_info_pub = rospy.Publisher(
'depth/frontright/camera_info', CameraInfo, queue_size=10)
self.left_depth_info_pub = rospy.Publisher(
'depth/left/camera_info', CameraInfo, queue_size=10)
self.right_depth_info_pub = rospy.Publisher(
'depth/right/camera_info', CameraInfo, queue_size=10)
# Status Publishers #
self.joint_state_pub = rospy.Publisher('joint_states',
JointState,
queue_size=10)
"""Defining a TF publisher manually because of conflicts between Python3 and tf"""
self.tf_pub = rospy.Publisher('tf', TFMessage, queue_size=10)
self.metrics_pub = rospy.Publisher('status/metrics',
Metrics,
queue_size=10)
self.lease_pub = rospy.Publisher('status/leases',
LeaseArray,
queue_size=10)
self.odom_twist_pub = rospy.Publisher('odometry/twist',
TwistWithCovarianceStamped,
queue_size=10)
self.feet_pub = rospy.Publisher('status/feet',
FootStateArray,
queue_size=10)
self.estop_pub = rospy.Publisher('status/estop',
EStopStateArray,
queue_size=10)
self.wifi_pub = rospy.Publisher('status/wifi',
WiFiState,
queue_size=10)
self.power_pub = rospy.Publisher('status/power_state',
PowerState,
queue_size=10)
self.battery_pub = rospy.Publisher('status/battery_states',
BatteryStateArray,
queue_size=10)
self.behavior_faults_pub = rospy.Publisher(
'status/behavior_faults', BehaviorFaultState, queue_size=10)
self.system_faults_pub = rospy.Publisher('status/system_faults',
SystemFaultState,
queue_size=10)
self.feedback_pub = rospy.Publisher('status/feedback',
Feedback,
queue_size=10)
rospy.Subscriber('cmd_vel', Twist, self.cmdVelCallback)
rospy.Subscriber('body_pose', Pose, self.bodyPoseCallback)
rospy.Service("claim", Trigger, self.handle_claim)
rospy.Service("release", Trigger, self.handle_release)
rospy.Service("stop", Trigger, self.handle_stop)
rospy.Service("self_right", Trigger, self.handle_self_right)
rospy.Service("sit", Trigger, self.handle_sit)
rospy.Service("stand", Trigger, self.handle_stand)
rospy.Service("power_on", Trigger, self.handle_power_on)
rospy.Service("power_off", Trigger, self.handle_safe_power_off)
rospy.Service("estop/hard", Trigger, self.handle_estop_hard)
rospy.Service("estop/gentle", Trigger, self.handle_estop_soft)
rospy.on_shutdown(self.shutdown)
self.auto_claim = rospy.get_param('~auto_claim', False)
self.auto_power_on = rospy.get_param('~auto_power_on', False)
self.auto_stand = rospy.get_param('~auto_stand', False)
if self.auto_claim:
self.spot_wrapper.claim()
if self.auto_power_on:
self.spot_wrapper.power_on()
if self.auto_stand:
self.spot_wrapper.stand()
while not rospy.is_shutdown():
self.spot_wrapper.updateTasks()
feedback_msg = Feedback()
feedback_msg.standing = self.spot_wrapper.is_standing
feedback_msg.sitting = self.spot_wrapper.is_sitting
feedback_msg.moving = self.spot_wrapper.is_moving
id = self.spot_wrapper.id
try:
feedback_msg.serial_number = id.serial_number
feedback_msg.species = id.species
feedback_msg.version = id.version
feedback_msg.nickname = id.nickname
feedback_msg.computer_serial_number = id.computer_serial_number
except:
pass
self.feedback_pub.publish(feedback_msg)
rate.sleep()