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)