private void PublishGroundTruth(List <Ros.Detection3D> detectedObjects)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
if (Time.time < nextSend)
{
return;
}
if (detectedObjects == null)
{
return;
}
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO || targetEnv == ROSTargetEnvironment.LGSVL)
{
var detectedObjectArrayMsg = new Ros.Detection3DArray()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
},
detections = detectedObjects,
};
DetectedObjectArrayWriter.Publish(detectedObjectArrayMsg);
nextSend = Time.time + 1.0f / frequency;
}
}
// Update is called in fixed Rate
void FixedUpdate()
{
if (is_connected)
{
var clock_msg = new Ros.Clock();
clock_msg.clock = Ros.Time.Now();
RosClockWriter.Publish(clock_msg);
}
}
void FixedUpdate()
{
if (Bridge != null && Bridge.Status == Comm.BridgeStatus.Connected)
{
var clock_msg = new Ros.Clock();
clock_msg.clock = Ros.Time.Now();
RosClockWriter.Publish(clock_msg);
}
}
void SendImage(byte[] data, int length)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
#if USE_COMPRESSED
var msg = new Ros.CompressedImage()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seqId++,
frame_id = FrameId,
},
format = "jpeg",
data = new Ros.PartialByteArray()
{
Array = data,
Length = length,
}
};
#else
byte[] temp = new byte[videoWidth * Reader.BytesPerPixel];
int stride = videoWidth * Reader.BytesPerPixel;
for (int y = 0; y < videoHeight / 2; y++)
{
int row1 = stride * y;
int row2 = stride * (videoHeight - 1 - y);
System.Array.Copy(data, row1, temp, 0, stride);
System.Array.Copy(data, row2, data, row1, stride);
System.Array.Copy(temp, 0, data, row2, stride);
}
var msg = new Ros.Image()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seqId++,
frame_id = FrameId,
},
height = (uint)videoHeight,
width = (uint)videoWidth,
encoding = Reader.BytesPerPixel == 3 ? "rgb8" : "rgba8",
is_bigendian = 0,
step = (uint)stride,
data = data,
};
#endif
ImageIsBeingSent = true;
VideoWriter.Publish(msg, () => ImageIsBeingSent = false);
}
public void LateUpdate()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
CarInfoWriter.Publish(new Ros.Pose()
{
position = new Ros.Point()
{
x = mainTransform.position.x,
y = mainTransform.position.y,
z = mainTransform.position.z,
},
orientation = new Ros.Quaternion()
{
x = mainTransform.rotation.x,
y = mainTransform.rotation.y,
z = mainTransform.rotation.z,
w = mainTransform.rotation.w,
},
});
UnityTimeWriter.Publish(Time.time);
linearVelocity = mainRigidbody.velocity;
angularVelocity = mainRigidbody.angularVelocity;
linearSpeed = linearVelocity.magnitude;
angularSpeed = angularVelocity.magnitude;
SimulatorCurrentVelocityWriter.Publish(new Ros.TwistStamped()
{
twist = new Ros.Twist()
{
linear = new Ros.Vector3()
{
x = linearSpeed,
y = 0.0,
z = 0.0,
},
angular = new Ros.Vector3()
{
x = 0.0,
y = 0.0,
z = angularSpeed,
}
}
});
}
private void SendPointCloud()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
range_array.Clear();
intensity_array.Clear();
return;
}
int FOV_take_1 = (int)(Mathf.Abs(FOVLeftLimit - FOVRightLimit) / Mathf.Abs(rotationAnglePerStep));
int FOV_skip = (int)(Mathf.Abs(-270 - FOVLeftLimit) / Mathf.Abs(rotationAnglePerStep));
int FOV_take_2 = 0;
if (FOV_take_1 + FOV_skip != (int)(360 / rotationAnglePerStep))
{
FOV_take_2 = (int)(360 / rotationAnglePerStep) - FOV_skip - FOV_take_1;
}
var msg = new Ros.LaserScan()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seqId++,
frame_id = frame_id,
},
angle_min = FOVLeftLimit * Mathf.PI / 180.0f,
angle_max = FOVRightLimit * Mathf.PI / 180.0f,
angle_increment = rotationAnglePerStep * Mathf.PI / 180.0f,
time_increment = 1.0f * 360.0f / (rotationSpeedHz * rotationAnglePerStep),
scan_time = 1.0f / rotationSpeedHz,
range_min = minRange,
range_max = maxRange,
ranges = (range_array.Take(FOV_take_2).Concat(range_array.Skip(FOV_skip).Take(FOV_take_1))).ToArray(),
intensities = (intensity_array.Take(FOV_take_2).Concat(intensity_array.Skip(FOV_skip).Take(FOV_take_1))).ToArray(),
};
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1)
{
AutowareWriterLaserScan.Publish(msg);
}
range_array.Clear();
intensity_array.Clear();
}
void SendMessage(int currentEndIndex, float currentEndAngle)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("SendMessage");
#endif
// Lidar x is forward, y is left, z is up
var worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero);
if (Compensated)
{
worldToLocal = worldToLocal * transform.worldToLocalMatrix;
}
if (CurrentRayCount != 1)
{
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
if (TargetEnvironment == ROSTargetEnvironment.APOLLO35)
{
var msg = new Apollo.Drivers.PointCloud()
{
Header = new Apollo.Common.Header()
{
// TimestampSec = measurement_time,
SequenceNum = Sequence++,
FrameId = FrameName,
},
FrameId = "lidar128",
IsDense = false,
MeasurementTime = measurement_time,
Height = 1,
Width = (uint)PointCloud.Length, // TODO is this right?
};
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
var pos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
pos = worldToLocal.MultiplyPoint3x4(pos);
msg.Point.Add(new Apollo.Drivers.PointXYZIT()
{
X = pos.x,
Y = pos.y,
Z = pos.z,
Intensity = (byte)(intensity * 255),
Timestamp = (ulong)measurement_time
});
}
;
Apollo35WriterPointCloud2.Publish(msg);
}
else
{
int count = 0;
unsafe
{
fixed(byte *ptr = RosPointCloud)
{
int offset = 0;
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
var pos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
*(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(pos);
*(ptr + offset + 16) = (byte)(intensity * 255);
offset += 32;
count++;
}
}
}
var msg = new Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = FrameName,
},
height = 1,
width = (uint)count,
fields = new Ros.PointField[] {
new Ros.PointField()
{
name = "x",
offset = 0,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "y",
offset = 4,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "z",
offset = 8,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "intensity",
offset = 16,
datatype = 2,
count = 1,
},
new Ros.PointField()
{
name = "timestamp",
offset = 24,
datatype = 8,
count = 1,
},
},
is_bigendian = false,
point_step = 32,
row_step = (uint)count * 32,
data = new Ros.PartialByteArray()
{
Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32),
},
is_dense = true,
};
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
ApolloWriterPointCloud2.Publish(msg);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
AutowareWriterPointCloud2.Publish(msg);
}
else
{
WriterPointCloud2.Publish(msg);
}
}
}
else // for planar lidar only (single ray lidar) publish LaserScan instead of PoinCloud2
{
float fixup = currentEndAngle;
FixupAngle = currentEndAngle;
float LaserScanMinAngle = -Mathf.PI;
float LaserScanMaxAngle = Mathf.PI;
float LaserScanMinRange = 0.0f;
float LaserScanMaxRange = 40.0f;
float[] range_array = new float[CurrentMeasurementsPerRotation];
float[] intensity_array = new float[CurrentMeasurementsPerRotation];
int offset = (int)(currentEndIndex - currentEndAngle / 360.0f * CurrentMeasurementsPerRotation);
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
range_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = Vector3.Distance(new Vector3(point.x, point.y, point.z), transform.position);
intensity_array[(i + offset + CurrentMeasurementsPerRotation) % CurrentMeasurementsPerRotation] = point.w;
}
// Populate LaserScan message
var msg = new Ros.LaserScan()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = FrameName,
},
angle_min = LaserScanMinAngle,
angle_max = LaserScanMaxAngle,
angle_increment = Mathf.Abs(LaserScanMaxAngle - LaserScanMinAngle) / CurrentMeasurementsPerRotation,
time_increment = 1.0f / (CurrentMeasurementsPerRotation * RotationFrequency),
scan_time = 1.0f / RotationFrequency,
range_min = LaserScanMinRange,
range_max = LaserScanMaxRange,
ranges = range_array, // TODO: Should make sure only sending range between min and max
intensities = intensity_array,
};
WriterLaserScan.Publish(msg);
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
}
void SendV_x()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected || !PublishMessage || !IsEnabled)
{
return;
}
var smsg = new Ros.V2x_Spat()//Spat message에 관한 pub
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = V2xFrameId,
},
msg_type = "SPAT_MSG_TYPE",
spat_id_region = id_region,
spat_movement_cnt = movement_cnt,
//v2x메세지 발행시 필요한 토픽
spat_signalgroup = single_group,
spat_movement_name = single_movement, // assume that movement state contains only one movement event
spat_eventstate = single_event_state, //0 : unavaliable/ 3: stop and remain/ 5 : permissive_movement_allowed
spat_minendtime = single_min_endtime,
};
SV2x.Publish(smsg);
var mmsg = new Ros.V2x_MAP()//MAP message에 관한 pub
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = V2xFrameId,
},
msg_type = "MAP_MSG_TYPE",
};
MV2x.Publish(mmsg);
var bmsg = new Ros.V2x_BSM()//BSM message에 관한 pub
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = V2xFrameId,
},
msg_type = "BSM_MSG_TYPE",
bsm_lat = lat,
bsm_lon = lon,
};
BV2x.Publish(bmsg);
var tmsg = new Ros.V2x_TIM()//TIM message에 관한 pub
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = V2xFrameId,
},
msg_type = "TIM_MSG_TYPE",
};
TV2x.Publish(tmsg);
}
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);
}
}
public void SendRadarData()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
var apolloHeader = new Ros.ApolloHeader()
{
timestamp_sec = (System.DateTime.UtcNow - originTime).TotalSeconds,
module_name = "conti_radar",
sequence_num = seqId
};
var radarPos = transform.position;
var radarAim = transform.forward;
var radarRight = transform.right;
radarObjList.Clear();
utilColList.Clear();
utilColList.AddRange(radarDetectedColliders.Keys);
utilColList.RemoveAll(c => c == null);
//Debug.Log("radarDetectedColliders.Count: " + radarDetectedColliders.Count);
System.Func <Collider, int> GetDynPropInt = ((col) => {
var trafAiMtr = col.GetComponentInParent <TrafAIMotor>();
if (trafAiMtr != null)
{
return(trafAiMtr.currentSpeed > 1.0f ? 0 : 1);
}
return(1);
});
System.Func <Collider, Vector3> GetLinVel = ((col) => {
var trafAiMtr = col.GetComponentInParent <TrafAIMotor>();
if (trafAiMtr != null)
{
return(trafAiMtr.currentVelocity);
}
else
{
return(col.attachedRigidbody == null ? Vector3.zero : col.attachedRigidbody.velocity);
}
});
for (int i = 0; i < utilColList.Count; i++)
{
Collider col = utilColList[i];
Vector3 point = radarDetectedColliders[col].point;
Vector3 relPos = point - radarPos;
Vector3 carVel = gameObject.GetComponentInParent <Rigidbody>().velocity;
Vector3 relVel = carVel - GetLinVel(col);
//Debug.Log("id to be assigned to obstacle_id is " + radarDetectedColliders[col].id);
Vector3 size = col.bounds.size;
// angle is orientation of the obstacle in degrees as seen by radar, counterclockwise is positive
double angle = -Vector3.SignedAngle(transform.forward, col.transform.forward, transform.up);
if (angle > 90)
{
angle -= 180;
}
else if (angle < -90)
{
angle += 180;
}
radarObjList.Add(new Ros.Apollo.Drivers.ContiRadarObs()
{
header = apolloHeader,
clusterortrack = false,
obstacle_id = radarDetectedColliders[col].id,
longitude_dist = Vector3.Project(relPos, radarAim).magnitude,
lateral_dist = Vector3.Project(relPos, radarRight).magnitude *(Vector3.Dot(relPos, radarRight) > 0 ? -1 : 1),
longitude_vel = Vector3.Project(relVel, radarAim).magnitude *(Vector3.Dot(relVel, radarAim) > 0 ? -1 : 1),
lateral_vel = Vector3.Project(relVel, radarRight).magnitude *(Vector3.Dot(relVel, radarRight) > 0 ? -1 : 1),
rcs = 11.0, //
dynprop = GetDynPropInt(col), // seem to be constant
longitude_dist_rms = 0,
lateral_dist_rms = 0,
longitude_vel_rms = 0,
lateral_vel_rms = 0,
probexist = 1.0, //prob confidence
meas_state = radarDetectedColliders[col].newDetection ? 1 : 2, //1 new 2 exist
longitude_accel = 0,
lateral_accel = 0,
oritation_angle = angle,
longitude_accel_rms = 0,
lateral_accel_rms = 0,
oritation_angle_rms = 0,
length = size.z,
width = size.x,
obstacle_class = size.z > 5 ? 2 : 1, // 0: point; 1: car; 2: truck; 3: pedestrian; 4: motorcycle; 5: bicycle; 6: wide; 7: unknown
});
}
var msg = new Ros.Apollo.Drivers.ContiRadar
{
header = apolloHeader,
contiobs = radarObjList,
object_list_status = new Ros.Apollo.Drivers.ObjectListStatus_60A
{
nof_objects = utilColList.Count,
meas_counter = 22800,
interface_version = 0
}
};
ApolloWriterContiRadar.Publish(msg);
++seqId;
}
void Update()
{
if (targetEnv != ROSTargetEnvironment.APOLLO && 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_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,
},
};
ApolloChassisWriter.Publish(apolloMessage);
}
}
void Update()
{
Vector3 pos = Target.transform.position;
double easting, northing;
var utc = System.DateTime.UtcNow.ToString("HHmmss.fff");
accuracy = 0.01f; // just a number to report
double altitude = pos.y; // above sea level
height = 0; // sea level to WGS84 ellipsoid
GetEastingNorthing(pos, out easting, out northing);
// MIT licensed conversion code from https://github.com/Turbo87/utm/blob/master/utm/conversion.py
double K0 = 0.9996;
double E = 0.00669438;
double E2 = E * E;
double E3 = E2 * E;
double E_P2 = E / (1.0 - E);
double SQRT_E = Math.Sqrt(1 - E);
double _E = (1 - SQRT_E) / (1 + SQRT_E);
double _E2 = _E * _E;
double _E3 = _E2 * _E;
double _E4 = _E3 * _E;
double _E5 = _E4 * _E;
double M1 = (1 - E / 4 - 3 * E2 / 64 - 5 * E3 / 256);
double P2 = (3.0 / 2 * _E - 27.0 / 32 * _E3 + 269.0 / 512 * _E5);
double P3 = (21.0 / 16 * _E2 - 55.0 / 32 * _E4);
double P4 = (151.0 / 96 * _E3 - 417.0 / 128 * _E5);
double P5 = (1097.0 / 512 * _E4);
double R = 6378137;
double x = easting;
double y = northing;
double m = y / K0;
double mu = m / (R * M1);
double p_rad = (mu +
P2 * Math.Sin(2 * mu) +
P3 * Math.Sin(4 * mu) +
P4 * Math.Sin(6 * mu) +
P5 * Math.Sin(8 * mu));
double p_sin = Math.Sin(p_rad);
double p_sin2 = p_sin * p_sin;
double p_cos = Math.Cos(p_rad);
double p_tan = p_sin / p_cos;
double p_tan2 = p_tan * p_tan;
double p_tan4 = p_tan2 * p_tan2;
double ep_sin = 1 - E * p_sin2;
double ep_sin_sqrt = Math.Sqrt(1 - E * p_sin2);
double n = R / ep_sin_sqrt;
double r = (1 - E) / ep_sin;
double c = _E * p_cos * p_cos;
double c2 = c * c;
double d = x / (n * K0);
double d2 = d * d;
double d3 = d2 * d;
double d4 = d3 * d;
double d5 = d4 * d;
double d6 = d5 * d;
double lat = (p_rad - (p_tan / r) *
(d2 / 2 -
d4 / 24 * (5 + 3 * p_tan2 + 10 * c - 4 * c2 - 9 * E_P2)) +
d6 / 720 * (61 + 90 * p_tan2 + 298 * c + 45 * p_tan4 - 252 * E_P2 - 3 * c2));
double lon = (d -
d3 / 6 * (1 + 2 * p_tan2 + c) +
d5 / 120 * (5 - 2 * c + 28 * p_tan2 - 3 * c2 + 8 * E_P2 + 24 * p_tan4)) / p_cos;
latitude_orig = (double)(lat * 180.0 / Math.PI);
longitude_orig = (double)(lon * 180.0 / Math.PI);
if (targetEnv == ROSTargetEnvironment.APOLLO && UTMZoneId > 0)
{
longitude_orig = longitude_orig + (UTMZoneId - 1) * 6 - 180 + 3;
}
latitude_read = latitude_orig;
longitude_read = longitude_orig;
if (targetEnv != ROSTargetEnvironment.APOLLO && 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.AUTOWARE)
{
char latitudeS = latitude_orig < 0.0f ? 'S' : 'N';
char longitudeS = longitude_orig < 0.0f ? 'W' : 'E';
double latitude = Math.Abs(latitude_orig);
double longitude = Math.Abs(longitude_orig);
latitude = Math.Floor(latitude) * 100 + (latitude % 1) * 60.0f;
longitude = Math.Floor(longitude) * 100 + (longitude % 1) * 60.0f;
var gga = string.Format("GPGGA,{0},{1:0.000000},{2},{3:0.000000},{4},{5},{6},{7},{8:0.000000},M,{9:0.000000},M,,",
utc,
latitude, latitudeS,
longitude, longitudeS,
1, // GPX fix
10, // sattelites tracked
accuracy,
altitude,
height);
var angles = Target.transform.eulerAngles;
float roll = -angles.z;
float pitch = -angles.x;
float yaw = angles.y;
var qq = string.Format("QQ02C,INSATT,V,{0},{1:0.000},{2:0.000},{3:0.000},",
utc,
roll,
pitch,
yaw);
// http://www.plaisance-pratique.com/IMG/pdf/NMEA0183-2.pdf
// 5.2.3 Checksum Field
byte ggaChecksum = 0;
for (int i = 0; i < gga.Length; i++)
{
ggaChecksum ^= (byte)gga[i];
}
byte qqChecksum = 0;
for (int i = 0; i < qq.Length; i++)
{
qqChecksum ^= (byte)qq[i];
}
var ggaMessage = new Ros.Sentence()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = FrameId,
},
sentence = "$" + gga + "*" + ggaChecksum.ToString("X2"),
};
AutowareWriterSentence.Publish(ggaMessage);
var qqMessage = new Ros.Sentence()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = FrameId,
},
sentence = qq + "@" + qqChecksum.ToString("X2"),
};
AutowareWriterSentence.Publish(qqMessage);
// Autoware - GPS Odometry
var quat = Quaternion.Euler(pitch, roll, yaw);
float forward_speed = Vector3.Dot(mainRigidbody.velocity, Target.transform.forward);
var odometryMessage = new Ros.Odometry()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = "odom",
},
child_frame_id = "base_link",
pose = new Ros.PoseWithCovariance()
{
pose = new Ros.Pose()
{
position = new Ros.Point()
{
x = easting + 500000,
y = northing,
z = 0.0, // altitude,
},
orientation = new Ros.Quaternion()
{
x = quat.x,
y = quat.y,
z = quat.z,
w = quat.w,
}
}
},
twist = new Ros.TwistWithCovariance()
{
twist = new Ros.Twist()
{
linear = new Ros.Vector3()
{
x = forward_speed, // mainRigidbody.velocity.x,
y = 0.0, // mainRigidbody.velocity.z,
z = 0.0,
},
angular = new Ros.Vector3()
{
x = 0.0,
y = 0.0,
z = -mainRigidbody.angularVelocity.y,
}
},
}
};
AutowareWriterOdometry.Publish(odometryMessage);
}
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
// Apollo - GPS Best Pose
System.DateTime GPSepoch = new System.DateTime(1980, 1, 6, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - GPSepoch).TotalSeconds + 18.0f;
var apolloMessage = new Ros.GnssBestPose()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time,
sequence_num = seq++,
},
measurement_time = measurement_time,
sol_status = 0,
sol_type = 50,
latitude = latitude_orig, // in degrees
longitude = longitude_orig, // in degrees
height_msl = height, // height above mean sea level in meters
undulation = 0, // undulation = height_wgs84 - height_msl
datum_id = 61, // datum id number
latitude_std_dev = accuracy, // latitude standard deviation (m)
longitude_std_dev = accuracy, // longitude standard deviation (m)
height_std_dev = accuracy, // height standard deviation (m)
base_station_id = "0", // base station id
differential_age = 2.0f, // differential position age (sec)
solution_age = 0.0f, // solution age (sec)
num_sats_tracked = 15, // number of satellites tracked
num_sats_in_solution = 15, // number of satellites used in solution
num_sats_l1 = 15, // number of L1/E1/B1 satellites used in solution
num_sats_multi = 12, // number of multi-frequency satellites used in solution
extended_solution_status = 33, // extended solution status - OEMV and
// greater only
galileo_beidou_used_mask = 0,
gps_glonass_used_mask = 51
};
ApolloWriterGnssBestPose.Publish(apolloMessage);
// Apollo - GPS odometry
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
var angles = Target.transform.eulerAngles;
float roll = angles.z;
float pitch = angles.x;
float yaw = -angles.y - Angle;
var quat = Quaternion.Euler(pitch, roll, yaw);
Vector3 worldVelocity = mainRigidbody.velocity;
var apolloGpsMessage = new Ros.Gps()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time,
sequence_num = seq++,
},
localization = new Ros.ApolloPose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
position = new Ros.PointENU()
{
x = easting + 500000, // East from the origin, in meters.
y = northing, // North from the origin, in meters.
z = altitude // Up from the WGS-84 ellipsoid, in
// meters.
},
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
orientation = new Ros.ApolloQuaternion()
{
qx = quat.x,
qy = quat.y,
qz = quat.z,
qw = quat.w,
},
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_velocity = new Ros.Point3D()
{
x = worldVelocity.x,
y = worldVelocity.z,
z = worldVelocity.y
},
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_acceleration = new Ros.Point3D(),
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
// angular_velocity = new Ros.Point3D(),
// Heading
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
// euler_angles = new Ros.Point3D()
}
};
ApolloWriterGps.Publish(apolloGpsMessage);
}
}
void SendImage(byte[] data, int length)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
if (TargetEnvironment == ROSTargetEnvironment.APOLLO35)
{
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
#if USE_COMPRESSED
var msg = new Apollo.Drivers.CompressedImage()
{
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time,
Version = 1,
Status = new Apollo.Common.StatusPb()
{
ErrorCode = Apollo.Common.ErrorCode.Ok,
},
},
MeasurementTime = measurement_time,
FrameId = FrameId,
// Format = "png",
Format = "jpg",
Data = ByteString.CopyFrom(data, 0, length),
};
#else
// TODO
#endif
ImageIsBeingSent = true;
CyberVideoWriter.Publish(msg, () => ImageIsBeingSent = false);
}
else
{
#if USE_COMPRESSED
var msg = new Ros.CompressedImage()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seqId++,
frame_id = FrameId,
},
format = "jpeg",
data = new Ros.PartialByteArray()
{
Array = data,
Length = length,
}
};
#else
byte[] temp = new byte[videoWidth * Reader.BytesPerPixel];
int stride = videoWidth * Reader.BytesPerPixel;
for (int y = 0; y < videoHeight / 2; y++)
{
int row1 = stride * y;
int row2 = stride * (videoHeight - 1 - y);
System.Array.Copy(data, row1, temp, 0, stride);
System.Array.Copy(data, row2, data, row1, stride);
System.Array.Copy(temp, 0, data, row2, stride);
}
var msg = new Ros.Image()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seqId++,
frame_id = FrameId,
},
height = (uint)videoHeight,
width = (uint)videoWidth,
encoding = Reader.BytesPerPixel == 3 ? "rgb8" : "rgba8",
is_bigendian = 0,
step = (uint)stride,
data = data,
};
#endif
ImageIsBeingSent = true;
VideoWriter.Publish(msg, () => ImageIsBeingSent = false);
}
}
public void OnBridgeAvailable(Comm.Bridge bridge)
{
Bridge = bridge;
Bridge.OnConnected += () =>
{
// tugbot
Bridge.AddReader <Ros.Twist>(CMD_VEL_TOPIC,
(Ros.Twist msg) =>
{
float WHEEL_SEPARATION = 0.515f;
float WHEEL_DIAMETER = 0.39273163f;
float SCALING_RATIO = 0.208f;
// Assuming that we only get linear in x and angular in z
double v = msg.linear.x;
double w = msg.angular.z;
wheelLeftVel = SCALING_RATIO * (float)(v - w * 0.5 * WHEEL_SEPARATION);
wheelRightVel = SCALING_RATIO * (float)(v + w * 0.5 * WHEEL_SEPARATION);
});
Bridge.AddReader <WheelsCmdStampedMsg>(WHEEL_CMD_TOPIC,
(WheelsCmdStampedMsg msg) =>
{
wheelLeftVel = msg.vel_left;
wheelRightVel = msg.vel_right;
});
// Autoware vehicle command
Bridge.AddReader <Ros.VehicleCmd>(AUTOWARE_CMD_TOPIC,
(Ros.VehicleCmd msg) =>
{
float WHEEL_SEPARATION = 0.1044197f;
//float WHEEL_DIAMETER = 0.065f;
float L_GAIN = 0.25f;
float A_GAIN = 8.0f;
// Assuming that we only get linear in x and angular in z
double v = msg.twist_cmd.twist.linear.x;
double w = msg.twist_cmd.twist.angular.z;
wheelLeftVel = (float)(L_GAIN * v - A_GAIN * w * 0.5 * WHEEL_SEPARATION);
wheelRightVel = (float)(L_GAIN * v + A_GAIN * w * 0.5 * WHEEL_SEPARATION);
});
Bridge.AddService <Ros.Srv.Empty, Ros.Srv.Empty>(CENTER_GRIPPER_SRV, msg =>
{
hook.CenterHook();
return(new Ros.Srv.Empty());
});
Bridge.AddService <Ros.Srv.SetBool, Ros.Srv.SetBoolResponse>(ATTACH_GRIPPER_SRV, msg =>
{
hook.EngageHook(msg.data);
return(new Ros.Srv.SetBoolResponse()
{
success = true, message = ""
});
});
string override_topic;
if (Bridge.Version == 1)
{
JostickRos1Writer = Bridge.AddWriter <Ros.Joy>(JOYSTICK_ROS1);
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1;
}
else
{
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2;
}
JoystickOverrideTopicWriter = Bridge.AddWriter <BoolStamped>(override_topic);
Bridge.AddReader <BoolStamped>(override_topic, stamped =>
{
ManualControl = stamped.data;
});
if (FirstConnection)
{
var stamp = new BoolStamped()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = "joystick",
},
data = true,
};
var topic = (Bridge.Version == 1) ? JOYSTICK_OVERRIDE_TOPIC_ROS1 : JOYSTICK_OVERRIDE_TOPIC_ROS2;
JoystickOverrideTopicWriter.Publish(stamp);
FirstConnection = false;
}
ManualControl = true;
};
}
void Update()
{
bool allowAgentControl = EventSystem.current.currentSelectedGameObject == null;
if (Input.GetKey(KeyCode.UpArrow) || Input.GetKey(KeyCode.DownArrow) ||
Input.GetKey(KeyCode.LeftArrow) || Input.GetKey(KeyCode.RightArrow))
{
if (MainCamera.gameObject.activeSelf && allowAgentControl)
{
controlMethod = ControlMethod.UnityKeyboard;
if (Input.GetKey(KeyCode.UpArrow))
{
vertical += Time.deltaTime * UnityVerticalSensitivity;
}
else if (Input.GetKey(KeyCode.DownArrow))
{
vertical -= Time.deltaTime * UnityVerticalSensitivity;
}
else
{
if (vertical > 0f)
{
vertical -= Time.deltaTime * UnityVerticalSensitivity;
if (vertical < 0f)
{
vertical = 0f;
}
}
else if (vertical < 0f)
{
vertical += Time.deltaTime * UnityVerticalSensitivity;
if (vertical > 0f)
{
vertical = 0f;
}
}
}
if (Input.GetKey(KeyCode.RightArrow))
{
horizontal += Time.deltaTime * UnityHorizontalSensitivity;
}
else if (Input.GetKey(KeyCode.LeftArrow))
{
horizontal -= Time.deltaTime * UnityHorizontalSensitivity;
}
else
{
if (horizontal > 0f)
{
horizontal -= Time.deltaTime * UnityHorizontalSensitivity;
if (horizontal < 0f)
{
horizontal = 0f;
}
}
else if (horizontal < 0f)
{
horizontal += Time.deltaTime * UnityHorizontalSensitivity;
if (vertical > 0f)
{
horizontal = 0f;
}
}
}
}
}
else
{
controlMethod = ControlMethod.ROS;
}
if (MainCamera.gameObject.activeSelf && allowAgentControl)
{
if (Input.GetKeyDown(KeyCode.Alpha1))
{
ManualControl = !ManualControl;
if (ManualControl)
{
wheelLeftVel = wheelRightVel = 0.0f;
}
var stamp = new BoolStamped()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = "joystick",
},
data = ManualControl,
};
if (Bridge.Version == 1)
{
var joy = new Ros.Joy()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = "joystick",
},
axes = new float[6],
buttons = new int[15],
};
joy.buttons[5] = 1;
JostickRos1Writer.Publish(joy);
JoystickOverrideTopicWriter.Publish(stamp);
}
else
{
JoystickOverrideTopicWriter.Publish(stamp);
}
}
}
if (Bridge != null && Bridge.Status != Comm.BridgeStatus.Connected)
{
wheelLeftVel = wheelRightVel = 0.0f;
}
vertical = Mathf.Clamp(vertical, -1f, 1f);
horizontal = Mathf.Clamp(horizontal, -1f, 1f);
}
private void PublishGroundTruth(List <Ros.Detection2D> detectedObjects)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
if (Time.time < nextSend)
{
return;
}
if (detectedObjects == null)
{
return;
}
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO || targetEnv == ROSTargetEnvironment.LGSVL)
{
var detectedObjectArrayMsg = new Ros.Detection2DArray()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
},
detections = detectedObjects,
};
DectectedObjectArrayWriter.Publish(detectedObjectArrayMsg);
nextSend = Time.time + 1.0f / frequency;
}
if (targetEnv == ROSTargetEnvironment.APOLLO35)
{
apollo.common.Detection2DArray cyberDetectionObjectArray = new apollo.common.Detection2DArray();
foreach (Ros.Detection2D rosDetection2D in detectedObjects)
{
apollo.common.Detection2D cyberDetection2D = new apollo.common.Detection2D()
{
header = new apollo.common.Header()
{
sequence_num = rosDetection2D.header.seq,
frame_id = rosDetection2D.header.frame_id,
timestamp_sec = (double)rosDetection2D.header.stamp.secs,
},
id = rosDetection2D.id,
label = rosDetection2D.label,
score = rosDetection2D.score,
bbox = new apollo.common.BoundingBox2D()
{
x = rosDetection2D.bbox.x,
y = rosDetection2D.bbox.y,
height = rosDetection2D.bbox.height,
width = rosDetection2D.bbox.width,
},
velocity = new apollo.common.Twist()
{
linear = new apollo.common.Vector3()
{
x = rosDetection2D.velocity.linear.x,
y = rosDetection2D.velocity.linear.y,
z = rosDetection2D.velocity.linear.z,
},
angular = new apollo.common.Vector3()
{
x = rosDetection2D.velocity.angular.x,
y = rosDetection2D.velocity.angular.y,
z = rosDetection2D.velocity.angular.z,
},
},
};
cyberDetectionObjectArray.detections.Add(cyberDetection2D);
}
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
cyberDetectionObjectArray.header = new apollo.common.Header()
{
timestamp_sec = measurement_time,
};
Apollo35DetectedObjectArrayWriter.Publish(cyberDetectionObjectArray);
nextSend = Time.time + 1.0f / frequency;
}
}
void SendMessage(int currentEndIndex, float currentEndAngle)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
// Lidar x is forward, y is left, z is up
var worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero);
worldToLocal = worldToLocal * transform.worldToLocalMatrix;
int count = 0;
unsafe
{
fixed(byte *ptr = RosPointCloud)
{
int offset = 0;
for (int i = 0; i < PointCloud.Length; i++)
{
var point = PointCloud[i];
if (point == Vector4.zero)
{
continue;
}
var pos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
*(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(pos);
*(ptr + offset + 16) = (byte)(intensity * 255);
offset += 32;
count++; //if (i == 0) { Debug.Log("<color=blue>:</color>"); }
}
}
}
var msg = new Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = FrameName,
},
height = 1,
width = (uint)count,
fields = new Ros.PointField[] {
new Ros.PointField()
{
name = "x",
offset = 0,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "y",
offset = 4,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "z",
offset = 8,
datatype = 7,
count = 1,
},
new Ros.PointField()
{
name = "intensity",
offset = 16,
datatype = 2,
count = 1,
},
new Ros.PointField()
{
name = "timestamp",
offset = 24,
datatype = 8,
count = 1,
},
},
is_bigendian = false,
point_step = 32,
row_step = (uint)count * 32,
data = new Ros.PartialByteArray()
{
Base64 = System.Convert.ToBase64String(RosPointCloud, 0, count * 32),
},
is_dense = true,
};
WriterPointCloud2.Publish(msg);
}
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;
UpdateValues();
double altitude = transform.position.y; // above sea level
var utc = System.DateTime.UtcNow.ToString("HHmmss.fff");
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
char latitudeS = latitude_orig < 0.0f ? 'S' : 'N';
char longitudeS = longitude_orig < 0.0f ? 'W' : 'E';
double latitude = Math.Abs(latitude_orig);
double longitude = Math.Abs(longitude_orig);
latitude = Math.Floor(latitude) * 100 + (latitude % 1) * 60.0f;
longitude = Math.Floor(longitude) * 100 + (longitude % 1) * 60.0f;
var gga = string.Format("GPGGA,{0},{1:0.000000},{2},{3:0.000000},{4},{5},{6},{7},{8:0.000000},M,{9:0.000000},M,,",
utc,
latitude, latitudeS,
longitude, longitudeS,
1, // GPX fix
10, // sattelites tracked
accuracy,
altitude,
height);
var angles = Target.transform.eulerAngles;
float roll = -angles.z;
float pitch = -angles.x;
float yaw = angles.y;
var qq = string.Format("QQ02C,INSATT,V,{0},{1:0.000},{2:0.000},{3:0.000},",
utc,
roll,
pitch,
yaw);
// http://www.plaisance-pratique.com/IMG/pdf/NMEA0183-2.pdf
// 5.2.3 Checksum Field
byte ggaChecksum = 0;
for (int i = 0; i < gga.Length; i++)
{
ggaChecksum ^= (byte)gga[i];
}
byte qqChecksum = 0;
for (int i = 0; i < qq.Length; i++)
{
qqChecksum ^= (byte)qq[i];
}
var ggaMessage = new Ros.Sentence()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = FrameId,
},
sentence = "$" + gga + "*" + ggaChecksum.ToString("X2"),
};
AutowareWriterSentence.Publish(ggaMessage);
var qqMessage = new Ros.Sentence()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = FrameId,
},
sentence = qq + "@" + qqChecksum.ToString("X2"),
};
AutowareWriterSentence.Publish(qqMessage);
// Autoware - GPS Odometry
var quat = Quaternion.Euler(pitch, roll, yaw);
float forward_speed = Vector3.Dot(mainRigidbody.velocity, Target.transform.forward);
var odometryMessage = new Ros.Odometry()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = "odom",
},
child_frame_id = "base_link",
pose = new Ros.PoseWithCovariance()
{
pose = new Ros.Pose()
{
position = new Ros.Point()
{
x = easting + 500000,
y = northing,
z = 0.0, // altitude,
},
orientation = new Ros.Quaternion()
{
x = quat.x,
y = quat.y,
z = quat.z,
w = quat.w,
}
}
},
twist = new Ros.TwistWithCovariance()
{
twist = new Ros.Twist()
{
linear = new Ros.Vector3()
{
x = forward_speed, // mainRigidbody.velocity.x,
y = 0.0, // mainRigidbody.velocity.z,
z = 0.0,
},
angular = new Ros.Vector3()
{
x = 0.0,
y = 0.0,
z = -mainRigidbody.angularVelocity.y,
}
},
}
};
AutowareWriterOdometry.Publish(odometryMessage);
}
else if (targetEnv == ROSTargetEnvironment.APOLLO)
{
// Apollo - GPS Best Pose
System.DateTime GPSepoch = new System.DateTime(1980, 1, 6, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - GPSepoch).TotalSeconds + 18.0f;
var apolloMessage = new Ros.GnssBestPose()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time,
sequence_num = seq++,
},
measurement_time = measurement_time,
sol_status = 0,
sol_type = 50,
latitude = latitude_orig, // in degrees
longitude = longitude_orig, // in degrees
height_msl = height, // height above mean sea level in meters
undulation = 0, // undulation = height_wgs84 - height_msl
datum_id = 61, // datum id number
latitude_std_dev = accuracy, // latitude standard deviation (m)
longitude_std_dev = accuracy, // longitude standard deviation (m)
height_std_dev = accuracy, // height standard deviation (m)
base_station_id = "0", // base station id
differential_age = 2.0f, // differential position age (sec)
solution_age = 0.0f, // solution age (sec)
num_sats_tracked = 15, // number of satellites tracked
num_sats_in_solution = 15, // number of satellites used in solution
num_sats_l1 = 15, // number of L1/E1/B1 satellites used in solution
num_sats_multi = 12, // number of multi-frequency satellites used in solution
extended_solution_status = 33, // extended solution status - OEMV and
// greater only
galileo_beidou_used_mask = 0,
gps_glonass_used_mask = 51
};
ApolloWriterGnssBestPose.Publish(apolloMessage);
// Apollo - GPS odometry
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
var angles = Target.transform.eulerAngles;
float roll = angles.z;
float pitch = angles.x;
float yaw = -angles.y - Angle;
var quat = Quaternion.Euler(pitch, roll, yaw);
Vector3 worldVelocity = mainRigidbody.velocity;
var apolloGpsMessage = new Ros.Gps()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time,
sequence_num = seq++,
},
localization = new Ros.ApolloPose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
position = new Ros.PointENU()
{
x = easting + 500000, // East from the origin, in meters.
y = northing, // North from the origin, in meters.
z = altitude // Up from the WGS-84 ellipsoid, in
// meters.
},
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
orientation = new Ros.ApolloQuaternion()
{
qx = quat.x,
qy = quat.y,
qz = quat.z,
qw = quat.w,
},
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_velocity = new Ros.Point3D()
{
x = worldVelocity.x,
y = worldVelocity.z,
z = worldVelocity.y
},
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_acceleration = new Ros.Point3D(),
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
// angular_velocity = new Ros.Point3D(),
// Heading
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
// euler_angles = new Ros.Point3D()
}
};
ApolloWriterGps.Publish(apolloGpsMessage);
}
else if (targetEnv == ROSTargetEnvironment.APOLLO35)
{
// Apollo - GPS Best Pose
System.DateTime GPSepoch = new System.DateTime(1980, 1, 6, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - GPSepoch).TotalSeconds + 18.0f;
var apolloMessage = new Apollo.Drivers.Gnss.GnssBestPose()
{
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time,
SequenceNum = seq++,
},
MeasurementTime = measurement_time,
SolStatus = (Apollo.Drivers.Gnss.SolutionStatus) 0,
SolType = (Apollo.Drivers.Gnss.SolutionType) 50,
Latitude = latitude_orig, // in degrees
Longitude = longitude_orig, // in degrees
HeightMsl = height, // height above mean sea level in meters
Undulation = 0, // undulation = height_wgs84 - height_msl
DatumId = (Apollo.Drivers.Gnss.DatumId) 61, // datum id number
LatitudeStdDev = accuracy, // latitude standard deviation (m)
LongitudeStdDev = accuracy, // longitude standard deviation (m)
HeightStdDev = accuracy, // height standard deviation (m)
BaseStationId = Google.Protobuf.ByteString.CopyFromUtf8("0"), //CopyFrom((byte)"0"), // base station id
DifferentialAge = 2.0f, // differential position age (sec)
SolutionAge = 0.0f, // solution age (sec)
NumSatsTracked = 15, // number of satellites tracked
NumSatsInSolution = 15, // number of satellites used in solution
NumSatsL1 = 15, // number of L1/E1/B1 satellites used in solution
NumSatsMulti = 12, // number of multi-frequency satellites used in solution
ExtendedSolutionStatus = 33, // extended solution status - OEMV and
// greater only
GalileoBeidouUsedMask = 0,
GpsGlonassUsedMask = 51
};
Apollo35WriterGnssBestPose.Publish(apolloMessage);
// Apollo - GPS odometry
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
var angles = Target.transform.eulerAngles;
float roll = angles.z;
float pitch = angles.x;
float yaw = -angles.y - Angle;
var quat = Quaternion.Euler(pitch, roll, yaw);
Vector3 worldVelocity = mainRigidbody.velocity;
var apolloGpsMessage = new Apollo.Localization.Gps()
{
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time,
SequenceNum = seq++,
},
Localization = new Apollo.Localization.Pose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
Position = new Apollo.Common.PointENU()
{
X = easting + 500000, // East from the origin, in meters.
Y = northing, // North from the origin, in meters.
Z = altitude // Up from the WGS-84 ellipsoid, in
// meters.
},
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
Orientation = new Apollo.Common.Quaternion()
{
Qx = quat.x,
Qy = quat.y,
Qz = quat.z,
Qw = quat.w,
},
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
LinearVelocity = new Apollo.Common.Point3D()
{
X = worldVelocity.x,
Y = worldVelocity.z,
Z = worldVelocity.y
},
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_acceleration = new Ros.Point3D(),
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
// angular_velocity = new Ros.Point3D(),
// Heading
// The heading is zero when the car is facing East and positive when facing North.
Heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
// euler_angles = new Ros.Point3D()
}
};
Apollo35WriterGps.Publish(apolloGpsMessage);
var apolloInsMessage = new Apollo.Drivers.Gnss.InsStat()
{
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time,
SequenceNum = 0,
},
InsStatus = 3,
PosType = 56
};
Apollo35WriterInsStat.Publish(apolloInsMessage);
}
}
private void PublishGroundTruth(List <Ros.Detection3D> detectedObjects)
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected)
{
return;
}
if (Time.time < nextSend)
{
return;
}
if (detectedObjects == null)
{
return;
}
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO || targetEnv == ROSTargetEnvironment.LGSVL)
{
var detectedObjectArrayMsg = new Ros.Detection3DArray()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
},
detections = detectedObjects,
};
DetectedObjectArrayWriter.Publish(detectedObjectArrayMsg);
nextSend = Time.time + 1.0f / frequency;
}
if (targetEnv == ROSTargetEnvironment.APOLLO35)
{
Apollo.Common.Detection3DArray cyberDetectionObjectArray = new Apollo.Common.Detection3DArray();
foreach (Ros.Detection3D rosDetection3D in detectedObjects)
{
Apollo.Common.Detection3D cyberDetection3D = new Apollo.Common.Detection3D()
{
Header = new Apollo.Common.Header()
{
SequenceNum = rosDetection3D.header.seq,
FrameId = rosDetection3D.header.frame_id,
TimestampSec = (double)rosDetection3D.header.stamp.secs,
},
Id = rosDetection3D.id,
Label = rosDetection3D.label,
Score = rosDetection3D.score,
Bbox = new Apollo.Common.BoundingBox3D()
{
Position = new Apollo.Common.Pose()
{
Position = new Apollo.Common.Point3D()
{
X = rosDetection3D.bbox.position.position.x,
Y = rosDetection3D.bbox.position.position.y,
Z = rosDetection3D.bbox.position.position.z,
},
Orientation = new Apollo.Common.Quaternion()
{
Qx = rosDetection3D.bbox.position.orientation.x,
Qy = rosDetection3D.bbox.position.orientation.y,
Qz = rosDetection3D.bbox.position.orientation.z,
Qw = rosDetection3D.bbox.position.orientation.w,
},
},
Size = new Apollo.Common.Vector3()
{
X = rosDetection3D.bbox.size.x,
Y = rosDetection3D.bbox.size.y,
Z = rosDetection3D.bbox.size.z,
},
},
Velocity = new Apollo.Common.Twist()
{
Linear = new Apollo.Common.Vector3()
{
X = rosDetection3D.velocity.linear.x,
Y = rosDetection3D.velocity.linear.y,
Z = rosDetection3D.velocity.linear.z,
},
Angular = new Apollo.Common.Vector3()
{
X = rosDetection3D.velocity.angular.x,
Y = rosDetection3D.velocity.angular.y,
Z = rosDetection3D.velocity.angular.z,
},
},
};
cyberDetectionObjectArray.Detections.Add(cyberDetection3D);
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
cyberDetectionObjectArray.Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time,
};
Apollo35DetectedObjectArrayWriter.Publish(cyberDetectionObjectArray);
nextSend = Time.time + 1.0f / frequency;
}
}
}
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);
}
}
public void FixedUpdate()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected || !PublishMessage || !IsEnabled)
{
return;
}
Vector3 currVelocity = transform.InverseTransformDirection(mainRigidbody.velocity);
Vector3 acceleration = (currVelocity - lastVelocity) / Time.fixedDeltaTime;
lastVelocity = currVelocity;
Vector3 angularVelocity = mainRigidbody.angularVelocity;
System.DateTime GPSepoch = new System.DateTime(1980, 1, 6, 0, 0, 0, System.DateTimeKind.Utc);
double measurement_time = (double)(System.DateTime.UtcNow - GPSepoch).TotalSeconds + 18.0f;
float measurement_span = (float)Time.fixedDeltaTime;
// Debug.Log(measurement_time + ", " + measurement_span);
// Debug.Log("Linear Acceleration: " + linear_acceleration.x.ToString("F1") + ", " + linear_acceleration.y.ToString("F1") + ", " + linear_acceleration.z.ToString("F1"));
// Debug.Log("Angular Velocity: " + angular_velocity.x.ToString("F1") + ", " + angular_velocity.y.ToString("F1") + ", " + angular_velocity.z.ToString("F1"));
var angles = Target.transform.eulerAngles;
float roll = -angles.z;
float pitch = -angles.x;
float yaw = -angles.y;
Quaternion orientation_unity = Quaternion.Euler(roll, pitch, yaw);
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
// for odometry frame position
odomPosition.x += currVelocity.z * Time.fixedDeltaTime * Mathf.Cos(yaw * (Mathf.PI / 180.0f));
odomPosition.y += currVelocity.z * Time.fixedDeltaTime * Mathf.Sin(yaw * (Mathf.PI / 180.0f));
acceleration += transform.InverseTransformDirection(Physics.gravity);
if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
var linear_acceleration = new Ros.Point3D()
{
x = acceleration.x,
y = acceleration.z,
z = -acceleration.y,
};
var angular_velocity = new Ros.Point3D()
{
x = -angularVelocity.z,
y = angularVelocity.x,
z = -angularVelocity.y,
};
ApolloWriterImu.Publish(new Ros.Apollo.Imu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time
},
measurement_time = measurement_time,
measurement_span = measurement_span,
linear_acceleration = linear_acceleration,
angular_velocity = angular_velocity
});
var apolloIMUMessage = new Ros.CorrectedImu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time
},
imu = new Ros.ApolloPose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
// position = new Ros.PointENU(),
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
// orientation = new Ros.ApolloQuaternion(),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_velocity = new Ros.Point3D(),
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_acceleration = linear_acceleration,
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
angular_velocity = angular_velocity,
// Heading
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
euler_angles = new Ros.Point3D()
{
x = roll * Mathf.Deg2Rad,
y = pitch * Mathf.Deg2Rad,
z = yaw * Mathf.Deg2Rad
}
}
};
ApolloWriterCorrectedImu.Publish(apolloIMUMessage);
}
else if (TargetRosEnv == ROSTargetEnvironment.APOLLO35)
{
var linear_acceleration = new Apollo.Common.Point3D()
{
X = acceleration.x,
Y = acceleration.z,
Z = -acceleration.y,
};
var angular_velocity = new Apollo.Common.Point3D()
{
X = -angularVelocity.z,
Y = angularVelocity.x,
Z = -angularVelocity.y,
};
Apollo35WriterImu.Publish(new Apollo.Drivers.Gnss.Imu()
{
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time
},
MeasurementTime = measurement_time,
MeasurementSpan = measurement_span,
LinearAcceleration = linear_acceleration,
AngularVelocity = angular_velocity
});
var apolloIMUMessage = new Apollo.Localization.CorrectedImu()
{
Header = new Apollo.Common.Header()
{
TimestampSec = measurement_time
},
Imu = new Apollo.Localization.Pose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
// position = new Ros.PointENU(),
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
// orientation = new Ros.ApolloQuaternion(),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_velocity = new Ros.Point3D(),
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
LinearAcceleration = linear_acceleration,
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
AngularVelocity = angular_velocity,
// Heading
// The heading is zero when the car is facing East and positive when facing North.
Heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
EulerAngles = new Apollo.Common.Point3D()
{
X = roll * Mathf.Deg2Rad,
Y = pitch * Mathf.Deg2Rad,
Z = yaw * Mathf.Deg2Rad,
}
}
};
Apollo35WriterCorrectedImu.Publish(apolloIMUMessage);
}
else if (TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1 || TargetRosEnv == ROSTargetEnvironment.AUTOWARE)
{
var imu_msg = new Ros.Imu()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence++,
frame_id = ImuFrameId,
},
orientation = new Ros.Quaternion()
{
x = orientation_unity.x,
y = orientation_unity.y,
z = orientation_unity.z,
w = orientation_unity.w,
},
orientation_covariance = new double[9],
angular_velocity = new Ros.Vector3()
{
x = angularVelocity.z,
y = -angularVelocity.x,
z = angularVelocity.y,
},
angular_velocity_covariance = new double[9],
linear_acceleration = new Ros.Vector3()
{
x = acceleration.z,
y = -acceleration.x,
z = acceleration.y,
},
linear_acceleration_covariance = new double[9],
};
AutowareWriterImu.Publish(imu_msg);
var odom_msg = new Ros.Odometry()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = Sequence,
frame_id = OdometryFrameId,
},
child_frame_id = OdometryChildFrameId,
pose = new Ros.PoseWithCovariance()
{
pose = new Ros.Pose()
{
position = new Ros.Point()
{
x = odomPosition.x,
y = odomPosition.y,
z = odomPosition.z,
},
orientation = new Ros.Quaternion()
{
x = orientation_unity.x,
y = orientation_unity.y,
z = orientation_unity.z,
w = orientation_unity.w,
},
},
covariance = new double[36],
},
twist = new Ros.TwistWithCovariance()
{
twist = new Ros.Twist()
{
linear = new Ros.Vector3(currVelocity.z, -currVelocity.x, currVelocity.y),
angular = new Ros.Vector3(angularVelocity.z, -angularVelocity.x, angularVelocity.y),
},
covariance = new double[36],
},
};
AutowareWriterOdometry.Publish(odom_msg);
}
}
public void FixedUpdate()
{
if (Bridge == null || Bridge.Status != Comm.BridgeStatus.Connected || !PublishMessage || !IsEnabled)
{
return;
}
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
if (IsFirstFixedUpdate)
{
lock (MessageQueue)
{
MessageQueue.Clear();
}
CurrTimestamp = System.DateTime.UtcNow - Unixepoch;
IsFirstFixedUpdate = false;
}
else
{
CurrTimestamp = CurrTimestamp.Add(Interval);
}
if (TimeSpan.Compare(CurrTimestamp, LastTimestamp) == -1) // if CurrTimestamp < LastTimestamp
{
return;
}
Vector3 currVelocity = transform.InverseTransformDirection(mainRigidbody.velocity);
Vector3 acceleration = (currVelocity - lastVelocity) / Time.fixedDeltaTime;
lastVelocity = currVelocity;
Vector3 angularVelocity = mainRigidbody.angularVelocity;
var angles = Target.transform.eulerAngles;
float roll = -angles.z;
float pitch = -angles.x;
float yaw = -angles.y;
Quaternion orientation_unity = Quaternion.Euler(roll, pitch, yaw);
double measurement_time = (double)CurrTimestamp.TotalSeconds;
float measurement_span = (float)(1 / Frequency);
// for odometry frame position
odomPosition.x += currVelocity.z * Time.fixedDeltaTime * Mathf.Cos(yaw * (Mathf.PI / 180.0f));
odomPosition.y += currVelocity.z * Time.fixedDeltaTime * Mathf.Sin(yaw * (Mathf.PI / 180.0f));
acceleration += transform.InverseTransformDirection(Physics.gravity);
if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
var linear_acceleration = new Ros.Point3D()
{
x = acceleration.x,
y = acceleration.z,
z = -acceleration.y,
};
var angular_velocity = new Ros.Point3D()
{
x = -angularVelocity.z,
y = angularVelocity.x,
z = -angularVelocity.y,
};
var apolloImuMsg = new Ros.Apollo.Imu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time
},
measurement_time = measurement_time,
measurement_span = measurement_span,
linear_acceleration = linear_acceleration,
angular_velocity = angular_velocity
};
var apolloCorrectedImuMsg = new Ros.CorrectedImu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = measurement_time
},
imu = new Ros.ApolloPose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
// position = new Ros.PointENU(),
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
// orientation = new Ros.ApolloQuaternion(),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_velocity = new Ros.Point3D(),
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_acceleration = linear_acceleration,
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
angular_velocity = angular_velocity,
// Heading
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
euler_angles = new Ros.Point3D()
{
x = roll * Mathf.Deg2Rad,
y = pitch * Mathf.Deg2Rad,
z = yaw * Mathf.Deg2Rad
}
}
};
lock (MessageQueue)
{
MessageQueue.Enqueue(new Tuple <TimeSpan, Action>(CurrTimestamp, () => {
ApolloWriterImu.Publish(apolloImuMsg);
ApolloWriterCorrectedImu.Publish(apolloCorrectedImuMsg);
}));
}
}
else if (TargetRosEnv == ROSTargetEnvironment.APOLLO35)
{
var linear_acceleration = new apollo.common.Point3D()
{
x = acceleration.x,
y = acceleration.z,
z = -acceleration.y,
};
var angular_velocity = new apollo.common.Point3D()
{
x = -angularVelocity.z,
y = angularVelocity.x,
z = -angularVelocity.y,
};
var apollo35ImuMsg = new apollo.drivers.gnss.Imu()
{
header = new apollo.common.Header()
{
timestamp_sec = measurement_time
},
measurement_time = measurement_time,
measurement_span = measurement_span,
linear_acceleration = linear_acceleration,
angular_velocity = angular_velocity
};
var apollo35CorrectedImuMsg = new apollo.localization.CorrectedImu()
{
header = new apollo.common.Header()
{
timestamp_sec = measurement_time
},
imu = new apollo.localization.Pose()
{
// Position of the vehicle reference point (VRP) in the map reference frame.
// The VRP is the center of rear axle.
// position = new Ros.PointENU(),
// A quaternion that represents the rotation from the IMU coordinate
// (Right/Forward/Up) to the
// world coordinate (East/North/Up).
// orientation = new Ros.ApolloQuaternion(),
// Linear velocity of the VRP in the map reference frame.
// East/north/up in meters per second.
// linear_velocity = new Ros.Point3D(),
// Linear acceleration of the VRP in the map reference frame.
// East/north/up in meters per second.
linear_acceleration = linear_acceleration,
// Angular velocity of the vehicle in the map reference frame.
// Around east/north/up axes in radians per second.
angular_velocity = angular_velocity,
// Heading
// The heading is zero when the car is facing East and positive when facing North.
heading = yaw, // not used ??
// Linear acceleration of the VRP in the vehicle reference frame.
// Right/forward/up in meters per square second.
// linear_acceleration_vrf = new Ros.Point3D(),
// Angular velocity of the VRP in the vehicle reference frame.
// Around right/forward/up axes in radians per second.
// angular_velocity_vrf = new Ros.Point3D(),
// Roll/pitch/yaw that represents a rotation with intrinsic sequence z-x-y.
// in world coordinate (East/North/Up)
// The roll, in (-pi/2, pi/2), corresponds to a rotation around the y-axis.
// The pitch, in [-pi, pi), corresponds to a rotation around the x-axis.
// The yaw, in [-pi, pi), corresponds to a rotation around the z-axis.
// The direction of rotation follows the right-hand rule.
euler_angles = new apollo.common.Point3D()
{
x = roll * Mathf.Deg2Rad,
y = pitch * Mathf.Deg2Rad,
z = yaw * Mathf.Deg2Rad,
}
}
};
lock (MessageQueue)
{
MessageQueue.Enqueue(new Tuple <TimeSpan, Action>(CurrTimestamp, () => {
Apollo35WriterImu.Publish(apollo35ImuMsg);
Apollo35WriterCorrectedImu.Publish(apollo35CorrectedImuMsg);
}));
}
}
else if (TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1 || TargetRosEnv == ROSTargetEnvironment.AUTOWARE)
{
var nanoSec = 1000000 * CurrTimestamp.TotalMilliseconds;
var autowareImuMsg = new Ros.Imu()
{
header = new Ros.Header()
{
stamp = new Ros.Time()
{
secs = (long)nanoSec / 1000000000,
nsecs = (uint)nanoSec % 1000000000,
},
seq = Sequence++,
frame_id = ImuFrameId,
},
orientation = new Ros.Quaternion()
{
x = orientation_unity.x,
y = orientation_unity.y,
z = orientation_unity.z,
w = orientation_unity.w,
},
orientation_covariance = new double[9],
angular_velocity = new Ros.Vector3()
{
x = angularVelocity.z,
y = -angularVelocity.x,
z = angularVelocity.y,
},
angular_velocity_covariance = new double[9],
linear_acceleration = new Ros.Vector3()
{
x = acceleration.z,
y = -acceleration.x,
z = acceleration.y,
},
linear_acceleration_covariance = new double[9],
};
var autowareOdomMsg = new Ros.Odometry()
{
header = new Ros.Header()
{
stamp = new Ros.Time()
{
secs = (long)nanoSec / 1000000000,
nsecs = (uint)nanoSec % 1000000000,
},
seq = Sequence,
frame_id = OdometryFrameId,
},
child_frame_id = OdometryChildFrameId,
pose = new Ros.PoseWithCovariance()
{
pose = new Ros.Pose()
{
position = new Ros.Point()
{
x = odomPosition.x,
y = odomPosition.y,
z = odomPosition.z,
},
orientation = new Ros.Quaternion()
{
x = orientation_unity.x,
y = orientation_unity.y,
z = orientation_unity.z,
w = orientation_unity.w,
},
},
covariance = new double[36],
},
twist = new Ros.TwistWithCovariance()
{
twist = new Ros.Twist()
{
linear = new Ros.Vector3(currVelocity.z, -currVelocity.x, currVelocity.y),
angular = new Ros.Vector3(angularVelocity.z, -angularVelocity.x, angularVelocity.y),
},
covariance = new double[36],
},
};
lock (MessageQueue)
{
MessageQueue.Enqueue(new Tuple <TimeSpan, Action>(CurrTimestamp, () => {
AutowareWriterImu.Publish(autowareImuMsg);
if (TargetRosEnv == ROSTargetEnvironment.DUCKIETOWN_ROS1)
{
AutowareWriterOdometry.Publish(autowareOdomMsg);
}
}));
}
}
}