interface_services.py

Go to the documentation of this file.

#!/usr/bin/env python

import copy
import time

import rospy
import os

from frame_editor.objects import *
from frame_editor.commands import *
from frame_editor.interface import Interface

from frame_editor.constructors_geometry import *
from frame_editor.constructors_std import *

from frame_editor.srv import *


class FrameEditor_Services(Interface):

    def __init__(self, frame_editor):

        self.editor = frame_editor

        rospy.Service("~align_frame", AlignFrame, self.callback_align_frame)
        rospy.Service("~edit_frame", EditFrame, self.callback_edit_frame)
        rospy.Service("~get_frame", GetFrame, self.callback_get_frame)
        rospy.Service("~remove_frame", RemoveFrame, self.callback_remove_frame)
        rospy.Service("~set_frame", SetFrame, self.callback_set_frame)
        rospy.Service("~set_parent", SetParentFrame, self.callback_set_parent_frame)
        rospy.Service("~copy_frame", CopyFrame, self.callback_copy_frame)

        rospy.Service("~load_yaml", LoadYaml, self.callback_load_yaml)
        rospy.Service("~save_yaml", SaveYaml, self.callback_save_yaml)


    def callback_align_frame(self, request):
        print("> Request to align frame {} with frame {} mode {}".format(request.name, request.source_name, request.mode))

        response = AlignFrameResponse()
        response.error_code = 0

        if request.name == "":
            print(" Error: No name given")
            response.error_code = 1

        elif request.source_name == "":
            print(" Error: No source name given")
            response.error_code = 3

        elif request.name not in self.editor.frames:
            print(" Error: Frame not found: {}".format(request.name))
            response.error_code = 2

        else:
            frame = self.editor.frames[request.name]

            m = request.mode
            mode = []
            if m & 1: mode.append("x")
            if m & 2: mode.append("y")
            if m & 4: mode.append("z")
            if m & 8: mode.append("a")
            if m & 16: mode.append("b")
            if m & 32: mode.append("c")

            self.editor.command(Command_AlignElement(self.editor, frame, request.source_name, mode))

        return response


    def callback_edit_frame(self, request):
        print("> Request to edit frame {}".format(request.name))

        response = EditFrameResponse()
        response.error_code = 0

        if request.name == "":
            ## Reset
            self.editor.command(Command_SelectElement(self.editor, None))

        elif request.name not in self.editor.frames:
            print(" Error: Frame not found: {}".format(request.name))
            response.error_code = 2

        else:
            ## Set
            self.editor.command(Command_SelectElement(self.editor, self.editor.frames[request.name]))

        return response


    def callback_get_frame(self, request):
        print("> Request to get frame {}".format(request.name))

        response = GetFrameResponse()
        response.error_code = 0

        if request.name == "":
            print(" Error: No name given")
            response.error_code = 1

        elif request.name not in self.editor.frames:
            print(" Error: Frame not found: {}".format(request.name))
            response.error_code = 2

        else:
            f = self.editor.frames[request.name]
            f.print_all()
            response.name = f.name
            response.parent = f.parent
            response.pose = ToPose(f.position, f.orientation)

        return response


    def callback_remove_frame(self, request):
        print("> Request to remove frame {}".format(request.name))

        response = RemoveFrameResponse()
        response.error_code = 0

        if request.name == "":
            print(" Error: No name given")
            response.error_code = 1

        elif request.name not in self.editor.frames:
            print(" Error: Frame not found: {}".format(request.name))
            response.error_code = 2

        else:
            self.editor.command(Command_RemoveElement(self.editor, self.editor.frames[request.name]))

        return response


    def callback_set_frame(self, request):
        print("> Request to set (or add) frame {} {}".format(request.name, request.parent))

        response = SetFrameResponse()

        if request.name == "":
            print(" Error: No name given")
            response.error_code = 1
            return response

        if request.parent == "":
            if request.name in self.editor.frames:
                request.parent = self.editor.frames[request.name].parent
            else:
                print("Error: No parent given and frame previously not existing")
                response.error_code = 2
                return response

        f = Frame(request.name,
                  FromPoint(request.pose.position),
                  FromQuaternion(request.pose.orientation),
                  request.parent)
        self.editor.command(Command_AddElement(self.editor, f))
        
        response.error_code = 0
        return response


    def callback_set_parent_frame(self, request):
        print("> Request to set parent_frame {} {}".format(request.name, request.parent))

        response = SetParentFrameResponse()
        response.error_code = 0

        if request.name == "":
            print(" Error: No frame_name given")
            response.error_code = 1

        elif request.parent == "":
            print(" Error: No parent_name given")
            response.error_code = 2

        else:
            f = self.editor.frames[request.name]
            self.editor.command(Command_SetParent(self.editor, f, request.parent, request.keep_absolute))

        return response

    def callback_load_yaml(self, request):
        print("> Request to load yaml file:'{}'".format(request.filename))

        response = LoadYamlResponse()
        try:
            self.editor.load_file(os.path.expanduser(request.filename))
            response.success = True
            response.message = "file loaded"
        except Exception as e:
            response.success = False
            response.message = "Exception: {}".format(str(e))

        return response

    def callback_save_yaml(self, request):
        print("> Request to save yaml file to:'{}'".format(request.filename))

        response = SaveYamlResponse()
        try:
            self.editor.save_file(os.path.expanduser(request.filename))
            response.success = True
            response.message = "file saved"
        except Exception as e:
            response.success = False
            response.message = "Exception: {}".format(str(e))

        return response

    def callback_copy_frame(self, request):
        print("> Request to copy frame '{}' with new name '{}' and new parent name '{}'".format(request.source_name, request.name, request.parent))

        response = CopyFrameResponse()
        response.error_code = 0

        if request.name == "":
            print(" Error: No name given")
            response.error_code = 1

        elif request.source_name == "":
            print(" Error: No source name given")
            response.error_code = 3

        else:
            t = time.time()

            try:
                # If not existing yet: create frame
                if request.name not in self.editor.frames:
                    print(">> add")

                    # No parent specified: use source's parent
                    if request.parent == "":
                        if request.source_name in self.editor.frames:
                            request.parent = self.editor.frames[request.source_name].parent
                        else:
                            print(" Error: No parent name given")
                            response.error_code = 3
                            return response

                    Frame.wait_for_transform(request.source_name, request.parent, rospy.Duration(1.0))
                    self.editor.command(Command_CopyElement(self.editor, request.name, request.source_name, request.parent))
                    Frame.wait_for_transform(request.parent, request.name, rospy.Duration(1.0))


                else:
                    frame = self.editor.frames[request.name]

                    Frame.wait_for_transform(request.source_name, request.parent, rospy.Duration(1.0))
                    if (request.parent != "") and (frame.parent != request.parent):
                        print(">> rebase")
                        self.editor.command(Command_RebaseElement(self.editor, frame, request.source_name, request.parent))
                    else:
                        print(">> align")
                        self.editor.command(Command_AlignElement(self.editor, frame, request.source_name, ['x', 'y', 'z', 'a', 'b', 'c']))
                    Frame.wait_for_transform(frame.parent, frame.name, rospy.Duration(1.0))

            except Exception as e:
                print("Error: unhandled exception {}".format(e))
                response.error_code = 9

            print(time.time() - t)

        return response

# eof