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 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)
