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 get_walking_params(max_linear_vel, max_rotation_vel):
max_vel_linear = geometry_pb2.Vec2(x=max_linear_vel, y=max_linear_vel)
max_vel_se2 = geometry_pb2.SE2Velocity(linear=max_vel_linear,
angular=max_rotation_vel)
vel_limit = geometry_pb2.SE2VelocityLimit(max_vel=max_vel_se2)
params = RobotCommandBuilder.mobility_params()
params.vel_limit.CopyFrom(vel_limit)
return params
def set_mobility_params(self,
body_height=0,
footprint_R_body=EulerZXY(),
locomotion_hint=1,
stair_hint=False,
external_force_params=None):
"""Define body, locomotion, and stair parameters.
Args:
body_height: Body height in meters
footprint_R_body: (EulerZXY) – The orientation of the body frame with respect to the footprint frame (gravity aligned framed with yaw computed from the stance feet)
locomotion_hint: Locomotion hint
stair_hint: Boolean to define stair motion
"""
self._mobility_params = RobotCommandBuilder.mobility_params(
body_height, footprint_R_body, locomotion_hint, stair_hint,
external_force_params)
def relative_move(dx,
dy,
dyaw,
frame_name,
robot_command_client,
robot_state_client,
stairs=False):
transforms = robot_state_client.get_robot_state(
).kinematic_state.transforms_snapshot
# Build the transform for where we want the robot to be relative to where the body currently is.
body_tform_goal = math_helpers.SE2Pose(x=dx, y=dy, angle=dyaw)
# We do not want to command this goal in body frame because the body will move, thus shifting
# our goal. Instead, we transform this offset to get the goal position in the output frame
# (which will be either odom or vision).
out_tform_body = get_se2_a_tform_b(transforms, frame_name, BODY_FRAME_NAME)
out_tform_goal = out_tform_body * body_tform_goal
# Command the robot to go to the goal point in the specified frame. The command will stop at the
# new position.
robot_cmd = RobotCommandBuilder.synchro_se2_trajectory_point_command(
goal_x=out_tform_goal.x,
goal_y=out_tform_goal.y,
goal_heading=out_tform_goal.angle,
frame_name=frame_name,
params=RobotCommandBuilder.mobility_params(stair_hint=stairs))
end_time = 10.0
cmd_id = robot_command_client.robot_command(lease=None,
command=robot_cmd,
end_time_secs=time.time() +
end_time)
# Wait until the robot has reached the goal.
while True:
feedback = robot_command_client.robot_command_feedback(cmd_id)
mobility_feedback = feedback.feedback.synchronized_feedback.mobility_command_feedback
if mobility_feedback.status != RobotCommandFeedbackStatus.STATUS_PROCESSING:
print("Failed to reach the goal")
return False
traj_feedback = mobility_feedback.se2_trajectory_feedback
if (traj_feedback.status == traj_feedback.STATUS_AT_GOAL
and traj_feedback.body_movement_status
== traj_feedback.BODY_STATUS_SETTLED):
print("Arrived at the goal.")
return True
time.sleep(1)
def __init__(self,
username,
password,
hostname,
logger,
rates={},
callbacks={}):
self._username = username
self._password = password
self._hostname = hostname
self._logger = logger
self._rates = rates
self._callbacks = callbacks
self._keep_alive = True
self._valid = True
self._mobility_params = RobotCommandBuilder.mobility_params()
self._is_standing = False
self._is_sitting = True
self._is_moving = False
self._last_stand_command = None
self._last_sit_command = None
self._last_motion_command = None
self._last_motion_command_time = None
self._front_image_requests = []
for source in front_image_sources:
self._front_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
self._side_image_requests = []
for source in side_image_sources:
self._side_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
self._rear_image_requests = []
for source in rear_image_sources:
self._rear_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
try:
self._sdk = create_standard_sdk('ros_spot')
except Exception as e:
self._logger.error("Error creating SDK object: %s", e)
self._valid = False
return
self._robot = self._sdk.create_robot(self._hostname)
try:
self._robot.authenticate(self._username, self._password)
self._robot.start_time_sync()
except RpcError as err:
self._logger.error("Failed to communicate with robot: %s", err)
self._valid = False
return
if self._robot:
# Clients
try:
self._robot_state_client = self._robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
self._graph_nav_client = self._robot.ensure_client(
GraphNavClient.default_service_name)
self._power_client = self._robot.ensure_client(
PowerClient.default_service_name)
self._lease_client = self._robot.ensure_client(
LeaseClient.default_service_name)
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)
except Exception as e:
self._logger.error("Unable to create client service: %s", e)
self._valid = False
return
# Store the most recent knowledge of the state of the robot based on rpc calls.
self._current_graph = None
self._current_edges = dict(
) #maps to_waypoint to list(from_waypoint)
self._current_waypoint_snapshots = dict(
) # maps id to waypoint snapshot
self._current_edge_snapshots = dict() # maps id to edge snapshot
self._current_annotation_name_to_wp_id = dict()
# Async Tasks
self._async_task_list = []
self._robot_state_task = AsyncRobotState(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("robot_state", 0.0)),
self._callbacks.get("robot_state", lambda: None))
self._robot_metrics_task = AsyncMetrics(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("metrics", 0.0)),
self._callbacks.get("metrics", lambda: None))
self._lease_task = AsyncLease(
self._lease_client, self._logger,
max(0.0, self._rates.get("lease", 0.0)),
self._callbacks.get("lease", lambda: None))
self._front_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("front_image", 0.0)),
self._callbacks.get("front_image", lambda: None),
self._front_image_requests)
self._side_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("side_image", 0.0)),
self._callbacks.get("side_image", lambda: None),
self._side_image_requests)
self._rear_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("rear_image", 0.0)),
self._callbacks.get("rear_image", lambda: None),
self._rear_image_requests)
self._idle_task = AsyncIdle(self._robot_command_client,
self._logger, 10.0, self)
self._estop_endpoint = None
self._async_tasks = AsyncTasks([
self._robot_state_task, self._robot_metrics_task,
self._lease_task, self._front_image_task,
self._side_image_task, self._rear_image_task, self._idle_task
])
self._robot_id = None
self._lease = None
def __init__(self,
username,
password,
token,
hostname,
logger,
rates={},
callbacks={}):
self._username = username
self._password = password
self._token = token
self._hostname = hostname
self._logger = logger
self._rates = rates
self._callbacks = callbacks
self._keep_alive = True
self._valid = True
self._mobility_params = RobotCommandBuilder.mobility_params()
self._is_standing = False
self._is_sitting = True
self._is_moving = False
self._last_stand_command = None
self._last_sit_command = None
self._last_motion_command = None
self._last_motion_command_time = None
self._front_image_requests = []
for source in front_image_sources:
self._front_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
self._side_image_requests = []
for source in side_image_sources:
self._side_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
self._rear_image_requests = []
for source in rear_image_sources:
self._rear_image_requests.append(
build_image_request(
source, image_format=image_pb2.Image.Format.FORMAT_RAW))
try:
self._sdk = create_standard_sdk('ros_spot')
except Exception as e:
self._logger.error("Error creating SDK object: %s", e)
self._valid = False
return
try:
self._sdk.load_app_token(self._token)
except Exception as e:
self._logger.error("Error loading developer token: %s", e)
self._valid = False
return
self._robot = self._sdk.create_robot(self._hostname)
try:
self._robot.authenticate(self._username, self._password)
self._robot.start_time_sync()
except RpcError as err:
self._logger.error("Failed to communicate with robot: %s", err)
self._valid = False
return
if self._robot:
# Clients
try:
self._robot_state_client = self._robot.ensure_client(
RobotStateClient.default_service_name)
self._robot_command_client = self._robot.ensure_client(
RobotCommandClient.default_service_name)
self._power_client = self._robot.ensure_client(
PowerClient.default_service_name)
self._lease_client = self._robot.ensure_client(
LeaseClient.default_service_name)
self._image_client = self._robot.ensure_client(
ImageClient.default_service_name)
self._estop_client = self._robot.ensure_client(
EstopClient.default_service_name)
except Exception as e:
self._logger.error("Unable to create client service: %s", e)
self._valid = False
return
# Async Tasks
self._async_task_list = []
self._robot_state_task = AsyncRobotState(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("robot_state", 0.0)),
self._callbacks.get("robot_state", lambda: None))
self._robot_metrics_task = AsyncMetrics(
self._robot_state_client, self._logger,
max(0.0, self._rates.get("metrics", 0.0)),
self._callbacks.get("metrics", lambda: None))
self._lease_task = AsyncLease(
self._lease_client, self._logger,
max(0.0, self._rates.get("lease", 0.0)),
self._callbacks.get("lease", lambda: None))
self._front_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("front_image", 0.0)),
self._callbacks.get("front_image", lambda: None),
self._front_image_requests)
self._side_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("side_image", 0.0)),
self._callbacks.get("side_image", lambda: None),
self._side_image_requests)
self._rear_image_task = AsyncImageService(
self._image_client, self._logger,
max(0.0, self._rates.get("rear_image", 0.0)),
self._callbacks.get("rear_image", lambda: None),
self._rear_image_requests)
self._idle_task = AsyncIdle(self._robot_command_client,
self._logger, 10.0, self)
self._estop_endpoint = EstopEndpoint(self._estop_client, 'ros',
9.0)
self._async_tasks = AsyncTasks([
self._robot_state_task, self._robot_metrics_task,
self._lease_task, self._front_image_task,
self._side_image_task, self._rear_image_task, self._idle_task
])
self._robot_id = None
self._lease = None
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()
