def PoseStamped(x=0, y=0, z=0, phi=0,
roll=0, pitch=0, yaw=0,
frame_id="/map", stamp=None, pointstamped=None):
"""Build a geometry_msgs.msgs.PoseStamped from any number of arguments.
Each value defaults to 0
>>> ps = PoseStamped(yaw=0.5)
>>> ps.pose
position:
x: 0
y: 0
z: 0
orientation:
x: 0.0
y: 0.0
z: 0.247403959255
w: 0.968912421711
"""
if not stamp:
stamp = rospy.get_rostime()
if pointstamped:
return gm.PoseStamped(header=pointstamped.header,
pose=gm.Pose(pointstamped.point, Quaternion()))
elif isinstance(x, number) and isinstance(y, number) and isinstance(z, number):
return gm.PoseStamped(header=Header(frame_id, stamp),
pose=Pose(x, y, z, phi, roll, pitch,yaw))
else:
raise ValueError("Either supply a number for x, y and z or a PointStamped to pointstamped")
def copDetection():
copName = "deckard"
robberName = "roy"
# Get list of objects and their locations
mapInfo = 'map2.yaml'
objLocations = getObjects(mapInfo)
vertexes = objLocations.values()
vertexKeys = objLocations.keys()
# TODO: GetPlan not working because move base does not provide the service (EITHER FIX IT OR FIND A NEW WAY TO GET DISTANCE)
# tol = .1
# robberName = "roy"
# robLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,1,0), geo_msgs.Quaternion(0,0,0,1)))
# destination = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,0,1)))
# print(robLoc)
# print(destination)
# # Get plan from make_plan service
# #rospy.wait_for_service("/" + robberName + "move_base/make_plan")
# try:
# planner = rospy.ServiceProxy("/" + robberName + "/move_base/make_plan", nav_srv.GetPlan)
# plan = planner(robLoc, destination, tol)
# poses = plan.plan.poses
# print(poses)
# except rospy.ServiceException, e:
# print "GetPlan service call failed: %s"%e
# return 0
# rospy.Subscriber("/" + copName + "/", geo_msgs.Pose, getCopLocation)
# rospy.Subscriber("/" + robberName + "/", geo_msgs.Pose, getRobberLocation)
copLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,1,0), geo_msgs.Quaternion(0,0,1,0)))
pastCopLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,1,0)))
robLoc = geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,0,0), geo_msgs.Quaternion(0,0,1,0)))
test_path = [geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(0,1,0), geo_msgs.Quaternion(0,0,0,1))),
geo_msgs.PoseStamped(std_msgs.Header(), geo_msgs.Pose(geo_msgs.Point(1,0,0), geo_msgs.Quaternion(0,0,0,1)))]
test_plans = {"kitchen": nav_msgs.Path(std_msgs.Header(), test_path)}
curCost, curDestination = chooseDestination(test_plans, copLoc, robLoc, pastCopLoc)
print("Minimum Cost: " + str(curCost) + " at " + curDestination)
#move base to curDestination
# state = mover_base.get_state()
# pathFailure = False
# while (state!=3 and pathFailure==False): # SUCCESSFUL
# #wait a second rospy.sleep(1)
# newCost = evaluatePath(test_plans[curDestination])
# if newCost > curCost*2:
# pathFailure = True
# rospy.loginfo("Just Reached " + vertexKeys[i])
# Floyd warshall stuff
mapGrid = np.load('mapGrid.npy')
floydWarshallCosts = np.load('floydWarshallCosts.npy')
evaluateFloydCost(copLoc, robLoc, floydWarshallCosts, mapGrid)
def make_kin_tree_from_joint(ps, jointname, ns='default_ns', valuedict=None):
rospy.logdebug("joint: %s" % jointname)
U = get_pr2_urdf()
joint = U.joints[jointname]
joint.origin = joint.origin or urdf.Pose()
if not joint.origin.position: joint.origin.position = [0, 0, 0]
if not joint.origin.rotation: joint.origin.rotation = [0, 0, 0]
parentFromChild = conversions.trans_rot_to_hmat(
joint.origin.position,
transformations.quaternion_from_euler(*joint.origin.rotation))
childFromRotated = np.eye(4)
if valuedict and jointname in valuedict:
if joint.joint_type == 'revolute' or joint.joint_type == 'continuous':
childFromRotated = transformations.rotation_matrix(
valuedict[jointname], joint.axis)
elif joint.joint_type == 'prismatic':
childFromRotated = transformations.translation_matrix(
np.array(joint.axis) * valuedict[jointname])
parentFromRotated = np.dot(parentFromChild, childFromRotated)
originFromParent = conversions.pose_to_hmat(ps.pose)
originFromRotated = np.dot(originFromParent, parentFromRotated)
newps = gm.PoseStamped()
newps.header = ps.header
newps.pose = conversions.hmat_to_pose(originFromRotated)
return make_kin_tree_from_link(newps,
joint.child,
ns=ns,
valuedict=valuedict)
def execute(self, ud):
start_time = rospy.get_time()
rate = rospy.Rate(2.5)
while not self.preempt_requested() and not rospy.is_shutdown():
segmented_objs = self.segment_srv(
only_surface=True).segmented_objects.objects
if segmented_objs:
table = segmented_objs[0]
pose = geo_msgs.PoseStamped(
table.point_cloud.header,
geo_msgs.Pose(table.center, table.orientation))
pose = TF2().transform_pose(pose, pose.header.frame_id, 'map')
width, length = table.width, table.depth
table_tf = to_transform(pose, 'table_frame')
TF2().publish_transform(table_tf)
if 'table' in ud:
table.name = ud['table'].name
ud['table'] = table
ud['table_pose'] = pose
rospy.loginfo("Detected table of size %.1f x %.1f at %s",
width, length, pose2d2str(pose))
return 'succeeded'
elif self.timeout and rospy.get_time() - start_time > self.timeout:
rospy.logwarn("No table detected after %g seconds",
rospy.get_time() - start_time)
return 'aborted'
try:
rate.sleep()
except rospy.ROSInterruptException:
break
rospy.logwarn(
"Detect table has been preempted after %g seconds (timeout %g)",
rospy.get_time() - start_time, self.timeout)
return 'preempted'
def read_table_file(file_name):
try:
data_file = open(file_name, 'r')
except:
return []
poses_data = data_file.readlines()
data_file.close()
tables = list()
for object in poses_data:
object = object.strip()
if len(object) == 0 or object.startswith('#'):
continue
print object
pose = geometry.PoseStamped()
[
name, secs, nsecs, frame_id, x, y, z, qx, qy, qz, qw, size_x,
size_y, size_z
] = object.split()
pose.header.frame_id = frame_id
pose.pose.position.x = float(x)
pose.pose.position.y = float(y)
pose.pose.position.z = float(z)
pose.pose.orientation.x = float(qx)
pose.pose.orientation.y = float(qy)
pose.pose.orientation.z = float(qz)
pose.pose.orientation.w = float(qw)
tables.append(
[pose, [float(size_x), float(size_y),
float(size_z)], name])
return tables
def set_cart_target(self, quat, xyz, ref_frame):
ps = gm.PoseStamped()
ps.header.frame_id = ref_frame
ps.header.stamp = rospy.Time(0)
ps.pose.position = gm.Point(xyz[0], xyz[1], xyz[2])
ps.pose.orientation = gm.Quaternion(*quat)
self.cart_command.publish(ps)
def getObjects(mapInfo):
with open(mapInfo, 'r') as stream:
try:
yamled = yaml.load(stream)
except yaml.YAMLError as exc:
print(exc)
# deletes info
del yamled['info']
# Populates dictionary with location names and their poses
objDict = yamled.values()
objLocations = {}
objNames = {}
for item in objDict:
itemName = item['name']
if itemName[0:4] != "wall":
x_loc = item['centroid_x'] + (item['width'] / 2 + .6) * math.cos(
math.radians(item['orientation']))
y_loc = item['centroid_y'] + (item['length'] / 2 + .6) * math.sin(
math.radians(item['orientation']))
quat = tf.transformations.quaternion_from_euler(
0, 0, item['orientation'] - 180)
itemLoc = geo_msgs.PoseStamped(
std_msgs.Header(),
geo_msgs.Pose(
geo_msgs.Point(x_loc, y_loc, 0),
geo_msgs.Quaternion(quat[0], quat[1], quat[2], quat[3])))
objLocations[itemName] = itemLoc
objNames[itemName] = ([item['value']])
return objLocations, objNames
def convert_pose(pose, from_frame, to_frame):
"""
Convert a pose or transform between frames using tf.
pose -> A geometry_msgs.msg/Pose that defines the robots position and orientation in a reference_frame
from_frame -> A string that defines the original reference_frame of the robot
to_frame -> A string that defines the desired reference_frame of the robot to convert to
"""
global tfBuffer, listener
if tfBuffer is None or listener is None:
_init_tf()
try:
trans = tfBuffer.lookup_transform(to_frame, from_frame, rospy.Time(0), rospy.Duration(1.0))
except:
rospy.logerr('FAILED TO GET TRANSFORM FROM %s to %s' % (to_frame, from_frame))
return None
spose = gmsg.PoseStamped()
spose.pose = pose
spose.header.stamp = rospy.Time().now
spose.header.frame_id = from_frame
p2 = tf2_geometry_msgs.do_transform_pose(spose, trans)
return p2.pose
def draw_trajectory(self, pose_array, angles, ns="default_ns"):
decimation = max(len(pose_array.poses) // 6, 1)
ps = gm.PoseStamped()
ps.header.frame_id = pose_array.header.frame_id
ps.header.stamp = rospy.Time.now()
handles = []
multiplier = 5.79
for (pose, angle) in zip(pose_array.poses, angles)[::decimation]:
ps.pose = deepcopy(pose)
pointing_axis = transformations.quaternion_matrix([
pose.orientation.x, pose.orientation.y, pose.orientation.z,
pose.orientation.w
])[:3, 0]
ps.pose.position.x -= .18 * pointing_axis[0]
ps.pose.position.y -= .18 * pointing_axis[1]
ps.pose.position.z -= .18 * pointing_axis[2]
root_link = "r_wrist_roll_link"
valuedict = {
"r_gripper_l_finger_joint": angle * multiplier,
"r_gripper_r_finger_joint": angle * multiplier,
"r_gripper_l_finger_tip_joint": angle * multiplier,
"r_gripper_r_finger_tip_joint": angle * multiplier,
"r_gripper_joint": angle
}
handles.extend(
self.place_kin_tree_from_link(ps, root_link, valuedict, ns=ns))
return handles
def execute(self, ud):
# Compose a list containing id, pose, distance and heading for all cubes within arm reach
objects = []
for obj in ud.objects:
if not obj.id.startswith('cube'):
continue
# Object's timestamp is irrelevant, and can trigger a TransformException if very recent; zero it!
obj_pose = geometry_msgs.PoseStamped(obj.header, obj.pose)
obj_pose.header.stamp = rospy.Time(0)
obj_pose = TF2().transform_pose(obj_pose, obj_pose.header.frame_id,
ud.arm_ref_frame)
distance = distance_2d(obj_pose.pose)
if distance > cfg.MAX_ARM_REACH:
rospy.logdebug("'%s' is out of reach (%d > %d)", obj.id,
distance, cfg.MAX_ARM_REACH)
continue
direction = heading(obj_pose.pose)
objects.append((obj, obj_pose, distance, direction))
# Check if we have at least 2 cubes to stack
if len(objects) < 2:
return 'retry'
# Sort them by increasing heading; we stack over the one most in front of the arm, called base
objects = sorted(objects, key=lambda x: abs(x[-1]))
place_pose = objects[0][1]
place_pose.pose.position.z += get_size_from_co(
objects[0][0])[2] + cfg.PLACING_HEIGHT_ON_TABLE
ud.place_pose = place_pose
ud.other_cubes = [obj[0] for obj in objects[1:]]
return 'succeeded'
def read_pose_stamped_file(file_name):
try:
data_file = open(file_name, 'r')
except:
return {}
poses_data = data_file.readlines()
data_file.close()
named_poses = dict()
for object in poses_data:
object = object.strip()
if len(object) == 0 or object.startswith('#'):
continue
print object
pose = geometry.PoseStamped()
[name, secs, nsecs, frame_id, x, y, z, qx, qy, qz, qw] = object.split()
pose.header.frame_id = frame_id
pose.pose.position.x = float(x)
pose.pose.position.y = float(y)
pose.pose.position.z = float(z)
pose.pose.orientation.x = float(qx)
pose.pose.orientation.y = float(qy)
pose.pose.orientation.z = float(qz)
pose.pose.orientation.w = float(qw)
named_poses[name] = pose
return named_poses
def maybe_check_plan(self, goal):
"""Returns True if move_base will be able to find a global plan.
This method executes the service passed to the constructor. If no
service name has been passed, always returns True. If the service
call fails for some reason, also returns False.
"""
self._maybe_initialize_check_plan_service()
if not self._check_plan_service:
return True
else:
# We pass an empty PoseStampt as start here. According to the
# move_base sourcecode, an empty frame_id means to use the
# robot's current pose in move_base's reference frame which is
# exactly what we want.
try:
plan = self._check_plan_service(
start=geometry_msgs.PoseStamped(),
goal=goal,
tolerance=0.0)
except rospy.ServiceException:
rospy.logwarn('Service call failed: %r' %
self._check_plan_service_name)
return
return len(plan.plan.poses) > 0
def interpolate_pose_stamped(pose_stamped_a, pose_stamped_b, frac):
apose = pose_stamped_a.pose
bpose = pose_stamped_b.pose
ap = np.matrix([apose.position.x, apose.position.y, apose.position.z])
bp = np.matrix([bpose.position.x, bpose.position.y, bpose.position.z])
ip = ap + (bp - ap) * frac
aq = [
apose.orientation.x, apose.orientation.y, apose.orientation.z,
apose.orientation.w
]
bq = [
bpose.orientation.x, bpose.orientation.y, bpose.orientation.z,
bpose.orientation.w
]
iq = tr.quaternion_slerp(aq, bq, frac)
ps = geo.PoseStamped()
ps.header = pose_stamped_b.header
ps.pose.position.x = ip[0, 0]
ps.pose.position.y = ip[0, 1]
ps.pose.position.z = ip[0, 2]
ps.pose.orientation.x = iq[0]
ps.pose.orientation.y = iq[1]
ps.pose.orientation.z = iq[2]
ps.pose.orientation.w = iq[3]
return ps
def _update_coem(self):
"""Extracts and stores a new explored center of mass in the map"""
# Get the latest occupancy grid map
latest_map = rospy.wait_for_message("/map", nav_msgs.OccupancyGrid)
# Build an opencv binary image, with 1 corresponding to space that has
# been marked as free
map_data = np.array(latest_map.data,
dtype=np.int8).reshape(latest_map.info.width,
latest_map.info.height)
free_space_map = ((map_data >= 0) & (map_data <= 50)).astype(np.uint8)
# Use "image moments" to compute the centre of mass
# TODO should probably incorporate orientation in coordinate calc...
ms = cv2.moments(free_space_map, True)
centre = np.array([ms['m10'], ms['m01']]) / ms['m00']
centre_coordinates = np.array([
latest_map.info.origin.position.x,
latest_map.info.origin.position.y
]) + latest_map.info.resolution * centre
# Update "centre of explored mass" in the abstract map
# TODO remove debug
self._abstract_map._spatial_layout._coem = centre_coordinates
self._debug_coem.publish(
geometry_msgs.PoseStamped(
header=std_msgs.Header(stamp=rospy.Time.now(), frame_id='map'),
pose=tools.xythToPoseMsg(centre_coordinates[0],
centre_coordinates[1], 0)))
def create_move_base_goal(p):
target = geometry_msgs.PoseStamped()
target.header.frame_id = p['frame_id']
target.header.stamp = rospy.Time.now()
target.pose = create_geo_pose(p)
goal = move_base_msgs.MoveBaseGoal(target)
return goal
def point_stamed_to_pose_stamped(pts, orientation=(0, 0, 0, 1)):
"""convert pointstamped to posestamped"""
ps = gm.PoseStamped()
ps.pose.position = pts.point
ps.pose.orientation = orientation
ps.header.frame_id = pts.header.frame_id
return ps
def vel_callback(msg):
global pose_lock, saved_pose
twist = gms.Twist()
twist.linear.x = msg.linear[0]
twist.linear.y = msg.linear[1]
twist.linear.z = msg.linear[2]
twist.angular.x = msg.angular[0]
twist.angular.y = msg.angular[1]
twist.angular.z = msg.angular[2]
twist_pub.publish(twist)
pose_lock.acquire()
ps = cp.copy(saved_pose)
pose_lock.release()
if not ps is None:
translation = ps.position
rotation = ps.orientation
cmd_pose = gms.PoseStamped()
cmd_pose.pose = gms.Pose(
position=translation,
orientation=rotation)
cmd_pose_pub.publish(cmd_pose)
transform = gms.Transform(translation, rotation)
cmd_traj = tms.MultiDOFJointTrajectory()
cmd_traj.points.append(tms.MultiDOFJointTrajectoryPoint())
cmd_traj.points[0].transforms.append(transform)
cmd_traj.points[0].velocities.append(twist)
cmd_traj_pub.publish(cmd_traj)
def goal_msg(_goal):
_msg = geom.PoseStamped()
_msg.header.stamp = rospy.Time.now()
_msg.header.frame_id = "map"
_msg.pose.position.x = _goal.x
_msg.pose.position.y = _goal.y
_msg.pose.position.z = 0
return _msg
def set_cart_pose(self, trans, rot, frame, msg_time):
ps = gm.PoseStamped()
for i, field in enumerate(['x', 'y', 'z']):
exec('ps.pose.position.%s = trans[%d]' % (field, i))
for i, field in enumerate(['x', 'y', 'z', 'w']):
exec('ps.pose.orientation.%s = rot[%d]' % (field, i))
ps.header.frame_id = frame
ps.header.stamp = rospy.Time(msg_time)
self.cart_pose_pub(ps)
def __init__(self, duration=None):
super().__init__(
node_name="move_base_controller",
action_name="move_base",
action_type=py_trees_actions.MoveBase,
generate_feedback_message=self.generate_feedback_message,
duration=duration)
self.pose = geometry_msgs.PoseStamped()
self.pose.pose.position = geometry_msgs.Point(x=0.0, y=0.0, z=0.0)
def to_pose3d(pose, timestamp=rospy.Time(), frame=None):
if isinstance(pose, geometry_msgs.Pose2D):
p = geometry_msgs.Pose(geometry_msgs.Point(pose.x, pose.y, 0.0),
quaternion_msg_from_yaw(pose.theta))
if not frame:
return p
return geometry_msgs.PoseStamped(std_msgs.Header(0, timestamp, frame),
p)
raise rospy.ROSException(
"Input parameter pose is not a geometry_msgs.Pose2D object")
def publishGoal(self, target):
goal = geomsg.PoseStamped()
goal.pose.position.x = target[0] / 100.
goal.pose.position.y = -target[1] / 100.
quaternion = tf.transformations.quaternion_from_euler(
0.0, 0.0, -target[2])
goal.pose.orientation.x = quaternion[0]
goal.pose.orientation.y = quaternion[1]
goal.pose.orientation.z = quaternion[2]
goal.pose.orientation.w = quaternion[3]
self.goal_pub.publish(goal)
def get_current_pose_stamped(self):
"""
:returns: Robot pose as a geometry_msgs.msg.PoseWithCovarianceStamped object
"""
m = gm.PoseStamped() #WithCovarianceStamped()
m.header.frame_id = "/map"
m.header.stamp = rospy.Time.now()
tr, rot = self._get_current_pose_helper()
m.pose.position = gm.Point(*tr)
m.pose.orientation = gm.Quaternion(*rot)
return m
def main():
rospy.init_node('pick_object')
sm = smach.StateMachine(outcomes=['picked',
'pick_failed',
'preempted',
'error'],
input_keys=['goal'],
output_keys=['feedback',
'result'])
with sm:
sm.userdata.pose_arm_default = geometry_msgs.PoseStamped()
sm.userdata.pose_arm_default.header.stamp = rospy.Time.now()
sm.userdata.pose_arm_default.header.frame_id = "/base_footprint"
sm.userdata.pose_arm_default.pose.position.x = 0.1
sm.userdata.pose_arm_default.pose.position.y = 0.0
sm.userdata.pose_arm_default.pose.position.z = 0.5
sm.userdata.pose_arm_default.pose.orientation.w = 1.0
sm.userdata.result = pick_and_place_msgs.PickObjectResult()
smach.StateMachine.add('ParseGoal',
ParseGoal(),
remapping={'goal':'goal',
'feedback':'feedback',
'object_pose':'object_pose'},
transitions={'parsed':'PickObject'})
sm_pick = pick_object_sm.createSM()
smach.StateMachine.add('PickObject',
sm_pick,
remapping={'object_name':'object_name',
'object_pose':'object_pose',
'pose_arm_default':'pose_arm_default'},
transitions={'picked':'picked',
'pick_failed':'pick_failed',
'preempted':'preempted',
'error':'error'})
asw = ActionServerWrapper('pick_object',
pick_and_place_msgs.PickObjectAction,
wrapped_container = sm,
goal_key = 'goal',
feedback_key = 'feedback',
result_key = 'result',
succeeded_outcomes = ['picked'],
aborted_outcomes = ['pick_failed','error'],
preempted_outcomes = ['preempted'])
asw.run_server()
rospy.spin()
return
def execute(self, userdata):
rospy.loginfo('Parsing pick object goal ...')
userdata.object_name = userdata.goal.object_name
object_pose = geometry_msgs.PoseStamped()
object_pose = userdata.goal.object_pose
userdata.object_pose = object_pose
feedback = pick_and_place_msgs.PickObjectFeedback()
feedback.process_state = 'PickObjectGoal parsed'
userdata.feedback = feedback
rospy.loginfo('Gripper goal prepared.')
return 'parsed'
def publish_pose(pose, stamp=None, frame_id='camera'):
msg = geom_msg.PoseStamped()
msg.header.frame_id = frame_id
msg.header.stamp = stamp if stamp is not None else rospy.Time.now()
tvec = pose.tvec
x, y, z, w = pose.quat.to_xyzw()
msg.pose.position = geom_msg.Point(tvec[0], tvec[1], tvec[2])
msg.pose.orientation = geom_msg.Quaternion(x, y, z, w)
_publish_pose(msg)
def draw(self):
origin = [self.cx, self.cy, self.cz]
quat = conversions.yaw_to_quat(self.theta)
pose = conversions.trans_rot_to_pose(origin, quat)
ps = gm.PoseStamped()
ps.pose = pose
ps.header.frame_id = "/base_link"
ps.header.stamp = rospy.Time(0)
print "(%.3f, %.3f, %.3f), (%.3f, %.3f, %.3f, %.3f), (%.3f, %.3f, %.3f)"%(
self.cx, self.cy, self.cz, quat[0], quat[1], quat[2], quat[3], self.sx/2., self.sy/2., self.sz/2.)
#rviz.draw_marker(ps, 0, type=Marker.CUBE, rgba=(0,1,0,.25), scale=(self.sx,self.sy,self.sz))
self.marker_handle = rviz.place_kin_tree_from_link(ps, "base_link")
def getRobberLocation(self, tfMsg):
poseMsg = geo_msgs.PoseStamped(
std_msgs.Header(),
geo_msgs.Pose(
geo_msgs.Point(tfMsg.transform.translation.x,
tfMsg.transform.translation.y,
tfMsg.transform.translation.z),
geo_msgs.Quaternion(tfMsg.transform.rotation.x,
tfMsg.transform.rotation.y,
tfMsg.transform.rotation.z,
tfMsg.transform.rotation.w)))
self.robLoc = poseMsg
def create_pose(x, y, z, xx, yy, zz, ww):
pose = geometry_msgs.PoseStamped()
pose.header.frame_id = "map"
pose.header.stamp = rospy.Time.now()
pose.pose.position.x = x
pose.pose.position.y = y
pose.pose.position.z = z
pose.pose.orientation.x = xx
pose.pose.orientation.y = yy
pose.pose.orientation.z = zz
pose.pose.orientation.w = ww
return pose
def headControlRequestCb(userdata, goal):
rospy.loginfo('Preparing head goal ...')
head_target = geometry_msgs.PoseStamped()
pose_new_stamp = userdata.goal_pose
try:
pose_new_stamp.header.stamp = userdata.tf_listener.getLatestCommonTime(
"sensor_3d_fixed_link", userdata.goal_pose.header.frame_id)
head_target = userdata.tf_listener.transformPose(
"sensor_3d_fixed_link", pose_new_stamp)
except tf.Exception, e:
rospy.logerr('Couldn`t transform requested pose!')
rospy.logerr('%s', e)