def goto_shelf2(self):
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-1.0, self.dist_to_shelfs, 0.0)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
self(shelf)
def relative_pose(self, position, orientation):
shelf = PoseStamped()
header = Header()
header.frame_id = 'base_footprint'
shelf.header = header
shelf.pose.position = Point(*position)
shelf.pose.orientation = Quaternion(*orientation)
self(shelf)
def goto_shelf4(self):
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-2.9, self.dist_to_shelfs, 0.000)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
# shelf.pose.orientation = Quaternion(*quaternion_from_euler(0,0,pi*.99))
self(shelf)
def goto_shelf1(self):
# self.client.cancel_all_goals()
shelf = PoseStamped()
header = Header()
header.frame_id = 'map'
shelf.header = header
shelf.pose.position = Point(-0.0, self.dist_to_shelfs, 0.0)
shelf.pose.orientation = Quaternion(0.0, 0.0, 0.0, 1.0)
self(shelf)
def np_array_to_pose_stampeds(self, nparray, orientation, header):
pose_list = []
for p in nparray:
pose = PoseStamped()
pose.pose.position = Point(*p)
pose.pose.orientation = orientation
pose.header = header
pose_list.append(pose)
return pose_list
def odomCallback(odomData):
global lastOdom,tfl,lastMapPose
lastOdom = odomData
temp = PoseStampedMsg()
temp.pose = lastOdom.pose.pose
temp.header = lastOdom.header
try:
#now lastmappose has map coords
tfl.transformPose("map",temp,lastMapPose)
except tf.TransformException:
rospy.roserror("Transform Error")
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))
