def scan(self):
""" The end effector starts scanning. """
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.SCAN
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending SCAN goal.')
self.client.send_goal(goal)
def point_to(self, target, center='/pi_camera_frame'):
""" Points end effector to a target.
:param target: The vicitim frame ID.
:param center: The center of the frame the we will use.
"""
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.TRACK
goal.point_of_interest = target
goal.center_point = center
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending TRACK goal.')
self.client.send_goal(goal)
def __init__(self):
self.goal = MoveEndEffectorGoal()
self.commands = [
MoveEndEffectorGoal.TEST, MoveEndEffectorGoal.PARK,
MoveEndEffectorGoal.TRACK, MoveEndEffectorGoal.LAX_TRACK,
MoveEndEffectorGoal.SCAN
]
def setUp(self):
rospy.init_node('effector_clients_test')
self.head_pub = Publisher('/mock/head', String)
self.client_instance = HeadClient()
self.effector_goal = MoveEndEffectorGoal()
self.effector_goal.command = 'TEST'
self.effector_goal.point_of_interest = '0,0,1'
rospy.sleep(1)
def slowly_point_to(self, target, center='/pi_camera_frame'):
"""
Points end effector to a target, slow enough so the image
is staying still.
:param target: The victim frame ID.
:param center: The center of the frame the we will use.
"""
goal = MoveEndEffectorGoal()
goal.command = MoveEndEffectorGoal.LAX_TRACK
goal.point_of_interest = target
goal.center_point = center
log.debug('Waiting for the EndEffector action server.')
self.client.wait_for_server()
log.info('Sending LAX TRACK goal.')
self.client.send_goal(goal)
sleep(5)
def __init__(self, verbose=False):
self.verbose = verbose
self.client = Client(topics.move_end_effector_controller,
MoveEndEffectorAction)
goal = MoveEndEffectorGoal(command=MoveEndEffectorGoal.TEST)
func = partial(retry_action,
client=self.client,
goal=goal,
timeout=20,
msg='Test end effector ')
setattr(self, 'test', func)
goal = MoveEndEffectorGoal(command=MoveEndEffectorGoal.PARK)
func = partial(retry_action,
client=self.client,
goal=goal,
timeout=20,
msg='Park end effector ')
setattr(self, 'park', func)
def setUp(self):
rospy.init_node('effector_clients_test')
self.linear_actuator_pub = Publisher('/mock/linear_actuator', String)
self.client_instance = LinearActuatorClient()
self.effector_goal = MoveEndEffectorGoal()
self.effector_goal.command = 'TEST'
self.effector_goal.point_of_interest = '0,0,1'
self.effector_goal.center_point = '0,1,0'
self.effector_goal.center_point = '0,1,0'
rospy.sleep(1)
def setUp(self):
rospy.init_node('test_move_end_effector_server_node')
self.effector_client = Client(move_end_effector_controller_topic,
MoveEndEffectorAction)
self.server = MoveEndEffectorServer()
self.goal = MoveEndEffectorGoal()
self.goal_maker = EffectorGoalMaker()
self.sensor_pub = Publisher('mock/sensor', String)
self.linear_actuator_pub = Publisher('mock/linear_actuator', String)
self.head_pub = Publisher('mock/head', String)
rospy.sleep(1)
def __init__(self):
self._name = "/PANDORA_CONTROL/PANDORA_END_EFFECTOR_CONTROLLER"
self.factory = ClientFactory()
self.current_clients = list()
self.current_goal = MoveEndEffectorGoal()
self.server = Server(move_end_effector_controller_topic,
MoveEndEffectorAction, self.execute_cb, False)
self.server.register_preempt_callback(self.preempt_cb)
self.server.start()
self.create_clients()
self.wait_for_servers()
def test_execute_cb(self):
''' Tests if server is set correctly (triggered by callback!) '''
self.sensor_pub.publish('success:1')
self.linear_actuator_pub.publish('success:1')
self.head_pub.publish('success:1')
self.goal = MoveEndEffectorGoal()
self.goal = self.goal_maker.create_goal()
self.server.wait_for_servers()
self.effector_client.send_goal(self.goal)
self.effector_client.wait_for_result()
self.assertEqual(self.effector_client.get_state(),
GoalStatus.SUCCEEDED)
self.sensor_pub.publish('abort:1')
self.linear_actuator_pub.publish('success:1')
self.head_pub.publish('success:1')
self.goal = MoveEndEffectorGoal()
self.goal = self.goal_maker.create_goal()
self.effector_client.send_goal(self.goal)
self.effector_client.wait_for_result()
self.assertEqual(self.effector_client.get_state(), GoalStatus.ABORTED)
self.sensor_pub.publish('preempt:1')
self.linear_actuator_pub.publish('success:1')
self.head_pub.publish('preempt:1')
self.goal = MoveEndEffectorGoal()
self.goal = self.goal_maker.create_goal()
self.effector_client.send_goal(self.goal)
self.effector_client.wait_for_result()
self.assertEqual(self.effector_client.get_state(),
GoalStatus.PREEMPTED)
self.sensor_pub.publish('abort:1')
self.linear_actuator_pub.publish('success:1')
self.head_pub.publish('preempt:1')
self.goal = MoveEndEffectorGoal()
self.goal = self.goal_maker.create_goal()
self.effector_client.send_goal(self.goal)
self.effector_client.wait_for_result()
self.assertEqual(self.effector_client.get_state(), GoalStatus.ABORTED)
def setUp(self):
rospy.init_node('test_move_end_effector_server_node')
self.server = MoveEndEffectorServer()
self.goal = MoveEndEffectorGoal()
rospy.sleep(1)