import roslibpy.actionlib

client = roslibpy.Ros(host='10.10.10.10', port=9090)
client.run()

print(f"Is ROS connected? {client.is_connected}")


def request(service_name, type, value=None):
    service = roslibpy.Service(client, service_name, type)
    request = roslibpy.ServiceRequest(value)
    return service.call(request)


request('niryo_robot_tools_commander/update_tool', 'std_srvs/Trigger')
request('niryo_robot_tools_commander/enable_tcp', "std_srvs/Bool", {'value': True})


action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
            'cmd_type': 1,
            'position': {
                'x': 0.2,
                'y': 0.2,
                'z': 0.02
            },
            'rpy': {
                'roll': 0,
                'pitch': 1.57,
                'yaw': 0
            }
        }}))

goal.send()
goal.wait(10)


action = roslibpy.actionlib.ActionClient(client, "/niryo_robot_tools_commander/action_server", '/niryo_robot_tools_commander/ToolAction')
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({
    'cmd': {
        'cmd_type': 2,
        'tool_id': 11,
        'max_torque_percentage': 100,
        'hold_torque_percentage': 30
    }
}))

goal.send()
goal.wait(10)

action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
            'cmd_type': 1,
            'position': {
                'x': 0.2,
                'y': 0.2,
                'z': 0.2
            },
            'rpy': {
                'roll': 0,
                'pitch': 1.57,
                'yaw': 0
            }
        }}))

goal.send()
goal.wait(10)

action = roslibpy.actionlib.ActionClient(client, '/niryo_robot_arm_commander/robot_action', '/niryo_robot_arm_commander/RobotMoveAction')
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({'cmd': {
            'cmd_type': 1,
            'position': {
                'x': -0.2,
                'y': -0.2,
                'z': 0.02
            },
            'rpy': {
                'roll': 0,
                'pitch': 1.57,
                'yaw': 0
            }
        }}))

goal.send()
goal.wait(10)

action = roslibpy.actionlib.ActionClient(client, "/niryo_robot_tools_commander/action_server", '/niryo_robot_tools_commander/ToolAction')
goal = roslibpy.actionlib.Goal(action, roslibpy.Message({
    'cmd': {
        'cmd_type': 1,
        'tool_id': 11,
        'max_torque_percentage': 100,
        'hold_torque_percentage': 30
    }
}))

goal.send()
goal.wait(10)