class AveragingSVR2(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.register_goal_callback(self.goal_cb)
self.reset_numbers()
rospy.Subscriber('number', Float32, self.execute_loop)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def goal_cb(self):
self._goal = self._action.accept_new_goal()
rospy.loginfo('goal callback %s' % (self._goal))
def preempt_cb(self):
rospy.loginfo('preempt callback')
self.reset_numbers()
self._action.set_preempted(text='message for preempt')
def reset_numbers(self):
self._samples = []
def execute_loop(self, msg):
if (not self._action.is_active()):
return
self._samples.append(msg.data)
feedback = AveragingAction().action_feedback.feedback
feedback.sample = len(self._samples)
feedback.data = msg.data
feedback.mean = sum(self._samples) / len(self._samples)
feedback.std_dev = self.std_dev(self._samples)
self._action.publish_feedback(feedback)
## sending result
if (len(self._samples) >= self._goal.samples):
result = AveragingAction().action_result.result
result.mean = sum(self._samples) / len(self._samples)
result.std_dev = self.std_dev(self._samples)
rospy.loginfo('result: %s' % (result))
self.reset_numbers()
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result,
text='message for aborted')
class AveragingSVR(object):
def __init__(self):
self._action = SimpleActionServer('averaging',
AveragingAction,
execute_cb=self.execute_cb,
auto_start=False)
self._action.register_preempt_callback(self.preempt_cb)
self._action.start()
def std_dev(self, lst):
ave = sum(lst) / len(lst)
return sum([x * x for x in lst]) / len(lst) - ave**2
def preempt_cb(self):
rospy.loginfo('preempt callback')
self._action.set_preempted(text='message for preempt')
def execute_cb(self, goal):
rospy.loginfo('execute callback: %s' % (goal))
feedback = AveragingAction().action_feedback.feedback
result = AveragingAction().action_result.result
## execute loop
rate = rospy.Rate(1 / (0.01 + 0.99 * random.random()))
samples = []
for i in range(goal.samples):
sample = random.random()
samples.append(sample)
feedback.sample = i
feedback.data = sample
feedback.mean = sum(samples) / len(samples)
feedback.std_dev = self.std_dev(samples)
self._action.publish_feedback(feedback)
rate.sleep()
if (not self._action.is_active()):
rospy.loginfo('not active')
return
## sending result
result.mean = sum(samples) / len(samples)
result.std_dev = self.std_dev(samples)
rospy.loginfo('result: %s' % (result))
if (result.mean > 0.5):
self._action.set_succeeded(result=result,
text='message for succeeded')
else:
self._action.set_aborted(result=result, text='message for aborted')
class AStarSearch(object):
def __init__(self, name):
rospy.loginfo("Starting {}".format(name))
self._as = SimpleActionServer(name,
TopoNavAction,
self.execute_cb,
auto_start=False)
self._as.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient("/waypoint_nav_node",
WayPointNavAction)
rospy.loginfo("Waiting for nav server")
self.client.wait_for_server()
self.nodes = set()
self.edges = defaultdict(list)
self.distances = {}
with open(rospy.get_param("~waypoint_yaml"), 'r') as f:
self.waypointInfo = yaml.load(f)
for waypoint, info in self.waypointInfo.items():
self.add_node(waypoint)
for edge in info["edges"]:
self.add_edge(waypoint, edge, 1.0)
self.waypointInfo[waypoint]["position"]["x"] *= 0.555
self.waypointInfo[waypoint]["position"]["y"] *= 0.555
self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped,
self.pose_cb)
self._as.start()
rospy.loginfo("Started {}".format(name))
def preempt_cb(self, *args):
self.client.cancel_all_goals()
def add_node(self, value):
self.nodes.add(value)
def add_edge(self, from_node, to_node, distance):
self.edges[from_node].append(to_node)
# self.edges[to_node].append(from_node)
self.distances[(from_node, to_node)] = distance
def pose_cb(self, msg):
closest = (None, None)
for k, v in self.waypointInfo.items():
dist = self.get_dist(msg, v["position"])
if closest[0] is None or dist < closest[0]:
closest = (dist, k)
self.closest = closest[1]
def get_dist(self, pose1, position):
return np.sqrt(
np.power(pose1.pose.position.x - position["x"], 2) +
np.power(pose1.pose.position.y - position["y"], 2))
def execute_cb(self, goal):
path = self.shortest_path(self.closest, goal.waypoint)[1]
print path
for p in path:
if not self._as.is_preempt_requested():
rospy.loginfo("Going to waypoint: {}".format(p))
target = PoseStamped()
target.header.stamp = rospy.Time.now()
target.header.frame_id = "map"
target.pose.position.x = self.waypointInfo[p]["position"]["x"]
target.pose.position.y = self.waypointInfo[p]["position"]["y"]
target.pose.orientation.x = self.waypointInfo[p][
"orientation"]["x"]
target.pose.orientation.y = self.waypointInfo[p][
"orientation"]["y"]
target.pose.orientation.z = self.waypointInfo[p][
"orientation"]["z"]
target.pose.orientation.w = self.waypointInfo[p][
"orientation"]["w"]
self.client.send_goal_and_wait(WayPointNavGoal(target))
if not self._as.is_preempt_requested():
self._as.set_succeeded(TopoNavResult())
else:
self._as.set_preempted()
def dijkstra(self, initial):
visited = {initial: 0}
path = {}
nodes = set(self.nodes)
while nodes:
min_node = None
for node in nodes:
if node in visited:
if min_node is None:
min_node = node
elif visited[node] < visited[min_node]:
min_node = node
if min_node is None:
break
nodes.remove(min_node)
current_weight = visited[min_node]
for edge in self.edges[min_node]:
weight = current_weight + self.distances[(min_node, edge)]
if edge not in visited or weight < visited[edge]:
visited[edge] = weight
path[edge] = min_node
return visited, path
def shortest_path(self, origin, destination):
visited, paths = self.dijkstra(origin)
full_path = deque()
_destination = paths[destination]
while _destination != origin:
full_path.appendleft(_destination)
_destination = paths[_destination]
full_path.appendleft(origin)
full_path.append(destination)
return visited[destination], list(full_path)
class MotionService(object):
def __init__(self):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() + '/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' + rospy.get_name +'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config, config_loader.targets[0].joint_state_topic, config_loader.targets[0].publisher_prefix)
else:
rospy.logerr('Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber('motion_command', ExecuteMotionCommand, self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(rospy.get_name() + '/motion_goal', ExecuteMotionAction, None, False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(self._preempt_motion_goal)
self._preempted = False
def _execute_motion_goal(self):
goal = self._action_server.accept_new_goal()
# Check if motion exists
if goal.motion_name not in self._motion_data:
result = ExecuteMotionResult()
result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN]
result.error_string = "The requested motion is unknown."
self._action_server.set_aborted(result)
return
# check if a valid time_factor is set, otherwise default to 1.0
if goal.time_factor <= 0.0:
goal.time_factor = 1.0
self._preempted = False
self._motion_publisher.publish_motion(self._motion_data[goal.motion_name], goal.time_factor, self._trajectory_finished)
print '[MotionService] New action goal received.'
def _preempt_motion_goal(self):
self._motion_publisher.stop_motion()
print '[MotionService] Preempting goal.'
self._preempted = True
def _trajectory_finished(self, error_codes, error_groups):
result = ExecuteMotionResult()
result.error_code = error_codes
error_string = ""
for error_code, error_group in zip(error_codes, error_groups):
error_string += error_group + ': ' + str(error_code) + '\n'
result.error_string = error_string
if self._preempted:
self._action_server.set_preempted(result)
else:
self._action_server.set_succeeded(result)
print '[MotionService] Trajectory finished with error code: \n', error_string
def _execute_motion(self, request):
response = ExecuteMotionResponse()
# check if a motion with this name exists
if request.motion_name not in self._motion_data:
print '[MotionService] Unknown motion name: "%s"' % request.motion_name
response.ok = False
response.finish_time.data = rospy.Time.now()
return response
# check if a valid time_factor is set, otherwise default to 1.0
if request.time_factor <= 0.0:
request.time_factor = 1.0
# find the duration of the requested motion
motion_duration = 0.0
for poses in self._motion_data[request.motion_name].values():
if len(poses) > 0:
endtime = poses[-1]['starttime'] + poses[-1]['duration']
motion_duration = max(motion_duration, endtime)
motion_duration *= request.time_factor
# execute motion
print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (request.motion_name, request.time_factor, motion_duration)
self._motion_publisher.publish_motion(self._motion_data[request.motion_name], request.time_factor)
# reply with ok and the finish_time of this motion
response.ok = True
response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(motion_duration)
return response
def start_server(self):
rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion, self._execute_motion)
self._action_server.start()
print '[MotionService] Waiting for calls...'
rospy.spin()
def motion_command_request_cb(self, command):
print "[MotionService] Initiate motion command via topic ..."
request = ExecuteMotion()
request.motion_name = command.motion_name
request.time_factor = command.time_factor
self._execute_motion(request)
print "[MotionService] Motion command complete"
class AudioFilePlayer(object):
def __init__(self):
rospy.loginfo("Initializing AudioFilePlayer...")
self.current_playing_process = None
self.afp_as = SimpleActionServer(rospy.get_name(),
AudioFilePlayAction,
self.as_cb,
auto_start=False)
self.afp_sub = rospy.Subscriber('~play',
String,
self.topic_cb,
queue_size=1)
# By default this node plays files using the aplay command
# Feel free to use any other command or flags
# by using the params provided
self.command = rospy.get_param('~command', 'play')
self.flags = rospy.get_param('~flags', '')
self.wrap_file_path = rospy.get_param('~wrap_file_path', True)
self.feedback_rate = rospy.get_param('~feedback_rate', 10)
self.afp_as.start()
# Needs to be done after start
self.afp_as.register_preempt_callback(self.as_preempt_cb)
rospy.loginfo(
"Done, playing files from action server or topic interface.")
def as_preempt_cb(self):
if self.current_playing_process:
self.current_playing_process.kill()
# Put the AS as cancelled/preempted
res = AudioFilePlayResult()
res.success = False
res.reason = "Got a cancel request."
self.afp_as.set_preempted(res, text="Cancel requested.")
def as_cb(self, goal):
initial_time = time.time()
self.play_audio_file(goal.filepath)
r = rospy.Rate(self.feedback_rate)
while not rospy.is_shutdown(
) and not self.current_playing_process.is_done():
feedback = AudioFilePlayFeedback()
curr_time = time.time()
feedback.elapsed_played_time = rospy.Duration(curr_time -
initial_time)
self.afp_as.publish_feedback(feedback)
r.sleep()
final_time = time.time()
res = AudioFilePlayResult()
if self.current_playing_process.is_succeeded():
res.success = True
res.total_time = rospy.Duration(final_time)
self.afp_as.set_succeeded(res)
else:
if self.afp_as.is_preempt_requested():
return
res.success = False
reason = "stderr: " + self.current_playing_process.get_stderr()
reason += "\nstdout: " + self.current_playing_process.get_stdout()
res.reason = reason
self.afp_as.set_aborted(res)
def topic_cb(self, data):
if self.current_playing_process:
if not self.current_playing_process.is_done():
self.current_playing_process.kill()
self.play_audio_file(data.data)
def play_audio_file(self, audio_file_path):
# Replace any ' or " characters with emptyness to avoid bad usage
audio_file_path = audio_file_path.replace("'", "")
audio_file_path = audio_file_path.replace('"', '')
if self.wrap_file_path:
full_command = self.command + " " + self.flags + " '" + audio_file_path + "'"
else:
full_command = self.command + " " + self.flags + " " + audio_file_path
rospy.loginfo("Playing audio file: " + str(audio_file_path) +
" with command: " + str(full_command))
self.current_playing_process = ShellCmd(full_command)
class MoveEndEffectorServer(object):
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 create_clients(self):
""" Imports all clients and creates a list of them """
for client in CLIENTS:
self.current_clients.append(self.factory.make_client(client))
def wait_for_servers(self):
""" Waits for every client to connect with their server """
for client in self.current_clients:
client.wait_server()
def execute_cb(self, goal):
""" Callback triggered by the arrival of a goal """
self.current_goal = goal
self.fill_goals()
self.send_goals()
self.wait_for_result()
rospy.logwarn("break from wait for result!")
self.checkGoalState()
def preempt_cb(self):
""" Preempting all goals """
for client in self.current_clients:
client.preempt_if_active()
def fill_goals(self):
""" Filling goals into every client respectively """
for client in self.current_clients:
client.fill_goal(self.current_goal)
def send_goals(self):
""" Sending goals to every client respectively """
for client in self.current_clients:
client.send_goal()
def wait_for_result(self):
for client in self.current_clients:
client.wait_result()
def success(self):
self.server.set_succeeded()
def abort(self):
self.server.set_aborted()
def preempt(self):
if self.server.is_active():
self.server.set_preempted()
else:
rospy.logerr("[" + self._name +
"] Preempt requested when server goal is not active")
def check_succeeded(self):
""" Checks if the final state of the goal must be set succeeded """
must_succeed = True
for client in self.current_clients:
must_succeed = must_succeed and client.has_succeeded()
return must_succeed
def check_aborted(self):
""" Checks if the final state of the goal must be set aborted """
must_abort = False
for client in self.current_clients:
must_abort = must_abort or client.has_aborted()
return must_abort
def check_preempted(self):
""" Checks if the final state of the goal must be set preempted """
must_preempt = False
for client in self.current_clients:
must_preempt = must_preempt or client.has_preempted()
return must_preempt
def check_recalled(self):
""" Checks if the final state of the goal must be set preempted """
must_recall = False
for client in self.current_clients:
must_recall = must_recall or client.has_been_recalled()
return must_recall
def checkGoalState(self):
""" Checking final state of goal """
if (self.check_succeeded()):
rospy.logwarn("check_succeeded")
self.success()
elif (self.check_aborted()):
rospy.logwarn("check_aborted")
self.abort()
elif (self.check_recalled()):
rospy.logwarn("check_recalled")
for client in self.current_clients:
rospy.logerr("[" + self._name + "] Client " +
client.get_name() + " is in state " +
str(client.client.get_state()))
rospy.logerr(
"[" + self._name +
"] One client at least is recalled, set server to aborted")
self.abort()
elif (self.check_preempted()):
rospy.logwarn("check_preempted")
self.preempt()
else:
rospy.logwarn("check_unexpected")
for client in self.current_clients:
rospy.logerr("[" + self._name + "] Client " +
client.get_name() + " is in state " +
str(client.client.get_state()))
rospy.logerr("[" + self._name +
"] Unexpected State, set server to aborted")
self.abort()
class ActionServerHandler(object):
"""
Interface to action server which is more useful for behaviors.
"""
def __init__(self, action_name, action_type):
self.goal_queue = Queue(1)
self.result_queue = Queue(1)
self.lock = Blackboard().lock
self.action_name = action_name
self.cancel_next_goal = False
self._as = SimpleActionServer(action_name, action_type, execute_cb=self.execute_cb, auto_start=False)
self._as.register_preempt_callback(self.cancel_cb)
self._as.start()
def cancel_cb(self):
self.cancel_next_goal = self._as.is_new_goal_available()
def execute_cb(self, goal):
"""
:type goal: MoveGoal
"""
with self.lock.acquire_timeout(0) as got_lock:
if got_lock and not self.cancel_next_goal:
self.my_state = Status.RUNNING
self.goal_queue.put(goal)
self.result_queue.get()()
else:
self.my_state = Status.FAILURE
r = self._as.action_server.ActionResultType()
r.error = DetectShelfLayersResult.SERVER_BUSY
print_with_prefix('rejected goal because server busy server busy', self.action_name)
self._as.set_aborted(r)
self.cancel_next_goal = False
def get_goal(self):
try:
goal = self.goal_queue.get_nowait()
return goal
except Empty:
return None
def has_goal(self):
return not self.goal_queue.empty()
def send_preempted(self, result=None):
def call_me_now():
self._as.set_preempted(result)
self.result_queue.put(call_me_now)
def send_aborted(self, result=None):
def call_me_now():
self._as.set_aborted(result)
self.result_queue.put(call_me_now)
def send_result(self, result=None):
"""
:type result: MoveResult
"""
def call_me_now():
self._as.set_succeeded(result)
self.result_queue.put(call_me_now)
def is_preempt_requested(self):
return self._as.is_preempt_requested()
class RobotPolicyExecutor():
def __init__(self, port, file_dir, file_name):
self.wait_for_result_dur = rospy.Duration(0.1)
self.top_nav_policy_exec = SimpleActionClient(
'topological_navigation/execute_policy_mode',
ExecutePolicyModeAction)
got_server = self.top_nav_policy_exec.wait_for_server(
rospy.Duration(1))
while not got_server:
rospy.loginfo(
"Waiting for topological navigation execute policy mode action server."
)
got_server = self.top_nav_policy_exec.wait_for_server(
rospy.Duration(1))
if rospy.is_shutdown():
return
self.policy_mode_pub = rospy.Publisher(
"mdp_plan_exec/current_policy_mode", NavRoute, queue_size=1)
self.current_waypoint_sub = rospy.Subscriber("current_node", String,
self.current_waypoint_cb)
self.closest_waypoint_sub = rospy.Subscriber("closest_node", String,
self.closest_waypoint_cb)
explicit_doors = rospy.get_param("mdp_plan_exec/explicit_doors", True)
forget_doors = rospy.get_param("mdp_plan_exec/forget_doors", True)
model_fatal_fails = rospy.get_param("mdp_plan_exec/model_fatal_fails",
True)
self.nav_before_action_exec = rospy.get_param(
"mdp_plan_exec/nav_before_action_exec", True
) #If True the robot will always navigate to a waypoint before trying to execute an action; if False robot can execute actions anywhere, if they are added without waypoint constraints.
self.mdp = TopMapMdp(explicit_doors=explicit_doors,
forget_doors=forget_doors,
model_fatal_fails=model_fatal_fails)
self.policy_utils = PolicyExecutionUtils(port, file_dir, file_name,
self.mdp)
self.action_executor = ActionExecutor()
self.cancelled = False
self.mdp_as = SimpleActionServer('mdp_plan_exec/execute_policy',
ExecutePolicyAction,
execute_cb=self.execute_policy_cb,
auto_start=False)
self.mdp_as.register_preempt_callback(self.preempt_policy_execution_cb)
self.mdp_as.start()
def current_waypoint_cb(self, msg):
self.current_waypoint = msg.data
def closest_waypoint_cb(self, msg):
self.closest_waypoint = msg.data
def execute_nav_policy(self, nav_policy_msg):
self.policy_mode_pub.publish(nav_policy_msg)
self.regenerated_nav_policy = False
self.top_nav_policy_exec.send_goal(
ExecutePolicyModeGoal(route=nav_policy_msg),
feedback_cb=self.top_nav_feedback_cb)
top_nav_running = True
while top_nav_running and not self.cancelled:
top_nav_running = not self.top_nav_policy_exec.wait_for_result(
self.wait_for_result_dur)
if not top_nav_running:
if self.regenerated_nav_policy:
nav_policy_msg = self.policy_utils.generate_current_nav_policy(
)
print(nav_policy_msg)
self.top_nav_policy_exec.send_goal(
ExecutePolicyModeGoal(route=nav_policy_msg),
feedback_cb=self.top_nav_feedback_cb)
top_nav_running = True
self.regenerated_nav_policy = False
else:
status = self.top_nav_policy_exec.get_state()
if self.cancelled:
status = GoalStatus.PREEMPTED
return status
def top_nav_feedback_cb(self, feedback):
executed_action = self.policy_mdp.get_current_action()
next_flat_state = None
if feedback.status == GoalStatus.SUCCEEDED:
next_flat_state = self.policy_utils.get_next_nav_policy_state(
feedback.current_wp, self.policy_mdp)
if next_flat_state is None:
rospy.logwarn(
"Error getting MDP next state: There is no transition modelling the state evolution. Looking for state in full state list..."
)
next_flat_state = self.policy_utils.get_next_state_from_wp_update(
self.policy_mdp, feedback.current_wp)
self.top_nav_policy_exec.cancel_all_goals()
if next_flat_state is not None:
self.regenerated_nav_policy = True
publish = next_flat_state != self.policy_mdp.current_flat_state
self.policy_mdp.set_current_state(next_flat_state)
if publish:
next_action = self.policy_mdp.get_current_action()
self.publish_feedback(executed_action, feedback.status,
next_action)
def execute_policy_cb(self, goal):
self.cancelled = False
self.policy_mdp = self.policy_utils.generate_policy_mdp(
goal.spec, self.closest_waypoint, rospy.Time.now())
if self.policy_mdp is None:
rospy.logerr("Failure to build policy for specification: " +
goal.spec.ltl_task)
self.mdp_as.set_aborted()
return
self.publish_feedback(None, None, self.policy_mdp.get_current_action())
if self.nav_before_action_exec:
starting_exec = True #used to make sure the robot executes calls topological navigation at least once before executing non-nav actions. This is too ensure the robot navigates to the exact pose of a waypoint before executing an action there
else:
starting_exec = False
while (self.policy_mdp.has_action_defined()
and not self.cancelled) or starting_exec:
next_action = self.policy_mdp.get_current_action()
if next_action in self.mdp.nav_actions or starting_exec:
starting_exec = False
current_nav_policy = self.policy_utils.generate_current_nav_policy(
self.policy_mdp)
status = self.execute_nav_policy(current_nav_policy)
rospy.loginfo(
"Topological navigation execute policy action server exited with status: "
+ GoalStatus.to_string(status))
if status != GoalStatus.SUCCEEDED:
self.policy_mdp.set_current_state(None)
break
else:
print("EXECUTE ACTION")
(status, state_update) = self.action_executor.execute_action(
self.mdp.action_descriptions[next_action])
executed_action = next_action
print(executed_action)
if not self.cancelled:
next_flat_state = self.policy_utils.get_next_state_from_action_outcome(
state_update, self.policy_mdp)
self.policy_mdp.set_current_state(next_flat_state)
if next_flat_state is None:
rospy.logerr(
"Error finding next state after action execution. Aborting..."
)
break
next_action = self.policy_mdp.get_current_action()
self.publish_feedback(executed_action, status, next_action)
if self.cancelled:
self.cancelled = False
rospy.loginfo("Policy execution preempted.")
self.mdp_as.set_preempted()
elif self.policy_mdp.current_flat_state is None:
rospy.loginfo("Policy execution failed.")
self.mdp_as.set_aborted()
else:
rospy.loginfo("Policy execution successful.")
self.mdp_as.set_succeeded()
def publish_feedback(self, executed_action, status, next_action):
(probability, prog_reward,
expected_time) = self.policy_mdp.get_guarantees_at_current_state()
self.mdp_as.publish_feedback(
ExecutePolicyFeedback(probability=probability,
expected_time=expected_time,
prog_reward=prog_reward,
current_waypoint=self.current_waypoint,
executed_action=executed_action,
execution_status=status,
next_action=next_action))
def preempt_policy_execution_cb(self):
self.top_nav_policy_exec.cancel_all_goals()
self.action_executor.cancel_all_goals()
self.cancelled = True
def main(self):
# Wait for control-c
rospy.spin()
if rospy.is_shutdown():
self.policy_utils.shutdown_prism(True)
class ActionPerformer(Node, ABC):
def __init__(self, name: str):
super().__init__(name, "action")
self.name = name
# Service for action point computation
self._action_point_server = Service(get_action_point_service(name),
ComputeActionPoint,
self._compute_action_point)
# Action server for this action
self._action_server = SimpleActionServer(get_action_server(name),
PerformAction,
auto_start=False)
self._action_server.register_goal_callback(self._on_goal)
self._action_server.register_preempt_callback(self._on_preempt)
self._action_server.start()
# Function managing the action
def returns(self, state=ActionStatus.DONE):
'''
Function to call when the action is interrupted
with a given state
'''
# Create result message
result = PerformResult()
result.state = state
# Send back to client
self._action_server.set_succeeded(result)
# Hidden handlers
def _compute_action_point(
self,
request: ComputeActionPointRequest) -> ComputeActionPointResponse:
# Extract data from request and call performer's function
result: ActionPoint = self.compute_action_point(
Argumentable().from_list(request.args), request.robot_pos)
response = ComputeActionPointResponse()
response.action_point = result
return response
def _on_goal(self):
if self._action_server.is_new_goal_available():
goal: PerformGoal = self._action_server.accept_new_goal()
# run action with given args
self.start(Argumentable().from_list(goal.arguments))
def _on_preempt(self):
self.cancel()
self._action_server.set_preempted()
# Function to be defined by inherited actions
@abstractmethod
def compute_action_point(self, args: Argumentable,
robot_pos: Pose2D) -> ActionPoint:
pass
@abstractmethod
def start(self, args: Argumentable):
pass
def cancel(self):
pass
class DummyInterface(object):
READY = -1
KILL_DETECT_SHELF_LAYER = 0
KILL_DETECT_FACINGS = 1
KILL_COUNT_PRODUCTS = 2
BUSY_DETECT_SHELF_LAYER = 3
BUSY_DETECT_FACINGS = 4
BUSY_COUNT_PRODUCTS = 5
FINISH = 6
ABORT = 7
def __init__(self):
self.query_shelf_systems_srv = rospy.Service(
'~query_shelf_systems', QueryShelfSystems,
self.query_shelf_systems_cb)
self.query_shelf_layers_srv = rospy.Service('~query_shelf_layers',
QueryShelfLayers,
self.query_shelf_layers_cb)
self.query_facings_srv = rospy.Service('~query_facings', QueryFacings,
self.query_facings_cb)
self.query_shelf_layer_detection_path_srv = rospy.Service(
'~query_detect_shelf_layers_path', QueryDetectShelfLayersPath,
self.query_detect_shelf_layers_path_cb)
self.query_facing_detection_path_srv = rospy.Service(
'~query_detect_facings_path', QueryDetectFacingsPath,
self.query_detect_facings_path_cb)
self.query_product_counting_path_srv = rospy.Service(
'~query_count_products_posture', QueryCountProductsPosture,
self.query_count_products_posture_cb)
self.finish_perception_srv = rospy.Service('~finish_perception',
FinishPerception,
self.finish_perception_cb)
self.query_reset_beliefstate_srv = rospy.Service(
'~reset_beliefstate', Trigger, lambda x: TriggerResponse())
self.detect_shelf_layer_as = SimpleActionServer(
'~detect_shelf_layers',
DetectShelfLayersAction,
execute_cb=self.detect_shelf_layers_cb,
auto_start=False)
self.detect_shelf_layer_as.register_preempt_callback(self.cancel_cb)
self.detect_facings_as = SimpleActionServer(
'~detect_facings',
DetectFacingsAction,
execute_cb=self.detect_facings_cb,
auto_start=False)
self.detect_facings_as.register_preempt_callback(self.cancel_cb)
self.count_products_as = SimpleActionServer(
'~count_products',
CountProductsAction,
execute_cb=self.count_products_cb,
auto_start=False)
self.count_products_as.register_preempt_callback(self.cancel_cb)
self.state_lock = Queue(1)
self.detect_shelf_layer_as.start()
self.detect_facings_as.start()
self.count_products_as.start()
self.set_ready(prefix='init')
self.set_names()
def set_names(self):
self.shelf_ids = ['shelf_535dc8']
self.layer_ids = ['layer_faa57d']
self.facing_ids = ['facing_92c6c2', 'facing_77bce2', 'facing_c4a62e']
self.shelf_to_layer = {'shelf_535dc8': ['layer_faa57d']}
self.layer_to_facing = {
'layer_faa57d':
['facing_92c6c2', 'facing_77bce2', 'facing_c4a62e']
}
self.product_counts = {}
# for shelf_id in self.shelf_ids:
# self.shelf_to_layer[shelf_id] = ['layer_{}'.format(rnd_hash()) for x in range(NUM_LAYER)]
# self.layer_ids.extend(self.shelf_to_layer[shelf_id])
# for layer_id in self.layer_ids:
# self.layer_to_facing[layer_id] = ['facing_{}'.format(rnd_hash()) for x in range(NUM_FACINGS)]
# self.facing_ids.extend(self.layer_to_facing[layer_id])
for facing_id in self.facing_ids:
self.product_counts[facing_id] = randint(0, 5)
def query_shelf_systems_cb(self, data):
"""
:type data: QueryShelfSystemsRequest
:rtype: QueryShelfSystemsResponse
"""
prefix = 'query_shelf_systems'
self.print_with_prefix('called', prefix)
r = QueryShelfSystemsResponse()
r.ids = self.shelf_ids
return r
def query_shelf_layers_cb(self, data):
"""
:type data: QueryShelfLayersRequest
:rtype: QueryShelfLayersResponse
"""
prefix = 'query_shelf_layers'
self.print_with_prefix('called', prefix)
r = QueryShelfLayersResponse()
if data.id in self.shelf_ids:
r.error = QueryShelfLayersResponse.SUCCESS
r.ids = self.shelf_to_layer[data.id]
else:
self.print_with_prefix('invalid id', prefix)
r.error = QueryShelfLayersResponse.INVALID_ID
return r
def query_facings_cb(self, data):
"""
:type data: QueryFacingsRequest
:rtype: QueryFacingsResponse
"""
prefix = 'query_facings'
self.print_with_prefix('called', prefix)
r = QueryFacingsResponse()
if data.id in self.layer_ids:
r.error = QueryFacingsResponse.SUCCESS
r.ids = self.layer_to_facing[data.id]
else:
self.print_with_prefix('invalid id', prefix)
r.error = QueryFacingsResponse.INVALID_ID
return r
def query_detect_shelf_layers_path_cb(self, data):
"""
:type data: QueryDetectShelfLayersPathRequest
:rtype: QueryDetectShelfLayersPathResponse
"""
self.print_with_prefix('called', 'query_detect_shelf_layers_path')
r = QueryDetectShelfLayersPathResponse()
if data.id in self.shelf_ids:
r.error = QueryDetectShelfLayersPathResponse.SUCCESS
base_pose = PoseStamped()
base_pose.header.frame_id = '/map'
base_pose.pose.position = Point(
rospy.get_param('shelf_detection_path/position/x'),
rospy.get_param('shelf_detection_path/position/y'),
rospy.get_param('shelf_detection_path/position/z'))
base_pose.pose.orientation = Quaternion(
rospy.get_param('shelf_detection_path/orientation/x'),
rospy.get_param('shelf_detection_path/orientation/y'),
rospy.get_param('shelf_detection_path/orientation/z'),
rospy.get_param('shelf_detection_path/orientation/w'))
for k in range(5):
posture = FullBodyPosture()
posture.base_pos = base_pose
posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param('shelf_detection_path/Torso_lin1')))
posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param('shelf_detection_path/Torso_lin2/gain')
* k))
posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param('shelf_detection_path/Torso_rot_1')))
r.path.postures.append(posture)
else:
r.error = QueryDetectShelfLayersPathResponse.INVALID_ID
return r
def query_detect_facings_path_cb(self, data):
"""
:type data: QueryDetectFacingsPathRequest
:rtype: QueryDetectFacingsPathResponse
"""
self.print_with_prefix('called', 'query_detect_facings_path')
r = QueryDetectFacingsPathResponse()
if data.id in self.layer_ids:
r.error = QueryDetectFacingsPathResponse.SUCCESS
# Start
posture = FullBodyPosture()
posture.base_pos.header.frame_id = '/map'
posture.base_pos.pose.position = Point(
rospy.get_param('detect_facing_path/start/position/x'),
rospy.get_param('detect_facing_path/start/position/y'),
rospy.get_param('detect_facing_path/start/position/z'))
posture.base_pos.pose.orientation = Quaternion(
rospy.get_param('detect_facing_path/start/orientation/x'),
rospy.get_param('detect_facing_path/start/orientation/y'),
rospy.get_param('detect_facing_path/start/orientation/z'),
rospy.get_param('detect_facing_path/start/orientation/w'))
posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param('detect_facing_path/start/Torso_lin1')))
posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param('detect_facing_path/start/Torso_lin2')))
posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param('detect_facing_path/start/Torso_rot_1')))
r.path.postures.append(posture)
# End
posture = FullBodyPosture()
posture.base_pos.header.frame_id = '/map'
posture.base_pos.pose.position = Point(
rospy.get_param('detect_facing_path/end/position/x'),
rospy.get_param('detect_facing_path/end/position/y'),
rospy.get_param('detect_facing_path/end/position/z'))
posture.base_pos.pose.orientation = Quaternion(
rospy.get_param('detect_facing_path/end/orientation/x'),
rospy.get_param('detect_facing_path/end/orientation/y'),
rospy.get_param('detect_facing_path/end/orientation/z'),
rospy.get_param('detect_facing_path/end/orientation/w'))
posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param('detect_facing_path/end/Torso_lin1')))
posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param('detect_facing_path/end/Torso_lin2')))
posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param('detect_facing_path/end/Torso_rot_1')))
r.path.postures.append(posture)
else:
r.error = QueryDetectFacingsPathResponse.INVALID_ID
return r
def query_count_products_posture_cb(self, data):
"""
:type data: QueryCountProductsPostureRequest
:rtype: QueryCountProductsPostureResponse
"""
self.print_with_prefix('called', 'query_count_products_posture')
r = QueryCountProductsPostureResponse()
if data.id in self.facing_ids:
r.error = QueryCountProductsPostureResponse.SUCCESS
if data.id == 'facing_92c6c2':
r.posture = FullBodyPosture()
r.posture.base_pos.header.frame_id = '/map'
r.posture.base_pos.pose.position = Point(
rospy.get_param(
'count_products_posture/facing_92c6c2/position/x'),
rospy.get_param(
'count_products_posture/facing_92c6c2/position/y'),
rospy.get_param(
'count_products_posture/facing_92c6c2/position/z'))
r.posture.base_pos.pose.orientation = Quaternion(
rospy.get_param(
'count_products_posture/facing_92c6c2/orientation/x'),
rospy.get_param(
'count_products_posture/facing_92c6c2/orientation/y'),
rospy.get_param(
'count_products_posture/facing_92c6c2/orientation/z'),
rospy.get_param(
'count_products_posture/facing_92c6c2/orientation/w'))
r.posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param(
'count_products_posture/facing_92c6c2/Torso_lin1'))
)
r.posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param(
'count_products_posture/facing_92c6c2/Torso_lin2'))
)
r.posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param(
'count_products_posture/facing_92c6c2/Torso_rot_1')
))
elif data.id == 'facing_77bce2':
r.posture = FullBodyPosture()
r.posture.base_pos.header.frame_id = '/map'
r.posture.base_pos.pose.position = Point(
rospy.get_param(
'count_products_posture/facing_77bce2/position/x'),
rospy.get_param(
'count_products_posture/facing_77bce2/position/y'),
rospy.get_param(
'count_products_posture/facing_77bce2/position/z'))
r.posture.base_pos.pose.orientation = Quaternion(
rospy.get_param(
'count_products_posture/facing_77bce2/orientation/x'),
rospy.get_param(
'count_products_posture/facing_77bce2/orientation/y'),
rospy.get_param(
'count_products_posture/facing_77bce2/orientation/z'),
rospy.get_param(
'count_products_posture/facing_77bce2/orientation/w'))
r.posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param(
'count_products_posture/facing_77bce2/Torso_lin1'))
)
r.posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param(
'count_products_posture/facing_77bce2/Torso_lin2'))
)
r.posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param(
'count_products_posture/facing_77bce2/Torso_rot_1')
))
elif data.id == 'facing_c4a62e':
r.posture = FullBodyPosture()
r.posture.base_pos.header.frame_id = '/map'
r.posture.base_pos.pose.position = Point(
rospy.get_param(
'count_products_posture/facing_c4a62e/position/x'),
rospy.get_param(
'count_products_posture/facing_c4a62e/position/y'),
rospy.get_param(
'count_products_posture/facing_c4a62e/position/z'))
r.posture.base_pos.pose.orientation = Quaternion(
rospy.get_param(
'count_products_posture/facing_c4a62e/orientation/x'),
rospy.get_param(
'count_products_posture/facing_c4a62e/orientation/y'),
rospy.get_param(
'count_products_posture/facing_c4a62e/orientation/z'),
rospy.get_param(
'count_products_posture/facing_c4a62e/orientation/w'))
r.posture.joints.append(
JointPosition(
'Torso_lin1',
rospy.get_param(
'count_products_posture/facing_c4a62e/Torso_lin1'))
)
r.posture.joints.append(
JointPosition(
'Torso_lin2',
rospy.get_param(
'count_products_posture/facing_c4a62e/Torso_lin2'))
)
r.posture.joints.append(
JointPosition(
'Torso_rot_1',
rospy.get_param(
'count_products_posture/facing_c4a62e/Torso_rot_1')
))
else:
r.error = QueryCountProductsPostureResponse.INVALID_ID
return r
def finish_perception_cb(self, data):
"""
:type data: FinishPerceptionRequest
:rtype: FinishPerceptionResponse
"""
prefix = 'finish_perception'
self.print_with_prefix('service called', prefix)
state = self.get_state(prefix)
if self.is_ready(state) or self.is_abort(state) or self.is_finish(
state):
self.put_state(state, prefix)
self.warn_with_prefix('server idle', prefix)
return FinishPerceptionResponse(
error=FinishPerceptionResponse.NO_RUNNING_JOB,
error_msg='no running job')
elif self.is_busy(state):
self.set_finish(prefix)
self.print_with_prefix(
'finishing {}'.format(self.state_code_to_name(state)), prefix)
return FinishPerceptionResponse(
error=FinishPerceptionResponse.SUCCESS)
else:
self.put_state(state, prefix)
self.print_with_prefix(
'called in unexpected state {}'.format(state), prefix)
def detect_shelf_layers_cb(self, data):
"""
:type data: DetectShelfLayersGoal
"""
result_type = DetectShelfLayersResult
action_server = self.detect_shelf_layer_as
prefix = 'detect_shelf_layers'
self.print_with_prefix('called', prefix)
r = result_type()
state = self.wait_for([
self.READY, self.KILL_DETECT_SHELF_LAYER, self.KILL_DETECT_FACINGS,
self.KILL_COUNT_PRODUCTS, self.BUSY_DETECT_SHELF_LAYER,
self.BUSY_COUNT_PRODUCTS, self.BUSY_DETECT_FACINGS
], prefix)
if state in [
self.KILL_DETECT_FACINGS, self.READY, self.KILL_COUNT_PRODUCTS
]:
if data.id in self.shelf_ids:
self.print_with_prefix('started', prefix)
self.put_state(self.BUSY_DETECT_SHELF_LAYER, prefix)
state = self.wait_for([self.ABORT, self.FINISH], prefix)
if self.is_abort(state):
r.error = 3
if action_server.new_goal:
self.set_ready(self.KILL_DETECT_SHELF_LAYER, prefix)
self.warn_with_prefix(
'aborted because new action of same type requested',
prefix)
else:
self.set_ready(prefix=prefix)
self.print_with_prefix('aborted', prefix)
action_server.set_aborted(r)
elif self.is_finish(state):
r.error = result_type.SUCCESS
r.ids = self.shelf_to_layer[data.id]
self.set_ready(prefix=prefix)
action_server.set_succeeded(r)
self.print_with_prefix('finished successful', prefix)
else:
r.error = result_type.INVALID_ID
r.error_msg = 'invalid shelf id: {}'.format(data.id)
self.set_ready(prefix=prefix)
self.print_with_prefix('invalid id {}'.format(data.id), prefix)
action_server.set_aborted(r)
elif self.is_busy(state):
self.set_abort(prefix)
self.print_with_prefix('aborted because server busy', prefix)
r.error = result_type.SERVER_BUSY
r.error_msg = 'aborted because server busy: {}'.format(
self.state_code_to_name(state))
action_server.set_aborted(r)
elif state == self.KILL_DETECT_SHELF_LAYER:
r.error = result_type.SERVER_BUSY
r.error_msg = 'aborted because server busy: {}'.format(
self.state_code_to_name(state))
self.set_ready(prefix=prefix)
self.print_with_prefix('aborted because server busy', prefix)
action_server.set_aborted(r)
def detect_facings_cb(self, data):
"""
:type data: DetectFacingsGoal
"""
result_type = DetectFacingsResult
action_server = self.detect_facings_as
prefix = 'detect_facings'
self.print_with_prefix('called', prefix)
r = result_type()
state = self.wait_for([
self.READY, self.KILL_DETECT_SHELF_LAYER, self.KILL_DETECT_FACINGS,
self.KILL_COUNT_PRODUCTS, self.BUSY_DETECT_SHELF_LAYER,
self.BUSY_COUNT_PRODUCTS, self.BUSY_DETECT_FACINGS
], prefix)
if state in [
self.KILL_DETECT_SHELF_LAYER, self.READY,
self.KILL_COUNT_PRODUCTS
]:
if data.id in self.layer_ids:
self.print_with_prefix('started', prefix)
self.put_state(self.BUSY_DETECT_FACINGS, prefix)
state = self.wait_for([self.ABORT, self.FINISH], prefix)
if self.is_abort(state):
r.error = 3
if action_server.new_goal:
self.set_ready(self.KILL_DETECT_FACINGS, prefix)
self.warn_with_prefix(
'aborted because new action of same type requested',
prefix)
else:
self.set_ready(prefix=prefix)
self.print_with_prefix('aborted', prefix)
action_server.set_aborted(r)
elif self.is_finish(state):
r.error = result_type.SUCCESS
r.ids = self.layer_to_facing[data.id]
self.set_ready(prefix=prefix)
action_server.set_succeeded(r)
self.print_with_prefix('finished successful', prefix)
else:
r.error = result_type.INVALID_ID
r.error_msg = 'invalid layer id: {}'.format(data.id)
self.set_ready(prefix=prefix)
self.print_with_prefix('invalid id {}'.format(data.id), prefix)
action_server.set_aborted(r)
elif self.is_busy(state):
self.set_abort(prefix)
self.print_with_prefix('aborted because server busy', prefix)
r.error = result_type.SERVER_BUSY
r.error_msg = 'aborted because server busy: {}'.format(
self.state_code_to_name(state))
action_server.set_aborted(r)
elif state == self.KILL_DETECT_FACINGS:
r.error = result_type.SERVER_BUSY
r.error_msg = 'aborted because server busy: {}'.format(
self.state_code_to_name(state))
self.set_ready(prefix=prefix)
self.print_with_prefix('aborted because server busy', prefix)
action_server.set_aborted(r)
def count_products_cb(self, data):
"""
:type data: CountProductsGoal
"""
result_type = CountProductsResult
action_server = self.count_products_as
prefix = 'count_products'
self.print_with_prefix('called', prefix)
r = result_type()
state = self.wait_for([
self.READY, self.KILL_DETECT_SHELF_LAYER, self.KILL_DETECT_FACINGS,
self.KILL_COUNT_PRODUCTS, self.BUSY_DETECT_SHELF_LAYER,
self.BUSY_COUNT_PRODUCTS, self.BUSY_DETECT_FACINGS
], prefix)
if state in [
self.KILL_DETECT_SHELF_LAYER, self.READY,
self.KILL_DETECT_FACINGS
]:
if data.id in self.facing_ids:
self.print_with_prefix('started', prefix)
# self.put_state(self.BUSY_COUNT_PRODUCTS, prefix)
self.set_finish(prefix)
state = self.wait_for([self.ABORT, self.FINISH], prefix)
if self.is_abort(state):
r.error = 3
if action_server.new_goal:
self.set_ready(self.KILL_COUNT_PRODUCTS, prefix)
self.warn_with_prefix(
'aborted because new action of same type requested',
prefix)
else:
self.set_ready(prefix=prefix)
self.print_with_prefix('aborted', prefix)
action_server.set_aborted(r)
elif self.is_finish(state):
r.error = result_type.SUCCESS
r.count = self.product_counts[data.id]
self.set_ready(prefix=prefix)
action_server.set_succeeded(r)
self.print_with_prefix('finished successful', prefix)
else:
r.error = result_type.INVALID_ID
r.error_msg = 'invalid facing id: {}'.format(data.id)
self.set_ready(prefix=prefix)
self.print_with_prefix('invalid id {}'.format(data.id), prefix)
action_server.set_aborted(r)
elif self.is_busy(state):
self.set_abort(prefix)
self.print_with_prefix('aborted because server busy', prefix)
r.error = result_type.SERVER_BUSY
r.error_msg = 'aborted because server busy: {}'.format(
self.state_code_to_name(state))
action_server.set_aborted(r)
elif state == self.KILL_COUNT_PRODUCTS:
r.error = result_type.SERVER_BUSY
r.error_msg = 'aborted because server busy: {}'.format(
self.state_code_to_name(state))
self.set_ready(prefix=prefix)
self.print_with_prefix('aborted because server busy', prefix)
action_server.set_aborted(r)
def cancel_cb(self):
prefix = 'cancel_callback'
self.print_with_prefix('called', prefix)
state = self.get_state(prefix)
if self.is_server_idle(state):
self.put_state(state, prefix)
self.print_with_prefix('server idle', prefix)
elif self.is_busy(state):
self.set_abort(prefix)
# self.state_lock.task_done()
self.print_with_prefix(
'aborting {}'.format(self.state_code_to_name(state)), prefix)
else:
self.put_state(state, prefix)
self.warn_with_prefix('called in unexpected state', prefix)
def get_state(self, prefix):
state = self.state_lock.get(timeout=ONE_YEAR)
# self.print_with_prefix('removed state token {}'.format(state), prefix)
# if task_done:
# self.state_lock.task_done()
return state
def put_state(self, state, prefix=''):
self.state_lock.put(state)
# self.print_with_prefix('added state {}'.format(state), prefix)
def is_ready(self, state):
return state in {
self.KILL_DETECT_SHELF_LAYER, self.KILL_DETECT_FACINGS,
self.KILL_COUNT_PRODUCTS, self.READY
}
def is_busy(self, state):
return state in {
self.BUSY_DETECT_SHELF_LAYER, self.BUSY_DETECT_FACINGS,
self.BUSY_COUNT_PRODUCTS
}
def is_abort(self, state):
return state == self.ABORT
def is_finish(self, state):
return state == self.FINISH
def is_server_idle(self, state):
return self.is_ready(state) or self.is_finish(state) or self.is_abort(
state)
def set_finish(self, prefix):
self.put_state(self.FINISH, prefix)
def set_ready(self, state=None, prefix=''):
if state is None:
self.put_state(self.READY, prefix)
else:
if state in [
self.KILL_DETECT_SHELF_LAYER, self.KILL_COUNT_PRODUCTS,
self.KILL_DETECT_FACINGS
]:
self.put_state(state, prefix)
def set_kill_next(self, state, prefix):
self.put_state(state, prefix)
def set_busy(self, state, prefix):
self.put_state(state, prefix)
def set_abort(self, prefix):
self.put_state(self.ABORT, prefix)
def wait_for(self, events, prefix):
state = None
while not rospy.is_shutdown():
state = self.check_state(events, prefix)
if state is not None:
break
else:
rospy.sleep(.5)
# try:
# self.state_lock.task_done()
# except:
# pass
return state
def check_state(self, events, prefix):
state = self.get_state(prefix)
if state in events:
return state
else:
self.put_state(state, prefix)
# self.state_lock.task_done()
return None
def state_code_to_name(self, code):
if code in {
self.READY, self.KILL_DETECT_SHELF_LAYER,
self.KILL_DETECT_FACINGS, self.KILL_COUNT_PRODUCTS
}:
return 'ready'
if code == self.BUSY_DETECT_SHELF_LAYER:
return 'detecting shelf layers'
if code == self.BUSY_DETECT_FACINGS:
return 'detecting shelf facings'
if code == self.BUSY_COUNT_PRODUCTS:
return 'counting products'
if code == self.ABORT:
return 'ABORT'
if code == self.FINISH:
return 'FINISH'
return 'UNKNOWN STATE'
def print_with_prefix(self, msg, prefix):
rospy.loginfo('[{}] {}'.format(prefix, msg))
def warn_with_prefix(self, msg, prefix):
rospy.logwarn('[{}] {}'.format(prefix, msg))
class MotionService(object):
def __init__(self):
rospy.init_node('motion_service')
# Load config
config_loader = RobotConfigLoader()
try:
robot_config_name = rospy.get_param(rospy.get_name() +
'/robot_config')
except KeyError:
rospy.logwarn('Could not find robot config param in /' +
rospy.get_name +
'/robot_config. Using default config for '
'Thor Mang.')
robot_config_name = 'thor'
config_loader.load_xml_by_name(robot_config_name + '_config.xml')
# Create publisher for first target
if len(config_loader.targets) > 0:
self._motion_publisher = MotionPublisher(
config_loader.robot_config,
config_loader.targets[0].joint_state_topic,
config_loader.targets[0].publisher_prefix)
else:
rospy.logerr(
'Robot config needs to contain at least one valid target.')
self._motion_data = MotionData(config_loader.robot_config)
# Subscriber to start motions via topic
self.motion_command_sub = rospy.Subscriber(
'motion_command', ExecuteMotionCommand,
self.motion_command_request_cb)
# Create action server
self._action_server = SimpleActionServer(
rospy.get_name() + '/motion_goal', ExecuteMotionAction, None,
False)
self._action_server.register_goal_callback(self._execute_motion_goal)
self._action_server.register_preempt_callback(
self._preempt_motion_goal)
self._preempted = False
def _execute_motion_goal(self):
goal = self._action_server.accept_new_goal()
# Check if motion exists
if goal.motion_name not in self._motion_data:
result = ExecuteMotionResult()
result.error_code = [ExecuteMotionResult.MOTION_UNKNOWN]
result.error_string = "The requested motion is unknown."
self._action_server.set_aborted(result)
return
# check if a valid time_factor is set, otherwise default to 1.0
if goal.time_factor <= 0.0:
goal.time_factor = 1.0
self._preempted = False
self._motion_publisher.publish_motion(
self._motion_data[goal.motion_name], goal.time_factor,
self._trajectory_finished)
print '[MotionService] New action goal received.'
def _preempt_motion_goal(self):
self._motion_publisher.stop_motion()
print '[MotionService] Preempting goal.'
self._preempted = True
def _trajectory_finished(self, error_codes, error_groups):
result = ExecuteMotionResult()
result.error_code = error_codes
error_string = ""
for error_code, error_group in zip(error_codes, error_groups):
error_string += error_group + ': ' + str(error_code) + '\n'
result.error_string = error_string
if self._preempted:
self._action_server.set_preempted(result)
else:
self._action_server.set_succeeded(result)
print '[MotionService] Trajectory finished with error code: \n', error_string
def _execute_motion(self, request):
response = ExecuteMotionResponse()
# check if a motion with this name exists
if request.motion_name not in self._motion_data:
print '[MotionService] Unknown motion name: "%s"' % request.motion_name
response.ok = False
response.finish_time.data = rospy.Time.now()
return response
# check if a valid time_factor is set, otherwise default to 1.0
if request.time_factor <= 0.0:
request.time_factor = 1.0
# find the duration of the requested motion
motion_duration = 0.0
for poses in self._motion_data[request.motion_name].values():
if len(poses) > 0:
endtime = poses[-1]['starttime'] + poses[-1]['duration']
motion_duration = max(motion_duration, endtime)
motion_duration *= request.time_factor
# execute motion
print '[MotionService] Executing motion: "%s" (time factor: %.3f, duration %.2fs)' % (
request.motion_name, request.time_factor, motion_duration)
self._motion_publisher.publish_motion(
self._motion_data[request.motion_name], request.time_factor)
# reply with ok and the finish_time of this motion
response.ok = True
response.finish_time.data = rospy.Time.now() + rospy.Duration.from_sec(
motion_duration)
return response
def start_server(self):
rospy.Service(rospy.get_name() + '/execute_motion', ExecuteMotion,
self._execute_motion)
self._action_server.start()
print '[MotionService] Waiting for calls...'
rospy.spin()
def motion_command_request_cb(self, command):
print "[MotionService] Initiate motion command via topic ..."
request = ExecuteMotion()
request.motion_name = command.motion_name
request.time_factor = command.time_factor
self._execute_motion(request)
print "[MotionService] Motion command complete"
class AudioFilePlayer(object):
def __init__(self,
volume_set_command='amixer -c 0 sset Master playback',
volume_get_command='amixer -c 0 sget Master playback'):
self.current_playing_process = None
self.volume_set_command = volume_set_command
self.volume_get_command = volume_get_command
self.curr_vol = 0
rospy.loginfo("Initializing AudioFilePlayer...")
rospy.loginfo("VolumeManager using base set command: '" +
self.volume_set_command + "'")
rospy.loginfo("VolumeManager using base get command: '" +
self.volume_get_command + "'")
# By default this node plays files using the aplay command
# Feel free to use any other command or flags
# by using the params provided
self.command = rospy.get_param('~/command', 'play')
self.flags = rospy.get_param('~/flags', '')
self.feedback_rate = rospy.get_param('~/feedback_rate', 10)
self.afp_as = SimpleActionServer(rospy.get_name(),
AudioFilePlayAction,
self.as_cb,
auto_start=False)
self.afp_sub = rospy.Subscriber('~play',
String,
self.topic_cb,
queue_size=1)
self.afp_as.start()
# Needs to be done after start
self.afp_as.register_preempt_callback(self.as_preempt_cb)
self.volume_sub = rospy.Subscriber('~set_volume',
Int8,
self.volume_cb,
queue_size=1)
# Get volume to start publishing
self.curr_vol = self.get_current_volume()
self.volume_listener = VolumeListener(self)
self.volume_pub = rospy.Publisher(
'~get_volume',
Int8,
subscriber_listener=self.volume_listener,
latch=True,
queue_size=1)
self.volume_timer = None
rospy.loginfo(
"Done, playing files from action server or topic interface.")
def as_preempt_cb(self):
if self.current_playing_process:
self.current_playing_process.kill()
# Put the AS as cancelled/preempted
res = AudioFilePlayResult()
res.success = False
res.reason = "Got a cancel request."
self.afp_as.set_preempted(res, text="Cancel requested.")
def as_cb(self, goal):
initial_time = time.time()
self.play_audio_file(goal.filepath)
r = rospy.Rate(self.feedback_rate)
while not rospy.is_shutdown(
) and not self.current_playing_process.is_done():
feedback = AudioFilePlayFeedback()
curr_time = time.time()
feedback.elapsed_played_time = rospy.Duration(curr_time -
initial_time)
self.afp_as.publish_feedback(feedback)
r.sleep()
final_time = time.time()
res = AudioFilePlayResult()
if self.current_playing_process.is_succeeded():
res.success = True
res.total_time = rospy.Duration(final_time)
self.afp_as.set_succeeded(res)
else:
if self.afp_as.is_preempt_requested():
return
res.success = False
reason = "stderr: " + self.current_playing_process.get_stderr()
reason += "\nstdout: " + self.current_playing_process.get_stdout()
res.reason = reason
self.afp_as.set_aborted(res)
def topic_cb(self, data):
if self.current_playing_process:
if not self.current_playing_process.is_done():
self.current_playing_process.kill()
self.play_audio_file(data.data)
def play_audio_file(self, audio_file_path):
# Replace any ' or " characters with emptyness to avoid bad usage
audio_file_path = audio_file_path.replace("'", "")
audio_file_path = audio_file_path.replace('"', '')
full_command = self.command + " " + self.flags + " '" + audio_file_path + "'"
rospy.loginfo("Playing audio file: " + str(audio_file_path) +
" with command: " + str(full_command))
self.current_playing_process = ShellCmd(full_command)
def curr_vol_cb(self, event):
self.curr_vol = self.get_current_volume()
self.volume_pub.publish(Int8(self.curr_vol))
def get_current_volume(self):
cmd = ShellCmd(self.volume_get_command)
while not cmd.is_done() and not rospy.is_shutdown():
rospy.sleep(0.1)
output = cmd.get_stdout()
rospy.logdebug("self.volume_get_command output: " + str(output))
# Output looks like:
# Simple mixer control 'Master',0
# Capabilities: pvolume pvolume-joined pswitch pswitch-joined
# Playback channels: Mono
# Limits: Playback 0 - 87
# Mono: Playback 44 [51%] [-32.25dB] [on]
pct_idx = output.index('%')
# At most 3 characters of 100%
pct_text = output[pct_idx - 3:pct_idx]
# Remove [ if it was less than 3 chars
pct_text = pct_text.replace('[', '')
# Remove spaces if it was less than 2 char
pct_text = pct_text.strip()
curr_vol = int(pct_text)
return curr_vol
def volume_cb(self, data):
if data.data > 100:
to_pct = 100
elif data.data < 0:
to_pct = 0
else:
to_pct = data.data
rospy.loginfo("Changing volume to: " + str(to_pct) + "%")
cmd_line = self.volume_set_command + " " + str(to_pct) + "%"
rospy.loginfo("Executing command: " + cmd_line)
cmd = ShellCmd(cmd_line)
while not cmd.is_done() and not rospy.is_shutdown():
rospy.sleep(0.1)
output = cmd.get_stdout()
rospy.logdebug("Set command output: " + str(output))
def enable_volume_service(self):
if not self.volume_timer:
rospy.loginfo("Enable audio volume subscription service.")
self.volume_timer = rospy.Timer(rospy.Duration(1.0),
self.curr_vol_cb)
def disable_volume_servce(self):
if self.volume_timer:
rospy.loginfo("Disable audio volume subscription service.")
self.volume_timer.shutdown()
self.volume_timer = None
class Server(object):
def __init__(self, name):
rospy.loginfo("Starting '{test}'.".format(test=name))
self._as = SimpleActionServer(name, WayPointNavAction, auto_start=False)
self._as.register_goal_callback(self.execute_cb)
self._as.register_preempt_callback(self.preempt_cb)
self.listener = tf.TransformListener()
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self.sub = None
self._as.start()
rospy.loginfo("Started '{test}'.".format(test=name))
def execute_cb(self):
self.goal = self._as.accept_new_goal()
print self.goal
self.sub = rospy.Subscriber("/orb_slam/pose", PoseStamped, self.pose_cb)
def preempt_cb(self, *args):
self.sub.unregister()
self.sub = None
t = Twist()
self.pub.publish(t)
self._as.set_preempted(WayPointNavResult())
def pose_cb(self, msg):
dist = self.get_dist(self.goal.goal, msg)
t = Twist()
new_goal = self._transform(self.goal.goal, "camera_pose")
print new_goal
print "Dist", dist
theta = self.get_theta(new_goal.pose)
print "Theta", theta
if dist < 0.02:
self._as.set_succeeded(WayPointNavResult())
self.sub.unregister()
self.sub = None
t.linear.x = 0.0
t.angular.z = 0.0
else:
t.linear.x = 0.03
t.angular.z = theta * .6
self.pub.publish(t)
def get_dist(self, pose1, pose2):
return np.sqrt(np.power(pose1.pose.position.x-pose2.pose.position.x, 2)+np.power(pose1.pose.position.y-pose2.pose.position.y, 2))
def _transform(self, msg, target_frame):
if msg.header.frame_id != target_frame:
try:
t = self.listener.getLatestCommonTime(target_frame, msg.header.frame_id)
msg.header.stamp = t
return self.listener.transformPose(target_frame, msg)
except (tf.Exception, tf.LookupException, tf.ConnectivityException) as ex:
rospy.logwarn(ex)
return None
else:
return msg
def get_theta(self, pose):
return np.arctan2(pose.position.y, pose.position.x)
