def compute_action_point(self, args: Argumentable,
robot_pos: Pose2D) -> ActionPoint:
# Compute start and end of action
res = ActionPoint()
res.start = robot_pos
res.end = robot_pos
return res
def fun(robot_position):
ret = Pose2D()
ret.x = robot_position.x * xf
ret.y = robot_position.y * yf
p = ActionPoint()
p.start = robot_position
p.end = ret
return p
def compute_action_point(self, args: Argumentable,
robot_pos: Pose2D) -> ActionPoint:
res = ActionPoint()
res.start = robot_pos
res.end = Pose2D()
res.end.x = args.get("x", float, 0)
res.end.y = args.get("y", float, 0)
res.end.theta = args.get("angle", float, 0)
return res
def action_point(self, origin: Pose2D) -> ActionPoint:
# Check if previously saved for this origin
if self.__action_point != None and self.__action_point_origin == origin:
return self.__action_point
if self.type == ActionGroup.BEST:
return self.best_child().action_point(origin)
start: Pose2D = None
current: Pose2D = origin
for child in self.children:
child_action_point = child.action_point(current)
current = child_action_point.end
if start == None:
start = child_action_point.start
# Save action point
self.__action_point = ActionPoint()
self.__action_point.start = start
self.__action_point.end = current
# And it's origin
self.__action_point_origin = origin
return self.__action_point
def setUp(self):
# Init some position
self.robot_position = Pose2D()
self.robot_position.x = 0
self.robot_position.y = 0
# Some example action point
self.action_point = ActionPoint()
self.action_point.start = self.robot_position
self.action_point.end = self.robot_position
# Monitored state of return
self.action_state = ActionStatus.IDLE
# Perform client
self.pc = PerformClient("test_template_performer_client", "actions")
self.pc.on_action_returns = self.action_returns
def setUp(self):
rospy.init_node('test_action', anonymous=True)
self.robot_position = Pose2D()
self.robot_position.x = 0
self.robot_position.y = 0
self.first_position = Pose2D()
self.first_position.x = 30
self.first_position.y = 10
self.second_position = Pose2D()
self.second_position.x = 90
self.second_position.y = 60
self.action_point = ActionPoint()
self.action_point.start = self.first_position
self.action_point.end = self.second_position