def test1_add_proj_elem(self): rospy.init_node("projection_element_test") rospy.wait_for_service("projection_start") # at this point we asume projector is connected try: # send projection element topic = rospy.resolve_name("add_projection_element") pub = rospy.Publisher(topic, Figure, queue_size=1) rospy.sleep(1) print("\n") print(topic) print(pub.get_num_connections()) if pub.get_num_connections() == 0: self.fail("subscriber for topic %s not available" % topic) # create circle circle = Figure() circle.figure_type = Figure.CIRCLE circle.projection_group = "figure_test" circle.figure_name = "circle_test" circle.position.x = 0 circle.position.y = 0 circle.position.z = 0 circle.angle.append(0) circle.size.append(50) # radius pub.publish(circle) rospy.sleep(1) except Exception as e: self.fail("%s" % e)
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_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 circle_def(self): """Publish new circle projection element on topic.""" try: circle = Figure() circle.figure_type = int(1) circle.projection_group = str(self.pe_group.text()) circle.figure_name = str(self.pe_id.text()) circle.position.x = float(self.pe_x.text()) circle.position.y = float(self.pe_y.text()) circle.size.append(float(self.pe_length.text())) self.add_proj_elem.publish(circle) except Exception as e: self.error_msg("Projection element Menu error",e)
def oval_def(self): """Publish new ellipse projection element on topic.""" try: oval = Figure() oval.figure_type = int(3) oval.projection_group = str(self.pe_group.text()) oval.figure_name = str(self.pe_id.text()) oval.position.x = float(self.pe_x.text()) oval.position.y = float(self.pe_y.text()) oval.size.append(float(self.pe_length.text())) oval.size.append(float(self.pe_height.text())) oval.angle.append(float(self.pe_angle.text())) self.add_proj_elem.publish(oval) except Exception as e: self.error_msg("Projection element Menu error",e)
def arc_def(self): """Publish new arc projection element on topic.""" try: arc = Figure() arc.figure_type = int(2) arc.projection_group = str(self.pe_group.text()) arc.figure_name = str(self.pe_id.text()) arc.position.x = float(self.pe_x.text()) arc.position.y = float(self.pe_y.text()) arc.size.append(float(self.pe_length.text())) arc.angle.append(float(self.pe_angle.text())) arc.angle.append(float(self.pe_end_angle.text())) self.add_proj_elem.publish(arc) except Exception as e: self.error_msg("Projection element Menu error",e)
def to_figure(self): """Return ProjectionElementParameters attributes converted to ROS msg format. Returns: object: ROS msg object with the parameters of the projection element """ figure = Figure() figure.projection_group = self.projection_group figure.figure_type = self.figure_type figure.figure_name = self.figure_name figure.position = self.position figure.size = self.size figure.angle = self.angle figure.text = self.text return figure
def text_def(self): """Publish new text projection element on topic.""" try: text = Figure() text.figure_type = int(4) text.projection_group = str(self.pe_group.text()) text.figure_name = str(self.pe_id.text()) text.position.x = float(self.pe_x.text()) text.position.y = float(self.pe_y.text()) text.size.append(float(self.pe_height.text())) text.size.append(float(self.pe_char_space.text())) text.angle.append(float(self.pe_angle.text())) text.text = str(self.pe_text.text()) self.add_proj_elem.publish(text) except Exception as e: self.error_msg("Projection element Menu error",e)
def test4_operations(self): viz = ZLPVisualizer() m = viz.base_marker("test_marker") figure = Figure() figure.position.x = 0 figure.position.y = 0 figure.position.z = 0 figure.size.append(100) # length figure.size.append(10) # height figure.angle.append(0) figure.angle.append(0) try: viz.translate(m, dx=1) self.assertEquals(m.pose.position.x, 1) self.assertEquals(m.pose.position.y, 0) viz.translate(m, dy=2) self.assertEquals(m.pose.position.x, 1) self.assertEquals(m.pose.position.y, 2) viz.translate(m, dy=-1) self.assertEquals(m.pose.position.y, 1) viz.rotate(m, 90) figure.figure_type = Figure.POLYLINE viz.scale(m, 2, figure) figure.figure_type = Figure.CIRCLE viz.scale(m, 2, figure) figure.figure_type = Figure.ARC viz.scale(m, 2, figure) figure.figure_type = Figure.OVAL viz.scale(m, 2, figure) figure.figure_type = Figure.TEXT viz.scale(m, 2, figure) except Exception as e: self.fail("%s" % e)