def getPathWVfromPoints(self, points, speeds):
print len(points)
pathWV = PathWithVelocity()
for i in range(len(points)):
poseWV = PoseStampedWithVelocity()
poseWV.pose.position = points[i]
if(i==len(points)-1):
poseWV.pose.orientation = pathWV.poses[-1].pose.orientation
else:
poseWV.pose.orientation = self.getOrientation(points[i], points[i+1])
poseWV.linear_velocity = Vector3(np.around(speeds[i], decimals=2), 0, 0)
pathWV.poses.append(poseWV)
return pathWV
def __init__(self):
# Define Adjustable Parameters
self.input_file = raw_input("Input File:\n")
self.output_file = self.input_file + ".json"
# Internal USE Variables - Modify with Consultation
self.pathWV = PathWithVelocity()
# Load File
self.pathWV = self.loadFile(self.input_file)
# Write File
self.writeFile(self.pathWV, self.output_file)
# Shutdown the Node
rospy.signal_shutdown("done")
def toPathWV(self, json_data):
loaded_pathWV = PathWithVelocity()
loaded_pathWV.header.frame_id = self.frame_global
i = 0
for p in json_data["pathWV"]:
poseWV = PoseStampedWithVelocity()
poseWV.header.seq = i
poseWV.header.stamp = rospy.Time.now()
poseWV.header.frame_id = self.frame_global
poseWV.pose.position = Point(p["x"], p["y"], 0)
poseWV.pose.orientation = Quaternion(p["qx"], p["qy"], p["qz"],
p["qw"])
poseWV.linear_velocity = Vector3(p["spd"], 0, 0)
loaded_pathWV.poses.append(poseWV)
i += 1
return loaded_pathWV
def loop(self):
pathWV = PathWithVelocity()
pathWV.header.frame_id = self.frame_global
pathWV.header.seq = 0
while not rospy.is_shutdown():
try:
# Looking for transform
transform = self.tf2Buffer.lookup_transform(
self.frame_global, self.frame_robot, rospy.Time())
except (tf2_ros.LookupException, tf2_ros.ConnectivityException,
tf2_ros.ExtrapolationException):
self.rate.sleep()
continue
poseWV = self.toPoseStampedWithVelocity(
transform.transform.translation, transform.transform.rotation,
self.x_spd, len(pathWV.poses))
pathWV.header.stamp = rospy.Time.now()
pathWV.header.seq += 1
# Record the 1st pose
if (len(pathWV.poses) == 0):
pathWV.poses.append(poseWV)
self.pathWV_pub.publish(pathWV)
# Record the poses that dist_interval are more than the threshold value
elif (self.dist2d(
pathWV.poses[len(pathWV.poses) - 1].pose.position,
poseWV.pose.position) > self.dist_interval):
pathWV.poses.append(poseWV)
self.pathWV_pub.publish(pathWV)
elif (self.ang2d(
pathWV.poses[len(pathWV.poses) - 1].pose.orientation,
poseWV.pose.orientation) > self.ang_interval):
pathWV.poses.append(poseWV)
self.pathWV_pub.publish(pathWV)
# print pathWV.header.seq
# For Saving USE only
self.save_path_WV = pathWV
# Running at rate [Hz]
self.rate.sleep()
def __init__(self):
# Adjustable Parameters
self.input_file = "/home/atlas80evo/catkin_ws/src/atlas80evo/traj/psa_test/1_A.path"
self.output_file = "/home/atlas80evo/catkin_ws/src/atlas80evo/traj/psa_test/1_A.path"
# Internal USE Variables - Modify with consultation
self.save_pathWV = PathWithVelocity()
# Step 1 - Load file
pts, spds = self.loadFile(self.input_file)
print "Step 1 - Done"
# Step 2 - Convert data
self.save_pathWV = self.getPathWVfromPoints(pts, spds)
print "Step 2 - Done"
# Step 3 - Write File
self.writeFile(self.output_file)
print "Step 3 - Done"
# Shut down this node
rospy.signal_shutdown("done")
def change_pathSRV(self, req_path):
loaded_pathWV = PathWithVelocity()
loaded_pathWV = self.loadFile(req_path.filename)
loaded_pathWV.header.stamp = rospy.Time.now()
self.pathWV_pub.publish(loaded_pathWV)
return True
def track_statusCB(self, msg):
# Report Done & Reset the Path to Empty when goal reached
if (msg.status == 3):
self.path_done_call()
self.pathWV_pub.publish(PathWithVelocity())
def path_cancelSRV(self, req):
self.pathWV_pub.publish(PathWithVelocity())
print "cancel to trajectory_server received......"
return ()