def test_create_se2_pose():
# Test creating an SE2Pose from a proto with from_obj()
proto_se2 = geometry_pb2.SE2Pose(position=geometry_pb2.Vec2(x=1, y=2),
angle=.2)
se2 = SE2Pose.from_obj(proto_se2)
assert type(se2) == SE2Pose
assert se2.x == proto_se2.position.x
assert se2.y == proto_se2.position.y
assert se2.angle == proto_se2.angle
# Test proto-like attribute access properties
pos = se2.position
assert type(pos) == geometry_pb2.Vec2
assert pos.x == proto_se2.position.x
assert pos.y == proto_se2.position.y
# Test going back to a proto message with to_proto()
new_proto_se2 = se2.to_proto()
assert type(new_proto_se2) == geometry_pb2.SE2Pose
assert new_proto_se2.position.x == proto_se2.position.x
assert new_proto_se2.position.y == proto_se2.position.y
assert new_proto_se2.angle == proto_se2.angle
# Test mutating an existing proto message to_obj()
proto_mut_se2 = geometry_pb2.SE2Pose()
se2.to_obj(proto_mut_se2)
assert se2.x == proto_mut_se2.position.x
assert se2.y == proto_mut_se2.position.y
assert se2.angle == proto_mut_se2.angle
def trajectory_command(goal_x, goal_y, goal_heading, frame, params=None, body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO):
"""Command robot to move to pose along a 2D plane. Pose can specified in the world
(kinematic odometry) frame or the robot body frame. The arguments body_height and
locomotion_hint are ignored if params argument is passed.
A trajectory command requires an end time. End time is not set in this function, but rather
is set externally before call to RobotCommandService.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params(body_height=body_height,
locomotion_hint=locomotion_hint)
any_params = RobotCommandBuilder._to_any(params)
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
pose = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
point = trajectory_pb2.SE2TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE2Trajectory(points=[point], frame=frame)
traj_command = robot_command_pb2.SE2TrajectoryCommand.Request(trajectory=traj)
mobility_command = robot_command_pb2.MobilityCommand.Request(
se2_trajectory_request=traj_command, params=any_params)
command = robot_command_pb2.RobotCommand(mobility_command=mobility_command)
return command
def test_synchro_se2_trajectory_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
goal_se2 = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
command = RobotCommandBuilder.synchro_se2_trajectory_command(
goal_se2, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
def test_synchro_se2_trajectory_command():
goal_x = 1.0
goal_y = 2.0
goal_heading = 3.0
frame = ODOM_FRAME_NAME
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
goal_se2 = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
command = RobotCommandBuilder.synchro_se2_trajectory_command(goal_se2, frame)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
# with a build_on_command
arm_command = RobotCommandBuilder.arm_stow_command()
command = RobotCommandBuilder.synchro_se2_trajectory_command(goal_se2, frame,
build_on_command=arm_command)
_check_se2_traj_command(command, goal_x, goal_y, goal_heading, frame, 1)
_test_has_arm(command.synchronized_command)
def trajectory_command(goal_x,
goal_y,
goal_heading,
frame_name,
params=None,
body_height=0.0,
locomotion_hint=spot_command_pb2.HINT_AUTO):
"""Command robot to move to pose along a 2D plane. Pose can specified in the world
(kinematic odometry) frame or the robot body frame. The arguments body_height and
locomotion_hint are ignored if params argument is passed.
A trajectory command requires an end time. End time is not set in this function, but rather
is set externally before call to RobotCommandService.
Args:
goal_x: Position X coordinate.
goal_y: Position Y coordinate.
goal_heading: Pose heading in radians.
frame_name: Name of the frame to use.
params(spot.MobilityParams): Spot specific parameters for mobility commands. If not set,
this will be constructed using other args.
body_height: Body height in meters.
locomotion_hint: Locomotion hint to use for the trajectory command.
Returns:
RobotCommand, which can be issued to the robot command service.
"""
if not params:
params = RobotCommandBuilder.mobility_params(
body_height=body_height, locomotion_hint=locomotion_hint)
any_params = RobotCommandBuilder._to_any(params)
position = geometry_pb2.Vec2(x=goal_x, y=goal_y)
pose = geometry_pb2.SE2Pose(position=position, angle=goal_heading)
point = trajectory_pb2.SE2TrajectoryPoint(pose=pose)
traj = trajectory_pb2.SE2Trajectory(points=[point])
traj_command = basic_command_pb2.SE2TrajectoryCommand.Request(
trajectory=traj, se2_frame_name=frame_name)
mobility_command = mobility_command_pb2.MobilityCommand.Request(
se2_trajectory_request=traj_command, params=any_params)
command = robot_command_pb2.RobotCommand(
mobility_command=mobility_command)
return command
def to_proto(self):
"""Converts the math_helpers.SE2Pose into an output of the protobuf geometry_pb2.SE2Pose."""
return geometry_pb2.SE2Pose(position=geometry_pb2.Vec2(x=self.x,
y=self.y),
angle=self.angle)
