class LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
def __init__(self):
self.path = rospy.get_param(
"~trajectory",
default=
"/home/nvidia/workspace/catkin_ws/src/ddl_0725/pf_localization/trajectories/2018-08-09-16-16-58.traj"
)
self.should_publish = bool(rospy.get_param("~publish", default=True))
self.pub_topic = rospy.get_param(
"~topic", default="/loaded_trajectory/recorded_path")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
if self.should_publish:
self.traj_pub = rospy.Publisher(self.pub_topic, Path, queue_size=1)
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# visualize the loaded trajectory for 5 seconds
self.trajectory.publish_viz(duration=3.0)
self.duration = rospy.Duration(1)
self.times = 0
self.srv_get_raw_path_ = rospy.Service('/get_raw_path', GetRawPath,
self.handle_get_raw_path)
# send the trajectory
if self.should_publish:
self.publish_trajectory()
def publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
while not rospy.is_shutdown():
if self.traj_pub.get_num_connections() > 0:
self.traj_pub.publish(self.trajectory.toPath())
time.sleep(self.duration.to_sec())
self.times = self.times + 1
if self.times > 20:
break
def handle_get_raw_path(self, req):
if not isinstance(self.trajectory, LineTrajectory):
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
return GetRawPathResponse(self.trajectory.toPath())
class LoadTrajectory(object):
""" Loads a trajectory from the file system and publishes it to a ROS topic.
"""
def __init__(self):
self.path = rospy.get_param(
"~trajectory",
default=
"/home/lenovo/zh/workspace/catkin_ws/src/ddl_0725/localization_0725/trajectories/2018-08-09-16-16-58.traj"
)
self.should_publish = bool(rospy.get_param("~publish", default=True))
self.pub_topic = rospy.get_param(
"~topic", default="/loaded_trajectory/recorded_path")
# initialize and load the trajectory
self.trajectory = LineTrajectory("/loaded_trajectory")
self.trajectory.load(self.path)
if self.should_publish:
self.traj_pub = rospy.Publisher(self.pub_topic, Path, queue_size=1)
# need to wait a short period of time before publishing the first message
time.sleep(0.5)
# visualize the loaded trajectory for 5 seconds
self.trajectory.publish_viz(duration=3.0)
self.duration = rospy.Duration(1)
# send the trajectory
if self.should_publish:
self.publish_trajectory()
def publish_trajectory(self):
print "Publishing trajectory to:", self.pub_topic
while not rospy.is_shutdown():
self.traj_pub.publish(self.trajectory.toPath())
time.sleep(self.duration.to_sec())
