def _move(self, left_x, left_y, right_x):
"""Commands the robot with a velocity command based on left/right stick values.
Args:
left_x: X value of left stick.
left_y: Y value of left stick.
right_x: X value of right stick.
"""
# Stick left_x controls robot v_y
v_y = -left_x * VELOCITY_BASE_SPEED
# Stick left_y controls robot v_x
v_x = left_y * VELOCITY_BASE_SPEED
# Stick right_x controls robot v_rot
v_rot = -right_x * VELOCITY_BASE_ANGULAR
# Recreate mobility_params with the latest information
self.mobility_params = RobotCommandBuilder.mobility_params(
body_height=self.body_height,
locomotion_hint=self.mobility_params.locomotion_hint,
stair_hint=self.mobility_params.stair_hint)
cmd = RobotCommandBuilder.velocity_command(v_x=v_x,
v_y=v_y,
v_rot=v_rot,
params=self.mobility_params)
self.command_client.robot_command_async(cmd,
end_time_secs=time.time() +
VELOCITY_CMD_DURATION)
def velocity_cmd_listener(self, twist):
"""Callback that sends instantaneous velocity [m/s] commands to Spot"""
MAX_MARCHING_TIME = 5
v_x = twist.linear.x
v_y = twist.linear.y
v_rot = twist.angular.z
all_zero = (v_x == 0) and (v_y == 0) and (v_rot == 0)
if (all_zero):
if self.t_last_non_zero_cmd < time.monotonic() - MAX_MARCHING_TIME:
# No need to send command, just return.
return
else:
self.t_last_non_zero_cmd = time.monotonic()
cmd = RobotCommandBuilder.velocity_command(v_x=v_x,
v_y=v_y,
v_rot=v_rot)
try:
self.command_client.robot_command(cmd,
end_time_secs=time.time() +
self.VELOCITY_CMD_DURATION)
except:
print("Invalid command: v_x=${},v_y=${},v_rot${}".format(
v_x, v_y, v_rot))
# rospy.loginfo(
# "Robot velocity cmd sent: v_x=${},v_y=${},v_rot${}".format(v_x, v_y, v_rot))
return []
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0):
self._start_robot_command(desc,
RobotCommandBuilder.velocity_command(
v_x=v_x, v_y=v_y, v_rot=v_rot,
body_height=self.height,
locomotion_hint=spot_command_pb2.HINT_AMBLE),
end_time_secs=time.time() + VELOCITY_CMD_DURATION)
def test_velocity_command():
v_x = 1.0
v_y = 2.0
v_rot = 3.0
command = RobotCommandBuilder.velocity_command(v_x, v_y, v_rot)
_test_has_mobility(command)
assert command.mobility_command.HasField("se2_velocity_request")
vel_cmd = command.mobility_command.se2_velocity_request
assert vel_cmd.velocity.linear.x == v_x
assert vel_cmd.velocity.linear.y == v_y
assert vel_cmd.velocity.angular == v_rot
assert vel_cmd.frame.base_frame == geometry_pb2.FRAME_BODY
def test_velocity_command():
v_x = 1.0
v_y = 2.0
v_rot = 3.0
command = RobotCommandBuilder.velocity_command(v_x, v_y, v_rot)
_test_has_mobility_deprecated(command)
assert command.mobility_command.HasField("se2_velocity_request")
vel_cmd = command.mobility_command.se2_velocity_request
assert vel_cmd.velocity.linear.x == v_x
assert vel_cmd.velocity.linear.y == v_y
assert vel_cmd.velocity.angular == v_rot
assert vel_cmd.se2_frame_name == BODY_FRAME_NAME
def velocity_cmd(self, v_x, v_y, v_rot, cmd_duration=0.1):
"""Send a velocity motion command to the robot.
Args:
v_x: Velocity in the X direction in meters
v_y: Velocity in the Y direction in meters
v_rot: Angular velocity around the Z axis in radians
cmd_duration: (optional) Time-to-live for the command in seconds. Default is 125ms (assuming 10Hz command rate).
"""
end_time = time.time() + cmd_duration
self._robot_command(RobotCommandBuilder.velocity_command(
v_x=v_x, v_y=v_y, v_rot=v_rot, params=self._mobility_params),
end_time_secs=end_time)
self._last_motion_command_time = end_time
def velocity_cmd_srv(self, twist):
"""Callback that sends instantaneous velocity [m/s] commands to Spot"""
v_x = twist.velocity.linear.x
v_y = twist.velocity.linear.y
v_rot = twist.velocity.angular.z
cmd = RobotCommandBuilder.velocity_command(
v_x=v_x,
v_y=v_y,
v_rot=v_rot
)
self.command_client.robot_command(
cmd,
end_time_secs=time.time() + self.VELOCITY_CMD_DURATION
)
rospy.loginfo(
"Robot velocity cmd sent: v_x=${},v_y=${},v_rot${}".format(v_x, v_y, v_rot))
return []
def _velocity_cmd_helper(self, desc='', v_x=0.0, v_y=0.0, v_rot=0.0):
self._start_robot_command(
desc,
RobotCommandBuilder.velocity_command(v_x=v_x, v_y=v_y,
v_rot=v_rot),
end_time_secs=time.time() + VELOCITY_CMD_DURATION)