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