def run(self):
"""
Continuously publish a pose as a fake vehicle
"""
rate = rospy.Rate(50)
while not rospy.is_shutdown():
if self.base_path is None:
continue
seq = 0
if self.pose is None:
next_pose = self.base_path.poses[seq]
else:
seq = utils.get_closest_waypoint_index(self.pose,
self.base_path.poses)
next_pose = self.base_path.poses[(seq + 1) %
(len(self.base_path.poses))]
self.pose = next_pose
# rospy.loginfo('Pose Seq = %d, x: %f, y % f' % (seq, self.pose.pose.position.x, self.pose.pose.position.y))
# Publish
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = 'world'
pose_stamped.header.seq = seq + 1
pose_stamped.pose = next_pose.pose
self.pose_pub.publish(pose_stamped)
rate.sleep()
def run(self):
"""
Continuously publish a pose as a fake vehicle
"""
rate = rospy.Rate(50)
prev = rospy.get_time()
while not rospy.is_shutdown():
curr = rospy.get_time()
if self.base_path is None:
continue
seq = None
if self.pose is None:
seq = 0
elif curr - prev < 5.0:
seq = utils.get_closest_waypoint_index(self.pose, self.base_path.poses)
else:
seq = utils.get_closest_waypoint_index(self.pose, self.base_path.poses) + 1
prev = curr
self.pose = self.base_path.poses[(seq)%(len(self.base_path.poses))]
q = quaternion_from_euler(0, 0, 3.1416)
self.pose.pose.orientation.x = q[0]
self.pose.pose.orientation.y = q[1]
self.pose.pose.orientation.z = q[2]
self.pose.pose.orientation.w = q[3]
# rospy.loginfo('Pose Seq = %d, x: %f, y % f' % (seq, self.pose.pose.position.x, self.pose.pose.position.y))
# Publish
pose_stamped = PoseStamped()
pose_stamped.header.frame_id = 'world'
pose_stamped.header.seq = seq
pose_stamped.pose = self.pose.pose
self.pose_pub.publish(pose_stamped)
rate.sleep()
def run(self):
"""
Continuously publish local path paths with target velocities
"""
rate = rospy.Rate(50)
self.ilane = int(rospy.get_param("~lane_index", '0'))
self.cruise_speed = int(rospy.get_param("~cruise_speed", '10'))
while not rospy.is_shutdown():
if self.base_path is None or self.pose is None or self.frame_id is None:
continue
# Where in base paths list the car is
car_index = utils.get_closest_waypoint_index(
self.pose_stamped, self.base_path.poses)
# Get subset paths ahead
lookahead_waypoints, lookahead_waypoints_display \
= utils.get_next_waypoints(self.base_path.poses, self.pose_stamped, car_index, LOOKAHEAD_WPS, self.ilane)
# Publish
path = utils.construct_path_object(self.frame_id,
lookahead_waypoints)
lane = Lane()
lane.header.frame_id = self.frame_id
for i in range(len(lookahead_waypoints)):
waypoint = Waypoint()
waypoint.pose = lookahead_waypoints[i]
waypoint.twist.header.frame_id = self.frame_id
waypoint.twist.twist.linear.x = self.cruise_speed
lane.waypoints.append(waypoint)
rospy.logdebug('Update local path and waypoints ...')
self.final_path_pub.publish(path)
self.final_path_points_pub.publish(lookahead_waypoints_display)
self.final_waypoints_pub.publish(lane)
self.car_index_pub.publish(car_index)
rate.sleep()