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 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 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 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 __init__(self, role_name): self.role_name = role_name self.vehicle_control_manual_override_publisher = rospy.Publisher( "/carla/{}/vehicle_control_manual_override".format(self.role_name), Bool, queue_size=1, latch=True, ) self.vehicle_control_manual_override = True self.auto_pilot_enable_publisher = rospy.Publisher( "/carla/{}/enable_autopilot".format(self.role_name), Bool, queue_size=1) self.vehicle_control_publisher = rospy.Publisher( "/carla/{}/vehicle_control_cmd_manual".format(self.role_name), CarlaEgoVehicleControl, queue_size=1, ) self._autopilot_enabled = False self._control = CarlaEgoVehicleControl() self.set_autopilot(self._autopilot_enabled) self._steer_cache = 0.0 self.set_vehicle_control_manual_override( self.vehicle_control_manual_override) # disable manual override
def run_step(self, target_speed): # pylint: disable=no-self-use,unused-argument """ Execute one step of navigation. :return: control """ control = CarlaEgoVehicleControl() return control
def __init__(self, role_name, hud): self.role_name = role_name self.hud = hud self._autopilot_enabled = False self._control = CarlaEgoVehicleControl() self._steer_cache = 0.0 self.vehicle_control_manual_override_publisher = rospy.Publisher( "/carla/{}/vehicle_control_manual_override".format(self.role_name), Bool, queue_size=1, latch=True) self.vehicle_control_manual_override = False self.auto_pilot_enable_publisher = rospy.Publisher( "/carla/{}/enable_autopilot".format(self.role_name), Bool, queue_size=1) self.vehicle_control_publisher = rospy.Publisher( "/carla/{}/vehicle_control_cmd_manual".format(self.role_name), CarlaEgoVehicleControl, queue_size=1) self.carla_status_subscriber = rospy.Subscriber( "/carla/status", CarlaStatus, self._on_new_carla_frame) self.set_autopilot(self._autopilot_enabled) self.set_vehicle_control_manual_override( self.vehicle_control_manual_override) # disable manual override
def __init__(self, role_name, hud): self.role_name = role_name self.hud = hud self.vehicle_control_manual_override_publisher = rospy.Publisher( "/carla/{}/vehicle_control_manual_override".format(self.role_name), Bool, queue_size=1, latch=True) self.vehicle_control_manual_override = False self.auto_pilot_enable_publisher = rospy.Publisher( "/carla/{}/enable_autopilot".format(self.role_name), Bool, queue_size=1) self.vehicle_control_publisher = rospy.Publisher( "/carla/{}/vehicle_control_cmd_manual".format(self.role_name), CarlaEgoVehicleControl, queue_size=1) self._autopilot_enabled = False self._control = CarlaEgoVehicleControl() self.set_autopilot(self._autopilot_enabled) self._steer_cache = 0.0 self.set_vehicle_control_manual_override( self.vehicle_control_manual_override) # disable manual override self.hud.notification("Press 'H' or '?' for help.", seconds=4.0)
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: 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, 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 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): 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 __init__(self, carla_actor, parent, communication, vehicle_control_applied_callback): """ Constructor :param carla_actor: carla actor object :type carla_actor: carla.Actor :param parent: the parent of this :type parent: carla_icv_bridge.Parent :param communication: communication-handle :type communication: carla_icv_bridge.communication """ super(EgoVehicle, self).__init__(carla_actor=carla_actor, parent=parent, communication=communication, prefix=carla_actor.attributes.get('role_name')) self.vehicle_info_published = False self.vehicle_control_override = False self._vehicle_control_applied_callback = vehicle_control_applied_callback self.sub1 = Subscriber(self.get_topic_prefix() + "/vehicle_control_cmd") self.sub2 = Subscriber(self.get_topic_prefix() + "/vehicle_control_cmd_manual") self.sub3 = Subscriber(self.get_topic_prefix() + "/vehicle_control_manual_override") self.sub4 = Subscriber(self.get_topic_prefix() + "/enable_autopilot") self.sub5 = Subscriber(self.get_topic_prefix() + "/twist_cmd") self.Sec_loop = 0.02 self.control_subscriber = CarlaEgoVehicleControl() self.manual_control_subscriber = CarlaEgoVehicleControl() self.control_override_subscriber = Bool() self.enable_autopilot_subscriber = Bool() self.twist_control_subscriber = Twist() self.pub1 = Publisher(self.get_topic_prefix() + "/vehicle_info") self.pub2 = Publisher(self.get_topic_prefix() + "/vehicle_status") self.update_command_thread = Thread( target=self._update_commands_thread) self.update_command_thread.start()
def dummy_agent(): rospy.init_node("dummy_agent") rate = rospy.Rate(20) # 20hz publisher = rospy.Publisher("/carla/hero/vehicle_control_cmd", CarlaEgoVehicleControl, queue_size=1) ready = rospy.Publisher("/carla/hero/status", Bool, latch=True, queue_size=1) ready.publish(True) while not rospy.is_shutdown(): control_msg = CarlaEgoVehicleControl() control_msg.header.stamp = rospy.Time.now() control_msg.throttle = 1.0 publisher.publish(control_msg) rate.sleep()
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): """ 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 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' # )
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 __init__(self, role_name, hud, node): self.role_name = role_name self.hud = hud self.node = node self._autopilot_enabled = False self._control = CarlaEgoVehicleControl() self._steer_cache = 0.0 if ROS_VERSION == 1: self.callback_group = None elif ROS_VERSION == 2: self.callback_group = ReentrantCallbackGroup() fast_qos = QoSProfile(depth=10) fast_latched_qos = QoSProfile(depth=10, durability=latch_on) # imported from ros_compat. self.vehicle_control_manual_override_publisher = \ self.node.new_publisher(Bool, "/carla/{}/vehicle_control_manual_override".format( self.role_name), qos_profile=fast_latched_qos, callback_group=self.callback_group) self.vehicle_control_manual_override = False self.auto_pilot_enable_publisher = \ self.node.new_publisher(Bool, "/carla/{}/enable_autopilot".format(self.role_name), qos_profile=fast_qos, callback_group=self.callback_group) self.vehicle_control_publisher = \ self.node.new_publisher(CarlaEgoVehicleControl, "/carla/{}/vehicle_control_cmd_manual".format(self.role_name), qos_profile=fast_qos, callback_group=self.callback_group) self.carla_status_subscriber = self.node.create_subscriber(CarlaStatus, "/carla/status", self._on_new_carla_frame, callback_group=self.callback_group) self.set_autopilot(self._autopilot_enabled) self.set_vehicle_control_manual_override( self.vehicle_control_manual_override) # disable manual override
def __init__(self, role_name, hud): self.role_name = role_name self.hud = hud self._autopilot_enabled = False self._control = CarlaEgoVehicleControl() self._deep = CarlaDeepData() self._steer_cache = 0.0 self.vehicle_deep_pb = rospy.Publisher("/carla/vehicle_deep_data", CarlaDeepData, queue_size=1) self.vehicle_deep_sub_status = rospy.Subscriber( "/carla/{}/vehicle_status".format(self.role_name), CarlaEgoVehicleStatus, self.update_deep) self.vehicle_deep_sub_image = rospy.Subscriber( "/carla/ego_vehicle/camera/rgb/front/image_color", Image, self.update_deep_image) self.vehicle_control_manual_override_publisher = rospy.Publisher( "/carla/{}/vehicle_control_manual_override".format(self.role_name), Bool, queue_size=1, latch=True) self.vehicle_control_manual_override = False self.auto_pilot_enable_publisher = rospy.Publisher( "/carla/{}/enable_autopilot".format(self.role_name), Bool, queue_size=1) self.vehicle_control_publisher = rospy.Publisher( "/carla/{}/vehicle_control_cmd_manual".format(self.role_name), CarlaEgoVehicleControl, queue_size=1) self.carla_status_subscriber = rospy.Subscriber( "/carla/status", CarlaStatus, self._on_new_carla_frame) self.set_autopilot(self._autopilot_enabled) self.set_vehicle_control_manual_override( self.vehicle_control_manual_override) # disable manual override
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 servo_fine(): 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) rospy.init_node('servo_controller', anonymous=True) rate = rospy.Rate(10) while not rospy.is_shutdown(): code_control = CarlaEgoVehicleControl() 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.publish(throttle_angle) break_pub.publish(break_angle) steer_pub.publish(steer_angle) rate.sleep()
def __init__(self, role_name, host, port): self.role_name = role_name client = carla.Client(host, port) self.world = client.get_world() self.map = self.world.get_map() self.vehicle_dyn = VehicleDynamics() self.state = None self.input = [0, 0] self.vehicle = None self.speed_control = PID(Kp=1.5, Ki=1.5, Kd=0., sample_time=0.05, output_limits=(0., 1.)) self.vehicle_control_cmd = CarlaEgoVehicleControl(throttle=0.) self.find_ego_vehicle() self.init_state() subControl = rospy.Subscriber('/carla/%s/vehicle_control'%role_name, CarlaEgoVehicleControl, self.controlCallback) # subControl = rospy.Subscriber('/carla/%s/vehicle_control'%role_name, CarlaEgoVehicleControl, self.controlCallback) subAckermann = rospy.Subscriber('/carla/%s/ackermann_control'%role_name, AckermannDrive, self.ackermannCallback)
def __init__(self, role_name, hud, node): self.role_name = role_name self.hud = hud self.node = node self._autopilot_enabled = False self._control = CarlaEgoVehicleControl() self._steer_cache = 0.0 fast_qos = QoSProfile(depth=10) fast_latched_qos = QoSProfile( depth=10, durability=DurabilityPolicy.TRANSIENT_LOCAL) self.vehicle_control_manual_override_publisher = self.node.new_publisher( Bool, "/carla/{}/vehicle_control_manual_override".format(self.role_name), qos_profile=fast_latched_qos) self.vehicle_control_manual_override = False self.auto_pilot_enable_publisher = self.node.new_publisher( Bool, "/carla/{}/enable_autopilot".format(self.role_name), qos_profile=fast_qos) self.vehicle_control_publisher = self.node.new_publisher( CarlaEgoVehicleControl, "/carla/{}/vehicle_control_cmd_manual".format(self.role_name), qos_profile=fast_qos) self.carla_status_subscriber = self.node.new_subscription( CarlaStatus, "/carla/status", self._on_new_carla_frame, qos_profile=10) self.set_autopilot(self._autopilot_enabled) self.set_vehicle_control_manual_override( self.vehicle_control_manual_override) # disable manual override
def throttle_control_callback(cmd): global throttle throttle = cmd.data if __name__ == '__main__': rospy.Subscriber('/carla/{}/cv_steer'.format(role_name), Float32, cv_steer_callback) rospy.Subscriber('/carla/{}/throttle_control'.format(role_name), Float32, throttle_control_callback) throttle_value = 0.0 if len(sys.argv) > 1: throttle_value = float(sys.argv[1]) #TODO: declare to publish CarlaEgoVehicleControl message to 'cmd_topic_name' topic. Set queue_size to 1. pub_cmd = rospy.Publisher(cmd_topic_name, CarlaEgoVehicleControl, queue_size=1) rospy.init_node('driver_byl24', anonymous=False) rate = rospy.Rate(20) while not rospy.is_shutdown(): #TODO: publish CarlaEgoVehicleControl message rospy.loginfo('name: Brandon Liu, throttle: {}, steer: {}'.format( throttle, cv_steer)) pub_cmd.publish( CarlaEgoVehicleControl(throttle=throttle, steer=cv_steer)) rate.sleep()
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()
import glob from geometry_msgs.msg import Twist sys.path.append( 'home/kankan/carla/Dist/CARLA_Shipping_0.9.7-2-g73f91e0b/LinuxNoEditor/PythonAPI/carla/dist/carla-0.9.7-py3.6-linux-x86_64.egg' ) import carla from carla_msgs.msg import CarlaEgoVehicleControl import std_msgs from pynput.keyboard import Key, Listener speed = 0.0 threshold = 0.5 steer = 0.0 vel_msg = CarlaEgoVehicleControl() velocity_publisher = rospy.Publisher('/carla/ego_vehicle/vehicle_control_cmd', CarlaEgoVehicleControl, queue_size=10) class KeyboardPublisher: def on_press(self, key): global steer #print(key) #<-- this prints out the key press and then crashes try: if key.char == 'd' and steer < 1: steer = steer + 0.2 if key.char == 'a' and steer > -1: steer = steer - 0.2 except AttributeError:
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
draw_lines(temp, R_avg) result = weighted_img(temp, src) #overlap cv2.imshow('result', result) #print cv2.waitKey(2) 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)