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