def twist_received(self, twist): """ receive twist and convert to carla vehicle control """ control = CarlaEgoVehicleControl() if twist == Twist(): #stop control.throttle = 0. control.brake = 1. control.steer = 0. else: if twist.linear.x > 0: control.throttle = min(TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x)/ \ TwistToVehicleControl.MAX_LON_ACCELERATION else: control.reverse = True control.throttle = max(-TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x)/ \ -TwistToVehicleControl.MAX_LON_ACCELERATION if twist.angular.z > 0: control.steer = -min(self.max_steering_angle, twist.angular.z)/ \ self.max_steering_angle else: control.steer = -max(-self.max_steering_angle, twist.angular.z)/ \ self.max_steering_angle #invert steer for reverse case if control.reverse: control.steer = -control.steer self.pub.publish(control)
def twist_received(self, twist): """ receive twist and convert to carla vehicle control """ control = CarlaEgoVehicleControl() if twist == Twist(): # stop control.throttle = 0. control.brake = 1. control.steer = 0. else: if twist.linear.x > 0: control.throttle = min( TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x ) / TwistToVehicleControl.MAX_LON_ACCELERATION else: control.reverse = True control.throttle = max( -TwistToVehicleControl.MAX_LON_ACCELERATION, twist.linear.x ) / -TwistToVehicleControl.MAX_LON_ACCELERATION if twist.angular.z > 0: control.steer = -min(self.max_steering_angle, twist.angular.z) / \ self.max_steering_angle else: control.steer = -max(-self.max_steering_angle, twist.angular.z) / \ self.max_steering_angle try: self.pub.publish(control) except rospy.ROSException as e: if not rospy.is_shutdown(): rospy.logwarn("Error while publishing control: {}".format(e))
def tick(self): target_speed = self._get_target_speed() speed_err = target_speed - self.speed cte = self._calc_lateral_error() # cross-track error max_delta = 100 steer = clamp(-1, self.steer_pid.update(cte), 1) # steer = clamp(self.prev_steer - max_delta, steer, self.prev_steer + max_delta) self.prev_steer = steer brake = 0 throttle = clamp(-1, self.throttle_pid.update(speed_err), 1) if throttle < 0: throttle = 0 brake = -throttle # public control command control = CarlaEgoVehicleControl() control.throttle = throttle control.steer = steer control.brake = brake control.hand_brake = False self._vehicle_control_publisher.publish(control) # publish debug info debug_info_msg = msg.ControllerDebugInfo() debug_info_msg.cross_track_error = cte debug_info_msg.speed_error = speed_err debug_info_msg.target_speed = target_speed self._debug_info_publisher.publish(debug_info_msg)
def run_step(self, target_speed): """ Execute one step of local planning which involves running the longitudinal and lateral PID controllers to follow the waypoints trajectory. """ if not self._waypoints_queue: control = CarlaEgoVehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False control.manual_gear_shift = False return control # Buffering the waypoints if not self._waypoint_buffer: for i in range(self._buffer_size): if self._waypoints_queue: self._waypoint_buffer.append( self._waypoints_queue.popleft()) else: break # current vehicle waypoint self._current_waypoint = self.get_waypoint(self._current_pose.position) # target waypoint target_route_point = self._waypoint_buffer[0] # for us redlight-detection self.target_waypoint = self.get_waypoint(target_route_point.position) target_point = PointStamped() target_point.header.frame_id = "map" target_point.point.x = target_route_point.position.x target_point.point.y = target_route_point.position.y target_point.point.z = target_route_point.position.z self._target_point_publisher.publish(target_point) # move using PID controllers control = self._vehicle_controller.run_step(target_speed, self._current_speed, self._current_pose, target_route_point) # purge the queue of obsolete waypoints max_index = -1 sampling_radius = target_speed * 1 / 3.6 # 1 seconds horizon min_distance = sampling_radius * self.MIN_DISTANCE_PERCENTAGE for i, route_point in enumerate(self._waypoint_buffer): if distance_vehicle(route_point, self._current_pose.position) < min_distance: max_index = i if max_index >= 0: for i in range(max_index + 1): self._waypoint_buffer.popleft() return control
def run_step(self, target_speed, current_speed, current_pose, waypoint): """ Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint at a given target_speed. :param target_speed: desired vehicle speed :param waypoint: target location encoded as a waypoint :return: distance (in meters) to the waypoint """ current_time = rospy.get_time() dt = current_time - self._last_control_time if dt == 0.0: dt = 0.000001 control = CarlaEgoVehicleControl() throttle = self._lon_controller.run_step(target_speed, current_speed, dt) steering = self._lat_controller.run_step(current_pose, waypoint, dt) self._last_control_time = current_time control.steer = steering control.throttle = throttle control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control
def run_step(self): """ Execute one step of navigation. """ control = CarlaEgoVehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 0.0 control.hand_brake = False if not self._agent: rospy.loginfo("Waiting for ego vehicle...") return control if not self._route_assigned and self._global_plan: rospy.loginfo("Assigning plan...") self._agent._local_planner.set_global_plan( # pylint: disable=protected-access self._global_plan.poses) self._route_assigned = True else: control, finished = self._agent.run_step(self._target_speed) if finished: self._global_plan = None self._route_assigned = False return control
def steer_cmd_pub(self, steer_cmd, brake_cmd, thorttle_cmd): msg = CarlaEgoVehicleControl() msg.header.stamp = rospy.Time.now() msg.steer = steer_cmd msg.brake = brake_cmd msg.throttle = thorttle_cmd self.pub_steer_cmd.publish(msg)
def emergency_stop(self): control_msg = CarlaEgoVehicleControl() control_msg.steer = 0.0 control_msg.throttle = 0.0 control_msg.brake = 1.0 control_msg.hand_brake = False control_msg.manual_gear_shift = False self._control_cmd_publisher.publish(control_msg)
def control_callback(msg): data = CarlaEgoVehicleControl() data.throttle = msg.velocity data.steer = msg.angle data.brake = 0 data.gear = 1 data.reverse = False data.manual_gear_shift = True pub.publish(data) vehicle_control_manual_override_publisher.publish(vehicle_control_manual_override) auto_pilot_enable_publisher.publish(autopilot_enabled)
def emergency_stop(self): # pylint: disable=no-self-use """ Send an emergency stop command to the vehicle :return: """ control = CarlaEgoVehicleControl() control.steer = 0.0 control.throttle = 0.0 control.brake = 1.0 control.hand_brake = False return control
def main(): ack = Ani() rospy.init_node('Carla_control') rate = rospy.Rate(10) while not rospy.is_shutdown(): cmd_msg = AckermannDrive() msg = CarlaEgoVehicleControl() cmd_msg.speed = ack.speed cmd_msg.steering_angle = ack.steering_angle msg.throttle = cmd_msg.speed msg.steer = cmd_msg.steering_angle ack.pub_callback(msg) rate.sleep()
def main(): global curr_vel client = carla.Client('localhost', 2000) client.set_timeout(5.0) world = client.get_world() m = world.get_map() debug = world.debug ego_control = CarlaEgoVehicleControl() ego_status = CarlaEgoVehicleStatus() rospy.init_node('driver', anonymous=True) pub = rospy.Publisher('/carla/ego_vehicle/vehicle_control_cmd', CarlaEgoVehicleControl, queue_size=1) rospy.Subscriber('/carla/ego_vehicle/vehicle_status', CarlaEgoVehicleStatus, status_callback, queue_size=1) rospy.Subscriber('/carla/ego_vehicle/odometry', Odometry, pos_callback, queue_size=1) waypoints = making_traj(m, debug) wp_xs = waypoints[0] wp_ys = waypoints[1] wp_yaws = waypoints[2] rate = rospy.Rate(10) while not rospy.is_shutdown(): # generate acceleration, and steering speed_error = target_speed - curr_vel acc = Kp * speed_error # caution y and yaw sign steer = stanley_control(curr_pos["DX"], -curr_pos["DY"], -curr_yaw, curr_vel, wp_xs, wp_ys, wp_yaws) print("x: {}, y: {}, yaw: {}".format(curr_pos["DX"], -curr_pos["DY"], -np.degrees(curr_yaw))) # Publishing Topic ego_control.throttle = acc ego_control.steer = steer pub.publish(ego_control) print("steering: {:.3f}".format(steer)) print('\n') rate.sleep()
def run_step(self, target_speed, current_speed, current_pose, waypoint): """ Execute one step of control invoking both lateral and longitudinal PID controllers to reach a target waypoint at a given target_speed. :param target_speed: desired vehicle speed :param waypoint: target location encoded as a waypoint :return: control signal (throttle and steering) """ control = CarlaEgoVehicleControl() throttle = self._lon_controller.run_step(target_speed, current_speed) steering = self._lat_controller.run_step(current_pose, waypoint) control.steer = -steering control.throttle = throttle control.brake = 0.0 control.hand_brake = False control.manual_gear_shift = False return control
def step(self, action): #print("step") # self.ego_vehicle.apply_control(carla.VehicleControl(throttle = action[0], steer = action[1], brake = action[2])) p = CarlaEgoVehicleControl() p.header.stamp = rospy.Time.now() p.throttle = action[0] p.steer = action[1] p.brake = 0 print(action[0]) print(action[1]) print(action[2]) act_pub = rospy.Publisher('/carla/ego_vehicle/vehicle_control_cmd', CarlaEgoVehicleControl, queue_size = 100) act_pub.publish(p) rospy.sleep(1) is_done = False if self.episode_start + SECONDS_PER_EPISODE < time.time(): is_done = True reward = None return OBS.get_observation(), reward, is_done, None # def close(self): # pass # try: # sys.path.append(glob.glob('../carla/dist/carla-*%d.%d-%s.egg' % ( # sys.version_info.major, # sys.version_info.minor, # 'win-amd64' if os.name == 'nt' else 'linux-x86_64'))[0]) # except IndexError # pass # register( # id='CarlaEnv-v0', # entry_point=':custom_environments.envs:CarlaEnv' # )
import tf import numpy as np from carla_msgs.msg import CarlaEgoVehicleControl if __name__ == '__main__': try: rospy.init_node('wp3_ego_vehicle_ctrl_cmd', anonymous=True) time.sleep(1) wp3_ego_vehicle_ctrl_cmd = rospy.Publisher( '/carla/ego_vehicle/vehicle_control_cmd', CarlaEgoVehicleControl, queue_size=10) time.sleep(1) msg = CarlaEgoVehicleControl() msg.throttle = 1.0 msg.steer = 0.0 rate = rospy.Rate(10) # 10hz while not rospy.is_shutdown(): wp3_ego_vehicle_ctrl_cmd.publish(msg) #waypoint_pub.publish("TEST") print('publishing to ego_vehicle ' + str(msg)) #time.sleep(0.2) rate.sleep() except rospy.ROSInterruptException: pass
def main(): """ main function """ role_name = rospy.get_param("~role_name", "ego_vehicle") code_control = CarlaEgoVehicleControl() vehicle_control_publisher = rospy.Publisher( "/carla/{}/vehicle_control_cmd_manual".format(role_name), CarlaEgoVehicleControl, queue_size=1) rospy.init_node('carla_manual_control', anonymous=True) # resolution should be similar to spawned camera with role-name 'view' view_subscriber = rospy.Subscriber( "/carla/{}/camera/rgb/view/image_color".format(role_name), Image, on_view_view_image) top_subscriber = rospy.Subscriber( "/carla/{}/camera/rgb/top/image_color".format(role_name), Image, on_view_top_image) throttle_pub = rospy.Publisher('acceleratorServo', Float32, queue_size=1) break_pub = rospy.Publisher('breakServo', Float32, queue_size=1) steer_pub = rospy.Publisher('steeringServo', Float32, queue_size=1) resolution = {"width": 1920, "height": 1080} pygame.init() pygame.font.init() pygame.display.set_caption("CARLA ROS manual control") world = None skip_first_frame = True prev_timestamp = 0 tot_target_reached = 0 num_min_waypoints = 21 try: start_timestamp = 0 display = pygame.display.set_mode( (resolution['width'], resolution['height']), pygame.HWSURFACE | pygame.DOUBLEBUF) hud = HUD(role_name, resolution['width'], resolution['height']) world = World(role_name, hud) controllerK = KeyboardControl(role_name, hud) clock = pygame.time.Clock() waypoints_file = WAYPOINTS_FILENAME waypoints_filepath =\ os.path.join(os.path.dirname(os.path.realpath(__file__)), WAYPOINTS_FILENAME) waypoints_np = None with open(waypoints_filepath) as waypoints_file_handle: waypoints = list( csv.reader(waypoints_file_handle, delimiter=',', quoting=csv.QUOTE_NONNUMERIC)) waypoints_np = np.array(waypoints) wp_goal_index = 0 local_waypoints = None path_validity = np.zeros((NUM_PATHS, 1), dtype=bool) controller = resources.controller2d.Controller2D(waypoints) bp = resources.behavioural_planner.BehaviouralPlanner( BP_LOOKAHEAD_BASE) lp = resources.local_planner.LocalPlanner(NUM_PATHS, PATH_OFFSET, CIRCLE_OFFSETS, CIRCLE_RADII, PATH_SELECT_WEIGHT, TIME_GAP, A_MAX, SLOW_SPEED, STOP_LINE_BUFFER) clock = pygame.time.Clock() frame = 0 current_timestamp = start_timestamp reached_the_end = False tx = 0 ty = 2.0 tz = 2.0 CameraFOV = 100 Cu = 1920 / 2 Cv = 1080 / 2 f = 1920 / (2 * np.tan(CameraFOV * np.pi / 360)) K = [[f, 0, Cu], [0, f, Cv], [0, 0, 1]] K_np = np.asmatrix(K) t = [[1, 0, 0, tx], [0, 1, 0, ty], [0, 0, 1, tz]] t_np = np.asmatrix(t) while not rospy.core.is_shutdown(): vehicle_control_publisher.publish(code_control) frame = frame + 1 clock.tick_busy_loop(60) if controllerK.parse_events(clock): return hud.tick(clock) world.render(display) pygame.display.flip() ##transform = world.role_name.get_transform() ##vel = world.role_name.get_velocity() prev_timestamp = current_timestamp current_timestamp = rospy.get_time() ##control = world.role_name.get_control() try: tf_listener = hud.tf_listener (position, quaternion) = tf_listener.lookupTransform( '/map', role_name, rospy.Time()) _, _, yaw = tf.transformations.euler_from_quaternion( quaternion) yaw = -math.degrees(yaw) x = position[0] y = -position[1] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): x = 0 y = 0 z = 0 yaw = 0 current_x = x current_y = y current_yaw = np.radians(yaw) velocity = hud.vehicle_status.velocity current_speed = velocity open_loop_speed = lp._velocity_planner.get_open_loop_speed( current_timestamp - prev_timestamp) #open_loop_speed = lp._velocity_planner.get_open_loop_speed(current_timestamp - prev_timestamp) if frame % 5 == 0: # try: # cv2.imshow("view_image",view_bgr_image) # cv2.imshow("top image",top_bgr_image) # #cv2.imshow("top image",top_bgr_image) # #cv2.waitKey(1) #except Exception as e: # print(e) #Lane detection ego_state = [ current_x + 2, current_y, current_yaw, open_loop_speed ] print(ego_state) bp.set_lookahead(BP_LOOKAHEAD_BASE + BP_LOOKAHEAD_TIME * open_loop_speed) bp.transition_state(waypoints, waypoints, ego_state, current_speed) goal_state_set = lp.get_goal_state_set(bp._goal_index, bp._goal_state, waypoints, ego_state) #print("goal_state_set",goal_state_set) paths_vehicle_coordinate, path_validity = lp.plan_paths( goal_state_set) # print("Paths_local",paths_vehicle_coordinate) paths = resources.local_planner.transform_paths( paths_vehicle_coordinate, ego_state) #print("Paths_global",paths) best_index = lp._collision_checker.select_best_path_index( paths, bp._goal_state_hyp) ################################################ Compute paths visualize##################### #Whole Code # try: # # paths_vehicle_coordinate_np = np.array(paths_vehicle_coordinate) # # Each_Path_length = len(paths_vehicle_coordinate_np[0][0]) # # pixel_coordinate_paths = [] # # for x in range(0,NUM_PATHS): # for i in range(0,Each_Path_length): # world_coordinate = [paths_vehicle_coordinate_np[x][1][i],1,paths_vehicle_coordinate_np[x][0][i],1] # world_coordinate_np = np.asmatrix(world_coordinate) # camera_coordinates = np.dot(np.dot(K_np,t),world_coordinate_np.T) # pixel_coordinates = [[camera_coordinates[0] / camera_coordinates[2]], # [camera_coordinates[1] / camera_coordinates[2]] # ] # pixel_coordinate_paths.append(pixel_coordinates) # pixel_coordinate_paths_np = np.array(pixel_coordinate_paths) # pixel_plot = pixel_coordinate_paths_np[:,:,0, 0, 0] # pix_1 = pixel_plot[0:49] # pix_2 = pixel_plot[49:98] # pix_3 = pixel_plot[98:147] # pix_4 = pixel_plot[147:196] # pix_5 = pixel_plot[196:245] # pix_6 = pixel_plot[245:294] # pix_7 = pixel_plot[294:343] # # line_1 = np.array((pix_1.T[0],pix_1.T[1])) # line_1 = pix_1.astype(int) # line_1 = line_1.reshape((-1,1,2)) # # line_2 = pix_2.astype(int) # line_2 = line_2.reshape((-1,1,2)) # # line_3 = pix_3.astype(int) # line_3 = line_3.reshape((-1,1,2)) #line_4 = pix_4.astype(int) #line_4 = line_4.reshape((-1,1,2)) #line_5 = pix_5.astype(int) #line_5 = line_5.reshape((-1,1,2)) #line_6 = pix_6.astype(int) #line_6 = line_6.reshape((-1,1,2)) #line_7 = pix_7.astype(int) # line_7 = line_7.reshape((-1,1,2)) # on_view_image_global_np = np.array(on_view_image_global) # view_bgr_image_np = np.array(on_view_image_global) # line_array = [line_1,line_2,line_3,line_4,line_5,line_6,line_7] # print(line_array) # print("works 1") # image = cv2.polylines(on_view_image_global_np,[line_1,line_2,line_3,line_4,line_5,line_6,line_7], # isClosed=False, # color=(255, 255, 255), # thickness=10, # lineType=cv2.LINE_AA) # print("Works here") # image = cv2.polylines(image,[line_array[best_index]], # isClosed=False, # color=(0, 255, 0), # thickness=10, # lineType=cv2.LINE_AA) # cv2.imshow("front image",image[:,:,::-1]) # cv2.waitKey(1) # #except Exception as e: # print(e) try: if best_index == None: best_path = lp._prev_best_path else: best_path = paths[best_index] lp._prev_best_path = best_path except: continue desired_speed = bp._goal_state[2] decelerate_to_stop = False print("Desired_speed", desired_speed) print("Current_speed", current_speed) local_waypoints = lp._velocity_planner.compute_velocity_profile( best_path, desired_speed, ego_state, current_speed) print("local_waypoints", local_waypoints) if local_waypoints != None: wp_distance = [] # distance array local_waypoints_np = np.array(local_waypoints) for i in range(1, local_waypoints_np.shape[0]): wp_distance.append( np.sqrt((local_waypoints_np[i, 0] - local_waypoints_np[i - 1, 0])**2 + (local_waypoints_np[i, 1] - local_waypoints_np[i - 1, 1])**2)) wp_distance.append(0) wp_interp = [] for i in range(local_waypoints_np.shape[0] - 1): wp_interp.append(list(local_waypoints_np[i])) num_pts_to_interp = int(np.floor(wp_distance[i] /\ float(INTERP_DISTANCE_RES)) - 1) wp_vector = local_waypoints_np[ i + 1] - local_waypoints_np[i] wp_uvector = wp_vector / np.linalg.norm(wp_vector[0:2]) for j in range(num_pts_to_interp): next_wp_vector = INTERP_DISTANCE_RES * float( j + 1) * wp_uvector wp_interp.append( list(local_waypoints_np[i] + next_wp_vector)) wp_interp.append(list(local_waypoints_np[-1])) controller.update_waypoints(wp_interp) pass if local_waypoints != None and local_waypoints != []: controller.update_values(current_x, current_y, current_yaw, current_speed, current_timestamp, frame) controller.update_controls() cmd_throttle, cmd_steer, cmd_brake = controller.get_commands() else: cmd_throttle = 0.0 cmd_steer = 0.0 cmd_brake = 0.0 if skip_first_frame and frame == 0: pass elif local_waypoints == None: pass else: ## if i % 5 == 0: wp_interp_np = np.array(wp_interp) path_indices = np.floor( np.linspace(0, wp_interp_np.shape[0] - 1, INTERP_MAX_POINTS_PLOT)) #if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop: # agent.reroute(spawn_points) # tot_target_reached += 1 # world.hud.notification("The target has been reached " + # str(tot_target_reached) + " times.", seconds=4.0) #elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop: # print("Target reached, mission accomplished...") # break #speed_limit = world.role_name.get_speed_limit() #agent.get_local_planner().set_speed(speed_limit) #cmd_throttle = 0.4 #cmd_steer = 0 #cmd_brake = 0 dist_to_last_waypoint = np.linalg.norm( np.array([ waypoints[-1][0] - current_x, waypoints[-1][1] - current_y ])) if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT: reached_the_end = True if reached_the_end: cmd_steer = np.fmax(np.fmin(0, 1.0), -1.0) cmd_throttle = np.fmax(np.fmin(0, 1.0), 0) cmd_brake = np.fmax(np.fmin(0.1, 1.0), 0) break #cmd_steer = np.fmax(np.fmin(cmd_steer, 1.0), -1.0) kmd_throttle = np.fmax(np.fmin(cmd_throttle, 1.0), 0) #cmd_brake = np.fmax(np.fmin(cmd_brake, 1.0), 0) print("Throttle before", cmd_throttle) print("Throttle after", kmd_throttle) code_control.throttle = cmd_throttle code_control.steer = cmd_steer code_control.brake = cmd_brake code_control.gear = 3 vehicle_control_publisher.publish(code_control) print("Code control", code_control) throttle_angle = code_control.throttle * 135 #Kind of makes sense but the maximum is 130 degrees lets say random guess :) #Ill have to change the zeros break_angle = code_control.brake * 135 steer_angle = ( code_control.steer * -135 ) + 95 #here we state that 0 is the 95 degrees so anything below tends towards 0 and viceversa print("steer", cmd_steer) throttle_pub.publish(throttle_angle) break_pub.publish(break_angle) steer_pub.publish(steer_angle) #world.role_name.apply_control(carla.VehicleControl(throttle=cmd_throttle, # steer=cmd_steer, # brake=cmd_brake)) if reached_the_end: print("Reached the end of path. Writing to controller_output...") else: print("Exceeded assessment time. Writing to controller_output...") finally: if world is not None: world.destroy() pygame.quit()
def main(): """ main function """ rospy.init_node('carla_manual_control', anonymous=True) role_name = rospy.get_param("~role_name", "ego_vehicle") vehicle_control_publisher = rospy.Publisher( "/carla/{}/vehicle_control_cmd_manual".format(role_name), CarlaEgoVehicleControl, queue_size=10) # resolution should be similar to spawned camera with role-name 'view' resolution = {"width": 800, "height": 600} pygame.init() pygame.font.init() pygame.display.set_caption("CARLA ROS manual control") world = None tf_listener = tf.TransformListener() code_control = CarlaEgoVehicleControl() vehicle_status = CarlaEgoVehicleStatus() skip_first_frame = True prev_timestamp = 0 tot_target_reached = 0 num_min_waypoints = 21 try: start_timestamp = 0 display = pygame.display.set_mode( (resolution['width'], resolution['height']), pygame.HWSURFACE | pygame.DOUBLEBUF) hud = HUD(role_name, resolution['width'], resolution['height']) world = World(role_name, hud) controllerK = KeyboardControl(role_name, hud) waypoints_file = WAYPOINTS_FILENAME waypoints_filepath =\ os.path.join(os.path.dirname(os.path.realpath(__file__)), WAYPOINTS_FILENAME) waypoints_np = None with open(waypoints_filepath) as waypoints_file_handle: waypoints = list( csv.reader(waypoints_file_handle, delimiter=',', quoting=csv.QUOTE_NONNUMERIC)) waypoints_np = np.array(waypoints) wp_goal_index = 0 local_waypoints = None path_validity = np.zeros((NUM_PATHS, 1), dtype=bool) controller = resources.controller2d.Controller2D(waypoints) bp = resources.behavioural_planner.BehaviouralPlanner( BP_LOOKAHEAD_BASE) lp = resources.local_planner.LocalPlanner(NUM_PATHS, PATH_OFFSET, CIRCLE_OFFSETS, CIRCLE_RADII, PATH_SELECT_WEIGHT, TIME_GAP, A_MAX, SLOW_SPEED, STOP_LINE_BUFFER) clock = pygame.time.Clock() frame = 0 current_timestamp = start_timestamp reached_the_end = False while not rospy.core.is_shutdown(): frame = frame + 1 clock.tick_busy_loop(60) world.render(display) if controllerK.parse_events(clock): return hud.tick(clock) pygame.display.flip() ##transform = world.role_name.get_transform() ##vel = world.role_name.get_velocity() prev_timestamp = current_timestamp current_timestamp = rospy.get_time() ##control = world.role_name.get_control() try: (position, quaternion) = tf_listener.lookupTransform( '/map', role_name, rospy.Time()) _, _, yaw = tf.transformations.euler_from_quaternion( quaternion) #yaw = -math.degrees(yaw) x = position[0] y = -position[1] except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): x = 0 y = 0 z = 0 yaw = 0 current_x = x current_y = y #current_yaw = np.radians(yaw) current_yaw = -yaw velocity = hud.vehicle_status.velocity current_speed = velocity print("current_speed", current_speed) open_loop_speed = lp._velocity_planner.get_open_loop_speed( current_timestamp - prev_timestamp) #open_loop_speed = lp._velocity_planner.get_open_loop_speed(current_timestamp - prev_timestamp) print("open_loop_speed", open_loop_speed) if frame % 5 == 0: #Lane detection ego_state = [ current_x, current_y, current_yaw, open_loop_speed ] bp.set_lookahead(BP_LOOKAHEAD_BASE + BP_LOOKAHEAD_TIME * open_loop_speed) bp.transition_state(waypoints, waypoints, ego_state, current_speed) goal_state_set = lp.get_goal_state_set(bp._goal_index, bp._goal_state, waypoints, ego_state) paths, path_validity = lp.plan_paths(goal_state_set) paths = resources.local_planner.transform_paths( paths, ego_state) best_index = lp._collision_checker.select_best_path_index( paths, bp._goal_state_hyp) try: if best_index == None: best_path = lp._prev_best_path else: best_path = paths[best_index] lp._prev_best_path = best_path except: continue desired_speed = bp._goal_state[2] decelerate_to_stop = False local_waypoints = lp._velocity_planner.compute_velocity_profile( best_path, desired_speed, ego_state, current_speed) if local_waypoints != None: wp_distance = [] # distance array local_waypoints_np = np.array(local_waypoints) for i in range(1, local_waypoints_np.shape[0]): wp_distance.append( np.sqrt((local_waypoints_np[i, 0] - local_waypoints_np[i - 1, 0])**2 + (local_waypoints_np[i, 1] - local_waypoints_np[i - 1, 1])**2)) wp_distance.append(0) wp_interp = [] for i in range(local_waypoints_np.shape[0] - 1): wp_interp.append(list(local_waypoints_np[i])) num_pts_to_interp = int(np.floor(wp_distance[i] /\ float(INTERP_DISTANCE_RES)) - 1) wp_vector = local_waypoints_np[ i + 1] - local_waypoints_np[i] wp_uvector = wp_vector / np.linalg.norm(wp_vector[0:2]) for j in range(num_pts_to_interp): next_wp_vector = INTERP_DISTANCE_RES * float( j + 1) * wp_uvector wp_interp.append( list(local_waypoints_np[i] + next_wp_vector)) wp_interp.append(list(local_waypoints_np[-1])) controller.update_waypoints(wp_interp) pass if local_waypoints != None and local_waypoints != []: controller.update_values(current_x, current_y, current_yaw, current_speed, current_timestamp, frame) controller.update_controls() cmd_throttle, cmd_steer, cmd_brake = controller.get_commands() else: cmd_throttle = 0.0 cmd_steer = 0.0 cmd_brake = 0.0 if skip_first_frame and frame == 0: pass elif local_waypoints == None: pass else: ## if i % 5 == 0: wp_interp_np = np.array(wp_interp) path_indices = np.floor( np.linspace(0, wp_interp_np.shape[0] - 1, INTERP_MAX_POINTS_PLOT)) #if len(agent.get_local_planner().waypoints_queue) < num_min_waypoints and args.loop: # agent.reroute(spawn_points) # tot_target_reached += 1 # world.hud.notification("The target has been reached " + # str(tot_target_reached) + " times.", seconds=4.0) #elif len(agent.get_local_planner().waypoints_queue) == 0 and not args.loop: # print("Target reached, mission accomplished...") # break #speed_limit = world.role_name.get_speed_limit() #agent.get_local_planner().set_speed(speed_limit) #cmd_throttle = 0.4 #cmd_steer = 0 #cmd_brake = 0 dist_to_last_waypoint = np.linalg.norm( np.array([ waypoints[-1][0] - current_x, waypoints[-1][1] - current_y ])) if dist_to_last_waypoint < DIST_THRESHOLD_TO_LAST_WAYPOINT: reached_the_end = True if reached_the_end: cmd_steer = np.fmax(np.fmin(0, 1.0), -1.0) cmd_throttle = np.fmax(np.fmin(0, 1.0), 0) cmd_brake = np.fmax(np.fmin(0.1, 1.0), 0) break cmd_steer = np.fmax(np.fmin(cmd_steer, 1.0), -1.0) cmd_throttle = np.fmax(np.fmin(cmd_throttle, 1.0), 0) cmd_brake = np.fmax(np.fmin(cmd_brake, 1.0), 0) #print("throttle",cmd_throttle) code_control.throttle = cmd_throttle code_control.steer = cmd_steer code_control.brake = cmd_brake #servo_fine() #servo_fine() #throttle_angle = code_control.throttle * 270 #steer_angle = code_control.steer * 270 #break_angle = code_control.brake * 270 #rospy.loginfo(throttle_angle) #rospy.loginfo(break_angle) #rospy.loginfo(steer_angle) #throttle_pub = rospy.Publisher('acceleratorServo', Float32, queue_size=10) #break_pub = rospy.Publisher('breakServo', Float32, queue_size=10) #steer_pub = rospy.Publisher('steeringServo', Float32, queue_size=10) #throttle_pub.publish(throttle_angle) #break_pub.publish(break_angle) #steer_pub.publish(steer_angle) #print(code_control.throttle) #print("Ros_throttle",code_control.throttle) vehicle_control_publisher.publish(code_control) #print(code_control) #world.role_name.apply_control(carla.VehicleControl(throttle=cmd_throttle, # steer=cmd_steer, # brake=cmd_brake)) if reached_the_end: print("Reached the end of path. Writing to controller_output...") else: print("Exceeded assessment time. Writing to controller_output...") finally: if world is not None: world.destroy() pygame.quit()
def Control(self): rate = rospy.Rate(10) lon_param = { 'K_P': 0.5, 'K_I': 0.5, 'K_D': 0 } # Set PID values for longitudinal controller lat_param = { 'K_P': 0.5, 'K_I': 0.3, 'K_D': 0 } # Set PID values for lateral controller vehicle_controller = VehiclePIDController( self.vehicle, lon_param, lat_param) # Calling vehicle controller class from controller.py i = 0 for k in range(1, len(self.current_route) ): # Iterate through all the waypoints in the route self.Distance(self.current_route[i][0], self.veh_pos.transform) self.Waypoints() rospy.Subscriber( '/machine_learning/output', Int16, self.Detection ) # Subscribes to topic for Stop sign detection. Need to run carla_detect_objects.py script to obtain detection while self.distance > 0.5: # Control the vehicle until the distance of the next waypoint and the vehicle is less than 0.5 m self.Actor( ) # Call Actor function to update vehicle's location control = vehicle_controller.run_step( 15, self.current_route[i] [0]) # Feeds the controller the waypoints one by one self.velocity = self.vehicle.get_velocity( ) # Get vehicle velocity if self.detection == 11: # Stop sign detection(apply brakes). Our ML has a class ID of 11 for stop signs print('Object detected, apply brakes') msg = CarlaEgoVehicleControl( ) # Ego vehicle's control message msg.throttle = 0 msg.steer = control.steer msg.brake = 1 msg.hand_brake = control.hand_brake msg.reverse = control.reverse msg.gear = 1 msg.manual_gear_shift = control.manual_gear_shift self.detection = None elif len(self.current_route) - 5 <= k <= len( self.current_route ): # If the ith waypoint is between the last waypoint minus five apply brakes msg = CarlaEgoVehicleControl() msg.throttle = 0 msg.steer = control.steer msg.brake = 1 msg.hand_brake = control.hand_brake msg.reverse = control.reverse msg.gear = 1 msg.manual_gear_shift = control.manual_gear_shift print('You arrived to your destination!!') else: # If neither scenario happen, keep driving msg = CarlaEgoVehicleControl() msg.throttle = control.throttle msg.steer = control.steer msg.brake = control.brake msg.hand_brake = control.hand_brake msg.reverse = control.reverse msg.gear = 1 msg.manual_gear_shift = control.manual_gear_shift self.Publisher(msg) rate.sleep() self.Distance( self.current_route[i][0], self.veh_pos.transform ) # Calculates the Euclidean distance between the vehicle and the next waypoint in every iteration i += 1
rospy.init_node('my_node') rospy.Subscriber('/carla/ego_vehicle/rgb_front/image', Image, callback, queue_size=1) pub = rospy.Publisher('/carla/ego_vehicle/vehicle_control_cmd_manual', CarlaEgoVehicleControl, queue_size=1) pub_msg = CarlaEgoVehicleControl() #rate = rospy.Rate() while not rospy.is_shutdown(): if right == 0: pub_msg.throttle = 0.5 pub_msg.steer = steer pub.publish(pub_msg) #rate.sleep() pub_msg.throttle = 0.3 pub_msg.steer = -steer pub.publish(pub_msg) #rate.sleep() pub_msg.throttle = 1 pub_msg.steer = 0 pub.publish(pub_msg) #rate.sleep() else: pub_msg.throttle = 0.3 pub_msg.steer = steer pub.publish(pub_msg) #rate.sleep()