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))