def readPoseArrayFromFile(filename): try: # read file with open(filename) as f: poseArrayString = f.read() # deserialize it into PoseArray pa = PoseArray() pa.deserialize(poseArrayString) return pa except: print "Returning no pose array for " + filename return None
class PathPublisher(): # Must have __init__(self) function for a class, similar to a C++ class constructor. def __init__(self): # Get the ~private namespace parameters from command li self.poseArray = PoseArray() self.waypointArray = PoseArray() self.filename = "/home/shart/marker_trajectories/hose/hose_waypoints_1.txt" self.pathPub = rospy.Publisher("/path_publisher/path", Path) self.posePub = rospy.Publisher("/path_publisher/poseArray", PoseArray) self.waypointPub = rospy.Publisher("/path_publisher/waypointArray", PoseArray) self.num_points = 10 self.frameOffset = Frame() def readPoseArray(self) : with open(self.filename) as f: pa = f.read() self.waypointArray.deserialize(pa) [self.path, self.poseArray] = processPath(self.waypointArray, self.num_points) print "path size: ", len(self.poseArray.poses) self.waypointArray.header.frame_id = self.frame_id self.poseArray.header.frame_id = self.frame_id self.path.header.frame_id = self.frame_id self.waypointArray.header.stamp = rospy.Time.now() self.poseArray.header.frame_id = rospy.Time.now() self.path.header.stamp = rospy.Time.now() def publishPath(self) : self.waypointArray.header.stamp = rospy.Time.now() self.poseArray.header.frame_id = rospy.Time.now() self.path.header.stamp = rospy.Time.now() self.pathPub.publish(self.path) self.posePub.publish(self.poseArray) self.waypointPub.publish(self.waypointArray) def setFile(self, fname) : self.filename = fname def setFrameID(self, fid) : self.frame_id = fid def setNumPoints(self, N) : self.num_points = N def applyFrameOffset(self, pose) : self.frameOffset = getFrameFromPose(pose) for i in range(len(self.poseArray.poses)) : F = getFrameFromPose(self.poseArray.poses[i]) Fnew = self.frameOffset*F self.poseArray.poses[i] = getPoseFromFrame(Fnew) self.path.poses[i].pose = self.poseArray.poses[i] for i in range(len(self.waypointArray.poses)) : F = getFrameFromPose(self.waypointArray.poses[i]) Fnew = self.frameOffset*F self.waypointArray.poses[i] = getPoseFromFrame(Fnew) for i in range(len(self.path.poses)) : self.path.poses[i].header.frame_id = self.frame_id
class PathPublisher(): # Must have __init__(self) function for a class, similar to a C++ class constructor. def __init__(self): # Get the ~private namespace parameters from command li self.poseArray = PoseArray() self.waypointArray = PoseArray() self.filename = "/home/shart/marker_trajectories/hose/hose_waypoints_1.txt" self.pathPub = rospy.Publisher("/path_publisher/path", Path) self.posePub = rospy.Publisher("/path_publisher/poseArray", PoseArray) self.waypointPub = rospy.Publisher("/path_publisher/waypointArray", PoseArray) self.num_points = 10 self.frameOffset = Frame() def readPoseArray(self): with open(self.filename) as f: pa = f.read() self.waypointArray.deserialize(pa) [self.path, self.poseArray] = processPath(self.waypointArray, self.num_points) print "path size: ", len(self.poseArray.poses) self.waypointArray.header.frame_id = self.frame_id self.poseArray.header.frame_id = self.frame_id self.path.header.frame_id = self.frame_id self.waypointArray.header.stamp = rospy.Time.now() self.poseArray.header.frame_id = rospy.Time.now() self.path.header.stamp = rospy.Time.now() def publishPath(self): self.waypointArray.header.stamp = rospy.Time.now() self.poseArray.header.frame_id = rospy.Time.now() self.path.header.stamp = rospy.Time.now() self.pathPub.publish(self.path) self.posePub.publish(self.poseArray) self.waypointPub.publish(self.waypointArray) def setFile(self, fname): self.filename = fname def setFrameID(self, fid): self.frame_id = fid def setNumPoints(self, N): self.num_points = N def applyFrameOffset(self, pose): self.frameOffset = getFrameFromPose(pose) for i in range(len(self.poseArray.poses)): F = getFrameFromPose(self.poseArray.poses[i]) Fnew = self.frameOffset * F self.poseArray.poses[i] = getPoseFromFrame(Fnew) self.path.poses[i].pose = self.poseArray.poses[i] for i in range(len(self.waypointArray.poses)): F = getFrameFromPose(self.waypointArray.poses[i]) Fnew = self.frameOffset * F self.waypointArray.poses[i] = getPoseFromFrame(Fnew) for i in range(len(self.path.poses)): self.path.poses[i].header.frame_id = self.frame_id