void Update() { if (targetEnv != ROSTargetEnvironment.APOLLO && targetEnv != ROSTargetEnvironment.AUTOWARE && targetEnv != ROSTargetEnvironment.LGSVL) { return; } if (Bridge == null || Bridge.Status != Ros.Status.Connected || !PublishMessage) { return; } if (Time.time < NextSend) { return; } if (targetEnv == ROSTargetEnvironment.APOLLO) { Vector3 vel = mainRigidbody.velocity; Vector3 eul = mainRigidbody.rotation.eulerAngles; float dir; if (eul.y >= 0) { dir = 45 * Mathf.Round((eul.y % 360) / 45.0f); } else { dir = 45 * Mathf.Round((eul.y % 360 + 360) / 45.0f); } // (TODO) check for leap second issues. var gps_time = DateTimeOffset.FromUnixTimeSeconds((long)gps.measurement_time).DateTime.ToLocalTime(); var apolloMessage = new Ros.Apollo.ChassisMsg() { engine_started = true, engine_rpm = controller.currentRPM, speed_mps = vel.magnitude, odometer_m = 0, fuel_range_m = 0, throttle_percentage = input_controller.throttle * 100, brake_percentage = input_controller.brake * 100, steering_percentage = -controller.steerInput * 100, // steering_torque_nm parking_brake = controller.handbrakeApplied, high_beam_signal = (controller.headlightMode == 2), low_beam_signal = (controller.headlightMode == 1), left_turn_signal = controller.leftTurnSignal, right_turn_signal = controller.rightTurnSignal, // horn wiper = (controller.wiperStatus != 0), // disengage_status driving_mode = Ros.Apollo.Chassis.DrivingMode.COMPLETE_AUTO_DRIVE, // error_code // gear_location // steering_timestamp // signal // engage_advice chassis_gps = new Ros.Apollo.Chassis.ChassisGPS() { latitude = gps.latitude_orig, longitude = gps.longitude_orig, gps_valid = gps.PublishMessage, year = gps_time.Year, month = gps_time.Month, day = gps_time.Day, hours = gps_time.Hour, minutes = gps_time.Minute, seconds = gps_time.Second, compass_direction = dir, pdop = 0.1, is_gps_fault = false, is_inferred = false, altitude = gps.height, heading = eul.y, hdop = 0.1, vdop = 0.1, quality = Ros.Apollo.Chassis.GpsQuality.FIX_3D, num_satellites = 15, gps_speed = vel.magnitude, }, header = new Ros.ApolloHeader() { timestamp_sec = Ros.Time.Now().secs, module_name = "chassis", sequence_num = seq, }, }; Bridge.Publish(ApolloTopic, apolloMessage); } if (targetEnv == ROSTargetEnvironment.LGSVL) { var simulatorMessage = new Ros.TwistStamped() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = seq++, frame_id = "", }, twist = new Ros.Twist() { linear = new Ros.Vector3() { x = controller.steerInput, y = 0, z = 0, }, angular = new Ros.Vector3() { x = 0, y = 0, z = 0, }, }, }; Bridge.Publish(SimulatorTopic, simulatorMessage); } }
void FixedUpdate() { float steerInput = input.SteerInput; float accelInput = input.AccelBrakeInput; var hasWorkingSteerwheel = (steerwheelInput != null && SteeringWheelInputController.available); if (!selfDriving || underKeyboardControl) //manual control or keyboard-interrupted self driving { //grab input values if (!selfDriving) { steerInput = input.SteerInput; accelInput = input.AccelBrakeInput; } else if (underKeyboardControl) { steerInput = keyboardInput.SteerInput; accelInput = keyboardInput.AccelBrakeInput; } if (underKeyboardControl || underSteeringWheelControl) { float k = 0.4f + 0.6f * controller.CurrentSpeed / 30.0f; accelInput = accelInput < 0.0f ? accelInput : accelInput *Mathf.Min(1.0f, k); } else { accelInput = -1; steerInput = 0; } if (hasWorkingSteerwheel) { steerwheelInput.SetAutonomousForce(0); } } else if (selfDriving && !underKeyboardControl) //autonomous control(uninterrupted) { if (hasWorkingSteerwheel) { var diff = Mathf.Abs(autoSteerAngle - steerInput); if (autoSteerAngle < steerInput) // need to steer left { steerwheelInput.SetAutonomousForce((int)(diff * 10000)); } else { steerwheelInput.SetAutonomousForce((int)(-diff * 10000)); } } //use autonomous command values if (!hasWorkingSteerwheel || steerwheelInput.autonomousBehavior == SteerWheelAutonomousFeedbackBehavior.OutputOnly || steerwheelInput.autonomousBehavior == SteerWheelAutonomousFeedbackBehavior.None) { accelInput = autoInputAccel; steerInput = autoSteerAngle; } else { //purpose of this is to use steering wheel as input even when in self-driving state accelInput += autoInputAccel; steerInput += autoSteerAngle; } } if (controller.driveMode == DriveMode.Controlled) { controller.accellInput = accelInput; } controller.steerInput = steerInput; if (isControlEnabled == true) // Publish control command for training { var simControl = new Ros.TwistStamped() { header = new Ros.Header() { stamp = Ros.Time.Now(), seq = seq++, frame_id = "", }, twist = new Ros.Twist() { linear = new Ros.Vector3() { x = controller.accellInput, y = 0, z = 0, }, angular = new Ros.Vector3() { x = controller.steerInput, y = 0, z = 0, } } }; LgsvlSimulatorCmdWriter.Publish(simControl); } }