# Copyright (c) 2020 Boston Dynamics, Inc.  All rights reserved.
#
# Downloading, reproducing, distributing or otherwise using the SDK Software
# is subject to the terms and conditions of the Boston Dynamics Software
# Development Kit License (20191101-BDSDK-SL).

"""Simple robot command capture tutorial."""

import sys
import math
import time
import bosdyn.client
import bosdyn.client.util
from bosdyn.client.robot_state import RobotStateClient
from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder, blocking_stand
from bosdyn.client.exceptions import ResponseError, InvalidRequestError, TimedOutError
from bosdyn.api import estop_pb2
from bosdyn.api import geometry_pb2 as geo
from bosdyn.client import math_helpers
from bosdyn.client.estop import EstopClient, EstopEndpoint, EstopKeepAlive
from bosdyn.client.frame_helpers import ODOM_FRAME_NAME, VISION_FRAME_NAME, get_odom_tform_body, get_vision_tform_body
from bosdyn.client.lease import LeaseClient, LeaseKeepAlive
from bosdyn.client.lease import Error as LeaseBaseError
from bosdyn.client import ResponseError, RpcError
from bosdyn.api import robot_command_pb2, basic_command_pb2

import ipdb as pdb
import numpy as np
import websockets
import asyncio
import json
import base64
import argparse

try:
    import camera
except:
    print ("!!!! Could not import camera, probably because pyrealsense2 is missing.")
    camera = None

SERVER_IP = "10.8.0.3"
# SERVER_IP = "172.25.123.147"
# SERVER_IP = "localhost"
SERVER_PORT = 8765

FORWARD_STEP_SIZE = 0.3
TURN_ANGLE = np.deg2rad(30.)

# TIME_TO_STABILIZE = 2.0  # 0.5
TIME_TO_STABILIZE = 5.0  # 0.5
USE_LAST_PUBLISHED_FRAME = True
# TIME_TO_STABILIZE = 3.0
# USE_LAST_PUBLISHED_FRAME = False

PRINT_TIMES = True


def verify_estop(robot):
    """Verify the robot is not estopped"""

    client = robot.ensure_client(EstopClient.default_service_name)
    if client.get_status().stop_level != estop_pb2.ESTOP_LEVEL_NONE:
        error_message = "Robot is estopped. Please use an external E-Stop client, such as the" \
        " estop SDK example, to configure E-Stop."
        robot.logger.error(error_message)
        raise Exception(error_message)


def _try_grpc(desc, thunk):
    try:
        return thunk()
    except (ResponseError, RpcError, LeaseBaseError) as err:
        print("Failed {}: {}".format(desc, err))
        return None


class SpotNavServer:
    def __init__(self, config):
        self.config = config
        self.last_published_frame = None

    def reset_last_frame(self):
        self.last_published_frame = None

    def manual_control_loop(self):
        # Create robot object.
        if self.config.norobot:
            robot_command_client = None
            robot_state_client = None
        else:
            sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster')
            sdk.load_app_token(self.config.app_token)
            robot = sdk.create_robot(self.config.hostname)
            robot.authenticate(self.config.username, self.config.password)

            # Check that an estop is connected with the robot so that the robot commands can be executed.
            verify_estop(robot)

            # Create the lease client.
            lease_client = robot.ensure_client(LeaseClient.default_service_name)
            lease = lease_client.acquire()
            robot.time_sync.wait_for_sync()
            lk = bosdyn.client.lease.LeaseKeepAlive(lease_client)

            # Setup clients for the robot state and robot command services.
            robot_state_client = robot.ensure_client(RobotStateClient.default_service_name)
            robot_command_client = robot.ensure_client(RobotCommandClient.default_service_name)

            # Power on the robot and stand it up.
            if not self.config.nopower:
                robot.power_on()
                blocking_stand(robot_command_client)

        # Initialize camera
        camera_pipeline = camera.init_camera()

        # Print pose
        self.get_pose(robot_state_client)
        is_server_on = False

        # Control loop
        while True:
            cmd = input('Command (wasd/qe/c/r):')

            if cmd == 'c':
                break
            elif cmd == 'r':
                print("Remote control..")
                try:
                    self.reset_last_frame()
                    if is_server_on:
                        asyncio.get_event_loop().run_forever()
                    else:
                        is_server_on = True
                        self.remote_control_loop(robot_command_client, robot_state_client,
                                                 camera_pipeline=camera_pipeline,
                                                 dummy=(robot_command_client is None))
                except KeyboardInterrupt:
                    print("ending remote control")
                    pass
            else:
                self.reset_last_frame()
                pos = self.execute_command(robot_command_client, robot_state_client, cmd, wait_to_stabilize=False)

        # move_relative(robot_command_client, robot_state_client, timeout=5.0)

        robot_cmd = RobotCommandBuilder.safe_power_off_command()
        robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + 2.)
        time.sleep(2.)

        return True

    def remote_control_loop(self, robot_command_client, robot_state_client, camera_pipeline=None, dummy=False):
        # Start websocket server
        print("Starting server at %s %d" % (SERVER_IP, SERVER_PORT))

        # https://websockets.readthedocs.io/en/stable/intro.html
        async def printcommand(websocket, path):
            cmd = await websocket.recv()
            print(f"< {cmd}")
            await websocket.send(cmd)
            # print (f"I have received {cmd}!")

        async def processcommand(websocket, path):
            async for cmd in websocket:
                #cmd = await websocket.recv()
                print(f"received {cmd}")
                if cmd == "ping":
                    await websocket.send("ping")
                else:
                    # Execute command
                    t = time.time()
                    if dummy:
                        print(f"should be executing < {cmd}")
                        pos = np.array([0., 0., 0., ], np.float32)
                        self.last_published_frame = None
                    elif cmd == "obs" or cmd == "4":
                        pass  # dont move just read observation
                        frame, pos = self.get_pose(robot_state_client)
                        if USE_LAST_PUBLISHED_FRAME:
                            self.last_published_frame = frame

                    else:
                        pos = self.execute_command(robot_command_client, robot_state_client, cmd)
                    time_move = time.time() - t
                    t = time.time()

                    # Get camera image
                    if camera_pipeline:
                        rgb, depth = camera.get_images(camera_pipeline)
                        # print (depth.shape, depth.dtype)
                    else:
                        rgb = np.zeros((360, 640, 3), np.uint8)
                        depth = np.zeros((360, 640), np.uint16)
                        # rgb = np.zeros((1, 1, 3), np.uint8)
                        # depth = np.zeros((1, 1), np.float32)
                    time_camera = time.time() - t
                    t = time.time()

                    # Send reply
                    # msg = rgb.tobytes()
                    encode = lambda x: base64.b64encode(x).decode('utf8')
                    msg = json.dumps(dict(rgb=encode(rgb),
                                          depth=encode(depth),
                                          pos=encode(pos), ))

                    # msg = json.dumps(dict(rgb=str(rgb.tobytes()), depth=str(depth.tobytes())))
                    await websocket.send(msg)

                    time_send = time.time() - t
                    t = time.time()
                    print ("Time move %.3f camera %.3f reply %.3f"%(time_move, time_camera, time_send))

        # start_server = websockets.serve(printcommand, "10.8.0.3", 8765)
        start_server = websockets.serve(processcommand, SERVER_IP, SERVER_PORT, write_limit=2 ** 24, max_size=2 ** 24)

        asyncio.get_event_loop().run_until_complete(start_server)
        asyncio.get_event_loop().run_forever()

    @staticmethod
    def command_to_relpose(cmd):
        x = 0.
        y = 0.
        yaw = 0.
        success = True
        if cmd == 'ww' or cmd == '1':
            x = FORWARD_STEP_SIZE
        elif cmd == 'ss':
            x = -FORWARD_STEP_SIZE
        elif cmd == 'aa':
            y = FORWARD_STEP_SIZE
        elif cmd == 'dd':
            y = -FORWARD_STEP_SIZE
        elif cmd == 'w':
            x = 2 * FORWARD_STEP_SIZE
        elif cmd == 's':
            x = -2 * FORWARD_STEP_SIZE
        elif cmd == 'a':
            y = 2 * FORWARD_STEP_SIZE
        elif cmd == 'd':
            y = -2 * FORWARD_STEP_SIZE
        elif cmd == 'q' or cmd == '2':
            yaw = TURN_ANGLE  # left
        elif cmd == 'e' or cmd == '3':
            yaw = -TURN_ANGLE  # right
        elif cmd == 'qq':
            yaw = 0.5 * TURN_ANGLE  # left
        elif cmd == 'ee':
            yaw = -0.5 * TURN_ANGLE  # right
        else:
            print("Unknown command")
            success = False
        return success, x, y, yaw

    def execute_command(self, robot_command_client, robot_state_client, cmd, wait_to_stabilize=True):
        _, pos0 = self.get_pose(robot_state_client)  # will print

        # Move to relative pose
        success, x, y, yaw = self.command_to_relpose(cmd)
        if success:
            self.move_relative(robot_command_client, robot_state_client, x=x, y=y, yaw=yaw, start_frame=self.last_published_frame)
        else:
            print("Unknown command")

        # Wait to stabilize
        time.sleep(TIME_TO_STABILIZE if wait_to_stabilize else 0)

        # Get pose
        frame, pos = self.get_pose(robot_state_client)
        if USE_LAST_PUBLISHED_FRAME:
            self.last_published_frame = frame

        # print difference
        pos_diff = pos - pos0
        print("diff %f %f %f" % (pos_diff[0], pos_diff[1], np.rad2deg(pos_diff[2])))

        return pos

    @staticmethod
    def get_pose(robot_state_client):
        if not robot_state_client:
            return None, np.zeros((3,), np.float32)

        # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand
        # the robot's current position in the vision frame.
        vision_tform_body = get_vision_tform_body(
            robot_state_client.get_robot_state().kinematic_state.transforms_snapshot)

        # Position and heading
        # position = (vision_tform_body.x, vision_tform_body.y, vision_tform_body.z)
        # # We get heading by rotating the forward pointing vector (1., 0., 0.)
        # heading = np.array(vision_tform_body.rotation.transform_point(1., 0., 0.), np.float32)

        # x y yaw
        pos = np.array((vision_tform_body.x, vision_tform_body.y, vision_tform_body.rotation.to_yaw()), np.float32)
        print ("pos %f %f %f"%(pos[0], pos[1], np.rad2deg(pos[2])))
        # print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw()))

        return vision_tform_body, pos

    @staticmethod
    def move_relative(robot_command_client, robot_state_client, x, y, yaw, start_frame=None, timeout=10.0, block=False, verbose=False):
        if start_frame is not None:
            vision_tform_body = start_frame
        else:
            # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand
            # the robot's current position in the vision frame.
            vision_tform_body = get_vision_tform_body(
                robot_state_client.get_robot_state().kinematic_state.transforms_snapshot)

        # We want to command a trajectory to go forward one meter in the x-direction of the body.
        # It is simple to define this trajectory relative to the body frame, since we know that will be
        # just 1 meter forward in the x-axis of the body.
        # Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the
        # rotation of the body at the goal to match the rotation of the body currently, so we do not need
        # to transform the rotation.
        rot_quat = math_helpers.Quat().from_yaw(yaw)

        body_tform_goal = math_helpers.SE3Pose(x=x, y=y, z=0, rot=rot_quat)
        # We can then transform this transform to get the goal position relative to the vision frame.
        vision_tform_goal = vision_tform_body * body_tform_goal

        # print ((vision_tform_body.x, vision_tform_body.y), (vision_tform_goal.x, vision_tform_goal.y), )
        # print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw()))

        # Command the robot to go to the goal point in the vision frame. The command will stop at the new
        # position in the vision frame.
        robot_cmd = RobotCommandBuilder.trajectory_command(  # synchro_se2_trajectory_command(  #
            goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(),
            frame_name=VISION_FRAME_NAME)
        # robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + timeout)
        command_id = robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + timeout)

        # This will only issue the command, but it is not blocking. So we wait and check status manually
        now = time.time()
        start_time = time.time()
        end_time = time.time() + timeout
        while now < end_time:
            time_until_timeout = end_time - now
            rpc_timeout = max(time_until_timeout, 1)
            start_call_time = time.time()
            try:
                response = robot_command_client.robot_command_feedback(command_id, timeout=rpc_timeout)
            except TimedOutError:
                # Excuse the TimedOutError and let the while check bail us out if we're out of time.
                print ("Response timeout error")
                pass
            else:
                # print (response.status, robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING)
                # pdb.set_trace()
                if response.status != robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING:
                    raise ValueError('Stand (ID {}) no longer processing (now {})'.format(
                        command_id, response.Status.Name(response.status)))

                if verbose: print (response.feedback.mobility_feedback.se2_trajectory_feedback.status, basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL)
                if (response.feedback.mobility_feedback.se2_trajectory_feedback.status ==
                        basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL):
                        # basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING):
                    if verbose: print (response)
                    break
            # delta_t = time.time() - start_call_time
            # time.sleep(max(min(delta_t, update_time), 0.0))
            time.sleep(0.1)
            now = time.time()
        if verbose: print ("Took %f sec"%(time.time() - start_time))

        # if block:
        #     time.sleep(timeout)

        # # Get new robot state information after moving the robot. Here we are getting the transform odom_tform_body,
        # # which describes the robot body's position in the odom frame.
        # odom_tform_body = get_odom_tform_body(
        #     robot_state_client.get_robot_state().kinematic_state.transforms_snapshot)
        #
        # # We want to command a trajectory to go backwards one meter and to the left one meter.
        # # It is simple to define this trajectory relative to the body frame, since we know that will be
        # # just 1 meter backwards (negative-value) in the x-axis of the body and one meter left (positive-value)
        # # in the y-axis of the body.
        # body_tform_goal = math_helpers.SE3Pose(x=-1, y=1, z=0, rot=math_helpers.Quat())
        # # We can then transform this transform to get the goal position relative to the odom frame.
        # odom_tform_goal = odom_tform_body * body_tform_goal
        # # Command the robot to go to the goal point in the odom frame. The command will stop at the new
        # # position in the odom frame.
        # robot_cmd = RobotCommandBuilder.trajectory_command(goal_x=odom_tform_goal.x,
        #                                                    goal_y=odom_tform_goal.y,
        #                                                    goal_heading=odom_tform_goal.rot.to_yaw(),
        #                                                    frame_name=ODOM_FRAME_NAME)
        # robot_command_client.robot_command(lease=None, command=robot_cmd,
        #                                    end_time_secs=time.time() + timeout)
        # time.sleep(timeout)


def main():
    parser = argparse.ArgumentParser()
    bosdyn.client.util.add_common_arguments(parser)
    parser.add_argument('--nopower', type=bool, default=False)
    parser.add_argument('--norobot', type=bool, default=False)
    config = parser.parse_args()

    server = SpotNavServer(config)
    server.manual_control_loop()


if __name__ == "__main__":
    if not main():
        sys.exit(1)

# Copyright (c) 2020 Boston Dynamics, Inc.  All rights reserved.
#
# Downloading, reproducing, distributing or otherwise using the SDK Software
# is subject to the terms and conditions of the Boston Dynamics Software
# Development Kit License (20191101-BDSDK-SL).

"""Simple robot command capture tutorial."""

import sys
import math
import time
import bosdyn.client
import bosdyn.client.util
from bosdyn.client.robot_state import RobotStateClient
from bosdyn.client.robot_command import RobotCommandClient, RobotCommandBuilder, blocking_stand
from bosdyn.client.exceptions import ResponseError, InvalidRequestError, TimedOutError
from bosdyn.api import estop_pb2
from bosdyn.api import geometry_pb2 as geo
from bosdyn.client import math_helpers
from bosdyn.client.estop import EstopClient, EstopEndpoint, EstopKeepAlive
from bosdyn.client.frame_helpers import ODOM_FRAME_NAME, VISION_FRAME_NAME, get_odom_tform_body, get_vision_tform_body
from bosdyn.client.lease import LeaseClient, LeaseKeepAlive
from bosdyn.client.lease import Error as LeaseBaseError
from bosdyn.client import ResponseError, RpcError
from bosdyn.api import robot_command_pb2, basic_command_pb2

import ipdb as pdb
import numpy as np
import websockets
import asyncio
import json
import base64
import argparse

try:
    import camera
except:
    print ("!!!! Could not import camera, probably because pyrealsense2 is missing.")
    camera = None

SERVER_IP = "10.8.0.3"
# SERVER_IP = "172.25.123.147"
# SERVER_IP = "localhost"
SERVER_PORT = 8765

FORWARD_STEP_SIZE = 0.3
TURN_ANGLE = np.deg2rad(30.)

# TIME_TO_STABILIZE = 2.0  # 0.5
TIME_TO_STABILIZE = 5.0  # 0.5
USE_LAST_PUBLISHED_FRAME = True
# TIME_TO_STABILIZE = 3.0
# USE_LAST_PUBLISHED_FRAME = False

PRINT_TIMES = True


def verify_estop(robot):
    """Verify the robot is not estopped"""

    client = robot.ensure_client(EstopClient.default_service_name)
    if client.get_status().stop_level != estop_pb2.ESTOP_LEVEL_NONE:
        error_message = "Robot is estopped. Please use an external E-Stop client, such as the" \
        " estop SDK example, to configure E-Stop."
        robot.logger.error(error_message)
        raise Exception(error_message)


def _try_grpc(desc, thunk):
    try:
        return thunk()
    except (ResponseError, RpcError, LeaseBaseError) as err:
        print("Failed {}: {}".format(desc, err))
        return None


class SpotNavServer:
    def __init__(self, config):
        self.config = config
        self.last_published_frame = None

    def reset_last_frame(self):
        self.last_published_frame = None

    def manual_control_loop(self):
        # Create robot object.
        if self.config.norobot:
            robot_command_client = None
            robot_state_client = None
        else:
            sdk = bosdyn.client.create_standard_sdk('RobotCommandMaster')
            sdk.load_app_token(self.config.app_token)
            robot = sdk.create_robot(self.config.hostname)
            robot.authenticate(self.config.username, self.config.password)

            # Check that an estop is connected with the robot so that the robot commands can be executed.
            verify_estop(robot)

            # Create the lease client.
            lease_client = robot.ensure_client(LeaseClient.default_service_name)
            lease = lease_client.acquire()
            robot.time_sync.wait_for_sync()
            lk = bosdyn.client.lease.LeaseKeepAlive(lease_client)

            # Setup clients for the robot state and robot command services.
            robot_state_client = robot.ensure_client(RobotStateClient.default_service_name)
            robot_command_client = robot.ensure_client(RobotCommandClient.default_service_name)

            # Power on the robot and stand it up.
            if not self.config.nopower:
                robot.power_on()
                blocking_stand(robot_command_client)

        # Initialize camera
        camera_pipeline = camera.init_camera()

        # Print pose
        self.get_pose(robot_state_client)
        is_server_on = False

        # Control loop
        while True:
            cmd = input('Command (wasd/qe/c/r):')

            if cmd == 'c':
                break
            elif cmd == 'r':
                print("Remote control..")
                try:
                    self.reset_last_frame()
                    if is_server_on:
                        asyncio.get_event_loop().run_forever()
                    else:
                        is_server_on = True
                        self.remote_control_loop(robot_command_client, robot_state_client,
                                                 camera_pipeline=camera_pipeline,
                                                 dummy=(robot_command_client is None))
                except KeyboardInterrupt:
                    print("ending remote control")
                    pass
            else:
                self.reset_last_frame()
                pos = self.execute_command(robot_command_client, robot_state_client, cmd, wait_to_stabilize=False)

        # move_relative(robot_command_client, robot_state_client, timeout=5.0)

        robot_cmd = RobotCommandBuilder.safe_power_off_command()
        robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + 2.)
        time.sleep(2.)

        return True

    def remote_control_loop(self, robot_command_client, robot_state_client, camera_pipeline=None, dummy=False):
        # Start websocket server
        print("Starting server at %s %d" % (SERVER_IP, SERVER_PORT))

        # https://websockets.readthedocs.io/en/stable/intro.html
        async def printcommand(websocket, path):
            cmd = await websocket.recv()
            print(f"< {cmd}")
            await websocket.send(cmd)
            # print (f"I have received {cmd}!")

        async def processcommand(websocket, path):
            async for cmd in websocket:
                #cmd = await websocket.recv()
                print(f"received {cmd}")
                if cmd == "ping":
                    await websocket.send("ping")
                else:
                    # Execute command
                    t = time.time()
                    if dummy:
                        print(f"should be executing < {cmd}")
                        pos = np.array([0., 0., 0., ], np.float32)
                        self.last_published_frame = None
                    elif cmd == "obs" or cmd == "4":
                        pass  # dont move just read observation
                        frame, pos = self.get_pose(robot_state_client)
                        if USE_LAST_PUBLISHED_FRAME:
                            self.last_published_frame = frame

                    else:
                        pos = self.execute_command(robot_command_client, robot_state_client, cmd)
                    time_move = time.time() - t
                    t = time.time()

                    # Get camera image
                    if camera_pipeline:
                        rgb, depth = camera.get_images(camera_pipeline)
                        # print (depth.shape, depth.dtype)
                    else:
                        rgb = np.zeros((360, 640, 3), np.uint8)
                        depth = np.zeros((360, 640), np.uint16)
                        # rgb = np.zeros((1, 1, 3), np.uint8)
                        # depth = np.zeros((1, 1), np.float32)
                    time_camera = time.time() - t
                    t = time.time()

                    # Send reply
                    # msg = rgb.tobytes()
                    encode = lambda x: base64.b64encode(x).decode('utf8')
                    msg = json.dumps(dict(rgb=encode(rgb),
                                          depth=encode(depth),
                                          pos=encode(pos), ))

                    # msg = json.dumps(dict(rgb=str(rgb.tobytes()), depth=str(depth.tobytes())))
                    await websocket.send(msg)

                    time_send = time.time() - t
                    t = time.time()
                    print ("Time move %.3f camera %.3f reply %.3f"%(time_move, time_camera, time_send))

        # start_server = websockets.serve(printcommand, "10.8.0.3", 8765)
        start_server = websockets.serve(processcommand, SERVER_IP, SERVER_PORT, write_limit=2 ** 24, max_size=2 ** 24)

        asyncio.get_event_loop().run_until_complete(start_server)
        asyncio.get_event_loop().run_forever()

    @staticmethod
    def command_to_relpose(cmd):
        x = 0.
        y = 0.
        yaw = 0.
        success = True
        if cmd == 'ww' or cmd == '1':
            x = FORWARD_STEP_SIZE
        elif cmd == 'ss':
            x = -FORWARD_STEP_SIZE
        elif cmd == 'aa':
            y = FORWARD_STEP_SIZE
        elif cmd == 'dd':
            y = -FORWARD_STEP_SIZE
        elif cmd == 'w':
            x = 2 * FORWARD_STEP_SIZE
        elif cmd == 's':
            x = -2 * FORWARD_STEP_SIZE
        elif cmd == 'a':
            y = 2 * FORWARD_STEP_SIZE
        elif cmd == 'd':
            y = -2 * FORWARD_STEP_SIZE
        elif cmd == 'q' or cmd == '2':
            yaw = TURN_ANGLE  # left
        elif cmd == 'e' or cmd == '3':
            yaw = -TURN_ANGLE  # right
        elif cmd == 'qq':
            yaw = 0.5 * TURN_ANGLE  # left
        elif cmd == 'ee':
            yaw = -0.5 * TURN_ANGLE  # right
        else:
            print("Unknown command")
            success = False
        return success, x, y, yaw

    def execute_command(self, robot_command_client, robot_state_client, cmd, wait_to_stabilize=True):
        _, pos0 = self.get_pose(robot_state_client)  # will print

        # Move to relative pose
        success, x, y, yaw = self.command_to_relpose(cmd)
        if success:
            self.move_relative(robot_command_client, robot_state_client, x=x, y=y, yaw=yaw, start_frame=self.last_published_frame)
        else:
            print("Unknown command")

        # Wait to stabilize
        time.sleep(TIME_TO_STABILIZE if wait_to_stabilize else 0)

        # Get pose
        frame, pos = self.get_pose(robot_state_client)
        if USE_LAST_PUBLISHED_FRAME:
            self.last_published_frame = frame

        # print difference
        pos_diff = pos - pos0
        print("diff %f %f %f" % (pos_diff[0], pos_diff[1], np.rad2deg(pos_diff[2])))

        return pos

    @staticmethod
    def get_pose(robot_state_client):
        if not robot_state_client:
            return None, np.zeros((3,), np.float32)

        # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand
        # the robot's current position in the vision frame.
        vision_tform_body = get_vision_tform_body(
            robot_state_client.get_robot_state().kinematic_state.transforms_snapshot)

        # Position and heading
        # position = (vision_tform_body.x, vision_tform_body.y, vision_tform_body.z)
        # # We get heading by rotating the forward pointing vector (1., 0., 0.)
        # heading = np.array(vision_tform_body.rotation.transform_point(1., 0., 0.), np.float32)

        # x y yaw
        pos = np.array((vision_tform_body.x, vision_tform_body.y, vision_tform_body.rotation.to_yaw()), np.float32)
        print ("pos %f %f %f"%(pos[0], pos[1], np.rad2deg(pos[2])))
        # print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw()))

        return vision_tform_body, pos

    @staticmethod
    def move_relative(robot_command_client, robot_state_client, x, y, yaw, start_frame=None, timeout=10.0, block=False, verbose=False):
        if start_frame is not None:
            vision_tform_body = start_frame
        else:
            # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand
            # the robot's current position in the vision frame.
            vision_tform_body = get_vision_tform_body(
                robot_state_client.get_robot_state().kinematic_state.transforms_snapshot)

        # We want to command a trajectory to go forward one meter in the x-direction of the body.
        # It is simple to define this trajectory relative to the body frame, since we know that will be
        # just 1 meter forward in the x-axis of the body.
        # Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the
        # rotation of the body at the goal to match the rotation of the body currently, so we do not need
        # to transform the rotation.
        rot_quat = math_helpers.Quat().from_yaw(yaw)

        body_tform_goal = math_helpers.SE3Pose(x=x, y=y, z=0, rot=rot_quat)
        # We can then transform this transform to get the goal position relative to the vision frame.
        vision_tform_goal = vision_tform_body * body_tform_goal

        # print ((vision_tform_body.x, vision_tform_body.y), (vision_tform_goal.x, vision_tform_goal.y), )
        # print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw()))

        # Command the robot to go to the goal point in the vision frame. The command will stop at the new
        # position in the vision frame.
        robot_cmd = RobotCommandBuilder.trajectory_command(  # synchro_se2_trajectory_command(  #
            goal_x=vision_tform_goal.x, goal_y=vision_tform_goal.y, goal_heading=vision_tform_goal.rot.to_yaw(),
            frame_name=VISION_FRAME_NAME)
        # robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + timeout)
        command_id = robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + timeout)

        # This will only issue the command, but it is not blocking. So we wait and check status manually
        now = time.time()
        start_time = time.time()
        end_time = time.time() + timeout
        while now < end_time:
            time_until_timeout = end_time - now
            rpc_timeout = max(time_until_timeout, 1)
            start_call_time = time.time()
            try:
                response = robot_command_client.robot_command_feedback(command_id, timeout=rpc_timeout)
            except TimedOutError:
                # Excuse the TimedOutError and let the while check bail us out if we're out of time.
                print ("Response timeout error")
                pass
            else:
                # print (response.status, robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING)
                # pdb.set_trace()
                if response.status != robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING:
                    raise ValueError('Stand (ID {}) no longer processing (now {})'.format(
                        command_id, response.Status.Name(response.status)))

                if verbose: print (response.feedback.mobility_feedback.se2_trajectory_feedback.status, basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL)
                if (response.feedback.mobility_feedback.se2_trajectory_feedback.status ==
                        basic_command_pb2.SE2TrajectoryCommand.Feedback.STATUS_AT_GOAL):
                        # basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING):
                    if verbose: print (response)
                    break
            # delta_t = time.time() - start_call_time
            # time.sleep(max(min(delta_t, update_time), 0.0))
            time.sleep(0.1)
            now = time.time()
        if verbose: print ("Took %f sec"%(time.time() - start_time))

        # if block:
        #     time.sleep(timeout)

        # # Get new robot state information after moving the robot. Here we are getting the transform odom_tform_body,
        # # which describes the robot body's position in the odom frame.
        # odom_tform_body = get_odom_tform_body(
        #     robot_state_client.get_robot_state().kinematic_state.transforms_snapshot)
        #
        # # We want to command a trajectory to go backwards one meter and to the left one meter.
        # # It is simple to define this trajectory relative to the body frame, since we know that will be
        # # just 1 meter backwards (negative-value) in the x-axis of the body and one meter left (positive-value)
        # # in the y-axis of the body.
        # body_tform_goal = math_helpers.SE3Pose(x=-1, y=1, z=0, rot=math_helpers.Quat())
        # # We can then transform this transform to get the goal position relative to the odom frame.
        # odom_tform_goal = odom_tform_body * body_tform_goal
        # # Command the robot to go to the goal point in the odom frame. The command will stop at the new
        # # position in the odom frame.
        # robot_cmd = RobotCommandBuilder.trajectory_command(goal_x=odom_tform_goal.x,
        #                                                    goal_y=odom_tform_goal.y,
        #                                                    goal_heading=odom_tform_goal.rot.to_yaw(),
        #                                                    frame_name=ODOM_FRAME_NAME)
        # robot_command_client.robot_command(lease=None, command=robot_cmd,
        #                                    end_time_secs=time.time() + timeout)
        # time.sleep(timeout)


def main():
    parser = argparse.ArgumentParser()
    bosdyn.client.util.add_common_arguments(parser)
    parser.add_argument('--nopower', type=bool, default=False)
    parser.add_argument('--norobot', type=bool, default=False)
    config = parser.parse_args()

    server = SpotNavServer(config)
    server.manual_control_loop()


if __name__ == "__main__":
    if not main():
        sys.exit(1)

# Copyright (c) 2020 Boston Dynamics, Inc.  All rights reserved. # # Downloading, reproducing, distributing or otherwise using the SDK Software # is subject to the terms and conditions of the Boston Dynamics Software # Development Kit License (20191101-BDSDK-SL). Simple robot command capture tutorial. # SERVER_IP = "172.25.123.147" # SERVER_IP = "localhost" # TIME_TO_STABILIZE = 2.0  # 0.5 # 0.5 # TIME_TO_STABILIZE = 3.0 # USE_LAST_PUBLISHED_FRAME = False Verify the robot is not estopped # Create robot object. # Check that an estop is connected with the robot so that the robot commands can be executed. # Create the lease client. # Setup clients for the robot state and robot command services. # Power on the robot and stand it up. # Initialize camera # Print pose # Control loop # move_relative(robot_command_client, robot_state_client, timeout=5.0) # Start websocket server # https://websockets.readthedocs.io/en/stable/intro.html # print (f"I have received {cmd}!") #cmd = await websocket.recv() # Execute command # dont move just read observation # Get camera image # print (depth.shape, depth.dtype) # rgb = np.zeros((1, 1, 3), np.uint8) # depth = np.zeros((1, 1), np.float32) # Send reply # msg = rgb.tobytes() # msg = json.dumps(dict(rgb=str(rgb.tobytes()), depth=str(depth.tobytes()))) # start_server = websockets.serve(printcommand, "10.8.0.3", 8765) # left # right # left # right # will print # Move to relative pose # Wait to stabilize # Get pose # print difference # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand # the robot's current position in the vision frame. # Position and heading # position = (vision_tform_body.x, vision_tform_body.y, vision_tform_body.z) # # We get heading by rotating the forward pointing vector (1., 0., 0.) # heading = np.array(vision_tform_body.rotation.transform_point(1., 0., 0.), np.float32) # x y yaw # print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw())) # Get robot state information. Specifically, we are getting the vision_tform_body transform to understand # the robot's current position in the vision frame. # We want to command a trajectory to go forward one meter in the x-direction of the body. # It is simple to define this trajectory relative to the body frame, since we know that will be # just 1 meter forward in the x-axis of the body. # Note that the rotation is just math_helpers.Quat(), which is the identity quaternion. We want the # rotation of the body at the goal to match the rotation of the body currently, so we do not need # to transform the rotation. # We can then transform this transform to get the goal position relative to the vision frame. # print ((vision_tform_body.x, vision_tform_body.y), (vision_tform_goal.x, vision_tform_goal.y), ) # print (np.rad2deg(vision_tform_body.rotation.to_yaw()), np.rad2deg(vision_tform_goal.rotation.to_yaw())) # Command the robot to go to the goal point in the vision frame. The command will stop at the new # position in the vision frame. # synchro_se2_trajectory_command(  # # robot_command_client.robot_command(lease=None, command=robot_cmd, end_time_secs=time.time() + timeout) # This will only issue the command, but it is not blocking. So we wait and check status manually # Excuse the TimedOutError and let the while check bail us out if we're out of time. # print (response.status, robot_command_pb2.RobotCommandFeedbackResponse.STATUS_PROCESSING) # pdb.set_trace() # basic_command_pb2.StandCommand.Feedback.STATUS_IS_STANDING): # delta_t = time.time() - start_call_time # time.sleep(max(min(delta_t, update_time), 0.0)) # if block: #     time.sleep(timeout) # # Get new robot state information after moving the robot. Here we are getting the transform odom_tform_body, # # which describes the robot body's position in the odom frame. # odom_tform_body = get_odom_tform_body( #     robot_state_client.get_robot_state().kinematic_state.transforms_snapshot) # # # We want to command a trajectory to go backwards one meter and to the left one meter. # # It is simple to define this trajectory relative to the body frame, since we know that will be # # just 1 meter backwards (negative-value) in the x-axis of the body and one meter left (positive-value) # # in the y-axis of the body. # body_tform_goal = math_helpers.SE3Pose(x=-1, y=1, z=0, rot=math_helpers.Quat()) # # We can then transform this transform to get the goal position relative to the odom frame. # odom_tform_goal = odom_tform_body * body_tform_goal # # Command the robot to go to the goal point in the odom frame. The command will stop at the new # # position in the odom frame. # robot_cmd = RobotCommandBuilder.trajectory_command(goal_x=odom_tform_goal.x, #                                                    goal_y=odom_tform_goal.y, #                                                    goal_heading=odom_tform_goal.rot.to_yaw(), #                                                    frame_name=ODOM_FRAME_NAME) # robot_command_client.robot_command(lease=None, command=robot_cmd, #                                    end_time_secs=time.time() + timeout) # time.sleep(timeout)