class MoveFloorButton:
def __init__(self):
self.tl = TransformListener()
self.perception_srv = rospy.Service("/clickable_world/detect_empty_floor",
PerceiveButtons,
self.do_perception)
self.percept_pub = rospy.Publisher("/clickable_world/floor_button_vis",
Marker)
self.goal_pub = rospy.Publisher("/clickable_world/move_floor_goal", PoseStamped)
self.action_srv = rospy.Service("/clickable_world/move_empty_floor",
ButtonAction,
self.do_action)
self.floor_seg_srv = rospy.ServiceProxy("/move_floor_detect",
SegmentFloor)
self.floor_move_client = actionlib.SimpleActionClient("move_base", MoveBaseAction)
rospy.loginfo("[move_floor_button] MoveFloorButton loaded.")
def do_perception(self, req):
# segment surfaces
rospy.loginfo("[move_floor_button] Segmenting floor")
self.surfaces = self.floor_seg_srv().surfaces
for i in range(len(self.surfaces)):
self.surfaces[i].color.r = 0
self.surfaces[i].color.g = 0
self.surfaces[i].color.b = 256
self.surfaces[i].color.a = 256
if len(self.surfaces) != 0:
self.percept_pub.publish(self.surfaces[0])
resp = PerceiveButtonsResponse()
resp.buttons = self.surfaces
return resp
def do_action(self, req):
rospy.loginfo("[move_floor_button] MoveFloorButton clicked!")
# put 3d pt in base frame
req.pixel3d.header.stamp = rospy.Time(0)
move_pt = self.tl.transformPoint("/base_link", req.pixel3d)
floor_move_goal = MoveBaseGoal()
floor_move_goal.target_pose.header.frame_id = move_pt.header.frame_id
floor_move_goal.target_pose.pose.position = move_pt.point
quat = tf_trans.quaternion_from_euler(0, 0, np.arctan2(move_pt.point.y, move_pt.point.x))
floor_move_goal.target_pose.pose.orientation = Quaternion(quat[0], quat[1], quat[2], quat[3])
floor_move_goal.target_pose.header.frame_id = "/base_link"
floor_move_goal.target_pose.header.stamp = rospy.Time.now()
try:
self.goal_pub.publish(floor_move_goal.target_pose)
self.floor_move_client.send_goal(floor_move_goal)
self.floor_move_client.wait_for_result()
cur_pose = self.floor_move_client.get_result()
except rospy.ROSInterruptException:
print "[move_floor_button] Move floor failed"
return ButtonActionResponse()
def filter_by_distance(features, max_distance): # type: (List[Feature], float) -> List[Feature]
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
filtered = []
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
if point.point.x <= max_distance:
filtered.append(feature)
return filtered
def feature_depths(features):
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
depths = []
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()),
rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
depths.append(np.linalg.norm([point.point.x, point.point.y, point.point.z]))
return depths
def select_center(features): # type: (List[Feature]) -> Feature
cam_pixel_to_point = rospy.ServiceProxy('cam_pixel_to_point', CamPixelToPoint)
tf_listener = TransformListener()
angles = []
center_ray = np.array([1.0, 0.0, 0.0])
for feature in features:
cam_pos = Vector3()
cam_pos.x, cam_pos.y = feature.centroid[0], feature.centroid[1]
point = cam_pixel_to_point(cam_pos).point # type: PointStamped
tf_listener.waitForTransform('camera_link', point.header.frame_id, rospy.Time(rospy.get_time()), rospy.Duration(1))
point = tf_listener.transformPoint('camera_link', point)
ray = np.array([point.point.x, point.point.y, point.point.z])
ray /= np.linalg.norm(ray)
angle = np.arccos(np.dot(center_ray, ray))
angles.append(abs(angle))
return features[np.argmin(angles)]
class ClosestPersonDetector(object):
"""
Merges the outputs from the leg_tracker package with the face_detector
package from WillowGarage. Then from the combined data, it publishes the
closest person.
"""
def __init__(self):
# Internal parameters
self.publish_rate = rospy.get_param("~publish_rate", 10.0)
self.listener = TransformListener()
self.desired_pose_frame = rospy.get_param("~desired_pose_frame",
"base_link")
self.position_match_threshold = rospy.get_param(
"~position_match_threshold", 1.0)
# Variables to keep track of state
self.closest_person = None
self.leg_detections = []
self.closest_person_lock = Lock()
self.leg_detections_lock = Lock()
# Internal helpers
self.person_face_distance_func = lambda A, B: np.sqrt(
(A.pose.position.x - B.pos.x)**2 +
(A.pose.position.y - B.pos.y)**2 +
(1.2 - B.pos.z)**2 # A person's face is about 4ft from the floor
)
self.leg_detection_is_closest_face = lambda detected_person: (
self.closest_person is not None and self.closest_person.
detection_context.pose_source == DetectionContext.POSE_FROM_FACE
and self.closest_person.id == detected_person.id)
# The subscribers and publishers
self.face_sub = rospy.Subscriber(
"face_detector/people_tracker_measurements_array",
PositionMeasurementArray, self.face_callback)
self.leg_sub = rospy.Subscriber("people_tracked", PersonArray,
self.leg_callback)
self.closest_person_pub = rospy.Publisher('~closest_person',
Person,
queue_size=10)
# Debug
self.debug_enabled = rospy.get_param("~debug", False)
if self.debug_enabled:
self.debug_pub1 = rospy.Publisher("~debug/1", Marker, queue_size=1)
self.debug_pub2 = rospy.Publisher("~debug/2", Marker, queue_size=1)
def leg_callback(self, msg):
closest_distance = np.inf
closest_person = None
# Iterate over the people and find the closest
with self.leg_detections_lock:
self.leg_detections = []
for detected_person in msg.people:
detected_pose = PoseStamped(header=msg.header,
pose=detected_person.pose)
try:
detected_pose = self.listener.transformPose(
self.desired_pose_frame, detected_pose)
except ExtrapolationException as e:
continue
# Add the person to the list of leg_detections
detected_person = Person(header=detected_pose.header,
id=str(detected_person.id),
pose=detected_pose.pose)
detected_person.detection_context.pose_source = DetectionContext.POSE_FROM_LEGS
with self.leg_detections_lock:
self.leg_detections.append(detected_person)
# If this is the closest person, save them. However, if this person
# has the same ID as the person with a face, prefer that
person_distance = np.sqrt(detected_pose.pose.position.x**2 +
detected_pose.pose.position.y**2)
if self.leg_detection_is_closest_face(
detected_person) or person_distance < closest_distance:
closest_distance = person_distance
closest_person = detected_person
# If the leg detection is the closest face, then disable new
# closest detections associations
if self.leg_detection_is_closest_face(detected_person):
closest_distance = -np.inf
# Debug
if self.debug_enabled and closest_person is not None:
marker = Marker(header=closest_person.header,
ns="debug",
id=1,
type=Marker.SPHERE,
action=Marker.ADD)
marker.pose = closest_person.pose
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 1.0
marker.color.g = 0.0
marker.color.b = 0.0
self.debug_pub2.publish(marker)
# Acquire a lock to the people and update the closest person's position
# We don't want to be staring at feet...
with self.closest_person_lock:
if closest_person is None:
self.closest_person = None
elif self.closest_person is None or self.closest_person.id != closest_person.id:
self.closest_person = closest_person
else: # self.closest_person.id == closest_person.id
self.closest_person.pose.position.x = closest_person.pose.position.x
self.closest_person.pose.position.y = closest_person.pose.position.y
def face_callback(self, msg):
# Iterate through the message and find the closest face
closest_face = None
closest_distance = np.inf
for face in msg.people:
pos = PointStamped(header=face.header, point=face.pos)
try:
pos = self.listener.transformPoint(self.desired_pose_frame,
pos)
except ExtrapolationException as e:
continue
# Find the closest face
distance = np.sqrt(pos.point.x**2 + pos.point.y**2 +
pos.point.z**2)
if distance < closest_distance:
closest_distance = distance
closest_face = face
closest_face.header = pos.header
closest_face.pos = pos.point
# Debug
if self.debug_enabled and closest_face is not None:
marker = Marker(header=closest_face.header,
ns="debug",
id=0,
type=Marker.SPHERE,
action=Marker.ADD)
marker.pose.position = closest_face.pos
marker.pose.orientation.w = 1.0
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.5
marker.color.a = 1.0
marker.color.r = 0.0
marker.color.g = 1.0
marker.color.b = 0.0
self.debug_pub1.publish(marker)
# Now associate the closest face to the leg detection that it is closest
# to
closest_person = None
with self.leg_detections_lock:
for detected_person in self.leg_detections:
if closest_face is not None and self.person_face_distance_func(
detected_person,
closest_face) < self.position_match_threshold:
closest_person = detected_person
closest_person.pose.position = closest_face.pos
break
# Now check the distance between the closest face and the closest leg.
# If the distance exceeds the threshold, then don't associate the face
# with the leg
with self.closest_person_lock:
if closest_person is None or self.closest_person is None:
pass
else:
self.closest_person = closest_person
self.closest_person.detection_context.pose_source = DetectionContext.POSE_FROM_FACE
def spin(self):
"""
Run the detector
"""
# Publish the detected closest person at the desired frequency
rate = rospy.Rate(self.publish_rate)
while not rospy.is_shutdown():
# Otherwise, check to see if we should publish the latest detection
with self.closest_person_lock:
if self.closest_person is not None:
self.closest_person_pub.publish(self.closest_person)
# Sleep
rate.sleep()
class GraspObjButton:
def __init__(self):
self.arm = rospy.get_param("arm", "r")
self.tl = TransformListener()
self.perception_srv = rospy.Service("/clickable_world/detect_objects",
PerceiveButtons,
self.do_perception)
self.action_srv = rospy.Service("/clickable_world/grasp_object",
ButtonAction,
self.do_action)
self.obj_seg_srv = rospy.ServiceProxy("/object_button_detect",
ObjectButtonDetect)
self.grasp_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp', OverheadGraspAction)
self.grasp_client.wait_for_server()
self.grasp_setup_client = actionlib.SimpleActionClient(self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction)
self.grasp_setup_client.wait_for_server()
def do_perception(self, req):
rospy.loginfo("[grasp_obj_button] Perceiving object buttons...")
# perceive object on table
self.objects = self.obj_seg_srv().objects
for i in range(len(self.objects)):
self.objects[i].color.r = 256
self.objects[i].color.g = 0
self.objects[i].color.b = 0
self.objects[i].color.a = 256
# return object buttons
resp = PerceiveButtonsResponse()
resp.buttons = self.objects
return resp
def do_action(self, req):
rospy.loginfo("[grasp_obj_button] Table clicked, grasping object...")
# put 3d pt in grasping frame
req.pixel3d.header.stamp = rospy.Time(0)
grasp_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d)
# move to grasp setup position
setup_goal = OverheadGraspSetupGoal()
setup_goal.disable_head = False
setup_goal.open_gripper = True
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
############################################################
# Creating grasp goal
grasp_goal = OverheadGraspGoal()
grasp_goal.is_grasp = True
grasp_goal.disable_head = False
grasp_goal.disable_coll = False
grasp_goal.grasp_type=OverheadGraspGoal.VISION_GRASP
grasp_goal.x = grasp_pt.point.x
grasp_goal.y = grasp_pt.point.y
grasp_goal.behavior_name = "overhead_grasp"
grasp_goal.sig_level = 0.999
############################################################
# grasp object
self.grasp_client.send_goal(grasp_goal)
self.grasp_client.wait_for_result()
result = self.grasp_client.get_result()
rospy.loginfo("[grasp_obj_button] Grasp result: %s" % result)
obj_in_hand = result.grasp_result == "Object grasped"
# go back to grasp setup
setup_goal.open_gripper = False
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
return ButtonActionResponse()
class NavHeadController:
def __init__(self):
self.has_goal = False
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient(
"head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.plan_sub = rospy.Subscriber(
"move_base/TrajectoryPlannerROS/local_plan", Path,
self.planCallback)
self.stat_sub = rospy.Subscriber("move_base/status", GoalStatusArray,
self.statCallback)
def statCallback(self, msg):
goal = False
for status in msg.status_list:
if status.status == GoalStatus.ACTIVE:
goal = True
break
self.has_goal = goal
def planCallback(self, msg):
# get the goal
pose_stamped = msg.poses[-1]
pose = pose_stamped.pose
# look ahead one meter
R = quaternion_matrix([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
point = [1, 0, 0, 1]
M = R * point
p = PointStamped()
p.header.frame_id = pose_stamped.header.frame_id
p.header.stamp = rospy.Time(0)
p.point.x += pose_stamped.pose.position.x + M[0, 0]
p.point.y += pose_stamped.pose.position.y + M[1, 0]
p.point.z += pose_stamped.pose.position.z + M[2, 0]
# transform to base_link
p = self.listener.transformPoint("base_link", p)
# update
with self.mutex:
if p.point.x < 0.65:
self.x = 0.65
else:
self.x = p.point.x
if p.point.y > 0.5:
self.y = 0.5
elif p.point.y < -0.5:
self.y = -0.5
else:
self.y = p.point.y
def loop(self):
while not rospy.is_shutdown():
if self.has_goal:
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "base_link"
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.0
goal.min_duration = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.75
self.client.send_goal(goal)
self.client.wait_for_result()
else:
rospy.sleep(1.0)
class PlaceObject:
def __init__(self):
self.arm = rospy.get_param("arm", "r")
self.tl = TransformListener()
self.perception_srv = rospy.Service(
"/clickable_world/place_table_perceive", PerceiveButtons, self.do_perception
)
self.action_srv = rospy.Service("/clickable_world/place_object", ButtonAction, self.do_action)
self.pc_capture_srv = rospy.ServiceProxy("/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty)
self.table_seg_srv = rospy.ServiceProxy("/table_detection/segment_surfaces", SegmentSurfaces)
self.grasp_client = actionlib.SimpleActionClient(self.arm + "_overhead_grasp", OverheadGraspAction)
self.grasp_client.wait_for_server()
self.grasp_setup_client = actionlib.SimpleActionClient(
self.arm + "_overhead_grasp_setup", OverheadGraspSetupAction
)
self.grasp_setup_client.wait_for_server()
def do_perception(self, req):
rospy.loginfo("[place_object] Perceiving table...")
# capture a few clouds
rate = rospy.Rate(5)
for i in range(5):
self.pc_capture_srv()
rate.sleep()
# segment surfaces
self.surfaces = self.table_seg_srv().surfaces
for i in range(len(self.surfaces)):
self.surfaces[i].color.r = 256
self.surfaces[i].color.g = 0
self.surfaces[i].color.b = 256
self.surfaces[i].color.a = 256
resp = PerceiveButtonsResponse()
resp.buttons = self.surfaces
return resp
def do_action(self, req):
rospy.loginfo("[place_object] Table clicked, placing object...")
# put 3d pt in grasping frame
req.pixel3d.header.stamp = rospy.Time(0)
place_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d)
# move to place setup position
setup_goal = OverheadGraspSetupGoal()
setup_goal.disable_head = False
setup_goal.open_gripper = False
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
############################################################
# Creating place goal
grasp_goal = OverheadGraspGoal()
grasp_goal.is_grasp = False
grasp_goal.disable_head = False
grasp_goal.disable_coll = False
grasp_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
grasp_goal.x = place_pt.point.x
grasp_goal.y = place_pt.point.y
grasp_goal.behavior_name = "overhead_grasp"
grasp_goal.sig_level = 0.999
############################################################
# place object
self.grasp_client.send_goal(grasp_goal)
self.grasp_client.wait_for_result()
result = self.grasp_client.get_result()
rospy.loginfo("[place_object] Place result: %s" % result)
obj_in_hand = result.grasp_result == "Object placed"
# go back to grasp setup
setup_goal.open_gripper = True
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
return ButtonActionResponse()
class NavHeadController:
def __init__(self):
self.last_vel_time = rospy.Time(0)
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path, self.planCallback)
self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist, self.velCallback)
def velCallback(self, msg):
vel = msg.linear.x * msg.linear.x
vel += msg.linear.y * msg.linear.y
vel += msg.angular.z * msg.angular.z
if vel > 0.0:
self.last_vel_time = rospy.Time.now()
def planCallback(self, msg):
# get the goal
pose_stamped = msg.poses[-1]
pose = pose_stamped.pose
# look ahead one meter
R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
point = [1, 0, 0, 1]
M = R*point
p = PointStamped()
p.header.frame_id = pose_stamped.header.frame_id
p.header.stamp = rospy.Time(0)
p.point.x += pose_stamped.pose.position.x + M[0,0]
p.point.y += pose_stamped.pose.position.y + M[1,0]
p.point.z += pose_stamped.pose.position.z + M[2,0]
# transform to base_link
p = self.listener.transformPoint("base_link", p)
# update
with self.mutex:
if p.point.x < 0.65:
self.x = 0.65
else:
self.x = p.point.x
if p.point.y > 0.5:
self.y = 0.5
elif p.point.y < -0.5:
self.y = -0.5
else:
self.y = p.point.y
def loop(self):
while not rospy.is_shutdown():
if abs((rospy.Time.now() - self.last_vel_time).to_sec()) < 1.0:
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "base_link"
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.0
goal.min_duration = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.75
self.client.send_goal(goal)
self.client.wait_for_result()
else:
rospy.sleep(1.0)
class NavHeadController:
def __init__(self):
# enables
self.enable_tilt = True
self.enable_look = True
self.has_goal = False
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient(
'head_controller/point_head', PointHeadAction)
self.client.wait_for_server()
self.service = rospy.Service('~configure', TiltControl,
self.serviceHandler)
self.plan_sub = rospy.Subscriber(
'move_base/TrajectoryPlannerROS/local_plan', Path,
self.planCallback)
self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray,
self.statCallback)
def serviceHandler(self, req):
self.enable_tilt = req.enable_tilt
self.enable_look = req.enable_look and self.enable_tilt # tilt has to be enabled to look
return TiltControlResponse()
def statCallback(self, msg):
goal = False
for status in msg.status_list:
if status.status == GoalStatus.ACTIVE:
goal = True
break
self.has_goal = goal
def planCallback(self, msg):
# Only need to do this if we are looking
if not self.enable_look:
return
# get the goal
pose_stamped = msg.poses[-1]
pose = pose_stamped.pose
# look ahead one meter
R = quaternion_matrix([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
point = [1, 0, 0, 1]
M = R * point
p = PointStamped()
p.header.frame_id = pose_stamped.header.frame_id
p.header.stamp = rospy.Time(0)
p.point.x += pose_stamped.pose.position.x + M[0, 0]
p.point.y += pose_stamped.pose.position.y + M[1, 0]
p.point.z += pose_stamped.pose.position.z + M[2, 0]
# transform to base_link
p = self.listener.transformPoint('base_link', p)
# update
with self.mutex:
if p.point.x < 0.65:
self.x = 0.65
else:
self.x = p.point.x
if p.point.y > 0.5:
self.y = 0.5
elif p.point.y < -0.5:
self.y = -0.5
else:
self.y = p.point.y
def loop(self):
while not rospy.is_shutdown():
if self.has_goal and self.enable_tilt:
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = 'base_link'
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.75
goal.min_duration = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.0
self.client.send_goal(goal)
self.client.wait_for_result()
else:
rospy.sleep(1.0)
class NavHeadController:
def __init__(self):
self.last_vel_time = rospy.Time(0)
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient(
"head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.plan_sub = rospy.Subscriber("/safe_teleop_base/local_plan", Path,
self.planCallback)
self.vel_sub = rospy.Subscriber("/teleop/cmd_vel/unsafe", Twist,
self.velCallback)
def velCallback(self, msg):
vel = msg.linear.x * msg.linear.x
vel += msg.linear.y * msg.linear.y
vel += msg.angular.z * msg.angular.z
if vel > 0.0:
self.last_vel_time = rospy.Time.now()
def planCallback(self, msg):
# get the goal
pose_stamped = msg.poses[-1]
pose = pose_stamped.pose
# look ahead one meter
R = quaternion_matrix([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])
point = [1, 0, 0, 1]
M = R * point
p = PointStamped()
p.header.frame_id = pose_stamped.header.frame_id
p.header.stamp = rospy.Time(0)
p.point.x += pose_stamped.pose.position.x + M[0, 0]
p.point.y += pose_stamped.pose.position.y + M[1, 0]
p.point.z += pose_stamped.pose.position.z + M[2, 0]
# transform to base_link
p = self.listener.transformPoint("base_link", p)
# update
with self.mutex:
if p.point.x < 0.65:
self.x = 0.65
else:
self.x = p.point.x
if p.point.y > 0.5:
self.y = 0.5
elif p.point.y < -0.5:
self.y = -0.5
else:
self.y = p.point.y
def loop(self):
while not rospy.is_shutdown():
if abs((rospy.Time.now() - self.last_vel_time).to_sec()) < 1.0:
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = "base_link"
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.0
goal.min_duration = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.75
self.client.send_goal(goal)
self.client.wait_for_result()
else:
rospy.sleep(1.0)
class Transformer:
def __init__(self):
self.__listener = TransformListener()
def __del__(self):
pass
def transform_to(self, pose_target, target_frame="/odom_combined"):
'''
Transforms the pose_target into the target_frame.
:param pose_target: object to transform as PoseStamped/PointStamped/Vector3Stamped/CollisionObject/PointCloud2
:param target_frame: goal frame id
:return: transformed object
'''
if pose_target.header.frame_id == target_frame:
return pose_target
odom_pose = None
i = 0
while odom_pose is None and i < 10:
try:
#now = rospy.Time.now()
#self.__listener.waitForTransform(target_frame, pose_target.header.frame_id, now, rospy.Duration(4))
if type(pose_target) is CollisionObject:
i = 0
new_co = deepcopy(pose_target)
for cop in pose_target.primitive_poses:
p = PoseStamped()
p.header = pose_target.header
p.pose.orientation = cop.orientation
p.pose.position = cop.position
p = self.transform_to(p, target_frame)
if p is None:
return None
new_co.primitive_poses[i].position = p.pose.position
new_co.primitive_poses[i].orientation = p.pose.orientation
i += 1
new_co.header.frame_id = target_frame
return new_co
if type(pose_target) is PoseStamped:
odom_pose = self.__listener.transformPose(target_frame, pose_target)
break
if type(pose_target) is Vector3Stamped:
odom_pose = self.__listener.transformVector3(target_frame, pose_target)
break
if type(pose_target) is PointStamped:
odom_pose = self.__listener.transformPoint(target_frame, pose_target)
break
if type(pose_target) is PointCloud2:
odom_pose = self.__listener.transformPointCloud(target_frame, pose_target)
break
except Exception, e:
print "tf error:::", e
rospy.sleep(0.5)
i += 1
rospy.logdebug("tf fail nr. " + str(i))
if odom_pose is None:
rospy.logerr("FUUUUUUUUUUUUUU!!!! f*****g tf shit!!!!")
return odom_pose
class PlaceObject:
def __init__(self):
self.arm = rospy.get_param("arm", "r")
self.tl = TransformListener()
self.perception_srv = rospy.Service(
"/clickable_world/place_table_perceive", PerceiveButtons,
self.do_perception)
self.action_srv = rospy.Service("/clickable_world/place_object",
ButtonAction, self.do_action)
self.pc_capture_srv = rospy.ServiceProxy(
"/table_detection/surf_seg_capture_pc", std_srvs.srv.Empty)
self.table_seg_srv = rospy.ServiceProxy(
"/table_detection/segment_surfaces", SegmentSurfaces)
self.grasp_client = actionlib.SimpleActionClient(
self.arm + '_overhead_grasp', OverheadGraspAction)
self.grasp_client.wait_for_server()
self.grasp_setup_client = actionlib.SimpleActionClient(
self.arm + '_overhead_grasp_setup', OverheadGraspSetupAction)
self.grasp_setup_client.wait_for_server()
def do_perception(self, req):
rospy.loginfo("[place_object] Perceiving table...")
# capture a few clouds
rate = rospy.Rate(5)
for i in range(5):
self.pc_capture_srv()
rate.sleep()
# segment surfaces
self.surfaces = self.table_seg_srv().surfaces
for i in range(len(self.surfaces)):
self.surfaces[i].color.r = 256
self.surfaces[i].color.g = 0
self.surfaces[i].color.b = 256
self.surfaces[i].color.a = 256
resp = PerceiveButtonsResponse()
resp.buttons = self.surfaces
return resp
def do_action(self, req):
rospy.loginfo("[place_object] Table clicked, placing object...")
# put 3d pt in grasping frame
req.pixel3d.header.stamp = rospy.Time(0)
place_pt = self.tl.transformPoint("/torso_lift_link", req.pixel3d)
# move to place setup position
setup_goal = OverheadGraspSetupGoal()
setup_goal.disable_head = False
setup_goal.open_gripper = False
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
############################################################
# Creating place goal
grasp_goal = OverheadGraspGoal()
grasp_goal.is_grasp = False
grasp_goal.disable_head = False
grasp_goal.disable_coll = False
grasp_goal.grasp_type = OverheadGraspGoal.MANUAL_GRASP
grasp_goal.x = place_pt.point.x
grasp_goal.y = place_pt.point.y
grasp_goal.behavior_name = "overhead_grasp"
grasp_goal.sig_level = 0.999
############################################################
# place object
self.grasp_client.send_goal(grasp_goal)
self.grasp_client.wait_for_result()
result = self.grasp_client.get_result()
rospy.loginfo("[place_object] Place result: %s" % result)
obj_in_hand = result.grasp_result == "Object placed"
# go back to grasp setup
setup_goal.open_gripper = True
self.grasp_setup_client.send_goal(setup_goal)
self.grasp_setup_client.wait_for_result()
return ButtonActionResponse()
class FollowFace:
def __init__(self):
self.moving = False
self.listener = TransformListener()
self.broadcaster = TransformBroadcaster()
self.client = actionlib.SimpleActionClient("head_controller/point_head", PointHeadAction)
self.client.wait_for_server()
self.face_sub = rospy.Subscriber("face_detector/people_tracker_measurements_array", PositionMeasurementArray, self.faceCallback)
self.lastFaceCallback = time.time()
self.lastHeadTarget = None
def faceCallback(self, msg):
self.lastFaceCallback = time.time()
# Find closest face to look at
closestFace = None
closestDistance = 999999999
for face in msg.people:
pos = PointStamped()
pos.header = face.header
pos.point = face.pos
try:
pos = self.listener.transformPoint("base_link", pos)
except ExtrapolationException:
return
distance = math.sqrt(pos.point.x ** 2 + pos.point.y ** 2 + pos.point.z ** 2)
if distance < closestDistance:
closestFace = pos
closestDistance = distance
if closestFace is None:
return
goal = PointHeadGoal()
goal.min_duration = rospy.Duration(0.0)
goal.target = closestFace
distance = 999999999
if self.lastHeadTarget is not None:
distance = math.sqrt(
(closestFace.point.x - self.lastHeadTarget.point.x) ** 2 +
(closestFace.point.y - self.lastHeadTarget.point.y) ** 2 +
(closestFace.point.z - self.lastHeadTarget.point.z) ** 2
)
# Prevents jitter from insignificant face movements
if distance > 0.02:
self.lastHeadTarget = goal.target
self.client.send_goal(goal)
self.client.wait_for_result()
def loop(self):
while not rospy.is_shutdown():
# Reset head if no face found in 1 second
if time.time() - self.lastFaceCallback > 1:
self.lastHeadTarget = None
# Reset head
pos = PointStamped()
pos.header.stamp = rospy.Time.now()
pos.header.frame_id = "base_link"
pos.point.x = 1
pos.point.y = 0
pos.point.z = 1.5
goal = PointHeadGoal()
goal.min_duration = rospy.Duration(0.5)
goal.target = pos
#self.client.cancel_all_goals()
self.client.send_goal(goal)
rospy.sleep(0.1)
class NavHeadController:
def __init__(self):
# enables
self.enable_tilt = True
self.enable_look = True
self.has_goal = False
# pose and lock
self.x = 1.0
self.y = 0.0
self.mutex = Lock()
self.listener = TransformListener()
self.client = actionlib.SimpleActionClient('head_controller/point_head', PointHeadAction)
self.client.wait_for_server()
self.service = rospy.Service('~configure', TiltControl, self.serviceHandler)
self.plan_sub = rospy.Subscriber('move_base/TrajectoryPlannerROS/local_plan', Path, self.planCallback)
self.stat_sub = rospy.Subscriber('move_base/status', GoalStatusArray, self.statCallback)
def serviceHandler(self, req):
self.enable_tilt = req.enable_tilt
self.enable_look = req.enable_look and self.enable_tilt # tilt has to be enabled to look
return TiltControlResponse()
def statCallback(self, msg):
goal = False
for status in msg.status_list:
if status.status == GoalStatus.ACTIVE:
goal = True
break
self.has_goal = goal
def planCallback(self, msg):
# Only need to do this if we are looking
if not self.enable_look:
return
# get the goal
pose_stamped = msg.poses[-1]
pose = pose_stamped.pose
# look ahead one meter
R = quaternion_matrix([pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w])
point = [1, 0, 0, 1]
M = R*point
p = PointStamped()
p.header.frame_id = pose_stamped.header.frame_id
p.header.stamp = rospy.Time(0)
p.point.x += pose_stamped.pose.position.x + M[0,0]
p.point.y += pose_stamped.pose.position.y + M[1,0]
p.point.z += pose_stamped.pose.position.z + M[2,0]
# transform to base_link
p = self.listener.transformPoint('base_link', p)
# update
with self.mutex:
if p.point.x < 0.65:
self.x = 0.65
else:
self.x = p.point.x
if p.point.y > 0.5:
self.y = 0.5
elif p.point.y < -0.5:
self.y = -0.5
else:
self.y = p.point.y
def loop(self):
while not rospy.is_shutdown():
if self.has_goal and self.enable_tilt:
goal = PointHeadGoal()
goal.target.header.stamp = rospy.Time.now()
goal.target.header.frame_id = 'base_link'
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.75
goal.min_duration = rospy.Duration(1.0)
self.client.send_goal(goal)
self.client.wait_for_result()
with self.mutex:
goal.target.point.x = self.x
goal.target.point.y = self.y
self.x = 1
self.y = 0
goal.target.point.z = 0.0
self.client.send_goal(goal)
self.client.wait_for_result()
else:
rospy.sleep(1.0)
