class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation',
GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
class Speak(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.texts = {"hello": "Hello, I am pepper!"}
self.predicate = rospy.get_param("~predicate", "said")
rospy.loginfo("Starting dialogue client")
self.client = SimpleActionClient("/dialogue_start", dialogueAction)
rospy.loginfo("Waiting for dialogue client")
self.client.wait_for_server()
rospy.loginfo("Dialogue client started")
self.pub = rospy.Publisher("/animated_speech", String, queue_size=10)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=SpeakAction,
execute_cb=self.execute_cb,
auto_start=False)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person",
TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
# self.pub.publish(self.texts[goal.text])
# rospy.sleep(3)
self.start_client.send_goal(TrackPersonGoal(id=goal.id))
self.client.send_goal(dialogueGoal(userID=int(goal.id.split("_")[1])))
res = SpeakResult()
res.result.append(
ActionResult(cond=self.predicate + "__" + goal.id + "__" +
goal.text,
truth_value=ActionResult.TRUE))
self._ps.set_succeeded(res)
def __init__(self):
rospy.loginfo("Starting '%s'." % ACTION_ID)
super(ROSPlanFeedbackServer, self).__init__()
self._sps = PNPSimplePluginServer(ACTION_ID,
ROSPlanAction,
self.execute_cb,
auto_start=False)
self.client = SimpleActionClient("/kcl_rosplan/start_planning",
PlanAction)
self.client.wait_for_server()
self.pub = rospy.Publisher("/kcl_rosplan/action_feedback",
ActionFeedback,
queue_size=1)
self.state_pub = rospy.Publisher("~current_state",
String,
queue_size=1,
latch=False)
self.state_pub.publish("")
self.commands_pub = rospy.Publisher("/kcl_rosplan/planning_commands",
String,
queue_size=1)
self.f = {
END: self.set_succeeded,
START: self.set_enabled,
FAIL: self.set_failed
}
self._sps.start()
rospy.loginfo("Started '%s'." % ACTION_ID)
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation', GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
self.client.send_goal_and_wait(GotoNodeGoal(target=goal.value))
result = self.client.get_state()
if result == GoalStatus.PREEMPTED:
return
if result == GoalStatus.SUCCEEDED:
self._ps.set_succeeded()
else:
self._ps.set_aborted()
def preempt_cb(self):
self.client.cancel_all_goals()
self._ps.set_preempted()
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self._as = PNPSimplePluginServer(name,
RouteDescriptionAction,
execute_cb=self.execute_cb,
auto_start=False)
self.listener = tf.TransformListener()
client = MongoClient(rospy.get_param("~db_host", "localhost"),
int(rospy.get_param("~db_port", 62345)))
rospy.loginfo("Creating move_base client")
self.move_client = SimpleActionClient("/move_base", MoveBaseAction)
self.move_client.wait_for_server()
rospy.loginfo("Move_base client connected")
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person",
TrackPersonAction)
self.start_client.wait_for_server()
self.stop_client = SimpleActionClient("/stop_tracking_person",
TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self.db_name = rospy.get_param("~db_name", "semantic_map")
self.collection_name = rospy.get_param("~collection_name", "idea_park")
self.semantic_map_name = rospy.get_param("~semantic_map_name")
self._as.start()
self.db = client[self.db_name]
self.tts = rospy.Publisher("/speech", String, queue_size=1)
self.joints = rospy.Publisher("/joint_angles",
JointAnglesWithSpeed,
queue_size=10)
rospy.loginfo("... done")
class ROSPlanFeedbackServer(PNPPlanningAbstractclass):
def __init__(self):
rospy.loginfo("Starting '%s'." % ACTION_ID)
super(ROSPlanFeedbackServer, self).__init__()
self._sps = PNPSimplePluginServer(ACTION_ID,
ROSPlanAction,
self.execute_cb,
auto_start=False)
self.client = SimpleActionClient("/kcl_rosplan/start_planning",
PlanAction)
self.client.wait_for_server()
self.pub = rospy.Publisher("/kcl_rosplan/action_feedback",
ActionFeedback,
queue_size=1)
self.state_pub = rospy.Publisher("~current_state",
String,
queue_size=1,
latch=False)
self.state_pub.publish("")
self.commands_pub = rospy.Publisher("/kcl_rosplan/planning_commands",
String,
queue_size=1)
self.f = {
END: self.set_succeeded,
START: self.set_enabled,
FAIL: self.set_failed
}
self._sps.start()
rospy.loginfo("Started '%s'." % ACTION_ID)
def execute_cb(self, goal):
rospy.loginfo("Setting action '%s' to '%s'" % (goal.id, goal.action))
self.f[goal.action](goal.id)
self._sps.set_succeeded()
def __publish(self, msg):
self.pub.publish(msg)
def __new_feedback_msg(self, id, status):
return ActionFeedback(action_id=id, status=status)
def set_succeeded(self, id):
self.__publish(self.__new_feedback_msg(id, "action achieved"))
def set_failed(self, id):
self.__publish(self.__new_feedback_msg(id, "action failed"))
def set_enabled(self, id):
self.__publish(self.__new_feedback_msg(id, "action enabled"))
def goal_state_reached(self):
print "#### GOAL"
self.state_pub.publish("goal")
def fail_state_reached(self):
print "#### FAIL"
self.commands_pub.publish("cancel")
self.client.send_goal(PlanGoal())
self.state_pub.publish("fail")
class DescribeRoute(object):
BASE_LINK = "base_link"
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.pub = rospy.Publisher("/cmd_vel", Twist, queue_size=10)
self._as = PNPSimplePluginServer(name,
RouteDescriptionAction,
execute_cb=self.execute_cb,
auto_start=False)
self.listener = tf.TransformListener()
client = MongoClient(rospy.get_param("~db_host", "localhost"),
int(rospy.get_param("~db_port", 62345)))
rospy.loginfo("Creating move_base client")
self.move_client = SimpleActionClient("/move_base", MoveBaseAction)
self.move_client.wait_for_server()
rospy.loginfo("Move_base client connected")
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person",
TrackPersonAction)
self.start_client.wait_for_server()
self.stop_client = SimpleActionClient("/stop_tracking_person",
TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self.db_name = rospy.get_param("~db_name", "semantic_map")
self.collection_name = rospy.get_param("~collection_name", "idea_park")
self.semantic_map_name = rospy.get_param("~semantic_map_name")
self._as.start()
self.db = client[self.db_name]
self.tts = rospy.Publisher("/speech", String, queue_size=1)
self.joints = rospy.Publisher("/joint_angles",
JointAnglesWithSpeed,
queue_size=10)
rospy.loginfo("... done")
def stand(self):
print "STANDING"
self.__call_service(
"/naoqi_driver/robot_posture/go_to_posture", GoToPosture,
GoToPostureRequest(GoToPostureRequest.STAND_INIT, 0.5))
j = JointAnglesWithSpeed()
j.joint_names = ['HeadYaw', 'HeadPitch']
j.joint_angles = [0., -.5]
j.speed = .05
self.joints.publish(j)
def __call_service(self, srv_name, srv_type, req):
while not rospy.is_shutdown():
try:
s = rospy.ServiceProxy(srv_name, srv_type)
s.wait_for_service(timeout=1.)
except rospy.ROSException, rospy.ServiceException:
rospy.logwarn(
"Could not communicate with '%s' service. Retrying in 1 second."
% srv_name)
rospy.sleep(1.)
else:
return s(req)
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._as = PNPSimplePluginServer(name,
FinishRouteDescriptionAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("... done")
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=FindInteractantAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self.pub = rospy.Publisher("/speech", String, queue_size=1)
self._ps.start()
rospy.loginfo("Done")
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.predicate = rospy.get_param("~predicate", "human_exists")
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=CheckHumanExistsAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self.listener = tf.TransformListener()
self.pub = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self._ps.start()
rospy.loginfo("Done")
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation',
GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
super(StartStopPeopleTracking, self).__init__()
self.listener = tf.TransformListener()
self.id = 0
self.target_frame = "base_link"
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=TrackPersonAction,
execute_cb=self.start_cb if "start" in name else self.stop_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=FindInteractantAction,
execute_cb=self.execute_cb,
auto_start=False
)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person", TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def __init__(self):
rospy.loginfo("Starting '%s'." % ACTION_ID)
super(ROSPlanFeedbackServer, self).__init__()
self._sps = PNPSimplePluginServer(
ACTION_ID,
ROSPlanAction,
self.execute_cb,
auto_start=False
)
self.client = SimpleActionClient("/kcl_rosplan/start_planning", PlanAction)
self.client.wait_for_server()
self.pub = rospy.Publisher(
"/kcl_rosplan/action_feedback",
ActionFeedback,
queue_size=1
)
self.state_pub = rospy.Publisher(
"~current_state",
String,
queue_size=1,
latch=False
)
self.state_pub.publish("")
self.commands_pub = rospy.Publisher(
"/kcl_rosplan/planning_commands",
String,
queue_size=1
)
self.f = {
END: self.set_succeeded,
START: self.set_enabled,
FAIL: self.set_failed
}
self._sps.start()
rospy.loginfo("Started '%s'." % ACTION_ID)
class TerminateInteraction(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=TerminateInteractionAction,
execute_cb=self.execute_cb,
auto_start=False
)
rospy.loginfo("Creating tracker client")
self.stop_client = SimpleActionClient("/stop_tracking_person", TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Disengaging human '%s'" % (goal.id,))
self.stop_client.send_goal(TrackPersonGoal())
res = TerminateInteractionResult()
res.result.append(ActionResult(cond="engaged__"+goal.interactant_id+"__"+goal.text, truth_value=False))
res.result.append(ActionResult(cond="said__"+goal.id+"__"+goal.text, truth_value=False))
res.result.append(ActionResult(cond="found_interactant__"+goal.interactant_id+"__"+goal.id, truth_value=False))
res.result.append(ActionResult(cond="human_exists__"+goal.id, truth_value=False))
res.result.append(ActionResult(cond="free_interactant_id__"+goal.interactant_id, truth_value=True))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
class FindInteractant(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=FindInteractantAction,
execute_cb=self.execute_cb,
auto_start=False
)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person", TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Finding interactant '%s'" % (goal.interactant_id,))
self.start_client.send_goal(TrackPersonGoal(id=goal.id, interactant_id=goal.interactant_id, no_turn=True))
res = FindInteractantResult()
res.result.append(ActionResult(cond="found_interactant__"+goal.interactant_id+"__"+goal.id, truth_value=True))
res.result.append(ActionResult(cond="free_interactant_id__"+goal.interactant_id, truth_value=False))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
class CheckHumanExistance(PNPSimplePluginServer):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.predicate = rospy.get_param("~predicate", "human_exists")
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=CheckHumanExistsAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("checking existance of '%s'" % (goal.id, ))
res = CheckHumanExistsResult()
try:
msg = rospy.wait_for_message("/naoqi_driver_node/people_detected",
PersonDetectedArray,
timeout=5.)
except rospy.ROSException as e:
rospy.logwarn(e)
res.result.append(
ActionResult(cond=self.predicate + "__" + goal.id,
truth_value=ActionResult.FALSE))
res.result.append(
ActionResult(cond="found_interactant__" + goal.interactant_id +
"__" + goal.id,
truth_value=True))
res.result.append(
ActionResult(cond="free_interactant_id__" +
goal.interactant_id,
truth_value=False))
else:
found = ActionResult.FALSE
int_id = int(goal.id.split("_")[1])
for p in msg.person_array:
if p.id == int_id:
found = ActionResult.TRUE
break
res.result.append(
ActionResult(cond=self.predicate + "__" + goal.id,
truth_value=found))
finally:
if self._ps.is_preempt_requested():
self._ps.set_preempted()
elif res.result[-1].truth_value:
self._ps.set_succeeded(res)
else:
print "##### ABORTED #####"
self._ps.set_aborted(res)
class SayServer(object):
__dict = {"hello": "Hello, I am pepper", "bye": "Asta la vista, baby"}
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self.pub = rospy.Publisher("/speech", String, queue_size=1)
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
text = self.__dict[goal.value]
print "Say:", text
self.pub.publish(text)
self._ps.set_succeeded(StringResult(goal.value))
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.listener = tf.TransformListener()
self.target_frame = rospy.get_param("~target_frame", "base_link")
with open(rospy.get_param("~config_file"), 'r') as f:
self.distance = yaml.load(f)["distances"]
self.move_client = SimpleActionClient("move_base", MoveBaseAction)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=QualitativeMovePepperAction,
execute_cb=self.execute_cb,
auto_start=False)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person",
TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self.pub = rospy.Publisher("/speech", String, queue_size=1)
self._ps.start()
rospy.loginfo("Done")
class MoveToWaypoint(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._as = PNPSimplePluginServer(
name,
MoveToWaypointAction,
execute_cb=self.execute_cb,
auto_start=False
)
self.msg_store = MessageStoreProxy(
database=rospy.get_param("~db_name", "semantic_map"),
collection=rospy.get_param("~collection_name", "waypoints")
)
rospy.loginfo("Creating tracker client")
self.stop_client = SimpleActionClient("/stop_tracking_person", TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self.client = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base client.")
self.client.wait_for_server()
self._as.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
self.stop_client.send_goal(TrackPersonGoal())
if goal.to != "none":
pose, _ = self.load({"waypoint_name": goal.to})
rospy.loginfo("Going from '%s' to '%s'" % (goal.from_, goal.to))
self.client.send_goal_and_wait(MoveBaseGoal(target_pose=pose))
res = MoveToWaypointResult()
res.result.append(ActionResult(cond="robot_at_waypoint__"+goal.to, truth_value=True))
res.result.append(ActionResult(cond="robot_at_waypoint__"+goal.from_, truth_value=False))
res.result.append(ActionResult(cond="robot_pose_unknown", truth_value=True))
self._as.set_succeeded(res)
def load(self, meta):
message = self.msg_store.query(PoseStamped._type, {}, meta)
if len(message) == 0:
raise Exception("Desired data set %s: %s not in datacentre."% meta.items()[0])
else:
return message[0][0], message[0][1]["_id"]
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self.listener = tf.TransformListener()
self.pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=1)
self._ps.start()
rospy.loginfo("Done")
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.texts = {"hello": "Hello, I am pepper!"}
self.predicate = rospy.get_param("~predicate", "said")
rospy.loginfo("Starting dialogue client")
self.client = SimpleActionClient("/dialogue_start", dialogueAction)
rospy.loginfo("Waiting for dialogue client")
self.client.wait_for_server()
rospy.loginfo("Dialogue client started")
self.pub = rospy.Publisher("/animated_speech", String, queue_size=10)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=SpeakAction,
execute_cb=self.execute_cb,
auto_start=False)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person",
TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
class FinishDescription(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._as = PNPSimplePluginServer(name,
FinishRouteDescriptionAction,
execute_cb=self.execute_cb,
auto_start=False)
self._as.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
res = FinishRouteDescriptionResult()
res.result.append(
ActionResult(cond="described_route__%s__%s" %
(goal.shop_id, goal.waypoint),
truth_value=False))
res.result.append(
ActionResult(cond="finished_description__%s__%s" %
(goal.shop_id, goal.waypoint),
truth_value=True))
self._as.set_succeeded(res)
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._as = PNPSimplePluginServer(
name,
MoveToWaypointAction,
execute_cb=self.execute_cb,
auto_start=False
)
self.msg_store = MessageStoreProxy(
database=rospy.get_param("~db_name", "semantic_map"),
collection=rospy.get_param("~collection_name", "waypoints")
)
rospy.loginfo("Creating tracker client")
self.stop_client = SimpleActionClient("/stop_tracking_person", TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self.client = SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("Waiting for move_base client.")
self.client.wait_for_server()
self._as.start()
rospy.loginfo("... done")
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self._ps.register_preempt_callback(self.preempt_cb)
self.client = SimpleActionClient('topological_navigation', GotoNodeAction)
self.client.wait_for_server()
self._ps.start()
rospy.loginfo("Done")
class SayServer(object):
__dict = {
"hello": "Hello, I am pepper",
"bye": "Asta la vista, baby"
}
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self.pub = rospy.Publisher("/speech", String, queue_size=1)
self._ps.start()
rospy.loginfo("Done")
def execute_cb(self, goal):
text = self.__dict[goal.value]
print "Say:", text
self.pub.publish(text)
self._ps.set_succeeded(StringResult(goal.value))
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False)
self.listener = tf.TransformListener()
self.pub = rospy.Publisher("/move_base_simple/goal",
PoseStamped,
queue_size=1)
self._ps.start()
rospy.loginfo("Done")
def get_transfrom(self, f1, f2):
try:
(trans, rot) = self.listener.lookupTransform(f1, f2, rospy.Time(0))
return trans, rot
except (tf.LookupException, tf.ConnectivityException,
tf.ExtrapolationException):
if not rospy.is_shutdown() and not self._ps.is_preempt_requested():
rospy.sleep(0.1)
return self.get_transfrom(f1, f2)
def get_euclidean_distance(self, p1, p2):
return np.sqrt(
np.square(p1[0] - p2[0]) \
+ np.square(p1[1] - p2[1])
)
def execute_cb(self, goal):
p = PoseStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = BASE_LINK
start, r = self.get_transfrom(BASE_LINK, ODOM)
p.pose.orientation = Quaternion(x=r[0], y=r[1], z=r[2], w=r[3])
if goal.value == "close":
p.pose.position.x = 1.0
elif goal.value == "far":
p.pose.position.x = -1.0
elif goal.value == "left":
p.pose.position.y = 1.0
elif goal.value == "right":
p.pose.position.y = -1.0
self.pub.publish(p)
timeout = p.header.stamp.to_sec() + 3.
while not rospy.is_shutdown() and not self._ps.is_preempt_requested() \
and timeout > rospy.Time.now().to_sec():
t, _ = self.get_transfrom(BASE_LINK, ODOM)
if self.get_euclidean_distance(start[:2], t[:2]) > .95:
break
rospy.sleep(.1)
self._ps.set_succeeded(StringResult(goal.value))
class NavServer(object):
def __init__(self, name):
rospy.loginfo("Starting '%s'." % name)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=StringAction,
execute_cb=self.execute_cb,
auto_start=False
)
self.listener = tf.TransformListener()
self.pub = rospy.Publisher("/move_base_simple/goal", PoseStamped, queue_size=1)
self._ps.start()
rospy.loginfo("Done")
def get_transfrom(self, f1, f2):
try:
(trans,rot) = self.listener.lookupTransform(f1, f2, rospy.Time(0))
return trans, rot
except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException):
if not rospy.is_shutdown() and not self._ps.is_preempt_requested():
rospy.sleep(0.1)
return self.get_transfrom(f1, f2)
def get_euclidean_distance(self, p1, p2):
return np.sqrt(
np.square(p1[0] - p2[0]) \
+ np.square(p1[1] - p2[1])
)
def execute_cb(self, goal):
p = PoseStamped()
p.header.stamp = rospy.Time.now()
p.header.frame_id = BASE_LINK
start, r = self.get_transfrom(BASE_LINK, ODOM)
p.pose.orientation = Quaternion(x=r[0], y=r[1], z=r[2], w=r[3])
if goal.value == "close":
p.pose.position.x = 1.0
elif goal.value == "far":
p.pose.position.x = -1.0
elif goal.value == "left":
p.pose.position.y = 1.0
elif goal.value == "right":
p.pose.position.y = -1.0
self.pub.publish(p)
timeout = p.header.stamp.to_sec() + 3.
while not rospy.is_shutdown() and not self._ps.is_preempt_requested() \
and timeout > rospy.Time.now().to_sec():
t, _ = self.get_transfrom(BASE_LINK, ODOM)
if self.get_euclidean_distance(start[:2], t[:2]) > .95:
break
rospy.sleep(.1)
self._ps.set_succeeded(StringResult(goal.value))
class StartStopPeopleTracking(PeopleTracking):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
super(StartStopPeopleTracking, self).__init__()
self.listener = tf.TransformListener()
self.id = 0
self.target_frame = "base_link"
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=TrackPersonAction,
execute_cb=self.start_cb if "start" in name else self.stop_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def start_cb(self, goal):
if goal.id != "":
self.id = goal.id.split('_')[1]
else:
self.id = rospy.wait_for_message(
"/naoqi_driver_node/people_detected",
PersonDetectedArray).person_array[0].id
if not goal.no_turn:
self.start()
req = SetTrackerTargetRequest()
req.target = req.PEOPLE
req.values.append(float(self.id))
self._call_service("/naoqi_driver/tracker/register_target",
SetTrackerTarget, req)
self._call_service("/naoqi_driver/tracker/set_mode", SetTrackerMode,
SetTrackerModeRequest(SetTrackerModeRequest.HEAD))
self._call_service("/naoqi_driver/tracker/track", StartTracker,
StartTrackerRequest(StartTrackerRequest.PEOPLE))
res = TrackPersonResult()
res.result.append(
ActionResult(cond="tracking__" + goal.id, truth_value=True))
res.result.append(ActionResult(cond="no_tracking", truth_value=False))
self._ps.set_succeeded(res)
def stop_cb(self, goal):
self.stop()
self._call_service("/naoqi_driver/tracker/stop_tracker", Empty,
EmptyRequest())
self._call_service("/naoqi_driver/tracker/unregister_all_targets",
Empty, EmptyRequest())
self.pub.publish(Twist())
res = TrackPersonResult()
res.result.append(
ActionResult(cond="tracking__" + goal.id, truth_value=False))
res.result.append(ActionResult(cond="no_tracking", truth_value=True))
self._ps.set_succeeded(res)
class EngageHuman(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=EngageHumanAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Engaging human '%s'" % (goal.id, ))
res = EngageHumanResult()
# Has to be done in the end of the dialogue so the controller is blocked till it is over.
res.result.append(
ActionResult(cond="engaged__" + goal.interactant_id + "__" +
goal.text,
truth_value=True))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
class FindInteractant(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=FindInteractantAction,
execute_cb=self.execute_cb,
auto_start=False)
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
rospy.loginfo("Finding interactant '%s'" % (goal.interactant_id, ))
res = FindInteractantResult()
res.result.append(
ActionResult(cond="found_interactant__" + goal.interactant_id +
"__" + goal.id,
truth_value=True))
res.result.append(
ActionResult(cond="free_interactant_id__" + goal.interactant_id,
truth_value=False))
if self._ps.is_preempt_requested():
self._ps.set_preempted()
else:
self._ps.set_succeeded(res)
class ROSPlanFeedbackServer(PNPPlanningAbstractclass):
def __init__(self):
rospy.loginfo("Starting '%s'." % ACTION_ID)
super(ROSPlanFeedbackServer, self).__init__()
self._sps = PNPSimplePluginServer(
ACTION_ID,
ROSPlanAction,
self.execute_cb,
auto_start=False
)
self.client = SimpleActionClient("/kcl_rosplan/start_planning", PlanAction)
self.client.wait_for_server()
self.pub = rospy.Publisher(
"/kcl_rosplan/action_feedback",
ActionFeedback,
queue_size=1
)
self.state_pub = rospy.Publisher(
"~current_state",
String,
queue_size=1,
latch=False
)
self.state_pub.publish("")
self.commands_pub = rospy.Publisher(
"/kcl_rosplan/planning_commands",
String,
queue_size=1
)
self.f = {
END: self.set_succeeded,
START: self.set_enabled,
FAIL: self.set_failed
}
self._sps.start()
rospy.loginfo("Started '%s'." % ACTION_ID)
def execute_cb(self, goal):
rospy.loginfo("Setting action '%s' to '%s'" % (goal.id, goal.action))
self.f[goal.action](goal.id)
self._sps.set_succeeded()
def __publish(self, msg):
self.pub.publish(msg)
def __new_feedback_msg(self, id, status):
return ActionFeedback(action_id=id, status=status)
def set_succeeded(self, id):
self.__publish(self.__new_feedback_msg(id, "action achieved"))
def set_failed(self, id):
self.__publish(self.__new_feedback_msg(id, "action failed"))
def set_enabled(self, id):
self.__publish(self.__new_feedback_msg(id, "action enabled"))
def goal_state_reached(self):
print "#### GOAL"
self.state_pub.publish("goal")
def fail_state_reached(self):
print "#### FAIL"
self.commands_pub.publish("cancel")
self.client.send_goal(PlanGoal())
self.state_pub.publish("fail")
class QualitativeMove(PNPSimplePluginServer):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self.listener = tf.TransformListener()
self.target_frame = rospy.get_param("~target_frame", "base_link")
with open(rospy.get_param("~config_file"), 'r') as f:
self.distance = yaml.load(f)["distances"]
self.move_client = SimpleActionClient("move_base", MoveBaseAction)
self._ps = PNPSimplePluginServer(
name=name,
ActionSpec=QualitativeMovePepperAction,
execute_cb=self.execute_cb,
auto_start=False)
rospy.loginfo("Creating tracker client")
self.start_client = SimpleActionClient("/start_tracking_person",
TrackPersonAction)
self.start_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
self.start_client.send_goal(TrackPersonGoal(id=goal.id, no_turn=True))
try:
msg = rospy.wait_for_message("/naoqi_driver_node/people_detected",
PersonDetectedArray,
timeout=5.)
except rospy.ROSException as e:
rospy.logwarn(e)
self._ps.set_aborted()
else:
int_id = int(goal.id.split("_")[1])
for p in msg.person_array:
if p.id == int_id:
try:
t = self.listener.getLatestCommonTime(
self.target_frame, msg.header.frame_id)
p_pose = PoseStamped(header=msg.header,
pose=p.person.position)
p_pose.header.stamp = t
bl_pose = self.listener.transformPose(
self.target_frame, p_pose)
except (tf.Exception, tf.LookupException,
tf.ConnectivityException) as ex:
rospy.logwarn(ex)
else:
target_dist = p.person.distance - self.distance[
goal.to]
d = target_dist / p.person.distance
theta = math.atan2(bl_pose.pose.position.y,
bl_pose.pose.position.x)
target_pose = bl_pose
target_pose.pose.position.x *= d
target_pose.pose.position.y *= d
target_pose.pose.orientation.x = 0.
target_pose.pose.orientation.y = 0.
target_pose.pose.orientation.z = math.sin(theta / 2.)
target_pose.pose.orientation.w = math.cos(theta / 2.)
print target_pose
self.move_client.send_goal_and_wait(
MoveBaseGoal(target_pose=target_pose))
finally:
break
res = QualitativeMovePepperResult()
res.result.append(
ActionResult(cond="robot_at_home", truth_value=False))
res.result.append(
ActionResult(cond="robot_pose_unknown", truth_value=True))
self._ps.set_succeeded(res)
class GoHome(object):
def __init__(self, name):
rospy.loginfo("Starting %s ..." % name)
self._ps = PNPSimplePluginServer(name=name,
ActionSpec=GoHomeAction,
execute_cb=self.execute_cb,
auto_start=False)
rospy.loginfo("Creating tracker client")
self.stop_client = SimpleActionClient("/stop_tracking_person",
TrackPersonAction)
self.stop_client.wait_for_server()
rospy.loginfo("Tracker client connected")
self._ps.register_preempt_callback(self.preempt_cb)
self._ps.start()
rospy.loginfo("... done")
def execute_cb(self, goal):
self.stop_client.send_goal(TrackPersonGoal())
if self.__call_service("/naoqi_driver/localization/is_data_available",
LocalizationCheck,
LocalizationCheckRequest()).result:
while not rospy.is_shutdown(
) and not self._ps.is_preempt_requested():
rospy.loginfo("Robot returning home")
res = self.__call_service(
"/naoqi_driver/localization/go_to_home",
LocalizationTrigger, LocalizationTriggerRequest())
if res.result != 0:
rospy.logwarn(self.__get_error_message(res.result))
break
else:
rospy.logwarn("Cannot go home. Pepper has never been localised.")
res = GoHomeResult()
res.result.append(ActionResult(cond="robot_at_home", truth_value=True))
res.result.append(
ActionResult(cond="robot_pose_unknown", truth_value=False))
self._ps.set_succeeded(res)
def preempt_cb(self):
rospy.loginfo("Preempting going home")
self.__call_service("/naoqi_driver/localization/stop_all", Empty,
EmptyRequest())
def __call_service(self, srv_name, srv_type, req):
while not rospy.is_shutdown():
try:
s = rospy.ServiceProxy(srv_name, srv_type)
s.wait_for_service(timeout=1.)
except rospy.ROSException:
rospy.logwarn(
"Could not communicate with '%s' service. Retrying in 1 second."
% srv_name)
rospy.sleep(1.)
else:
return s(req)
def __get_error_message(self, code):
return self.__call_service(
"/naoqi_driver/localization/get_message_from_error_code",
LocalizationGetErrorMessage,
LocalizationGetErrorMessageRequest(code)).error_message