public override void OverridePositionAndOrientation(Vector3 newPosition, Quaternion newOrientation)
{
_rosLogger.PublishData(new StringMsg("Odometry overwritten"));
//Unity repositioning for the virtual robot.
transform.SetPositionAndRotation(newPosition, newOrientation);
//reset initial poses for the odom calculation. This is the equivalent of moving the robot in the physical room.
// We simply move the local coords.
InitialPosition = newPosition;
InitialRotation = newOrientation;
//Publishing to odo_calib_pose for initial gps position, just like the arlobot controller.
GeoPointWGS84 wgs84 = newPosition.ToUTM().ToWGS84();
PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(
new PoseMsg(
new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
));
OdometryMsg odometryUpdate = new OdometryMsg(pose);
_rosOdometryOverride.PublishData(odometryUpdate);
}
public void ReceivedOdometryUpdate(ROSBridgeMsg data)
{
//In WGS84
OdometryMsg nav = (OdometryMsg)data;
GeoPointWGS84 geoPoint = new GeoPointWGS84
{
latitude = nav._pose._pose._position.GetY(),
longitude = nav._pose._pose._position.GetX(),
altitude = nav._pose._pose._position.GetZ(),
};
Quaternion orientation = new Quaternion(
x: nav._pose._pose._orientation.GetX(),
z: nav._pose._pose._orientation.GetY(),
y: nav._pose._pose._orientation.GetZ(),
w: nav._pose._pose._orientation.GetW()
);
_odometryDataToConsume = new OdometryData
{
Position = geoPoint,
Orientation = orientation
};
_hasOdometryDataToConsume = true;
}
private void OnAddFiducialGPSValuesChanged(string value)
{
Vector3 rotation = new Vector3(float.Parse(_addFidRotX.text, CultureInfo.InvariantCulture),
float.Parse(_addFidRotY.text, CultureInfo.InvariantCulture), float.Parse(_addFidRotZ.text, CultureInfo.InvariantCulture));
int id = int.Parse(_addFidId.text);
GeoPointWGS84 gps = new GeoPointWGS84
{
altitude = double.Parse(_addFidAlt.text, CultureInfo.InvariantCulture),
latitude = double.Parse(_addFidLat.text, CultureInfo.InvariantCulture),
longitude = double.Parse(_addFidLon.text, CultureInfo.InvariantCulture)
};
Vector3 unityPos = gps.ToUTM().ToUnity();
_addFidPosX.text = unityPos.x.ToString(CultureInfo.InvariantCulture);
_addFidPosY.text = unityPos.y.ToString(CultureInfo.InvariantCulture);
_addFidPosZ.text = unityPos.z.ToString(CultureInfo.InvariantCulture);
if (CurrentUIState == UIState.PlacingFiducial)
{
FiducialController.Instance.PlaceOrUpdateNewFiducial(int.Parse(_addFidId.text), unityPos, rotation);
}
else if (CurrentUIState == UIState.UpdatingFiducial)
{
FiducialController.Instance.UpdateFiducial(id, unityPos, rotation);
}
}
public static GeoPointMercator ToMercator(this GeoPointWGS84 geoPoint)
{
GeoPointMercator mercator = new GeoPointMercator(_transformationFromWGS84ToMercator.MathTransform.Transform(geoPoint.ToArray()))
{
altitude = geoPoint.altitude
};
return(mercator);
}
public static GeoPointUTM ToUTM(this GeoPointWGS84 geoPoint)
{
GeoPointUTM utm = new GeoPointUTM(_transformationFromWGS84ToUTM.MathTransform.Transform(geoPoint.ToArray()))
{
altitude = geoPoint.altitude
};
return(utm);
}
public static GeoPointWGS84 ToWGS84(this GeoPointUTM geoPoint)
{
GeoPointWGS84 wgs84 = new GeoPointWGS84(_transformationFromUTMToUGS84.MathTransform.Transform(geoPoint.ToArray()))
{
altitude = geoPoint.altitude
};
return(wgs84);
}
private void Move(Vector3 position)
{
GeoPointWGS84 point = position.ToUTM().ToWGS84();
_rosLocomotionWaypoint.PublishData(point);
_currentWaypoint = position;
CurrenLocomotionType = RobotLocomotionType.WAYPOINT;
_rosLocomotionWaypointState.PublishData(ROSLocomotionWaypointState.RobotWaypointState.RUNNING);
CurrentRobotLocomotionState = RobotLocomotionState.MOVING;
}
/// <param name="data">X: Angular speed in meter/s, Y: Linear speed in meter/s</param>
public void PublishData(GeoPointWGS84 data)
{
if (_publisher == null)
{
return;
}
NavSatFixMsg navSatFix = new NavSatFixMsg(data.latitude, data.longitude, data.altitude);
_publisher.PublishData(navSatFix);
}
// Update is called once per frame
void Update()
{
if (_test)
{
GeoPointUTM coordinatesUtm = _testPoint.transform.position.ToUTM();
Debug.Log("Test point - UTM: " + coordinatesUtm);
GeoPointWGS84 wgs84 = coordinatesUtm.ToWGS84();
Debug.Log("Test point - WGS84: " + wgs84);
GeoPointMercator utm = coordinatesUtm.ToMercator();
Debug.Log("Test point - Mercator: " + utm);
_test = false;
}
if (_testCoord)
{
GameObject point = GameObject.CreatePrimitive(PrimitiveType.Sphere);
point.transform.position = _wgsPointToTest.ToUTM().ToUnity();
_testCoord = false;
}
if (_testDistance)
{
Debug.Log(Vector3.Distance(_testDistancePointA.position, _testDistancePointB.position) + "meters");
_testDistance = false;
}
if (_testOrientation)
{
transform.LookAt(_testOrientationPoint);
_testOrientation = false;
}
if (_testDistancePoint)
{
GameObject point = GameObject.CreatePrimitive(PrimitiveType.Sphere);
point.transform.position = transform.position + transform.forward * _distance;
_testDistancePoint = false;
}
if (_moveToDistance)
{
Ray ray = new Ray(transform.position, transform.forward);
RaycastHit hit;
if (Physics.Raycast(ray, out hit, float.PositiveInfinity))
{
float offset = hit.distance - _moveDistance;
transform.position = transform.position + transform.forward * offset;
}
_moveToDistance = false;
}
Debug.DrawRay(transform.position, transform.forward * 100);
}
//publishing on the /odom topic
// the newposition should be the difference between the current position
//and the InitialPosition of the robot in Unity vec3, effectively simulating the local space coords of odom
//The geopoint conversion is in here.
public void PublishOdometryData(Vector3 newPosition, Quaternion newOrientation)
{
GeoPointWGS84 wgs84 = newPosition.ToUTM().ToWGS84();
PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(
new PoseMsg(
new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
));
OdometryMsg odometryUpdate = new OdometryMsg(pose);
// _rosOdometry.PublishData(odometryUpdate);
}
public void ReceivedWaypoint(ROSBridgeMsg newGoal)
{
NavSatFixMsg waypoint = (NavSatFixMsg)newGoal;
GeoPointWGS84 coordinate = new GeoPointWGS84()
{
latitude = waypoint._latitude,
longitude = waypoint._longitude,
altitude = waypoint._altitude
};
goal = coordinate.ToUTM().ToUnity();
goal_set = true;
subState = "STOP";
}
public override void OverridePositionAndOrientation(Vector3 newPosition, Quaternion newOrientation)
{
GeoPointWGS84 wgs84 = newPosition.ToUTM().ToWGS84();
PoseWithCovarianceMsg pose = new PoseWithCovarianceMsg(
new PoseMsg(
new PointMsg(wgs84.longitude, wgs84.latitude, wgs84.altitude),
new QuaternionMsg(newOrientation.x, newOrientation.z, newOrientation.y, newOrientation.w)
));
OdometryMsg odometryOverride = new OdometryMsg(pose);
_rosOdometryOverride.PublishData(odometryOverride);
}
private void OnUpdateFiducialMouseClick(Transform fiducialToUpdate)
{
_addFiducialPanel.SetActive(true);
_addFidPosX.text = fiducialToUpdate.position.x.ToString(CultureInfo.InvariantCulture);
_addFidPosY.text = fiducialToUpdate.position.y.ToString(CultureInfo.InvariantCulture);
_addFidPosZ.text = fiducialToUpdate.position.z.ToString(CultureInfo.InvariantCulture);
_addFidRotX.text = fiducialToUpdate.eulerAngles.x.ToString(CultureInfo.InvariantCulture);
_addFidRotY.text = fiducialToUpdate.eulerAngles.y.ToString(CultureInfo.InvariantCulture);
_addFidRotZ.text = fiducialToUpdate.eulerAngles.z.ToString(CultureInfo.InvariantCulture);
GeoPointWGS84 gps = fiducialToUpdate.position.ToUTM().ToWGS84();
_addFidLon.text = gps.longitude.ToString(CultureInfo.InvariantCulture);
_addFidAlt.text = gps.altitude.ToString(CultureInfo.InvariantCulture);
_addFidLat.text = gps.latitude.ToString(CultureInfo.InvariantCulture);
_addFidId.text = FiducialController.Instance.StartUpdateFiducial(fiducialToUpdate).ToString();
_donePanel.SetActive(false);
}
//Publisher to /robot_gps_pose every interval time
private IEnumerator SendTransformUpdate(float interval)
{
while (true)
{
//Add the Noise here. Unity should have the ACTUAL position of the physical world
// and what it publishes and we see on the webmap is the where it thinks it is.
GeoPointWGS84 wgs = transform.position.ToUTM().ToWGS84();
Quaternion rot = transform.rotation;
PoseMsg pose = new PoseMsg(new PointMsg(wgs.longitude, wgs.latitude, wgs.altitude),
new QuaternionMsg(rot.x, rot.y, rot.z, rot.w));
PoseWithCovarianceMsg poseWithCovariance = new PoseWithCovarianceMsg(pose, new double[36]);
OdometryMsg odometry = new OdometryMsg(poseWithCovariance);
odometry._pose = poseWithCovariance;
_rosOdometryGPSPose.PublishData(odometry);
yield return(new WaitForSeconds(interval));
}
}
private void OnAddFiducialMouseClick(Vector3 clickPos)
{
clickPos = new Vector3(clickPos.x, FiducialController.Instance.ZeroFiducial.Position.ToUTM().ToUnity().y, clickPos.z);
_addFidPosX.text = clickPos.x.ToString(CultureInfo.InvariantCulture);
_addFidPosY.text = clickPos.y.ToString(CultureInfo.InvariantCulture);
_addFidPosZ.text = clickPos.z.ToString(CultureInfo.InvariantCulture);
_addFidRotX.text = "0.0";
_addFidRotY.text = "0.0";
_addFidRotZ.text = "0.0";
GeoPointWGS84 gps = clickPos.ToUTM().ToWGS84();
_addFidLon.text = gps.longitude.ToString(CultureInfo.InvariantCulture);
_addFidAlt.text = gps.altitude.ToString(CultureInfo.InvariantCulture);
_addFidLat.text = gps.latitude.ToString(CultureInfo.InvariantCulture);
Vector3 position = new Vector3(float.Parse(_addFidPosX.text, CultureInfo.InvariantCulture),
float.Parse(_addFidPosY.text, CultureInfo.InvariantCulture), float.Parse(_addFidPosZ.text, CultureInfo.InvariantCulture));
FiducialController.Instance.PlaceOrUpdateNewFiducial(int.Parse(_addFidId.text), position, Vector3.zero);
}
private void OnAddFiducialPositionValuesChanged(string value)
{
Vector3 position = new Vector3(float.Parse(_addFidPosX.text, CultureInfo.InvariantCulture),
float.Parse(_addFidPosY.text, CultureInfo.InvariantCulture), float.Parse(_addFidPosZ.text, CultureInfo.InvariantCulture));
Vector3 rotation = new Vector3(float.Parse(_addFidRotX.text, CultureInfo.InvariantCulture),
float.Parse(_addFidRotY.text, CultureInfo.InvariantCulture), float.Parse(_addFidRotZ.text, CultureInfo.InvariantCulture));
int id = int.Parse(_addFidId.text);
GeoPointWGS84 gps = position.ToUTM().ToWGS84();
_addFidLon.text = gps.longitude.ToString(CultureInfo.InvariantCulture);
_addFidAlt.text = gps.altitude.ToString(CultureInfo.InvariantCulture);
_addFidLat.text = gps.latitude.ToString(CultureInfo.InvariantCulture);
if (CurrentUIState == UIState.PlacingFiducial)
{
FiducialController.Instance.PlaceOrUpdateNewFiducial(int.Parse(_addFidId.text), position, rotation);
}
else if (CurrentUIState == UIState.UpdatingFiducial)
{
FiducialController.Instance.UpdateFiducial(id, position, rotation);
}
}
/// <summary>
/// Instantiates point object from WGS84 or Mercator coordinates, by converting them to UCS
/// </summary>
/// <param name="isMercator">Is coordinate in Mercator, otherwise WGS84</param>
/// <returns>Point</returns>
private GameObject InstantiatePoint(JSONObject coordinate)
{
GeoPointUTM geoPointUtm;
var geoPointWGS84 = new GeoPointWGS84
{
longitude = Double.Parse(coordinate[0].ToString(), NumberStyles.Float),
latitude = Double.Parse(coordinate[1].ToString(), NumberStyles.Float),
};
geoPointUtm = geoPointWGS84.ToUTM();
GameObject pointObject = new GameObject("point");
if (!GeoUtils.UtmOriginSet)
{
GeoUtils.UtmOrigin = geoPointUtm;
pointObject.name = "Zero";
}
pointObject.transform.position = geoPointUtm.ToUnity();
return(pointObject);
}
/// <summary>
/// Transforms WGS84 coordinate and GeoRotation into fiducialEntryMessage.
/// </summary>
public FiducialMapEntryMsg CreateFiducialMapEntryMessage(int fiducialId, GeoPointWGS84 wgsPoint, GeoRotation geoRotation)
{
return(new FiducialMapEntryMsg(fiducial_id: fiducialId, x: wgsPoint.longitude, y: wgsPoint.latitude, z: wgsPoint.altitude,
rx: geoRotation.east, ry: geoRotation.north, rz: geoRotation.heading));
}
public void GetPath(GeoPointWGS84 from, GeoPointWGS84 to)
{
string url = string.Format(PATH_SEARCH, from.latitude, from.longitude, to.latitude, to.longitude).Replace(',', '.');
StartCoroutine(GetWWW(url, CreateRouteFromPath));
}
