public void LateUpdate()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
Bridge.Publish(CAR_INFO_TOPIC, 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,
},
});
Bridge.Publish(UNITY_TIME_TOPIC, Time.time);
linearVelocity = mainRigidbody.velocity;
angularVelocity = mainRigidbody.angularVelocity;
linearSpeed = linearVelocity.magnitude;
angularSpeed = angularVelocity.magnitude;
Bridge.Publish(SIM_CUR_VELOCITY_TOPIC, 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 PublishGroundTruth(List <Ros.Detection3D> detectedObjects)
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
if (Time.time < nextSend)
{
return;
}
if (detectedObjects == null)
{
return;
}
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO)
{
var detectedObjectArrayMsg = new Ros.Detection3DArray()
{
detections = detectedObjects,
};
Bridge.Publish(objects3DTopicName, detectedObjectArrayMsg);
nextSend = Time.time + 1.0f / frequency;
}
}
void SendImage(byte[] data, int length)
{
if (Bridge == null || Bridge.Status != Ros.Status.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;
Bridge.Publish(TopicName, msg, () => ImageIsBeingSent = false);
}
public void FixedUpdate()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected || !PublishMessage)
{
return;
}
Vector3 currVelocity = transform.InverseTransformDirection(mainRigidbody.velocity);
Vector3 acceleration = (currVelocity - lastVelocity) / Time.fixedDeltaTime;
lastVelocity = currVelocity;
var linear_acceleration = new Ros.Point3D()
{
x = acceleration.x,
y = acceleration.z,
z = -Physics.gravity.y
};
Vector3 angularVelocity = mainRigidbody.angularVelocity;
var angular_velocity = new Ros.Point3D()
{
x = 0.0,
y = 0.0,
z = -angularVelocity.y
};
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"));
Bridge.Publish(ImuTopic, new Ros.Imu()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = Ros.Time.Now().secs
},
measurement_time = measurement_time,
measurement_span = measurement_span,
linear_acceleration = linear_acceleration,
angular_velocity = angular_velocity
});
}
private void PublishGroundTruth(List <Ros.Detection3D> detectedObjects)
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
var detectedObjectArrayMsg = new Ros.Detection3DArray()
{
detections = detectedObjects,
};
if (targetEnv == ROSTargetEnvironment.AUTOWARE || targetEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.Publish(objects3DTopicName, detectedObjectArrayMsg);
}
}
void SendPointCloud()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
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)
{
Bridge.Publish(topicName, msg);
}
range_array.Clear();
intensity_array.Clear();
}
private void PublishGroundTruth()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
detectedObjects.Clear();
detectedObjects = lidarDetectedColliders.Values.ToList();
var detectedObjectArrayMsg = new Ros.DetectedObjectArray()
{
objects = detectedObjects,
};
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Publish(objects3DTopicName, detectedObjectArrayMsg);
}
}
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);
}
}
public void FixedUpdate()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected || !PublishMessage || !isEnabled)
{
return;
}
Vector3 currVelocity = transform.InverseTransformDirection(mainRigidbody.velocity);
Vector3 acceleration = (currVelocity - lastVelocity) / Time.fixedDeltaTime;
lastVelocity = currVelocity;
var linear_acceleration = new Ros.Point3D()
{
x = acceleration.z,
y = -acceleration.x,
z = acceleration.y + Physics.gravity.y
};
Vector3 angularVelocity = mainRigidbody.angularVelocity;
var angular_velocity = new Ros.Point3D()
{
x = angularVelocity.z,
y = -angularVelocity.x,
z = angularVelocity.y
};
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));
if (TargetRosEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.Publish(ImuTopic, 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 * 0.01745329252,
y = pitch * 0.01745329252,
z = yaw * 0.01745329252
}
}
};
Bridge.Publish(ApolloIMUOdometryTopic, apolloIMUMessage);
}
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 + Physics.gravity.y,
},
linear_acceleration_covariance = new double[9],
};
Bridge.Publish(ImuTopic, 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],
},
};
Bridge.Publish(OdometryTopic, odom_msg);
}
}
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,
}
}
};
Bridge.Publish(SIMULATOR_CMD_TOPIC, simControl);
}
}
public void SendRadarData()
{
if (Bridge == null || Bridge.Status != Ros.Status.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);
for (int i = 0; i < radarDetectedColliders.Count; i++)
{
Collider col = utilColList[i];
Vector3 point = radarDetectedColliders[col].point;
Vector3 relPos = point - radarPos;
Vector3 relVel = col.attachedRigidbody == null ? Vector3.zero : col.attachedRigidbody.velocity;
//Debug.Log("id to be assigned to obstacle_id is " + radarDetectedColliders[col].id);
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,
longitude_vel = Vector3.Project(relVel, radarAim).magnitude,
lateral_vel = Vector3.Project(relVel, radarRight).magnitude,
rcs = 11.0, //
dynprop = 0, // 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 = 0,
longitude_accel_rms = 0,
lateral_accel_rms = 0,
oritation_angle_rms = 0,
length = 2.0,
width = 2.4,
obstacle_class = 1, // single type but need to find car number
});
}
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
}
};
Bridge.Publish(ApolloTopicName, msg);
++seqId;
}
void SendMessage(int currentEndIndex, float currentEndAngle)
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("SendMessage");
#endif
Matrix4x4 worldToLocal;
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
// local.Set(local.x, local.z, local.y);
worldToLocal = new Matrix4x4(new Vector4(1, 0, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(0, 1, 0, 0), Vector4.zero);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
// local.Set(local.z, -local.x, local.y);
worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero);
}
else
{
worldToLocal = Matrix4x4.identity;
}
if (Compensated)
{
worldToLocal = worldToLocal * transform.worldToLocalMatrix;
}
if (CurrentRayCount != 1)
{
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 UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("Publish");
#endif
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
Bridge.Publish(ApolloTopicName, msg);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Publish(AutowareTopicName, msg);
}
else
{
Bridge.Publish(TopicName, msg);
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
}
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,
};
Bridge.Publish(TopicName, msg);
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
}
public void SendRadarData()
{
if (Bridge == null || Bridge.Status != Ros.Status.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);
BoxCollider boxCol = (BoxCollider)(col);
Vector3 size = boxCol == null ? Vector3.zero : boxCol.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
}
};
Bridge.Publish(ApolloTopicName, msg);
++seqId;
}
void Update()
{
if (targetEnv != ROSTargetEnvironment.APOLLO && targetEnv != ROSTargetEnvironment.AUTOWARE)
{
return;
}
if (Bridge == null || Bridge.Status != Ros.Status.Connected || !PublishMessage)
{
return;
}
if (Time.time < NextSend)
{
return;
}
NextSend = Time.time + 1.0f / Frequency;
Vector3 pos = Target.transform.position;
var utc = System.DateTime.UtcNow.ToString("HHmmss.fff");
float accuracy = 0.01f; // just a number to report
float altitude = pos.y; // above sea level
float height = 0; // sea level to WGS84 ellipsoid
double easting = pos.x * Scale + OriginEasting;
double northing = pos.z * Scale + OriginNorthing;
if (targetEnv == ROSTargetEnvironment.APOLLO && OriginEasting != 0.0f)
{
easting = easting - 500000;
}
// 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;
float latitude_orig = (float)(lat * 180.0 / Math.PI);
float longitude_orig = (float)(lon * 180.0 / Math.PI);
if (targetEnv == ROSTargetEnvironment.APOLLO && UTMZoneId > 0)
{
longitude_orig = longitude_orig + (UTMZoneId - 1) * 6 - 180 + 3;
}
//
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
char latitudeS = latitude_orig < 0.0f ? 'S' : 'N';
char longitudeS = longitude_orig < 0.0f ? 'W' : 'E';
float latitude = Mathf.Abs(latitude_orig);
float longitude = Mathf.Abs(longitude_orig);
latitude = Mathf.Floor(latitude) * 100 + (latitude % 1) * 60.0f;
longitude = Mathf.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"),
};
Bridge.Publish(AutowareTopic, ggaMessage);
var qqMessage = new Ros.Sentence()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = FrameId,
},
sentence = qq + "@" + qqChecksum.ToString("X2"),
};
Bridge.Publish(AutowareTopic, qqMessage);
}
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
// Apollo - GPS Best Pose
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;
var apolloMessage = new Ros.GnssBestPose()
{
header = new Ros.ApolloHeader()
{
timestamp_sec = Ros.Time.Now().secs,
sequence_num = (int)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
};
Bridge.Publish(ApolloTopic, apolloMessage);
}
}
void SendMessage()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("SendMessage");
#endif
var worldToLocal = Matrix4x4.TRS(transform.position, transform.rotation, Vector3.one).inverse;
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
// local.Set(local.x, local.z, local.y);
worldToLocal = new Matrix4x4(new Vector4(1, 0, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(0, 1, 0, 0), Vector4.zero) * worldToLocal;
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
// local.Set(local.z, -local.x, local.y);
worldToLocal = new Matrix4x4(new Vector4(0, -1, 0, 0), new Vector4(0, 0, 1, 0), new Vector4(1, 0, 0, 0), Vector4.zero) * worldToLocal;
}
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 worldPos = new Vector3(point.x, point.y, point.z);
float intensity = point.w;
*(Vector3 *)(ptr + offset) = worldToLocal.MultiplyPoint3x4(worldPos);
*(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 = "velodyne", // needed for Autoware
},
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 UNITY_EDITOR
UnityEngine.Profiling.Profiler.BeginSample("Publish");
#endif
if (TargetEnvironment == ROSTargetEnvironment.APOLLO)
{
Bridge.Publish(ApolloTopicName, msg);
}
else if (TargetEnvironment == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Publish(AutowareTopicName, msg);
}
else
{
Bridge.Publish(TopicName, msg);
}
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
#if UNITY_EDITOR
UnityEngine.Profiling.Profiler.EndSample();
#endif
}
public void FixedUpdate()
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected || !PublishMessage || !isEnabled)
{
return;
}
Vector3 currVelocity = transform.InverseTransformDirection(mainRigidbody.velocity);
Vector3 acceleration = (currVelocity - lastVelocity) / Time.fixedDeltaTime;
lastVelocity = currVelocity;
var linear_acceleration = new Ros.Point3D()
{
x = acceleration.x,
y = acceleration.z,
z = -Physics.gravity.y
};
Vector3 angularVelocity = mainRigidbody.angularVelocity;
var angular_velocity = new Ros.Point3D()
{
x = 0.0,
y = 0.0,
z = -angularVelocity.y
};
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"));
Bridge.Publish(ImuTopic, new Ros.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 angles = Target.transform.eulerAngles;
float roll = angles.z;
float pitch = angles.x;
float yaw = -angles.y;
System.DateTime Unixepoch = new System.DateTime(1970, 1, 1, 0, 0, 0, System.DateTimeKind.Utc);
measurement_time = (double)(System.DateTime.UtcNow - Unixepoch).TotalSeconds;
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 * 0.01745329252,
y = pitch * 0.01745329252,
z = yaw * 0.01745329252
}
}
};
Bridge.Publish(ApolloIMUOdometryTopic, apolloIMUMessage);
}
void SendPointCloud(List <VelodynePointCloudVertex> pointCloud)
{
if (Bridge == null || Bridge.Status != Ros.Status.Connected)
{
return;
}
var pointCount = pointCloud.Count;
byte[] byteData = new byte[32 * pointCount];
for (int i = 0; i < pointCount; i++)
{
var local = sensorLocalspaceTransform.InverseTransformPoint(pointCloud[i].position);
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
local.Set(local.z, -local.x, local.y);
}
else
{
local.Set(local.x, local.z, local.y);
}
var scaledPos = local * exportScaleFactor;
var x = System.BitConverter.GetBytes(scaledPos.x);
var y = System.BitConverter.GetBytes(scaledPos.y);
var z = System.BitConverter.GetBytes(scaledPos.z);
//var intensity = System.BitConverter.GetBytes(pointCloud[i].color.maxColorComponent);
//var intensity = System.BitConverter.GetBytes((float)(((int)pointCloud[i].color.r) << 16 | ((int)pointCloud[i].color.g) << 8 | ((int)pointCloud[i].color.b)));
//var intensity = System.BitConverter.GetBytes((byte)pointCloud[i].distance);
var intensity = System.BitConverter.GetBytes((byte)(pointCloud[i].color.grayscale * 255));
var ring = System.BitConverter.GetBytes(pointCloud[i].ringNumber);
var ts = System.BitConverter.GetBytes((double)0.0);
System.Buffer.BlockCopy(x, 0, byteData, i * 32 + 0, 4);
System.Buffer.BlockCopy(y, 0, byteData, i * 32 + 4, 4);
System.Buffer.BlockCopy(z, 0, byteData, i * 32 + 8, 4);
System.Buffer.BlockCopy(intensity, 0, byteData, i * 32 + 16, 1);
System.Buffer.BlockCopy(ts, 0, byteData, i * 32 + 24, 8);
}
var msg = new Ros.PointCloud2()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seqId++,
frame_id = "velodyne", // needed for Autoware
},
height = 1,
width = (uint)pointCount,
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)pointCount * 32,
data = byteData,
is_dense = true,
};
if (targetEnv == ROSTargetEnvironment.AUTOWARE)
{
Bridge.Publish(topicName, msg);
}
if (targetEnv == ROSTargetEnvironment.APOLLO)
{
Bridge.Publish(ApolloTopicName, msg);
}
}
public void OnRosConnected()
{
Bridge.Subscribe(WHEEL_CMD_TOPIC,
(WheelsCmdStampedMsg msg) =>
{
wheelLeftVel = msg.vel_left;
wheelRightVel = msg.vel_right;
});
// Autoware vehicle command
Bridge.Subscribe(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);
});
string override_topic;
if (Bridge.Version == 1)
{
Bridge.AddPublisher <Ros.Joy>(JOYSTICK_ROS1);
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1;
}
else
{
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2;
}
Bridge.AddPublisher <BoolStamped>(override_topic);
Bridge.Subscribe <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;
Bridge.Publish(topic, stamp);
FirstConnection = false;
}
}
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 != Ros.Status.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"),
};
Bridge.Publish(AutowareTopic, ggaMessage);
var qqMessage = new Ros.Sentence()
{
header = new Ros.Header()
{
stamp = Ros.Time.Now(),
seq = seq++,
frame_id = FrameId,
},
sentence = qq + "@" + qqChecksum.ToString("X2"),
};
Bridge.Publish(AutowareTopic, 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,
}
},
}
};
Bridge.Publish(AutowareOdometryTopic, 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
};
Bridge.Publish(ApolloTopic, 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()
}
};
Bridge.Publish(ApolloGPSOdometryTopic, apolloGpsMessage);
}
}
public void OnRosConnected()
{
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 = ""
});
});
// tugbot
Bridge.Subscribe(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.Subscribe(WHEEL_CMD_TOPIC,
(WheelsCmdStampedMsg msg) =>
{
wheelLeftVel = msg.vel_left;
wheelRightVel = msg.vel_right;
});
// Autoware vehicle command
Bridge.Subscribe(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);
});
string override_topic;
if (Bridge.Version == 1)
{
Bridge.AddPublisher <Ros.Joy>(JOYSTICK_ROS1);
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS1;
}
else
{
override_topic = JOYSTICK_OVERRIDE_TOPIC_ROS2;
}
Bridge.AddPublisher <BoolStamped>(override_topic);
Bridge.Subscribe <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;
Bridge.Publish(topic, stamp);
FirstConnection = false;
}
ManualControl = true;
}
