def setUp(self):
     # check projector is on network
     if not ip_open("192.168.10.11",9090):
         self.skipTest("projector is not found on network")
     license_path = (os.path.dirname(os.path.dirname(os.path.abspath(__file__)))+'/lic/1900027652.lic')
     self.projector_manager = ZLPProjectorManager(projector_IP = "192.168.10.10", 
                                                  server_IP = "192.168.10.11", 
                                                  connection_port = 9090, 
                                                  lic_path=license_path)
Пример #2
0
    def __init__(self, projector_IP, server_IP, connection_port, license_path):
        """Initialize the ZLPProjectorROS object."""

        self.lic_path = license_path

        self.projector = ZLPProjectorManager(projector_IP, server_IP, connection_port, self.lic_path)

        self.run_viz = False

        self.STD_WAIT_TIME = CoordinateSystemParameters().DEFAULT_SHOW_TIME
        
        rospy.set_param('projector_connected', False)
    def test2_no_server_IP(self):

        projector_manager = ZLPProjectorManager(projector_IP = "192.168.10.10", 
                                                server_IP = "", 
                                                connection_port = 9090, 
                                                lic_path="")

        try:
            projector_manager.connect_and_setup()
            self.fail("error test2")

        except Exception:
            pass
    def test3_diconnection(self):

        license_path = (os.path.dirname(os.path.dirname(os.path.abspath(__file__)))+'/lic/1900027652.lic')
        projector_manager = ZLPProjectorManager(projector_IP = "192.168.10.10", 
                                                server_IP = "192.168.10.11", 
                                                connection_port = 9090, 
                                                lic_path=license_path)

        try:
            projector_manager.connect_and_setup()
            status = projector_manager.get_connection_status()
            if status:
                projector_manager.deactivate()
                projector_manager.client_server_disconnect()
                pass
            else:
                self.fail("error test3")

        except Exception:
            self.fail("error test3")
Пример #5
0
class ZLPProjectorROS(object):
    """This class initilizes the services and implements the functionalities of the node.

    Args:
        projector_IP (str): IP number of projector device
        server_IP (str): IP number of service running at projector device
        connection_port (int): connection port number 
        license_path (str): path of license file

    Attributes:
        lic_path (str): folder path where Z-Laser license file is located
        projector (object): ZLPProjectorManager object from projector_manager library
        run_viz (bool): true if visualizer is running, false otherwise
        STD_WAIT_TIME (int): predefined number of projection seconds in reference system definition
    """
    def __init__(self, projector_IP, server_IP, connection_port, license_path):
        """Initialize the ZLPProjectorROS object."""

        self.lic_path = license_path

        self.projector = ZLPProjectorManager(projector_IP, server_IP, connection_port, self.lic_path)

        self.run_viz = False

        self.STD_WAIT_TIME = CoordinateSystemParameters().DEFAULT_SHOW_TIME
        
        rospy.set_param('projector_connected', False)

    def open_services(self):
        """Open ROS services that allow projector device control."""
        self.connect       = rospy.Service('connect', Trigger, self.connection_cb)
        self.disconnect    = rospy.Service('disconnect', Trigger, self.disconnection_cb)
        self.start_proj    = rospy.Service('projection_start', Trigger, self.projection_start_cb)
        self.stop_proj     = rospy.Service('projection_stop', Trigger, self.projection_stop_cb)

        self.manual_cs     = rospy.Service('define_coordinate_system', CoordinateSystem, self.manual_define_coord_sys_cb)
        self.auto_cs       = rospy.Service('search_targets', CoordinateSystem, self.auto_search_target_cb)

        self.get_cs_list   = rospy.Service('coordinate_system_list', CoordinateSystemList, self.get_coord_sys_list_cb)

        self.set_cs        = rospy.Service('set_coordinate_system', CoordinateSystemName, self.set_coord_sys_cb)
        self.rem_cs        = rospy.Service('remove_coordinate_system', CoordinateSystemName, self.remove_coord_sys_cb)
        self.show_cs       = rospy.Service('show_active_coordinate_system', CoordinateSystemShow, self.show_coord_sys_cb)

        self.hide_proj_elem   = rospy.Service('hide_projection_element', ProjectionElement, self.hide_proj_elem_cb)
        self.unhide_proj_elem = rospy.Service('unhide_projection_element', ProjectionElement, self.unhide_proj_elem_cb)
        self.remove_proj_elem = rospy.Service('remove_projection_element', ProjectionElement, self.remove_proj_elem_cb)
        self.monit_proj_elem  = rospy.Service('monitor_projection_element', ProjectionElement, self.keyboard_monitor_proj_elem_cb)
        
        self.scan_pointer  = rospy.Service('scan_pointer', Trigger, self.scan_pointer_cb)
        
        self.add_proj_elem   = rospy.Subscriber("add_projection_element", Figure, self.add_fig_cb)
        self.add_pointer     = rospy.Subscriber("add_pointer", Figure, self.add_pointer_cb)

        rospy.loginfo("Use ROS Services: \n\t\t\trosservice list")

    def set_viz_run(self, run):
        """Set status of run_viz attribute and define ROS publisher if required.

        Args:
            run (bool): true if visulizer is running, false otherwise
        """
        if run:
            self.run_viz = True
            self.viz_monitor_fig = rospy.Publisher('/zlaser_viz/monitor_projection_element', Figure, queue_size=10)

    def connection_cb(self, req):
        """Callback of ROS service to connect to ZLP-Server, transfer license and activate projector.

        Args:
            req (object): trigger request ROS service object 

        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to connect projector.")
        try:
            self.projector.client_server_connect()       
            self.projector.load_license(self.lic_path)
            self.projector.activate()
            self.projector.geotree_operator_create()
            rospy.loginfo("Projector connected.")
            rospy.set_param('projector_connected', True)
            return TriggerResponse(True, "Projector connected.")
        
        except Exception as e:
            rospy.logerr(e)
            return TriggerResponse(False, str(e))

    def disconnection_cb(self, req):
        """Callback of ROS service to deactivate projector and disconnect from ZLP-Server.

        Args:
            req (object): trigger request ROS service object 

        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to disconnect projector.")
        try:
            self.projector.deactivate()
            self.projector.client_server_disconnect()
            rospy.loginfo("Projector disconnected.")
            rospy.set_param('projector_connected', False)
            return TriggerResponse(True, "Projector disconnected.")

        except Exception as e:
            rospy.logerr(e)
            return TriggerResponse(False, str(e))

    def projection_start_cb(self, req):
        """Callback of ROS service to start projection of elements related to the active reference system on the surface.
        (see :func:`set_coord_sys_cb`)

        Args:
            req (object): trigger request ROS service object 

        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to start projection")
        try:
            self.projector.start_projection()

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/projection_start")
                viz_start_proj = rospy.ServiceProxy('/zlaser_viz/projection_start', Trigger)
                viz_start_proj()

            return TriggerResponse(True, "Projection started.")

        except Exception as e:
            rospy.logerr(e)
            return TriggerResponse(False, str(e))
    
    def projection_stop_cb(self, req):
        """Callback of ROS service to stop projection of all elements.

        Args:
            req (object): trigger request ROS service object 

        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to stop projection")
        try:
            self.projector.stop_projection()

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/projection_stop")
                viz_stop_proj = rospy.ServiceProxy('/zlaser_viz/projection_stop', Trigger)
                viz_stop_proj()

            return TriggerResponse(True, "Projection stopped.")

        except Exception as e:
            rospy.logerr(e)
            return TriggerResponse(False, str(e))

    def manual_define_coord_sys_cb(self, req):
        """Callback of ROS service to define a new reference system, stating the points coordinates manually by the user.

        Args:
            req (object): object with the necessary parameters to define a new coordinate system
            
        Returns:
            tuple[list, bool, str]: the first value in the returned tuple is a list of the user reference points T0, T1, T2, T3,
            the second is a bool success value and the third s an information message string
        """
        rospy.loginfo("Received request to create a new coordinate system manually. Please wait for the system to indicate the end.")
        
        try:
            params = CoordinateSystemParameters.req_to_param(req)
            self.projector.define_coordinate_system(params, False)
            
            self.set_rosparam_coordinate_system(params)

            cs = self.projector.get_coordinate_system_params(params.name)
            self.projector.cs_frame_create(cs)
            self.projector.cs_axes_create(cs)
            message = "Coordinate system correctly defined:"
            rospy.loginfo(message)

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/define_coordinate_system")
                viz_manual_cs = rospy.ServiceProxy('/zlaser_viz/define_coordinate_system', CoordinateSystem)
                viz_manual_cs(req)
            
            rospy.loginfo("Projecting demonstration")
            self.projector.show_coordinate_system()
            rospy.sleep(self.STD_WAIT_TIME)
            self.projector.hide_coordinate_system()
            self.projector.show_frame()
            rospy.sleep(self.STD_WAIT_TIME)
            self.projector.hide_frame()

            return CoordinateSystemResponse(cs.T, True, message)

        except Exception as e:
            rospy.logerr(e)
            return CoordinateSystemResponse([], False, str(e))

    def auto_search_target_cb(self,req):
        """Callback of ROS service to define a new reference system by scanning the targets automatically with the projector.

        Args:
            req (object): object with the necessary parameters to define a new coordinate system by scanning targets
            
        Returns:
            tuple[list, bool, str]: the first value in the returned tuple is a list of the user reference points T0, T1, T2, T3,
            the second is a bool success value and the third s an information message string
        """
        rospy.loginfo("Received request to scan a new reference system.")

        try:
            params = CoordinateSystemParameters.req_to_param(req)
            self.projector.define_coordinate_system(params,True)

            coordinate_system_scanned = self.projector.get_coordinate_system_params(params.name)
            self.set_rosparam_coordinate_system(coordinate_system_scanned)

            self.projector.cs_frame_create(coordinate_system_scanned)
            self.projector.cs_axes_create(coordinate_system_scanned)
            message = "Coordinate system correctly scanned:"
            rospy.loginfo(message)

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/define_coordinate_system")
                viz_manual_cs = rospy.ServiceProxy('/zlaser_viz/define_coordinate_system', CoordinateSystem)
                cs_scanned_req = CoordinateSystemParameters.param_to_req(coordinate_system_scanned)
                viz_manual_cs(cs_scanned_req)

            rospy.loginfo("Projecting demonstration")
            self.projector.show_coordinate_system()
            rospy.sleep(self.STD_WAIT_TIME)
            self.projector.hide_coordinate_system()
            self.projector.show_frame()
            rospy.sleep(self.STD_WAIT_TIME)
            self.projector.hide_frame()

            return CoordinateSystemResponse(coordinate_system_scanned.T, True, message)

        except Exception as e:
            rospy.logerr(e)
            return CoordinateSystemResponse([], False, str(e))

    def get_coord_sys_list_cb(self, req):
        """Callback of ROS service to get the list of defined reference systems.

        Args:
            req (object): ROS service CoordinateSystemList request object is empty
        
        Returns:
            tuple[bool, str, list, str]: the first value in the returned tuple is a bool success value, the second 
            is an information message string, the third is a list of the defined reference systems and the last is
            the name of the active reference system
        """
        rospy.loginfo("Received request to get the coordinate system list at projector")
        
        cs_list = []
        try:
            cs_list,active_cs = self.projector.get_coordinate_systems_list()
            return CoordinateSystemListResponse(True,"Coordinate system list:",cs_list,active_cs)

        except Exception as e:
            rospy.logerr(e)
            return CoordinateSystemListResponse(False,str(e),cs_list,active_cs)

    def set_coord_sys_cb(self, req):
        """Callback of ROS service to set the indicated reference system as 'active reference system'.
        It means that services as projection_start or show_active_coordinate_system, etc. automatically use this 
        active reference system to perform their task.
        The rest of coordinate systems are defined and stored in the projector, staying on background until
        any is set as active again.

        Args:
            req (object): object with the necessary parameters to identify a coordinate system
            
        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to set coordinate system.")
        if not req.name:
            return CoordinateSystemNameResponse(False,"Please, specify name")

        try:
            self.projector.set_coordinate_system(req.name)

            cs = self.projector.get_coordinate_system_params(req.name)
            self.set_rosparam_coordinate_system(cs)

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/set_coordinate_system")
                viz_set_cs = rospy.ServiceProxy('/zlaser_viz/set_coordinate_system', CoordinateSystemName)
                viz_set_cs(req)
            
            return CoordinateSystemNameResponse(True,"Set coordinate system")
                    
        except Exception as e:
            rospy.logerr(e)
            return CoordinateSystemNameResponse(False,str(e))

    def show_coord_sys_cb(self,req):
        """Callback of ROS service to project reference points, origin axes and frame of the active reference system.

        Args:
            req (object): object with the necessary parameters to identify a reference system
            
        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Request to project active coordinate system.")
        if req.secs == 0:
            return CoordinateSystemShowResponse(False,"Please, specify seconds")

        try:
            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/show_active_coordinate_system")
                viz_show_cs = rospy.ServiceProxy('/zlaser_viz/show_active_coordinate_system', CoordinateSystemShow)
                viz_show_cs(req)

            rospy.loginfo("Projecting demonstration")
            self.projector.show_coordinate_system()
            rospy.sleep(req.secs)
            self.projector.hide_coordinate_system()
            self.projector.show_frame()
            rospy.sleep(req.secs)
            self.projector.hide_frame()

            return CoordinateSystemShowResponse(True,"Coordinate system showed")
        
        except Exception as e:
            rospy.logerr(e)
            return CoordinateSystemShowResponse(False,str(e))

    def remove_coord_sys_cb(self,req):
        """Callback of ROS service to remove a reference system.

        Args:
            req (object): object with the necessary parameters to identify a reference system
            
        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to remove coordinate system")
        if not req.name:
            return CoordinateSystemNameResponse(False,"Please, specify name")
        
        try:
            active_cs = self.projector.remove_coordinate_system(req.name)

            if active_cs:
                self.set_rosparam_coordinate_system(CoordinateSystemParameters())

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/remove_coordinate_system")
                viz_rem_cs = rospy.ServiceProxy('/zlaser_viz/remove_coordinate_system', CoordinateSystemName)
                viz_rem_cs(req)

            return CoordinateSystemNameResponse(True,"Coordinate system removed")
        
        except Exception as e:
            rospy.logerr(e)
            return CoordinateSystemNameResponse(False,str(e))

    def add_fig_cb(self,msg):
        """Callback of ROS topic to define a new projection element.

        Args:
            msg (object): object with the necessary parameters to define a new projection element
        """
        rospy.loginfo("Received request to add a new projection element to the active coordinate system.")

        if not msg.projection_group or not msg.figure_name:
            rospy.logerr("projection_group or figure_name request is empty.")
        else:
            try:
                if msg.figure_type == Figure.POLYLINE:
                    self.projector.create_polyline(msg)
                    rospy.loginfo("Line added correctly.")
                elif Figure.CIRCLE <= msg.figure_type <= Figure.OVAL:
                    self.projector.create_curve(msg)
                    rospy.loginfo("Curve added correctly.")
                elif msg.figure_type == Figure.TEXT:
                    self.projector.create_text(msg)
                    rospy.loginfo("Text added correctly.")
                elif msg.figure_type == Figure.POINTER:
                    rospy.warn("Pointer has other purpose.")
                else:
                    rospy.logerr("Figure type does not exist.")

                rospy.loginfo("Figure added correctly.")
            
            except Exception as e:
                rospy.logerr(e)

    def hide_proj_elem_cb(self,req):
        """Callback of ROS service to hide specific projection element from active reference system.

        Args:
            req (object): object with the necessary parameters to identify a projection element
            
        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to hide figure.")

        if not req.projection_group or not req.figure_name:
            return ProjectionElementResponse(False,"group_name or figure_name request is empty")

        try:        
            self.projector.hide_proj_elem(req)

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/hide_projection_element")
                viz_hide_figure = rospy.ServiceProxy('/zlaser_viz/hide_projection_element', ProjectionElement)
                viz_hide_figure(req)

            return ProjectionElementResponse(True,"Figure hidden")

        except Exception as e:
            rospy.logerr(e)
            return ProjectionElementResponse(False,str(e))

    def unhide_proj_elem_cb(self,req):
        """Callback of ROS service to unhide specific projection element from active reference system.

        Args:
            req (object): object with the necessary parameters to identify a projection element
            
        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to unhide figure.")

        if not req.projection_group or not req.figure_name:
            return ProjectionElementResponse(False,"group_name or figure_name request is empty")

        try:        
            self.projector.unhide_proj_elem(req)

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/unhide_projection_element")
                viz_unhide_figure = rospy.ServiceProxy('/zlaser_viz/unhide_projection_element', ProjectionElement)
                viz_unhide_figure(req)

            return ProjectionElementResponse(True,"Figure unhidden")

        except Exception as e:
            rospy.logerr(e)
            return ProjectionElementResponse(False,str(e))

    def remove_proj_elem_cb(self,req):
        """Callback of ROS service to remove specific figure from active reference system.

        Args:
            req (object): object with the necessary parameters to identify a projection element
            
        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to remove figure.")

        if not req.projection_group or not req.figure_name:
            return ProjectionElementResponse(False,"group_name or figure_name request is empty")

        try:        
            self.projector.remove_proj_elem(req)

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/remove_projection_element")
                viz_remove_figure = rospy.ServiceProxy('/zlaser_viz/remove_projection_element', ProjectionElement)
                viz_remove_figure(req)

            return ProjectionElementResponse(True,"Figure removed")

        except Exception as e:
            rospy.logerr(e)
            return ProjectionElementResponse(False,str(e))

    def keyboard_monitor_proj_elem_cb(self, req):
        """Callback of ROS service to apply different transformations (translation, rotation, scalation) 
        to a specific projection element on real time by the use of keyboard.

        Args:
            req (object): object with the necessary parameters to identify a projection element
            
        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to monitor figure. PRESS PRESS 'ESC' TO FINISH MONITORING.")

        if not req.projection_group or not req.figure_name:
            return ProjectionElementResponse(False,"group_name or figure_name request is empty")
        
        try:
            # Send info to viz
            if self.run_viz:
                proj_elem = self.projector.get_proj_elem(req)
                figure = proj_elem.to_figure()
                
                if self.viz_monitor_fig.get_num_connections() < 1:
                    return ProjectionElementResponse(False,"No subscribers")

                self.viz_monitor_fig.publish(figure)

            self.projector.monitor_proj_elem(req)
            rospy.loginfo("Monitoring ENDED.")
            return ProjectionElementResponse(True,"Figure monitored")
        
        except Exception as e:
            rospy.logerr(e)
            return ProjectionElementResponse(False,str(e))

    def add_pointer_cb(self, msg):
        """Callback of ROS topic to define a new pointer.

        Args:
            msg (object): object with the necessary parameters to define a new pointer
        """
        rospy.loginfo("Received request to add a new pointer to the active coordinate system.")

        if msg.figure_type == Figure.POINTER:
            if not msg.figure_name:
                rospy.logerr("figure_name request is empty.")
            else:
                try:
                    self.projector.create_pointer(msg)
                    rospy.loginfo("Pointer added correctly.")
                
                except Exception as e:
                    rospy.logerr(e)
        else:
            rospy.logerr("Pointer figure type is required.")

    def pointer_cb_example(self, name, reflection):
        """Callback example for reflection state change, events handler. It is used for running code when
        pointer is reflected.

        Args:
            name (str): name of the pointer that changed state
            reflection (bool): true if a reflection was detected; False otherwise 
        """
        rospy.loginfo("On pointer_cb_example. Detected pointer: %s", name)
        self.projector.stop_projection()

    def scan_pointer_cb(self, req):
        """Callback of ROS service to start pointers scanning.

        Args:
            req (object): trigger request ROS service object 

        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        rospy.loginfo("Received request to scan pointer.")
        
        try:
            self.projector.scan_pointer(self.pointer_cb_example)
            rospy.loginfo("Pointer scanning is active.")
            return TriggerResponse(True, "Pointer scanning is active.")
        
        except Exception as e:
            rospy.logerr(e)
            return TriggerResponse(False, str(e))

    def set_rosparam_coordinate_system(self, cs_params):
        """Set rosparams from given reference system.

        Args:
            cs_params (object): object with the parameters of a reference system
        """
        rospy.set_param('coordinate_system_name', cs_params.name)
        rospy.set_param('coordinate_system_distance', cs_params.distance)
        for i in range(4):
            rospy.set_param('P%d/x' % i, cs_params.P[i].x)
            rospy.set_param('P%d/y' % i, cs_params.P[i].y)
        rospy.set_param('T0/x', cs_params.T[0].x)
        rospy.set_param('T0/y', cs_params.T[0].y)
        rospy.set_param('coordinate_system_resolution', cs_params.resolution)
        
    def read_rosparam_coordinate_system(self):
        """Get parameters of the active reference system from rosparams.

        Returns:
            cs_params (object): object with the parameters of the active reference system
        """
        cs_params            = CoordinateSystemParameters()
        cs_params.name       = rospy.get_param('coordinate_system_name', "default_cs")
        cs_params.distance   = rospy.get_param('coordinate_system_distance', 1500)
        cs_params.P[0].x     = rospy.get_param('P0/x', -100)
        cs_params.P[0].y     = rospy.get_param('P0/y', -100)
        cs_params.P[1].x     = rospy.get_param('P1/x', -100)
        cs_params.P[1].y     = rospy.get_param('P1/y',  100)
        cs_params.P[2].x     = rospy.get_param('P2/x',  100)
        cs_params.P[2].y     = rospy.get_param('P2/y',  100)
        cs_params.P[3].x     = rospy.get_param('P3/x',  100)
        cs_params.P[3].y     = rospy.get_param('P3/y', -100)
        cs_params.T[0].x     = rospy.get_param('T0/x',    0)
        cs_params.T[0].y     = rospy.get_param('T0/y',    0)
        cs_params.resolution = rospy.get_param('coordinate_system_resolution', 1000)
        return cs_params

    def setup_projector(self):
        """Setup projector at initialization (connect to ZLP-Service, transfer license file and activate projector).
 
        Returns:
            bool: success value. True if success, False otherwise.
        """
        rospy.loginfo("Setting projector up")
        try:
            self.projector.connect_and_setup()
            rospy.loginfo("Projector connected.")
            rospy.set_param('projector_connected', True)
            return False
        
        except Exception as e:
            rospy.logerr(e)
            return True

    def initialize_coordinate_system(self):
        """Initial projector setup with the factory or an user predefined reference system from configuration files."""
        params = self.read_rosparam_coordinate_system()
        try:
            self.projector.define_coordinate_system(params, False)
            
            cs = self.projector.get_coordinate_system_params(params.name)
            self.projector.cs_frame_create(cs)
            self.projector.cs_axes_create(cs)
            rospy.loginfo("Coordinate System [{}] loaded".format(params.name))

            # Send info to viz
            if self.run_viz:
                rospy.wait_for_service("/zlaser_viz/define_coordinate_system")
                viz_manual_cs = rospy.ServiceProxy('/zlaser_viz/define_coordinate_system', CoordinateSystem)
                cs_req = CoordinateSystemParameters.param_to_req(params)
                viz_manual_cs(cs_req)

            rospy.loginfo("Projecting demonstration")
            self.projector.show_coordinate_system()
            rospy.sleep(self.STD_WAIT_TIME)
            self.projector.hide_coordinate_system()
            self.projector.show_frame()
            rospy.sleep(self.STD_WAIT_TIME)
            self.projector.hide_frame()

        except Exception as e:
            if "InvalidRelativePath" in str(e):
                rospy.logwarn("Possible error: check coordinate system name is not empty")
            rospy.logerr(e)

    def shutdown_handler(self):
        """Handler to close connection when node exits.
 
        Returns:
            tuple[bool, str]: the first value in the returned tuple is a bool success value and the second value in the tuple 
            is an information message string
        """
        try:
            status = self.projector.get_connection_status()
            if status:
                rospy.loginfo("Disconnecting before shutdown...")
                self.projector.deactivate()
                self.projector.client_server_disconnect()
                rospy.loginfo("Projector disconnected.")
                return TriggerResponse(True, "Projector disconnected.")

        except Exception as e:
            rospy.logerr(e)
            return TriggerResponse(False, str(e))
class TestProjectorManager(unittest.TestCase):
    def setUp(self):
        # check projector is on network
        if not ip_open("192.168.10.11", 9090):
            self.skipTest("projector is not found on network")
        license_path = (
            os.path.dirname(os.path.dirname(os.path.abspath(__file__))) +
            '/lic/1900027652.lic')
        self.projector_manager = ZLPProjectorManager(
            projector_IP="192.168.10.10",
            server_IP="192.168.10.11",
            connection_port=9090,
            lic_path=license_path)

    def tearDown(self):
        self.projector_manager.deactivate()
        self.projector_manager.client_server_disconnect()

    # test creation of invalid coordinate system
    def test1_create_wrong_coordinate_system(self):
        # coordinate system
        cs_params = CoordinateSystemParameters()
        cs_params.name = "coordinate_system_test"
        cs_params.distance = 1500
        cs_params.P[0].x = 100
        cs_params.P[0].y = 250
        cs_params.P[1].x = 200
        cs_params.P[1].y = 0
        cs_params.P[2].x = 200
        cs_params.P[2].y = 200
        cs_params.P[3].x = 0
        cs_params.P[3].y = 200
        cs_params.T[0].x = 0
        cs_params.T[0].y = 0
        cs_params.resolution = 1000

        try:
            self.projector_manager.connect_and_setup()
            status = self.projector_manager.get_connection_status()
            if status:
                self.projector_manager.define_coordinate_system(
                    cs_params, False)
                self.fail("Library accepted invalid data")
            else:
                self.fail("connection error")

        except Exception as e:
            pass

    # test creation of valid coordinate system
    def test2_create_coordinate_system(self):
        name1 = "coordinate_system_test_1"
        name2 = "coordinate_system_test_2"
        # coordinate system 1
        cs_params1 = CoordinateSystemParameters()
        cs_params1.name = name1
        cs_params1.distance = 1500
        cs_params1.P[0].x = -100
        cs_params1.P[0].y = -100
        cs_params1.P[1].x = 100
        cs_params1.P[1].y = -100
        cs_params1.P[2].x = 100
        cs_params1.P[2].y = 100
        cs_params1.P[3].x = -100
        cs_params1.P[3].y = 100
        cs_params1.T[0].x = 0
        cs_params1.T[0].y = 0
        cs_params1.resolution = 1000
        # coordinate system 2
        cs_params2 = deepcopy(cs_params1)
        cs_params2.name = name2
        cs_params2.distance = 2000

        try:
            self.projector_manager.connect_and_setup()
            status = self.projector_manager.get_connection_status()
            if status:
                # create coordinate system 1
                self.projector_manager.define_coordinate_system(
                    cs_params1, False)
                cs_read = self.projector_manager.get_coordinate_system_params(
                    name1)
                self.assertEquals(cs_read.distance, cs_params1.distance)
                self.assertEquals(cs_read.name, name1)
                # create coordinate system 2
                self.projector_manager.define_coordinate_system(
                    cs_params2, False)
                cs_read = self.projector_manager.get_coordinate_system_params(
                    name2)
                self.assertEquals(cs_read.distance, cs_params2.distance)
                self.assertEquals(cs_read.name, name2)
                # change to coordinate system 1 and remove 2
                self.projector_manager.set_coordinate_system(name1)
                cs_list, cs_active = self.projector_manager.get_coordinate_systems_list(
                )
                self.assertEquals(cs_active, name1)
                self.projector_manager.remove_coordinate_system(name2)
                pass
            else:
                self.fail("connection error")

        except Exception as e:
            self.fail("Exception raised: {}".format(e))

    # test creation of axes and frame coordinate system
    def test3_create_coordinate_system(self):
        name = "coordinate_system_test"
        # coordinate system
        cs_params = CoordinateSystemParameters()
        cs_params.name = name
        cs_params.distance = 1500
        cs_params.P[0].x = -100
        cs_params.P[0].y = -100
        cs_params.P[1].x = 100
        cs_params.P[1].y = -100
        cs_params.P[2].x = 100
        cs_params.P[2].y = 100
        cs_params.P[3].x = -100
        cs_params.P[3].y = 100
        cs_params.T[0].x = 0
        cs_params.T[0].y = 0
        cs_params.resolution = 1000

        try:
            self.projector_manager.connect_and_setup()
            status = self.projector_manager.get_connection_status()
            if status:
                # create coordinate system
                self.projector_manager.define_coordinate_system(
                    cs_params, False)
                cs_read = self.projector_manager.get_coordinate_system_params(
                    name)
                self.assertEquals(cs_read.distance, cs_params.distance)
                # create axes and frame
                self.projector_manager.cs_axes_create(cs_params)
                self.projector_manager.cs_frame_create(cs_params)
                pass
            else:
                self.fail("connection error")

        except Exception as e:
            self.fail("Exception raised: {}".format(e))
class TestProjectorManager(unittest.TestCase):

    def setUp(self):
        # check projector is on network
        if not ip_open("192.168.10.11",9090):
            self.skipTest("projector is not found on network")
        license_path = (os.path.dirname(os.path.dirname(os.path.abspath(__file__)))+'/lic/1900027652.lic')
        self.projector_manager = ZLPProjectorManager(projector_IP = "192.168.10.10", 
                                                     server_IP = "192.168.10.11", 
                                                     connection_port = 9090, 
                                                     lic_path=license_path)

    def tearDown(self):
        self.projector_manager.deactivate()
        self.projector_manager.client_server_disconnect()

    # test creation of projection elements
    def test1_create_projection_elements(self):
        # coordinate system
        cs_params            = CoordinateSystemParameters()
        cs_params.name       = "coordinate_system_test"
        cs_params.distance   = 1500
        cs_params.P[0].x     = -100
        cs_params.P[0].y     = -100
        cs_params.P[1].x     =  100
        cs_params.P[1].y     = -100
        cs_params.P[2].x     =  100
        cs_params.P[2].y     =  100
        cs_params.P[3].x     = -100
        cs_params.P[3].y     =  100
        cs_params.T[0].x     =    0
        cs_params.T[0].y     =    0
        cs_params.resolution = 1000
        
        # common
        figure = Figure()
        figure.projection_group = "figure_test"
        figure.position.x = 0      
        figure.position.y = 0        
        figure.position.z = 0        
        figure.angle.append(0)
        # polyline
        polyline = deepcopy(figure)
        polyline.figure_type = Figure.POLYLINE
        polyline.figure_name = "polyline_test"
        polyline.size.append(100) # length
        # circle
        circle = deepcopy(figure)
        circle.figure_type = Figure.CIRCLE
        circle.figure_name = "circle_test"
        circle.size.append(50) # radius
        # text
        text = deepcopy(figure)
        text.figure_type = Figure.TEXT
        text.figure_name = "text_test"
        text.size.append(10) # heigh
        text.size.append(2) # char spacing
        text.text = "TEST"

        try:
            self.projector_manager.connect_and_setup()
            status = self.projector_manager.get_connection_status()
            if status:
                self.projector_manager.define_coordinate_system(cs_params, False)
                self.projector_manager.create_polyline(polyline)
                self.projector_manager.create_curve(circle)
                self.projector_manager.create_text(text)
                pass
            else:
                self.fail("connection error")

        except Exception as e:
            self.fail("Exception raised: {}".format(e))

    # test write and read data of projection element
    def test2_projection_element(self):    
        # coordinate system
        cs_params            = CoordinateSystemParameters()
        cs_params.name       = "coordinate_system_test"
        cs_params.distance   = 1500
        cs_params.P[0].x     = -100
        cs_params.P[0].y     = -100
        cs_params.P[1].x     =  100
        cs_params.P[1].y     = -100
        cs_params.P[2].x     =  100
        cs_params.P[2].y     =  100
        cs_params.P[3].x     = -100
        cs_params.P[3].y     =  100
        cs_params.T[0].x     =    0
        cs_params.T[0].y     =    0
        cs_params.resolution = 1000

        # polyline
        polyline = Figure()
        polyline.figure_type = Figure.POLYLINE
        polyline.projection_group = "figure_test"
        polyline.figure_name = "polyline_test"
        polyline.position.x = 0.0      
        polyline.position.y = 0.0        
        polyline.position.z = 0.0        
        polyline.angle.append(0)
        polyline.size.append(100) # length

        # proj_elem
        proj_elem = ProjectionElementRequest()
        proj_elem.figure_type = Figure.POLYLINE
        proj_elem.projection_group = "figure_test"
        proj_elem.figure_name = "polyline_test"

        try:
            self.projector_manager.connect_and_setup()
            status = self.projector_manager.get_connection_status()
            if status:
                self.projector_manager.define_coordinate_system(cs_params, False)
                self.projector_manager.create_polyline(polyline)
                figure = self.projector_manager.get_proj_elem(proj_elem)
                self.assertEqual(figure.size[0], polyline.size[0])
                self.assertEqual(figure.angle[0], polyline.angle[0])
                self.assertEqual(figure.position.x, polyline.position.x)
                # if figure is ok, do operations
                self.projector_manager.hide_proj_elem(proj_elem)
                self.projector_manager.unhide_proj_elem(proj_elem)
                self.projector_manager.remove_proj_elem(proj_elem)
            else:
                self.fail("connection error")

        except Exception as e:
            self.fail("Exception raised: {}".format(e))

    # test send and read data of pointer projection element
    def test3_define_pointer(self):
        
        # coordinate system
        cs_params            = CoordinateSystemParameters()
        cs_params.name       = "coordinate_system_test"
        cs_params.distance   = 1500
        cs_params.P[0].x     = -100
        cs_params.P[0].y     = -100
        cs_params.P[1].x     =  100
        cs_params.P[1].y     = -100
        cs_params.P[2].x     =  100
        cs_params.P[2].y     =  100
        cs_params.P[3].x     = -100
        cs_params.P[3].y     =  100
        cs_params.T[0].x     =    0
        cs_params.T[0].y     =    0
        cs_params.resolution = 1000        

        # pointer
        pointer = Figure()
        pointer.figure_type = Figure.POINTER
        pointer.projection_group = "figure_test"
        pointer.figure_name = "pointer_test"
        pointer.position.x = 0.0      
        pointer.position.y = 0.0        
        pointer.position.z = 0.0
        pointer.size.append(5) # height
        pointer.angle.append(0)

        # proj_elem
        proj_elem = ProjectionElementRequest()
        proj_elem.figure_type = Figure.POINTER
        proj_elem.projection_group = "figure_test"
        proj_elem.figure_name = "pointer_test"

        try:
            self.projector_manager.connect_and_setup()
            status = self.projector_manager.get_connection_status()
            if status:
                self.projector_manager.define_coordinate_system(cs_params, False)
                self.projector_manager.create_pointer(pointer)
                figure = self.projector_manager.get_proj_elem(proj_elem)
                self.assertEqual(figure.size[0], pointer.size[0])
                self.assertEqual(figure.position.x, pointer.position.x)
            else:
                self.fail("connection error")

        except Exception as e:
            self.fail("Exception raised: {}".format(e))