def execute(self, userdata=None):
# Store current base position
base_loc = self._robot.base.get_location()
base_pose = base_loc.frame
# Create automatic side detection state and execute
# self._robot.speech.speak("I am now going to look for the table", block=False)
# automatic_side_detection = AutomaticSideDetection2(self._robot)
# side = automatic_side_detection.execute({})
# self._robot.head.look_at_standing_person()
# self._robot.speech.speak("The {} is to my {}".format(self._location_id, side))
# self._robot.head.cancel_goal()
# if side == "left":
# base_pose.M.DoRotZ(math.pi / 2)
# elif side == "right":
# base_pose.M.DoRotZ(-math.pi / 2)
# Add to param server
loc_dict = {'x': base_pose.p.x(), 'y': base_pose.p.y(), 'phi': base_pose.M.GetRPY()[2]}
rospy.set_param("/restaurant_locations/{name}".format(name=self._location_id), loc_dict)
self._visualize_location(base_pose, self._location_id)
self._robot.ed.update_entity(id=self._location_id, frame_stamped=FrameStamped(base_pose, "/map"),
type="waypoint")
return "done"
def execute(self, userdata=None):
return "succeeded"
e = self._trashbin.resolve()
if not e:
raise Exception("trashbin designator is empty")
# get original entity pose
frame_original = self._robot.ed.get_entity(id=e.id)._pose
# inspect and update entity
states.look_at_segmentation_area(self._robot, self._robot.ed.get_entity(id=e.id), 'on_top_of')
self._robot.ed.update_kinect("{} {}".format('on_top_of', e.id))
frame_updated = self._robot.ed.get_entity(id=e.id)._pose
# update entity with original orientation
frame_updated.M = frame_original.M
new_frame = FrameStamped(frame_updated, "map")
# new_frame.header.stamp
self._robot.ed.update_entity(self._robot, e.id, frame_stamped=new_frame)
if e:
return "succeeded"
else:
return "failed"
def execute(self, userdata=None):
self._robot.speech.speak("Is the bar on my left or on my right?")
self._robot.head.look_at_standing_person()
base_pose = self._robot.base.get_location().frame
result = None
while not result:
result = self._robot.ears.recognize(
'<side>', {'side': ['left', 'right']},
time_out=rospy.Duration(10)) # Wait 100 secs
if result.result == "left":
base_pose.M.DoRotZ(math.pi / 2)
elif result.result == "right":
base_pose.M.DoRotZ(-math.pi / 2)
else:
print "\n\n WHUT?? No left or right lol? \n\n"
print result
print "\n"
self._robot.ed.update_entity(id="beverages",
kdl_frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
return "done"
def move_table(userdata=None, manipulate_machine=None):
""" Moves the entities for this challenge to the correct poses"""
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup:
(tup[0].frame.p - robot_pose.frame.p).Norm())
cabinet_id = ENTITY_POSES[0][2]
table_id = ENTITY_POSES[0][3]
# Update the world model
robot.ed.update_entity(id="balcony_shelf",
frame_stamped=FrameStamped(
kdl.Frame(kdl.Rotation(),
kdl.Vector(0.0, 3.0,
0.0)),
frame_id="map"))
robot.ed.update_entity(id=cabinet_id,
frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=table_id,
frame_stamped=ENTITY_POSES[0][1])
# Update designators
cabinet.id_ = ENTITY_POSES[0][2]
room.id_ = ENTITY_POSES[0][4]
# Update manipulate machine
manipulate_machine.place_entity_designator.id_ = cabinet_id
manipulate_machine.place_designator._area = ENTITY_POSES[0][5]
manipulate_machine.place_designator.place_location_designator.id = cabinet_id
manipulate_machine.table_designator.id_ = table_id
manipulate_machine.cabinet.id_ = ENTITY_POSES[0][2]
return "done"
def test_describe_near_objects4(self):
self.robot.base.get_location = lambda: FrameStamped(
kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(7.5, 2, 0)), "/map"
)
self.assertEqual("We now enter the bedroom",
self.tour_guide.describe_near_objects())
self.assertEqual("On our right you can see the bookcase",
self.tour_guide.describe_near_objects())
self.assertEqual("", self.tour_guide.describe_near_objects())
def execute(self, userdata=None):
# Stop the base
self._robot.base.local_planner.cancelCurrentPlan()
base_pose = self._robot.base.get_location().frame
choices = knowledge.guiding_choices
self._robot.head.look_at_standing_person()
self._robot.speech.speak("Location and side?")
result = self._robot.ears.recognize(
knowledge.guiding_spec, choices,
time_out=rospy.Duration(10)) # Wait 100 secs
self._robot.head.cancel_goal()
if result:
if "continue" in result.choices:
return "continue"
if "side" in result.choices and "location" in result.choices:
side = result.choices["side"]
location = result.choices["location"]
self._robot.head.look_at_standing_person()
# self._robot.speech.speak("%s %s?"%(location, side))
# result = self._robot.ears.recognize("(yes|no)",{})
# self._robot.head.cancel_goal()
# if not result or result.result == "no":
# self._robot.speech.speak("Sorry", block=False)
# return "continue"
self._robot.speech.speak("%s %s, it is!" %
(location, side))
self._robot.head.cancel_goal()
if side == "left":
base_pose.M.DoRotZ(math.pi / 2)
elif side == "right":
base_pose.M.DoRotZ(-math.pi / 2)
loc_dict = {
'x': base_pose.p.x(),
'y': base_pose.p.y(),
'phi': base_pose.M.GetRPY()[2]
}
rospy.set_param(
"/restaurant_locations/{name}".format(name=location),
loc_dict)
visualize_location(base_pose, location)
self._robot.ed.update_entity(
id=location,
kdl_frame_stamped=FrameStamped(base_pose, "/map"),
type="waypoint")
return "done"
return "continue"
def store_pose(userdata=None):
base_loc = robot.base.get_location()
base_pose = base_loc.frame
location_id = INFORMATION_POINT_ID
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
return "succeeded"
def _resolve(self):
# ToDo: Make this happen for the bin in the chosen room...
try:
frame = self._entity_des.resolve()._pose
except Exception:
rospy.logerr(Exception.message)
return None
frame.p.z(self._drop_height)
return FrameStamped(frame, "/map")
def _resolve(self):
frame = None
# Query ed
try:
frame = self._robot.ed.get_entity(id="trashbin")._pose
except:
return None
frame.p.z(self._drop_height)
return FrameStamped(frame, "/map")
def __init__(self, *args, **kwargs):
self.move = mock.MagicMock()
self.force_drive = mock.MagicMock()
self.get_location = lambda: FrameStamped(random_kdl_frame(), "/map")
self.set_initial_pose = mock.MagicMock()
self.go = mock.MagicMock()
self.reset_costmap = mock.MagicMock()
self.cancel_goal = mock.MagicMock()
self.analyzer = mock.MagicMock()
self.global_planner = mock.MagicMock()
self.local_planner = mock.MagicMock()
self.local_planner.getStatus = mock.MagicMock(return_value="arrived") #always arrive for now
self.global_planner.getPlan = mock.MagicMock(return_value=["dummy_plan"]) #always arrive for now
def load_waypoints(robot, filename="/param/locations.yaml"):
rp = rospkg.rospack.RosPack()
restaurant_package = rp.get_path("challenge_restaurant")
locations = rosparam.load_file(restaurant_package+filename)
rospy.set_param("/restaurant_locations/", locations)
for tablename in tables.values():
location = locations[0][0][0][0]['locations'][tablename] #Don't ask why there's so many subindices to use...
base_pose = msg_constructors.PoseStamped(location['x'], location['y'], z=0)
base_pose.pose.orientation = transformations.euler_z_to_quaternion(location['phi'])
visualize_location(base_pose, tablename)
robot.ed.update_entity(id=tablename, kdlFrameStamped=FrameStamped(base_pose, "/map"), type="waypoint")
def _set_waypoint(self, intersection_point, person_position):
""" Puts the goal as a waypoint in ED
:param intersection_point: geometry_msgs PointStamped of the last raytrace intersection (in map)
:param person_position: geometry_msgs PointStamped of the last measured person position (in map)
"""
yaw = math.atan2(person_position.point.y - intersection_point.point.y,
person_position.point.x - intersection_point.point.x)
position = kdl.Vector(intersection_point.point.x,
intersection_point.point.y, 0.0)
orientation = kdl.Rotation.RPY(0.0, 0.0, yaw)
waypoint = FrameStamped(frame=kdl.Frame(orientation, position),
frame_id="/map")
self.robot.ed.update_entity(id=self.waypoint_id,
type="waypoint",
frame_stamped=waypoint)
# import ipdb;ipdb.set_trace()
return
def __init__(self, robot_name, tf_listener, *args, **kwargs):
super(Base, self).__init__(robot_name, tf_listener)
self.move = mock.MagicMock()
self.turn_towards = mock.MagicMock()
self.force_drive = mock.MagicMock()
self.get_location = lambda: FrameStamped(random_kdl_frame(), "/map")
self.set_initial_pose = mock.MagicMock()
self.wait_for_motion_done = mock.MagicMock()
self.go = mock.MagicMock()
self.reset_costmap = mock.MagicMock()
self.cancel_goal = mock.MagicMock()
self.analyzer = mock.MagicMock()
self.global_planner = mock.MagicMock()
self.local_planner = mock.MagicMock()
self.local_planner.getStatus = mock.MagicMock(
return_value="arrived") # always arrive for now
self.global_planner.getPlan = mock.MagicMock(
return_value=["dummy_plan"]) # always arrive for now
def test_initialize(self):
# initialize is run in setUp, but robot.base.get_location has been replaced, so running it again
self.robot.base.get_location = lambda: FrameStamped(
kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(3.5, 3.5, 0)),
"/map")
self.tour_guide.initialize()
self.assertListEqual(self.tour_guide._passed_room_ids,
[self._kitchen.id])
self.assertListEqual(self.tour_guide._passed_furniture_ids,
[self._cabinet.id])
self.assertListEqual(sorted(self.tour_guide._furniture_entities),
sorted([self._cabinet, self._bookcase]))
self.assertListEqual(sorted(self.tour_guide._room_entities),
sorted([self._kitchen, self._bedroom]))
self.assertDictEqual(self.tour_guide._furniture_entities_room, {
self._kitchen: [self._cabinet],
self._bedroom: [self._bookcase]
})
def _resolve(self):
"""
:return: Where can an object be placed
:returns: FrameStamped
"""
place_location = self.place_location_designator.resolve()
place_frame = FrameStamped(frame=place_location._pose, frame_id="/map")
# points_of_interest = []
if self._area:
vectors_of_interest = self.determine_points_of_interest_with_area(place_location, self._area)
else:
vectors_of_interest = self.determine_points_of_interest(place_frame.frame, z_max=place_location.shape.z_max,
convex_hull=place_location.shape.convex_hull)
assert all(isinstance(v, FrameStamped) for v in vectors_of_interest)
open_POIs = filter(self.is_poi_occupied, vectors_of_interest)
return self.select_best_feasible_poi(open_POIs)
def move_table(userdata=None):
""" 'Locks' a locking designator """
# For now, don't do anything
return "done"
# Move away the cabinet
robot.ed.update_entity(id="cabinet",
frame_stamped=FrameStamped(frame=kdl.Frame(kdl.Rotation(),
kdl.Vector(12.0, 0, 0)),
frame_id="map"))
# Determine where to perform the challenge
robot_pose = robot.base.get_location()
ENTITY_POSES.sort(key=lambda tup: (tup[0].frame.p - robot_pose.frame.p).Norm())
# Update the world model
robot.ed.update_entity(id=CABINET, frame_stamped=ENTITY_POSES[0][0])
robot.ed.update_entity(id=TABLE, frame_stamped=ENTITY_POSES[0][1])
return "done"
def _resolve(self):
"""
:return: Where can an object be placed
:returns: FrameStamped
"""
place_location = self.place_location_designator.resolve()
place_frame = FrameStamped(frame=place_location._pose, frame_id="/map")
# points_of_interest = []
if self._area:
vectors_of_interest = self._determine_points_of_interest_with_area(place_location, self._area)
else:
vectors_of_interest = self._determine_points_of_interest(place_frame.frame, z_max=place_location.shape.z_max,
convex_hull=place_location.shape.convex_hull)
assert all(isinstance(v, FrameStamped) for v in vectors_of_interest)
open_POIs = filter(lambda pose: self._is_poi_unoccupied(pose, place_location), vectors_of_interest)
base_pose = self.robot.base.get_location()
arm = self.arm_designator.resolve()
open_POIs_dist = [(poi, self._distance_to_poi_area_heuristic(poi, base_pose, arm)) for poi in open_POIs]
# We don't care about small differences
open_POIs_dist.sort(key=lambda tup:tup[1])
open_POIs_dist = [f for f in open_POIs_dist if (f[1] - open_POIs_dist[0][1]) < self._nav_threshold]
open_POIs_dist.sort(key=lambda tup: tup[0].edge_score, reverse=True) # sorts in place
for poi in open_POIs_dist:
if self._distance_to_poi_area(poi[0], arm):
selection = self._create_selection_marker(poi[0])
self.marker_pub.publish(MarkerArray([selection]))
return poi[0]
rospy.logerr("Could not find an empty spot")
return None
def _resolve(self):
place_location = self.place_location_designator.resolve()
place_frame = FrameStamped(frame=place_location._pose, frame_id="/map")
# points_of_interest = []
if self._area:
vectors_of_interest = self.determine_points_of_interest_with_area(
place_location, self._area)
else:
vectors_of_interest = self.determine_points_of_interest(
place_frame.frame,
z_max=place_location.shape.z_max,
convex_hull=place_location.shape.convex_hull)
assert all(isinstance(v, FrameStamped) for v in vectors_of_interest)
def is_poi_occupied(frame_stamped):
entities_at_poi = self.robot.ed.get_entities(
center_point=frame_stamped.extractVectorStamped(),
radius=self._spacing)
return not any(entities_at_poi)
open_POIs = filter(is_poi_occupied, vectors_of_interest)
def distance_to_poi_area(frame_stamped):
# Derived from navigate_to_place
radius = math.hypot(self.robot.grasp_offset.x,
self.robot.grasp_offset.y)
x = frame_stamped.frame.p.x()
y = frame_stamped.frame.p.y()
ro = "(x-%f)^2+(y-%f)^2 < %f^2" % (x, y, radius + 0.075)
ri = "(x-%f)^2+(y-%f)^2 > %f^2" % (x, y, radius - 0.075)
pos_constraint = PositionConstraint(constraint=ri + " and " + ro,
frame=frame_stamped.frame_id)
plan_to_poi = self.robot.base.global_planner.getPlan(
pos_constraint)
if plan_to_poi:
distance = len(plan_to_poi)
# print "Distance to {fs}: {dist}".format(dist=distance, fs=frame_stamped.frame.p)
else:
distance = None
return distance
# List with tuples containing both the POI and the distance the
# robot needs to travel in order to place there
open_POIs_dist = [(poi, distance_to_poi_area(poi))
for poi in open_POIs]
# Feasible POIS: discard
feasible_POIs = []
for tup in open_POIs_dist:
if tup[1]:
feasible_POIs.append(tup)
if any(feasible_POIs):
feasible_POIs.sort(key=lambda tup: tup[1]) # sorts in place
# We don't care about small differences
nav_threshold = 0.5 / 0.05 # Distance (0.5 m) divided by resolution (0.05)
feasible_POIs = [
f for f in feasible_POIs
if (f[1] - feasible_POIs[0][1]) < nav_threshold
]
feasible_POIs.sort(key=lambda tup: tup[0].edge_score, reverse=True)
best_poi = feasible_POIs[0][0] # Get the POI of the best match
selection = self.create_selection_marker(best_poi)
self.marker_pub.publish(MarkerArray([selection]))
return best_poi
else:
rospy.logerr("Could not find an empty spot")
return None
# ROS
import PyKDL as kdl
# TU/e Robotics
from robot_skills.util.kdl_conversions import FrameStamped
from robocup_knowledge import knowledge_loader
from collections import namedtuple
# Common knowledge
common = knowledge_loader.load_knowledge("common")
# Detection
cabinet_amcl = ["cabinet", "storage_shelf"]
cabinet_poses = [
FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0),
kdl.Vector(0.144, 3.274, 0.0)),
frame_id="map"),
FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, math.pi),
kdl.Vector(4.8, 2.5, 0.0)),
frame_id="map")
]
object_shelves = ["shelf3", "shelf4", "shelf5"]
object_types = [obj["name"] for obj in common.objects]
# Placing
default_place_entity = cabinet_amcl
"""
EntityConfiguration defines a name, an pose estimate for the Entity and which volumes of that entity to use for
manipulation
"""
EntityConfiguration = namedtuple(
# ROS
import PyKDL as kdl
# TU/e Robotics
from robot_skills.util.kdl_conversions import FrameStamped
from robocup_knowledge import knowledge_loader
# Common knowledge
common = knowledge_loader.load_knowledge("common")
# Detection
cabinet_amcl = ["kitchen_shelf", "kitchen_shelf"]
cabinet_poses = [
FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, -0.0, 0.1292036732),
kdl.Vector(2.39, -4.125, 0)),
frame_id="map"),
FrameStamped(frame=kdl.Frame(kdl.Rotation(), kdl.Vector(-2.27, -7.55,
0.0)),
frame_id="map")
]
object_shelves = ["shelf2", "shelf3", "shelf4", "shelf5"]
object_types = [obj["name"] for obj in common.objects]
# Grasping
grasp_surface = ["kitchen_table", "bistro_table"]
room = ["kitchen", "balcony"]
#
# Placing
default_place_entity = cabinet_amcl
default_place_area = ["shelf3", "shelf3"]
import robot_smach_states.util.designators as ds
from robot_smach_states import Grab
if __name__ == "__main__":
parser = argparse.ArgumentParser(description="Put an imaginary object in the world model and grasp it using the "
"'Grab' smach state")
parser.add_argument("x", type=float, help="x-coordinate (in map) of the imaginary object")
parser.add_argument("y", type=float, help="y-coordinate (in map) of the imaginary object")
parser.add_argument("z", type=float, help="z-coordinate (in map) of the imaginary object")
parser.add_argument("--robot", default="hero", help="Robot name (amigo, hero, sergio)")
args = parser.parse_args()
rospy.init_node("test_grasping")
robot = get_robot(args.robot)
entity_id = "test_item"
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 0.0), kdl.Vector(args.x, args.y, args.z)),
frame_id="/map")
robot.ed.update_entity(id=entity_id, frame_stamped=pose)
item = ds.EdEntityDesignator(robot, id=entity_id)
arm = ds.UnoccupiedArmDesignator(robot, {})
grab_state = Grab(robot, item, arm)
grab_state.execute()
def pose(self):
""" Returns the pose of the Entity as a FrameStamped"""
return FrameStamped(frame=self._pose, frame_id=self.frame_id)
def get_grasp_pose(self, entity, arm):
""" Computes the most suitable grasp pose to grasp the specified entity with the specified arm
:param entity: entity to grasp
:param arm: arm to use
:return FrameStamped with grasp pose in map frame
"""
candidates = []
starttime = rospy.Time.now()
# ToDo: divide into functions
''' Create a grasp vector for every side of the convex hull '''
''' First: check if container actually has a convex hull '''
if entity.shape is None:
rospy.logerr(
"Entity {0} has no shape. We need to do something with this".
format(entity.id))
return False
''' Second: turn points into KDL objects and offset chull to get it in map frame '''
center_pose = entity._pose #TODO: Access to private member
# chull_obj = [point_msg_to_kdl_vector(p) for p in entity.shape._convex_hull] # convex hull in object frame
# chull = offsetConvexHull(chull_obj, center_pose) # convex hull in map frame
chull = offsetConvexHull(entity.shape.convex_hull,
center_pose) # convex hull in map frame
# import ipdb;ipdb.set_trace()
''' Get robot pose as a kdl frame (is required later on) '''
robot_frame = self._robot.base.get_location()
robot_frame_inv = robot_frame.frame.Inverse()
''' Loop over lines of chull '''
for i in xrange(len(chull)):
j = (i + 1) % len(chull)
dx = chull[j].x() - chull[i].x()
dy = chull[j].y() - chull[i].y()
if math.hypot(dx, dy) < 0.0001:
# Points are probably too close to get a decent angle estimate
continue
yaw = math.atan2(dx, -dy)
''' Filter on object width '''
# Normalize
n = math.hypot(dx, dy) # Norm
vx = dx / n
vy = dy / n
# Loop over all points
wmin = 0
wmax = 0
for c in chull:
# Compute vector
tx = c.x() - chull[i].x()
ty = c.y() - chull[i].y()
# Perform projection
# ToDo: continue when offset in x direction is too large
offset = tx * vx + ty * vy
# Update min and max
wmin = min(wmin, offset)
wmax = max(wmax, offset)
width = wmax - wmin
if width > self._width_treshold:
continue
else:
score = 1.0
''' Compute candidate vector '''
# Middle between point i and point j
# x = 0.5 * ( chull[i].x() + chull[j].x() )
# y = 0.5 * ( chull[i].y() + chull[j].y() )
# cvec = kdl.Frame(kdl.Rotation.RPY(0, 0, yaw),
# kdl.Vector(x, y, entity.pose.position.z))
# Divide width in two
# * kdl.Vector(0.5 * (wmin+wmax, 0, 0)
tvec = FrameStamped(kdl.Frame(
kdl.Rotation.RPY(0, 0, yaw),
kdl.Vector(chull[i].x(), chull[i].y(),
entity.pose.frame.p.z())),
frame_id="/map") # Helper frame
cvec = FrameStamped(kdl.Frame(
kdl.Rotation.RPY(0, 0, yaw),
tvec.frame * kdl.Vector(0, -0.5 * (wmin + wmax), 0)),
frame_id="/map")
''' Optimize over yaw offset w.r.t. robot '''
# robot_in_map * entity_in_robot = entity_in_map
# --> entity_in_robot = robot_in_map^-1 * entity_in_map
gvec = robot_frame_inv * cvec.frame
(R, P, Y) = gvec.M.GetRPY()
rscore = 1.0 - (abs(Y) / math.pi)
score = min(score, rscore)
candidates.append({'vector': cvec, 'score': score})
candidates = sorted(candidates,
key=lambda candidate: candidate['score'],
reverse=True)
self.visualize(candidates)
rospy.loginfo("GPD took %f seconds" %
(rospy.Time.now() - starttime).to_sec())
return candidates[0]['vector']
def execute(self, userdata=None):
point_entity = self.point_entity_designator.resolve()
if not point_entity:
rospy.logerr("Could not resolve grab_entity")
return "failed"
arm = self.arm_designator.resolve()
if not arm:
rospy.logerr("Could not resolve arm")
return "failed"
goal_map = VectorStamped(0, 0, 0, frame_id=point_entity.id)
try:
# Transform to base link frame
goal_bl = goal_map.projectToFrame(self.robot.base_link_frame, tf_listener=self.robot.tf_listener)
if goal_bl is None:
rospy.logerr('Transformation of goal to base failed')
return 'failed'
except tf2_ros.TransformException as tfe:
rospy.logerr('Transformation of goal to base failed: {0}'.format(tfe))
return 'failed'
# Make sure the torso and the arm are done
self.robot.torso.wait_for_motion_done(cancel=True)
arm.wait_for_motion_done(cancel=True)
# This is needed because the head is not entirely still when the
# look_at_point function finishes
rospy.sleep(rospy.Duration(0.5))
# Resolve the entity again because we want the latest pose
updated_point_entity = self.point_entity_designator.resolve()
rospy.loginfo("ID to update: {0}".format(point_entity.id))
if not updated_point_entity:
rospy.logerr("Could not resolve the updated grab_entity, "
"this should not happen [CHECK WHY THIS IS HAPPENING]")
point_entity = self.associate(original_entity=point_entity)
else:
rospy.loginfo("Updated pose of entity (dx, dy, dz) : (%f, %f, %f)" %
(updated_point_entity.pose.frame.p.x() - point_entity.pose.frame.p.x(),
updated_point_entity.pose.frame.p.y() - point_entity.pose.frame.p.y(),
updated_point_entity.pose.frame.p.z() - point_entity.pose.frame.p.z()))
point_entity = updated_point_entity
# - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Grasp point determination
goal_bl = self._get_pointing_pose(point_entity)
# Grasp
rospy.loginfo('Start pointing')
for x_offset in [-0.15, 0.0]: # Hack because Hero does not pre-grasp reliably
_goal_bl = FrameStamped(goal_bl.frame * kdl.Frame(kdl.Vector(x_offset, 0.0, 0.0)), goal_bl.frame_id)
if not arm.send_goal(_goal_bl, timeout=20, pre_grasp=False, allowed_touch_objects=[point_entity.id]):
self.robot.speech.speak('I am sorry but I cannot move my arm to the object position', block=False)
rospy.logerr('Grasp failed')
arm.reset()
return 'failed'
# Retract
rospy.loginfo('Start retracting')
roll = 0.0
goal_bl.frame.p.x(goal_bl.frame.p.x() - 0.1) # Retract 10 cm
goal_bl.frame.p.z(goal_bl.frame.p.z() + 0.05) # Go 5 cm higher
goal_bl.frame.M = kdl.Rotation.RPY(roll, 0.0, 0.0) # Update the roll
rospy.loginfo("Start retract")
if not arm.send_goal(goal_bl, timeout=0.0, allowed_touch_objects=[point_entity.id]):
rospy.logerr('Failed retract')
arm.wait_for_motion_done()
self.robot.base.force_drive(-0.125, 0, 0, 2.0)
# Close gripper
arm.wait_for_motion_done(cancel=True)
# Carrying pose
arm.send_joint_goal('carrying_pose', timeout=0.0)
# Reset head
self.robot.head.cancel_goal()
return "succeeded"
def main():
rospy.init_node("demo")
random.seed()
skip = rospy.get_param('~skip', False)
restart = rospy.get_param('~restart', False)
robot_name = rospy.get_param('~robot_name')
no_of_tasks = rospy.get_param('~number_of_tasks', 0)
time_limit = rospy.get_param('~time_limit', 0)
if no_of_tasks == 0:
no_of_tasks = 999
rospy.loginfo("[DEMO] Parameters:")
rospy.loginfo("[DEMO] robot_name = {}".format(robot_name))
if skip:
rospy.loginfo("[DEMO] skip = {}".format(skip))
if no_of_tasks:
rospy.loginfo("[DEMO] number_of_tasks = {}".format(no_of_tasks))
if restart:
rospy.loginfo("[DEMO] running a restart")
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
if robot_name == 'amigo':
from robot_skills.amigo import Amigo as Robot
elif robot_name == 'sergio':
from robot_skills.sergio import Sergio as Robot
elif robot_name == 'hero':
from robot_skills.hero import Hero as Robot
else:
raise ValueError('unknown robot')
robot = Robot()
action_client = ActionClient(robot.robot_name)
knowledge = load_knowledge('challenge_demo')
no_of_tasks_performed = 0
user_instruction = "What can I do for you?"
report = ""
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
# Start
if not skip and not restart:
# Wait for door, enter arena
s = StartChallengeRobust(robot, knowledge.initial_pose)
s.execute()
# Move to the start location
robot.speech.speak("Let's see if my operator has a task for me!",
block=False)
if restart:
robot.speech.speak(
"Performing a restart. So sorry about that last time!",
block=False)
# - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - -
finished = False
start_time = rospy.get_time()
trigger = WaitForTrigger(robot, ["gpsr"], "/" + robot_name + "/trigger")
while True:
# Navigate to the GPSR meeting point
if not skip:
robot.speech.speak("Moving to the meeting point.", block=False)
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot,
id=knowledge.starting_pose),
radius=0.3)
nwc.execute()
# Report to the user and ask for a new task
# Report to the user
robot.head.look_at_standing_person()
robot.speech.speak(report, block=True)
timeout_count = 0
while True:
rospy.loginfo("Waiting for trigger")
trigger.execute()
base_loc = robot.base.get_location()
base_pose = base_loc.frame
print base_pose
location_id = "starting_point"
robot.ed.update_entity(id=location_id,
frame_stamped=FrameStamped(
base_pose, "/map"),
type="waypoint")
robot.head.look_at_standing_person()
robot.speech.speak(user_instruction, block=True)
# Listen for the new task
while True:
try:
sentence, semantics = robot.hmi.query(
description="",
grammar=knowledge.grammar,
target=knowledge.grammar_target)
timeout_count = 0
break
except hmi.TimeoutException:
robot.speech.speak(
random.sample(knowledge.not_understood_sentences,
1)[0])
if timeout_count >= 3:
robot.hmi.restart_dragonfly()
timeout_count = 0
rospy.logwarn("[GPSR] Dragonfly restart")
else:
timeout_count += 1
rospy.logwarn(
"[GPSR] Timeout_count: {}".format(timeout_count))
# check if we have heard this correctly
robot.speech.speak('I heard %s, is this correct?' % sentence)
try:
if 'no' == robot.hmi.query('', 'T -> yes | no', 'T').sentence:
robot.speech.speak('Sorry')
continue
except hmi.TimeoutException:
# robot did not hear the confirmation, so lets assume its correct
break
break
# Dump the output json object to a string
task_specification = json.dumps(semantics)
# Send the task specification to the action server
task_result = action_client.send_task(task_specification)
print task_result.missing_field
# # Ask for missing information
# while task_result.missing_field:
# request_missing_field(knowledge.task_result.missing_field)
# task_result = action_client.send_task(task_specification)
# Write a report to bring to the operator
report = task_result_to_report(task_result)
robot.lights.set_color(0, 0, 1) # be sure lights are blue
robot.head.look_at_standing_person()
for arm in robot.arms.itervalues():
arm.reset()
arm.send_gripper_goal('close', 0.0)
robot.torso.reset()
rospy.loginfo("Driving back to the starting point")
nwc = NavigateToWaypoint(robot=robot,
waypoint_designator=EntityByIdDesignator(
robot=robot, id=location_id),
radius=0.3)
nwc.execute()
smach.StateMachine.add('PREPARE_POINT', states.PrepareEdGrasp(robot, arm, item),
transitions={'succeeded': 'POINT',
'failed': 'RESET_FAILURE'})
smach.StateMachine.add('POINT', PointAt(robot, arm, item),
transitions={'succeeded': 'done',
'failed': 'RESET_FAILURE'})
smach.StateMachine.add("RESET_FAILURE", states.ResetOnFailure(robot, arm),
transitions={'done': 'failed'})
if __name__ == "__main__":
rospy.init_node("test_grasping")
robot = get_robot_from_argv(index=1)
entity_id = "test_item"
pose = FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0, 0, -1.57), kdl.Vector(2.6, -0.95, 0.8)), frame_id="/map")
robot.ed.update_entity(id=entity_id, frame_stamped=pose)
item = ds.EdEntityDesignator(robot, id=entity_id)
arm = ds.UnoccupiedArmDesignator(robot, {})
grab_state = IdentifyObject(robot, item, arm)
grab_state.execute()
# ROS
import PyKDL as kdl
# TU/e Robotics
from robot_skills.util.kdl_conversions import FrameStamped
from robocup_knowledge import knowledge_loader
# Common knowledge
common = knowledge_loader.load_knowledge("common")
# Detection
cabinet_amcl = "bookcase"
cabinet_poses = [
FrameStamped(frame=kdl.Frame(kdl.Rotation(), kdl.Vector(0.144, 3.574,
0.0)),
frame_id="map"),
FrameStamped(frame=kdl.Frame(kdl.Rotation.RPY(0.0, 0.0, 1.570796),
kdl.Vector(2.271, -1.258, 0.0)),
frame_id="map")
]
object_shelves = ["shelf2", "shelf3", "shelf4", "shelf5"]
object_types = [obj["name"] for obj in common.objects]
# Grasping
grasp_surface = "cabinet"
room = "dining_room"
# Placing
default_place_entity = cabinet_amcl
default_place_area = "shelf2"
def test_initialize2(self):
self.robot.base.get_location = lambda: FrameStamped(
kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(20, 20, 2)), "/map"
)
self.tour_guide.initialize()
self.assertListEqual(self.tour_guide._passed_room_ids, [])
def test_describe_near_objects2(self):
self.robot.base.get_location = lambda: FrameStamped(
kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(6, 4, 0)), "/map")
self.assertEqual("We now enter the bedroom",
self.tour_guide.describe_near_objects())
def setUp(self):
self.robot.base.get_location = lambda: FrameStamped(
kdl.Frame(kdl.Rotation().Identity(), kdl.Vector(-1, -1, 0)), "/map"
)
self.tour_guide.initialize()