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; }