void FixedUpdate() { // Publish Frequency Limit if (lastUpdateTime.AddSeconds(1.0f / Frequency) > DateTime.Now) { return; } lastUpdateTime = DateTime.Now; // Sanity Check if (MapOrigin == null || Bridge == null || Bridge.Status != Status.Connected) { return; } // Get Data for Packing float speed = RigidBody.velocity.magnitude; var gps = MapOrigin.GetGpsLocation(transform.position); if (creep_state == true) { creep_state = false; } else { temp_vehicle_speed = speed * 3.60f; } // Pack message data msg = new ChassisData() { stamp = DateTime.Now, frame_id = "", steering_torque = 0.0f, engine_rpm = Dynamics.CurrentRPM, vehicle_speed = temp_vehicle_speed, throttle_percent = Dynamics.AccellInput > 0 ? Dynamics.AccellInput : 0, brake_percent = Dynamics.AccellInput < 0 ? -Dynamics.AccellInput : 0, steering_angle = -Dynamics.CurrentSteerAngle * Dynamics.SteeringRatio, brake_status = false, cruise_start = false, cruise_cancel = false, takeover = false, }; //speed during creep behaviour if (msg.brake_percent == 0.0f && msg.vehicle_speed >= 7.92f && msg.throttle_percent == 0.0f) { creep_state = true; msg.vehicle_speed = 2.2f * 3.60f; temp_vehicle_speed = msg.vehicle_speed; } // Write Data to Message Queue Writer.Write(msg, null); }
public static Snowball.ChassisDataMsg ConvertFrom(ChassisData data) { return(new Snowball.ChassisDataMsg() { header = new Snowball.Header() { stamp = new Snowball.Time(data.stamp), frame_id = data.frame_id }, steering_torque = data.steering_torque, engine_rpm = data.engine_rpm, vehicle_speed = data.vehicle_speed, throttle_percent = (float)data.throttle_percent, brake_percent = (float)data.brake_percent, steering_angle = (float)data.steering_angle, brake_status = data.brake_status, cruise_start = data.cruise_start, cruise_cancel = data.cruise_cancel, takeover = data.takeover, }); }