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)