Esempio n. 1
0
    void Update()
    {
        if (targetEnv != ROSTargetEnvironment.APOLLO && targetEnv != ROSTargetEnvironment.APOLLO35 && targetEnv != ROSTargetEnvironment.AUTOWARE)
        {
            return;
        }

        if (Bridge == null || Bridge.Status != Comm.BridgeStatus.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,
                    longitude         = gps.longitude,
                    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,
                },
            };

            ApolloChassisWriter.Publish(apolloMessage);
        }

        else if (targetEnv == ROSTargetEnvironment.APOLLO35)
        {
            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();

            System.DateTime Unixepoch        = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
            double          measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;

            var apolloMessage = new Apollo.Canbus.Chassis()
            {
                EngineStarted      = true,
                EngineRpm          = controller.currentRPM,
                SpeedMps           = vel.magnitude,
                OdometerM          = 0,
                FuelRangeM         = 0,
                ThrottlePercentage = input_controller.throttle * 100,
                BrakePercentage    = input_controller.brake * 100,
                SteeringPercentage = -controller.steerInput * 100,
                // steering_torque_nm
                ParkingBrake    = controller.handbrakeApplied,
                HighBeamSignal  = (controller.headlightMode == 2),
                LowBeamSignal   = (controller.headlightMode == 1),
                LeftTurnSignal  = controller.leftTurnSignal,
                RightTurnSignal = controller.rightTurnSignal,
                // horn
                Wiper = (controller.wiperStatus != 0),
                // disengage_status
                DrivingMode = Apollo.Canbus.Chassis.Types.DrivingMode.CompleteAutoDrive,
                // error_code
                // gear_location
                // steering_timestamp
                // signal
                // engage_advice

                ChassisGps = new Apollo.Canbus.ChassisGPS()
                {
                    Latitude         = gps.latitude,
                    Longitude        = gps.longitude,
                    GpsValid         = 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,
                    CompassDirection = dir,
                    Pdop             = 0.1,
                    IsGpsFault       = false,
                    IsInferred       = false,
                    Altitude         = gps.height,
                    Heading          = eul.y,
                    Hdop             = 0.1,
                    Vdop             = 0.1,
                    Quality          = Apollo.Canbus.GpsQuality.Fix3D,
                    NumSatellites    = 15,
                    GpsSpeed         = vel.magnitude,
                },

                Header = new Apollo.Common.Header()
                {
                    TimestampSec = measurement_time,
                    ModuleName   = "chassis",
                    SequenceNum  = seq,
                },
            };

            Apollo35ChassisWriter.Publish(apolloMessage);
        }
    }
Esempio n. 2
0
    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);
        }
    }
Esempio n. 3
0
    void Update()
    {
        if (targetEnv != ROSTargetEnvironment.APOLLO && targetEnv != ROSTargetEnvironment.APOLLO35 && targetEnv != ROSTargetEnvironment.AUTOWARE)
        {
            return;
        }

        if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected || !PublishMessage)
        {
            return;
        }

        if (Time.time < NextSend)
        {
            return;
        }
        NextSend = Time.time + 1.0f / Frequency;

        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();

            float accel         = input_controller.controller.accellInput * 100;
            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 = accel > 0 ? accel : 0,
                brake_percentage    = accel < 0 ? -accel : 0,
                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 = controller.InReverse ? Ros.Apollo.Chassis.GearPosition.GEAR_REVERSE : Ros.Apollo.Chassis.GearPosition.GEAR_DRIVE,
                // steering_timestamp
                // signal
                // engage_advice

                chassis_gps = new Ros.Apollo.Chassis.ChassisGPS()
                {
                    latitude          = gps.latitude,
                    longitude         = gps.longitude,
                    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,
                },
            };

            ApolloChassisWriter.Publish(apolloMessage);
        }

        else if (targetEnv == ROSTargetEnvironment.APOLLO35)
        {
            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();

            System.DateTime Unixepoch        = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
            double          measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;

            float accel         = input_controller.controller.accellInput * 100;
            var   apolloMessage = new apollo.canbus.Chassis()
            {
                engine_started      = true,
                engine_rpm          = controller.currentRPM,
                speed_mps           = vel.magnitude,
                odometer_m          = 0,
                fuel_range_m        = 0,
                throttle_percentage = accel > 0 ? accel : 0,
                brake_percentage    = accel < 0 ? -accel : 0,
                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 = apollo.canbus.Chassis.DrivingMode.COMPLETE_AUTO_DRIVE,
                // error_code
                gear_location = controller.InReverse ? apollo.canbus.Chassis.GearPosition.GEAR_REVERSE : apollo.canbus.Chassis.GearPosition.GEAR_DRIVE,
                // steering_timestamp
                // signal
                // engage_advice

                chassis_gps = new apollo.canbus.ChassisGPS()
                {
                    latitude          = gps.latitude,
                    longitude         = gps.longitude,
                    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           = apollo.canbus.GpsQuality.FIX_3D,
                    num_satellites    = 15,
                    gps_speed         = vel.magnitude,
                },

                header = new apollo.common.Header()
                {
                    timestamp_sec = measurement_time,
                    module_name   = "chassis",
                    sequence_num  = seq,
                },
            };

            Apollo35ChassisWriter.Publish(apolloMessage);
        }
    }