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
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))
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))
def test2_coordinate_system(self): cs = CoordinateSystemParameters() try: # this fields should be available cs.name = "test" cs.distance = 120.5 cs.resolution = 200 self.assertEquals(len(cs.P), 4) self.assertEquals(len(cs.T), 4) except Exception as e: self.fail("Exception raised: {}".format(e))
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))
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))
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 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 test3_req_conversions(self): req = CoordinateSystemRequest() req.name = "utils_test" req.distance = 10 req.T0 = 5 param = CoordinateSystemParameters() param.name = "utils_test_inverse" param.distance = 20 p = Point(0, 2, 0) param.T = [p, p, p, p] try: read_param = CoordinateSystemParameters.req_to_param(req) self.assertEquals(read_param.name, req.name) self.assertEquals(read_param.distance, req.distance) self.assertEquals(read_param.T[0], req.T0) read_req = CoordinateSystemParameters.param_to_req(param) self.assertEquals(read_req.name, param.name) self.assertEquals(read_req.distance, param.distance) self.assertEquals(read_req.T0, param.T[0]) except Exception as e: self.fail("Exception raised: {}".format(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 __init__(self): """Initialize the ZLPVisualizer object.""" self.cs_marker_array = MarkerArray() self.pe_marker_array = MarkerArray() self.active_cs = "" self.cs_reference = "" self.STD_WAIT_TIME = CoordinateSystemParameters().DEFAULT_SHOW_TIME self.figures_list = ProjectionElementParameters().figures_list self.scale_factor = 1
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 __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 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))
def get_coordinate_system_params(self, cs_name): """Get parameters values of a defined coordinate system. Args: cs_name (str): name of reference coordinate system Raises: SystemError """ cs_params, success, message = self.cs_element.get_cs( cs_name, CoordinateSystemParameters()) if not success: raise SystemError(message) return cs_params
def test2_create_frame(self): viz = ZLPVisualizer() cs_params = CoordinateSystemParameters() 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 try: axis_x, axis_y = viz.coord_sys_axes(cs_params) frame = viz.coord_sys_frame(cs_params) self.assertEquals(len(axis_x.points), 2) self.assertEquals(len(axis_y.points), 2) self.assertEquals(len(frame.points), 5) except Exception as e: self.fail("%s" % e)