def publish(self, waypoints): lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time(0) lane.waypoints = waypoints self.final_waypoints_pub.publish(lane)
def publish_waypoints(self, closest_idx): lane = Lane() lane.header = self.base_waypoints.header lane.waypoints = self.base_waypoints.waypoints[ closest_idx:closest_idx + LOOKAHEAD_WPS] self.final_waypoints_pub.publish(lane)
def loop(self): if self.waypoints != None: # Find waypoint directly ahead of us and assign it to self.wpt_ahead self.find_next_waypoint() # Initialize the final waypoints ahead of us self.final_wpts = Lane() if False: print('WPT Ahead ', "x: ", self.wpt_ahead.pose.pose.position.x, "y: ", self.wpt_ahead.pose.pose.position.y, "idx: ", self.wpt_ahead_idx) print('WPT List Length: ', len(self.waypoints)) # Form final waypoint list starting from the waypoint directly ahead final_wpt_idx = self.wpt_ahead_idx + LOOKAHEAD_WPS if final_wpt_idx < len( self.waypoints ): # protect against wrapping around the waypoints array self.final_wpts.waypoints = self.waypoints[ self.wpt_ahead_idx:final_wpt_idx] else: # Handle the case when the base waypoint indicies wrap back to zero self.final_wpts.waypoints = self.waypoints[ self.wpt_ahead_idx:len(self.waypoints)] idx_prev_0 = len(self.waypoints) - self.wpt_ahead_idx idx_past_0 = LOOKAHEAD_WPS - idx_prev_0 for idx in range(idx_past_0): self.final_wpts.waypoints.append(self.waypoints[idx]) # Find the index of the stopline in the final waypoints list self.num_wpts_to_stopline = self.light_ahead_idx - self.wpt_ahead_idx if self.num_wpts_to_stopline < 0: self.num_wpts_to_stopline = 30. if self.num_wpts_to_stopline == 0: self.num_wpts_to_stopline = 1. if self.num_wpts_to_stopline > 30: self.num_wpts_to_stopline = 30. idx = 0 fwpt_idx = 0 wpt_ahead = self.final_wpts.waypoints[0] if self.light_ahead_idx >= 0: wpt_stopline = self.waypoints[self.light_ahead_idx] dis_wptahead2stopline = math.sqrt( (wpt_ahead.pose.pose.position.x - wpt_stopline.pose.pose.position.x)**2 + (wpt_ahead.pose.pose.position.y - wpt_stopline.pose.pose.position.y)**2) else: dis_wptahead2stopline = 999. self.target_speed_mps = self.current_velocity vel_plan = [] dist_plan = [] for idx, wpt in enumerate(self.final_wpts.waypoints): # Two Checks before we set deceleration profile: # 1) Check if we are NOT past the white stop line in the middle of the intersection => dont stop in middle # 2) Check if we close enough to the stop line to start braking # If either are false then just continue at normal speed #in_intersection = dis_wptahead2stopline < 3. # arbitrary tuned number start_braking = math.fabs( dis_wptahead2stopline) < STOP_DIST_ERR if start_braking: #and not in_intersection: # For each waypoint compute in order: # Distance to the stoplight from waypoint # Distance from waypoint to stopline in front of stoplight # Desired deceleration # Final velocity command for waypoint # Find the distance from the final wpt to the stoplight dis_wpt2stopline = math.sqrt( (wpt.pose.pose.position.x - wpt_stopline.pose.pose.position.x)**2 + (wpt.pose.pose.position.y - wpt_stopline.pose.pose.position.y)**2) # Find the desired final stop position of the car self.target_speed_mps = self.target_speed_mps - 2.0 * 1. / self.num_wpts_to_stopline * dis_wpt2stopline if self.target_speed_mps < 3.0: # if the velocity is small, just command vel to 0 self.target_speed_mps = 0.0 if idx >= self.num_wpts_to_stopline: # if the final wpts are ahead of the stopline set the commanded vel to 0 self.target_speed_mps = 0.0 if idx < self.num_wpts_to_stopline and dis_wpt2stopline < 2.: # if the stopline is 2m ahead just stop self.target_speed_mps = 0.0 braking = self.current_velocity > 1e-2 else: # Car goes at normal speed self.target_speed_mps = self.base_wpt_spd_mps #SPEED_MPH*MPH2MPS braking = False dis_wpt2stopline = 999. # Set speeds in waypoint list wpt.twist.twist.linear.x = self.target_speed_mps vel_plan.append(self.target_speed_mps) # for debugging dist_plan.append(dis_wpt2stopline) #Publish final waypoints self.final_wpts.header.stamp = rospy.Time.now() self.final_waypoints_pub.publish(self.final_wpts) if DEBUG_TGTSPD and braking: #print('Distance Plan: {0:.1f}--{1:.1f}--{2:.1f}--{3:.1f}--{4:.1f}--{5:.1f}--{6:.1f}--{7:.1f}--{8:.1f}--{9:.1f}--{10:.1f}--{11:.1f}--{12:.1f}--{13:.1f}--{14:.1f}--{15:.1f}--{16:.1f}--{17:.1f}--{18:.1f}--{19:.1f}--{20:.1f}--{21:.1f}--{22:.1f}--{23:.1f}--{24:.1f}--{25:.1f}--{26:.1f}--{27:.1f}--{28:.1f}--{29:.1f}'.format(\ #dist_plan[0], dist_plan[1],dist_plan[2],dist_plan[3],dist_plan[4],dist_plan[5],dist_plan[6],dist_plan[7],dist_plan[8],dist_plan[9],dist_plan[10],\ #dist_plan[11],dist_plan[12],dist_plan[13],dist_plan[14],dist_plan[15],dist_plan[16],dist_plan[17],dist_plan[18],dist_plan[19],dist_plan[20],\ #dist_plan[21],dist_plan[22],dist_plan[23],dist_plan[24],dist_plan[25],dist_plan[26],dist_plan[27],dist_plan[28],dist_plan[29])) print('Velocity Plan: {0:.1f}--{1:.1f}--{2:.1f}--{3:.1f}--{4:.1f}--{5:.1f}--{6:.1f}--{7:.1f}--{8:.1f}--{9:.1f}--{10:.1f}--{11:.1f}--{12:.1f}--{13:.1f}--{14:.1f}--{15:.1f}--{16:.1f}--{17:.1f}--{18:.1f}--{19:.1f}--{20:.1f}--{21:.1f}--{22:.1f}--{23:.1f}--{24:.1f}--{25:.1f}--{26:.1f}--{27:.1f}--{28:.1f}--{29:.1f}'.format(\ vel_plan[0],vel_plan[1],vel_plan[2],vel_plan[3],vel_plan[4],vel_plan[5],vel_plan[6],vel_plan[7],vel_plan[8],vel_plan[9],vel_plan[10],\ vel_plan[11],vel_plan[12],vel_plan[13],vel_plan[14],vel_plan[15],vel_plan[16],vel_plan[17],vel_plan[18],vel_plan[19],vel_plan[20],\ vel_plan[21],vel_plan[22],vel_plan[23],vel_plan[24],vel_plan[25],vel_plan[26],vel_plan[27],vel_plan[28],vel_plan[29])) #Topics to publish for debugging if DEBUG_TOPICS: self.debug_currpos = PoseStamped() self.debug_currpos.header.stamp = rospy.Time.now() self.debug_currpos.pose.position.x = self.pos_x self.debug_currpos.pose.position.y = self.pos_y self.debug_currpos.pose.position.z = self.pos_z self.debug_currpos.pose.orientation = self.current_orient self.debug_currentpos_pub.publish(self.debug_currpos) pass
def loop(self): rate = rospy.Rate(5) # 5Hz while not rospy.is_shutdown(): if self.pose is not None and self.waypoints is not None and self.traffic is not None and self.current_velocity is not None: ego_theta = 2.*math.acos(self.pose.orientation.w) ego_vx = np.dot(np.array([[math.cos(ego_theta),-math.sin(ego_theta)], [math.sin(ego_theta), math.cos(ego_theta)]]), np.array([1,0])) ego_vy = np.dot(np.array([[0,-1],[1,0]]), ego_vx) #rospy.logerr(ego_theta) # publish final_waypoints min_distance = 1e9 initial_wp_index = 0 for k in xrange(len(self.waypoints)): i = k if self.previous_initial_wp_index is not None: i = (i + self.previous_initial_wp_index) % len(self.waypoints) waypoint = self.waypoints[i].pose.pose.position np_waypoint = np.array([waypoint.x,waypoint.y]) np_ego_pose = np.array([self.pose.position.x,self.pose.position.y]) np_diff = np_waypoint - np_ego_pose distance = np.linalg.norm(np_diff) param = np.dot(np.linalg.inv(np.vstack((ego_vx,ego_vy)).transpose()), np_diff) # If the waypoint is in front of vehicle and also the closest one, # update the index. if(param[0] > 0 and distance < min_distance): min_distance = distance initial_wp_index = i % len(self.waypoints) if self.previous_initial_wp_index is not None: break # publish final_waypoints = Lane() final_waypoints.header = std_msgs.msg.Header() final_waypoints.header.stamp = rospy.Time.now() actual_speed = self.current_velocity.linear.x for i in xrange(LOOKAHEAD_WPS): index = (initial_wp_index + i) % len(self.waypoints) final_waypoints.waypoints.append(self.waypoints[index]) self.previous_initial_wp_index = initial_wp_index v_limit = rospy.get_param('/waypoint_loader/velocity') / 3.6 # Speed limit given by ROS parameter v_limit *= 0.9 # Set margin to not exceed speed limit. v0 = min(20./2.24, v_limit) # This program allows maximum spped of 20mph. if self.traffic == -1: for i in xrange(LOOKAHEAD_WPS): self.set_waypoint_velocity(final_waypoints.waypoints, i, v0) else: # t0 # v0 ---------------- # \ # \ # \ a0 # \ # \ # \___________ v=0 # t1 # target velocity diagram # a0 = 2.5 # m/s^2 target acceleration margin = 10 # m target margin before stop line r0 = self.distance(self.waypoints,initial_wp_index,self.traffic) - margin # target position to stop stop_space = v0*v0/(2*a0) s_start_brake = r0 - stop_space #t1 = 0.5*(2.*r0/v0 + v0/a0) #t0 = 0.5*(2.*r0/v0 - v0/a0) #rospy.loginfo("DBG STOP_S: {0} BRAKE_S {1} TOTAL {2}".format(stop_space,s_start_brake,r0)) for i in xrange(LOOKAHEAD_WPS): r = self.distance(self.waypoints,initial_wp_index,initial_wp_index+i) if r <= s_start_brake: #v0 * t0: v = v0 elif s_start_brake < r and r < r0: ds = r-s_start_brake #rospy.loginfo("DBG DS: {0} v0 {1} ".format(ds,v0)) delta = v0*v0 - 2*a0*ds t = (v0 - math.sqrt(delta))/a0 v = v0 - a0*t #v = math.sqrt(2.*a0*v0*t0 + v0*v0 - 2.*a0*r) else: v = 0 #rospy.loginfo("SPEED: {0}".format(v)) self.set_waypoint_velocity(final_waypoints.waypoints, i, v) #rospy.loginfo("SPEED: -------------") self.final_waypoints_pub.publish(final_waypoints) rate.sleep()
def generate_final_waypoints(self): select_wps = [] start_idx = -1 end_idx = -1 # waypoints from base waypoints if self.waypoints: # waypoint closest to car start_idx = self.get_closest_waypoint(self.curr_pose) # slice LOOKAHEAD_WPS number of waypoints from base waypoints end_idx = start_idx + LOOKAHEAD_WPS if end_idx > self.waypoints_count: select_wps = self.waypoints[start_idx:] + self.waypoints[0:end_idx % self.waypoints_count] else: select_wps = self.waypoints[start_idx:end_idx] # rospy.logwarn('final waypoints range: (' + str(start_idx) + ',' + str(end_idx % self.waypoints_count) + ')') final_wps = Lane() final_wps.waypoints = select_wps # if upcoming red light -> Decelerate (to full stop) # else -> Accelerate (to max velocity) # Deceleration if self.stop_idx != -1: for i in range(len(select_wps)): self.set_waypoint_velocity(select_wps, i, 0.0) stop_idx_in_select_wps = self.stop_idx - start_idx # Using sudden stop mechnism. It is just a step responce and performs quite well # for i in range(stop_idx_in_select_wps-np.int(MAX_SPEED*1.5)): # self.set_waypoint_velocity(select_wps, i, MAX_SPEED) for i in range(stop_idx_in_select_wps-np.int(MAX_SPEED*0.5)): self.set_waypoint_velocity(select_wps, i, MAX_SPEED/10) for i in range(stop_idx_in_select_wps-np.int(MAX_SPEED*1.0)): self.set_waypoint_velocity(select_wps, i, MAX_SPEED/2) for i in range(stop_idx_in_select_wps-np.int(MAX_SPEED*3.0)): self.set_waypoint_velocity(select_wps, i, MAX_SPEED) # Acceleration else: # Acceleration should be gradual. Although throttle=1.0 works OK in the simulator # it would be rough for Carla # We can use increments in steps as used for deceleration for i in range(len(select_wps)): self.set_waypoint_velocity(select_wps, i, MAX_SPEED) # This should be smaller than 10 if DEBUG: v = [] for w in select_wps: v.append(w.twist.twist.linear.x) rospy.logwarn('Velocities of selected waypoints:') rospy.logwarn('{}'.format(v)) return final_wps
def publish_waypoints(self, final_waypoints): lane = Lane() lane.header = self.base_waypoints.header lane.waypoints = final_waypoints self.final_waypoints_pub.publish(lane)
def publish_waypoints(self): lane = Lane() final_lane = self.generate_lane() self.final_waypoints_pub.publish(final_lane)
def process_waypoints(self, event): if not self.pose: rospy.logwarn('no pose has been set') return None if not self.current_velocity: rospy.logwarn('no velocity has been set') return None if not self.waypoints: rospy.logwarn('no waypoints has been set') return None # get nearest waypoint begin = self.get_nearest_waypoint() end = begin + LOOKAHEAD_WPS end = min(end, len(self.waypoints)) rospy.loginfo("begin {}, end {}, waypoints len {}".format( begin, end, len(self.waypoints))) waypointsArr = [] epsilon = 1.0 distance_to_red_light = 0 distance_to_stop = 0 velocity = 0 previous_target_velocity = 0 for i in range(begin, end): target_velocity = self.max_velocity if self.red_light_waypoint > 0: distance_to_red_light = self.distance(self.waypoints, i, self.red_light_waypoint) distance_to_stop = max( 0, distance_to_red_light - self.stop_distance_to_traffic_light) velocity = self.get_waypoint_velocity(self.waypoints[i]) # slow down getting closer to intersection if distance_to_stop > 0 and distance_to_stop < self.decelerating_distance: target_velocity *= distance_to_red_light / self.decelerating_distance # push down the brakes a bit harder on the second half if distance_to_stop > 0 and distance_to_stop < self.decelerating_distance * 0.5: target_velocity *= distance_to_stop / self.decelerating_distance # keep a minimum target velocity target_velocity = max(2.0, target_velocity) # perform the brake motion if distance_to_stop > 0.5 and distance_to_stop <= 2: target_velocity = 1.0 if distance_to_stop > 0 and distance_to_stop <= 0.5: target_velocity = 0.33 elif distance_to_stop == 0: target_velocity = 0.0 # accelerate smoothly start_waypoint_velocity = self.get_waypoint_velocity( self.waypoints[begin]) previous_target_velocity = start_waypoint_velocity if i == begin else previous_target_velocity curr_waypoint_velocity = self.get_waypoint_velocity( self.waypoints[i]) rospy.logdebug( "{} = start v {}, current v {}, prev v {}, target v {}".format( i, start_waypoint_velocity, curr_waypoint_velocity, previous_target_velocity, target_velocity)) if start_waypoint_velocity == 0 and curr_waypoint_velocity == 0 and previous_target_velocity == 0: target_velocity = (0.25 * target_velocity + 0.75 * previous_target_velocity) elif previous_target_velocity < target_velocity: target_velocity = (0.1 * target_velocity + 0.9 * previous_target_velocity) # make sure target_velocity doesn't suprass max allowed velocity if target_velocity > 4.0: target_velocity = self.max_velocity # make sure target velocity is within bounds target_velocity = min(max(0, target_velocity), self.max_velocity) # store previous target velocity previous_target_velocity = target_velocity rospy.loginfo( "waypoint {}, decelerating distance {}, distance to red light {}, " "distance to stop {}, velocity {}, target velocity {}".format( i, self.decelerating_distance, distance_to_red_light, distance_to_stop, velocity, target_velocity)) self.set_waypoint_velocity(self.waypoints, i, target_velocity) waypointsArr.append(self.waypoints[i]) lane = Lane() lane.waypoints = waypointsArr self.final_waypoints_pub.publish(lane)
def update_waypoints(self): if self.current_pose is None or self.base_waypoints is None: return closest_waypoint_index = self.get_closest_waypoint_index() target_vel = self.get_waypoint_velocity( self.base_waypoints[closest_waypoint_index]) distance_to_light = None output_waypoints = [] curr_traffic_light = self.trafficlight if curr_traffic_light is not None and closest_waypoint_index is not None: distance_to_light = self.distance(self.base_waypoints, closest_waypoint_index, curr_traffic_light) if distance_to_light is not None and distance_to_light < TRAFFIC_LIGHT_RANGE: velocity = target_vel * distance_to_light / TRAFFIC_LIGHT_RANGE if velocity < 1: velocity = 0 if curr_traffic_light - closest_waypoint_index > 2: output_waypoints.append( self.clone_waypoint( self.base_waypoints[closest_waypoint_index])) self.set_waypoint_velocity(output_waypoints, -1, velocity) for i in range(closest_waypoint_index + 1, curr_traffic_light - 2): waypoint_index = i % len(self.base_waypoints) segment_distance = self.distance(self.base_waypoints, waypoint_index - 1, waypoint_index) decel_velocity = (segment_distance / distance_to_light ) * self.current_velocity velocity -= decel_velocity if velocity < 1: velocity = 0 output_waypoints.append( self.clone_waypoint( self.base_waypoints[waypoint_index])) self.set_waypoint_velocity(output_waypoints, -1, velocity) if curr_traffic_light == closest_waypoint_index: output_waypoints.append( self.clone_waypoint( self.base_waypoints[curr_traffic_light])) self.set_waypoint_velocity(output_waypoints, -1, 0) else: output_waypoints.append( self.clone_waypoint( self.base_waypoints[curr_traffic_light - 1])) self.set_waypoint_velocity(output_waypoints, -1, 0) output_waypoints.append( self.clone_waypoint( self.base_waypoints[curr_traffic_light])) self.set_waypoint_velocity(output_waypoints, -1, 0) if len(output_waypoints) < LOOKAHEAD_WPS: remaining_wps = LOOKAHEAD_WPS - len(output_waypoints) for i in range(curr_traffic_light, curr_traffic_light + remaining_wps): waypoint_index = i % len(self.base_waypoints) output_waypoints.append( self.clone_waypoint( self.base_waypoints[waypoint_index])) self.set_waypoint_velocity(output_waypoints, -1, 0) else: for i in range(closest_waypoint_index, closest_waypoint_index + LOOKAHEAD_WPS): waypoint_index = i % len(self.base_waypoints) output_waypoints.append( self.clone_waypoint(self.base_waypoints[waypoint_index])) self.set_waypoint_velocity(output_waypoints, -1, target_vel) lane = Lane() self.final_waypoints = output_waypoints lane.waypoints = output_waypoints rospy.loginfo(rospy.get_name() + ': waypoints published') self.final_waypoints_pub.publish(lane)
def get_lane_object(id, waypoints): lane = Lane() lane.header.frame_id = id lane.header.stamp = rospy.Time.now() lane.waypoints = waypoints return lane
def publish_waypoints(self): # TODO : This function publishes the next waypoints if self.received_waypoints and self.load_current_pose: next_wp_idx = self.get_next_waypoint() #rospy.loginfo("next_wp_idx = %s"% (next_wp_idx)) array_final_waypoints = Lane() red_light_idx_in_final_waypoints = None for idx_waypt in range(LOOKAHEAD_WPS): # Check if drive more than one round idx_waypt_to_append = (next_wp_idx + idx_waypt) % len( self.base_waypoints.waypoints) #Get the information about the waypoint that we will append waypt_to_append = self.base_waypoints.waypoints[ idx_waypt_to_append] # print "TARGET SPEED for each point:", self.get_waypoint_velocity(waypt_to_append) # Append the waypoint to the array of waypoints. array_final_waypoints.waypoints.append(waypt_to_append) #rospy.loginfo("self.cur_red_light_wp_idx = %s, idx_waypt_to_append = %s"% (self.cur_red_light_wp_idx, idx_waypt_to_append)) if (self.cur_red_light_wp_idx and (self.cur_red_light_wp_idx == idx_waypt_to_append)): red_light_idx_in_final_waypoints = idx_waypt #for i in range(5): # print "-------Index:", i," ;target velocity:", self.get_waypoint_velocity(array_final_waypoints.waypoints[i]) #Once the final waypoints is built, check for red-lights and reduce the velocieties of way points to gracefully halt the vehicle at red light if (red_light_idx_in_final_waypoints): total_dist_to_red_light = self.distance( array_final_waypoints.waypoints, 0, red_light_idx_in_final_waypoints) rospy.loginfo("Number of wps to stopline %s" % (red_light_idx_in_final_waypoints)) for i in range(len(array_final_waypoints.waypoints)): if (i < red_light_idx_in_final_waypoints - 13): #reduce the velocity dist = self.distance(array_final_waypoints.waypoints, i, red_light_idx_in_final_waypoints ) #distance to red light decel = 1 # decelerate at 1 m2/s2 vel = math.sqrt( 2 * decel * dist ) #based on distance, calcualte velocity and set it in waypoint if (vel < self.get_waypoint_velocity( array_final_waypoints.waypoints[i])): self.set_waypoint_velocity( array_final_waypoints.waypoints, i, vel) #set velocity else: self.set_waypoint_velocity( array_final_waypoints.waypoints, i, 0) else: for i in range(len(array_final_waypoints.waypoints)): self.set_waypoint_velocity(array_final_waypoints.waypoints, i, self.speed_limit) # for i in range(5): # print "-------Index:", i," ;target velocity:", self.get_waypoint_velocity(array_final_waypoints.waypoints[i]) #TEST START #if(red_light_idx_in_final_waypoints == 1): # self.cur_red_light_wp_idx = (next_wp_idx + 200) % len(self.base_waypoints.waypoints) #TEST END # Publish the Lane info to the /final_waypoints topic self.final_waypoints_pub.publish(array_final_waypoints)
def loop(self): rate = rospy.Rate(self.loop_frequency) while not rospy.is_shutdown(): # Stay still until traffic lights can be detected if not self.tl_detector_initialized: rospy.logwarn( 'Waypoint updater holding until traffic light detector is initialized' ) rate.sleep() continue if self.current_pose != None and self.base_waypoints != None: xyz_position = self.current_pose.position quaternion_orientation = self.current_pose.orientation p = xyz_position qo = quaternion_orientation p_list = [p.x, p.y, p.z] qo_list = [qo.x, qo.y, qo.z, qo.w] euler = euler_from_quaternion(qo_list) yaw_rad = euler[2] closest_waypoint_idx = None closest_waypoint_dist = None for idx in range(len(self.base_waypoints)): wcx, wcy = self.get_on_car_waypoint_x_y(p, yaw_rad, idx) if closest_waypoint_idx is None: closest_waypoint_idx = idx closest_waypoint_dist = math.sqrt(wcx**2 + wcy**2) else: curr_waypoint_dist = math.sqrt(wcx**2 + wcy**2) if curr_waypoint_dist < closest_waypoint_dist: # closest_waypoint_idx = idx closest_waypoint_dist = curr_waypoint_dist wcx, wcy = self.get_on_car_waypoint_x_y( p, yaw_rad, closest_waypoint_idx) while wcx < 0.: closest_waypoint_idx = (closest_waypoint_idx + 1) % self.base_waypoints_count wcx, wcy = self.get_on_car_waypoint_x_y( p, yaw_rad, closest_waypoint_idx) next_waypoints = [] for loop_idx in range(LOOKAHEAD_WPS): wp_idx = (loop_idx + closest_waypoint_idx) % self.base_waypoints_count next_waypoints.append(self.get_waypoint_to_sent(wp_idx)) rospy.loginfo('INDEX {} wc [{:.3f},{:.3f}]'.format( closest_waypoint_idx, wcx, wcy)) lane = Lane() lane.header.frame_id = '/world' lane.header.stamp = rospy.Time(0) lane.waypoints = self.adjust_velocity_to_stop( next_waypoints, closest_waypoint_idx) self.final_waypoints_pub.publish(lane) rate.sleep()