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