def finish(self, action, sender):
actions.get(action).tags["STATUS"] = "FINISH"
actions.get(action).tags["EXECUTER"] = sender
# if "ECUEIL" in action:
# for gob in actions[action]["GOBS"]:
# actions[action]["STATUS"] = "FINISHED"
# actions[gob]["EXECUTER"] = sender
return True
def preempt(self, action, sender):
actions.get(action).tags["STATUS"] = "PREEMPT"
actions.get(action).tags["EXECUTER"] = sender
# if "ECUEIL" in action:
# for gob in actions[action]["GOBS"]:
# actions[action]["STATUS"] = "PREEMPTED"
# actions[gob]["EXECUTER"] = sender
return True
def release(self, action, sender):
actions.get(action).tags["STATUS"] = "RELEASE"
actions.get(action).tags["EXECUTER"] = sender
# if "ECUEIL" in action:
# for gob in actions[action]["GOBS"]:
# actions[action]["STATUS"] = "RELEASED"
# actions[gob]["EXECUTER"] = None
# Don't add failing action again
return True
def confirm_current_action(self):
"""Confirm an action for the BT."""
if self.current_action is None:
return False
self.change_action_status_request.action = self.current_action
self.change_action_status_request.request = "CONFIRM"
response = self.synchronous_call(self.change_action_status_client,
self.change_action_status_request)
if response is None:
return False
actions.get(self.current_action).finish_action(self)
self.get_logger().info(f"CONFIRM {self.current_action}")
self.current_action = None
return response.success
def drop_current_action(self):
"""Drop an action for the BT."""
if self.current_action is None:
return False
self.change_action_status_request.action = self.current_action
self.change_action_status_request.request = "DROP"
response = self.synchronous_call(self.change_action_status_client,
self.change_action_status_request)
if response is None:
return False
actions.get(self.current_action).release_action(self)
self.get_logger().info(f"DROP {self.current_action}")
self.current_action = None
return response.success
def compute_best_action(self, action_list):
"""Calculate best action to choose from its distance to the robot and the time passed."""
if self.inventory_full():
return "CHENAL"
if not action_list:
self.get_logger().info("No actions left, aborting.")
return None
self.get_logger().info(f"Available actions: {action_list}")
coefficient_list = []
position = self.get_position()
for action_id in action_list:
action = actions.get(action_id)
coefficient = 0
distance = np.sqrt((action.position[0] - position[0])**2 +
(action.position[1] - position[1])**2)
coefficient += 100 * (1 - distance / 3.6)
coefficient += get_time_coeff(
self.get_clock().now().nanoseconds * 1e-9 - self.start_time,
action,
self.strategy_mode.value,
)
coefficient_list.append(coefficient)
# Return best action based on the one with a maximum coefficient
best_action = action_list[coefficient_list.index(
max(coefficient_list))]
return best_action
def preempt_action(self, action):
"""Preempt an action for the BT."""
if action is None:
return False
self.change_action_status_request.action = str(action)
self.change_action_status_request.request = "PREEMPT"
response = self.synchronous_call(self.change_action_status_client,
self.change_action_status_request)
if response is None:
return False
self.current_action = action if response.success else None
actions.get(self.current_action).preempt_action(self, actions)
self.get_logger().info(f"PREEMPT {self.current_action}")
if self.current_action is not None:
self.set_goal_pose()
return response.success
def set_goal_pose(self, z=0.0):
"""Set goal pose of action in blackboard."""
msg = NavigateToPose_Goal()
msg.pose.header.frame_id = "map"
orientation = actions.get(
self.current_action).get_initial_orientation(self)
position = actions.get(self.current_action).get_initial_position(self)
self.get_logger().info(f"Goal objective updated to:")
self.get_logger().info(f"Position x: {position[0]}, y: {position[1]}")
self.get_logger().info(f"Orientation {orientation} radians")
msg.pose.pose.position.x = position[0]
msg.pose.pose.position.y = position[1]
msg.pose.pose.position.z = z
rot = Rotation.RotZ(orientation)
q = rot.GetQuaternion()
msg.pose.pose.orientation.x = q[0]
msg.pose.pose.orientation.y = q[1]
msg.pose.pose.orientation.z = q[2]
msg.pose.pose.orientation.w = q[3]
self.blackboard.goal = msg
def start_actuator_action(self):
"""Start the actuator action after the robot has reached its destination."""
self.get_logger().info(f"START ACTUATOR {self.current_action}")
return actions.get(self.current_action).start_actuator(self)