def setPolar (self, distance, azimuth, elevation_angle, reference_geo_point):
"""
Sets position from polar position, relative to given reference position.
Parameters
----------
distance: float
distance in meters
azimuth: float
azimuth in radians
elevation_angle: float
elevation angle (up) in radians
reference_geo_point: nv.GeoPoint
Reference position for ENU origin
Returns
-------
"""
assert isinstance(reference_geo_point, nv.GeoPoint)
n = distance * math.cos(azimuth)
e = distance * math.sin(azimuth)
d = distance * math.sin(-elevation_angle)
local_frame = nv.FrameN(reference_geo_point)
d_rg2 = local_frame.Pvector(np.r_[n, e, d]).to_ecef_vector()
self.__ecef_pos = reference_geo_point.to_ecef_vector() + d_rg2.to_ecef_vector()
self.__geopoint = self.__ecef_pos.to_geo_point()
def getLatLongDelta(self, n, e):
local_frame = nv.FrameN(self.__geopoint) # North-East-Down frame
local_p = local_frame.Pvector(np.r_[n, e, 0])
ecef_pos = self.__ecef_pos + local_p.to_ecef_vector()
local_geo = ecef_pos.to_geo_point() # type: nv.GeoPoint
return abs(self.__geopoint.latitude_deg-local_geo.latitude_deg)[0], abs(self.__geopoint.longitude_deg-local_geo.longitude_deg)[0]
def setNED(self, n, e, d, reference_geo_point):
"""
Sets position from north-east-down position, relative to given reference position.
Parameters
----------
n: float
position north component
e: float
position east component
d: float
position down component
reference_geo_point: nv.GeoPoint
Reference position for ENU origin
Returns
-------
"""
assert isinstance(reference_geo_point, nv.GeoPoint)
local_frame = nv.FrameN(reference_geo_point) # North-East-Down frame
local_p = local_frame.Pvector(np.r_[n, e, d])
self.__ecef_pos = reference_geo_point.to_ecef_vector() + local_p.to_ecef_vector()
self.__geopoint = self.__ecef_pos.to_geo_point()
def ortho_azimuth(self, other_trap_center):
# Azimuth of line orthogonal to the divider
path_ab_e = nvector.diff_positions(self.trap_center, other_trap_center)
frame_n = nvector.FrameN(self.trap_center)
path_ab_n = path_ab_e.change_frame(frame_n)
path_ab_n = path_ab_n.pvector.ravel()
azimuth = numpy.arctan2(path_ab_n[1], path_ab_n[0])
return azimuth
def load_line(self, pointA, pointB):
self.path = nvector.GeoPath(pointA, pointB)
# Ick. This looks horrible.
p_AB_E = nvector.diff_positions(pointA, pointB)
frame_N = nvector.FrameN(pointA)
p_AB_N = p_AB_E.change_frame(frame_N)
p_AB_N = p_AB_N.pvector.ravel()
azimuth = numpy.arctan2(p_AB_N[1], p_AB_N[0])
self.course = numpy.rad2deg(azimuth) % 360
#print 'azimuth:', azimuth, self.course
self.course_distance = self.path.positionB.distance_and_azimuth(self.path.positionA)[0]
self.offset = 0.0
def ConvLLA2NED(self, LLA_mat):
LLA_ned = np.zeros(LLA_mat.shape)
for i in range(LLA_mat.shape[1]):
point = self.wgs84.GeoPoint(latitude=LLA_mat[0,i], longitude=LLA_mat[1,i], z=LLA_mat[2,i], degrees=True)
p_AB_E = nv.diff_positions(self.pointref, point)
frame_N = nv.FrameN(self.pointref)
p_AB_N = p_AB_E.change_frame(frame_N)
p_AB_N = p_AB_N.pvector.ravel()
LLA_ned[:,i] = p_AB_N
return LLA_ned
def rtk_callback(msg):
lat = msg.latitude
lon = msg.longitude
alt = msg.altitude
if 0.0 in [lat, lon, alt]:
rospy.logwarn_throttle(2, ('{}: Received a msg with lat, lon, alt'
': 0, 0, 0'.format(NODE_NAME)))
return
current_geopoint = WGS84.GeoPoint(
latitude=lat,
longitude=lon,
z=alt,
degrees=True)
current_stamped_geopoint = StampedGeoPoint(current_geopoint, msg.header.stamp)
if len(measurement_history) == 0:
measurement_history.append(current_stamped_geopoint)
else:
last_measurement = measurement_history[-1]
if last_measurement != current_stamped_geopoint:
measurement_history.append(current_stamped_geopoint)
else:
return
if reference_stamped_geopoint is None and autoset_geo_reference:
set_geo_reference()
if reference_stamped_geopoint is None:
rospy.logwarn_throttle(2, '{}: RTK Reference is not set!'.format(NODE_NAME))
return
'''get transformation from intial to current in Earth coordinate frame'''
transform_in_E = nv.diff_positions(
reference_stamped_geopoint.geopoint,
current_stamped_geopoint.geopoint)
'''get transformation in NED frame'''
reference_frame = nv.FrameN(reference_stamped_geopoint.geopoint)
transform_in_NED = transform_in_E.change_frame(reference_frame)
transform_in_NED = transform_in_NED.pvector.ravel()
'''get transformation in ENU frame (ROS compatible)'''
transform_in_ENU = [
transform_in_NED[1],
transform_in_NED[0],
transform_in_NED[2]
]
h = Header()
h.stamp = current_stamped_geopoint.stamp
h.frame_id = map_frame_id
p_stamped = PointStamped()
p_stamped.header = h
p_stamped.point.x = transform_in_ENU[0]
p_stamped.point.y = transform_in_ENU[1]
p_stamped.point.z = transform_in_ENU[2]
local_rtk_enu_position_pub.publish(p_stamped)
if(publish_tf):
t_stamped = TransformStamped()
t_stamped.header = h
t_stamped.child_frame_id = baselink_translation_frame_id
t_stamped.transform = Transform()
t_stamped.transform.translation = Vector3(*transform_in_ENU)
t_stamped.transform.rotation = Quaternion(0, 0, 0, 1)
tf_broadcaster.sendTransform(t_stamped)
def gps_callback(msg):
lat = msg.latitude
lon = msg.longitude
alt = msg.altitude
if 0.0 in [lat, lon, alt]:
rospy.logwarn_throttle(2, ('{}: Received a msg with lat, lon, alt'
': 0, 0, 0'.format(node_name)))
return
current_geopoint = WGS84.GeoPoint(latitude=lat,
longitude=lon,
z=alt,
degrees=True)
current_stamped_geopoint = StampedGeoPoint(current_geopoint,
msg.header.stamp)
global last_gps_message
if last_gps_message is None:
last_gps_message = current_stamped_geopoint
else:
if last_gps_message != current_stamped_geopoint:
last_gps_message = current_stamped_geopoint
else:
return
if reference_stamped_geopoint is None:
rospy.logwarn_throttle(
2, '{}: GPS Reference is not set!'.format(node_name))
return
'''get transformation from intial to current in Earth coordinate frame'''
transform_in_E = nv.diff_positions(reference_stamped_geopoint.geopoint,
current_stamped_geopoint.geopoint)
'''get transformation in NED frame'''
reference_frame = nv.FrameN(reference_stamped_geopoint.geopoint)
transform_in_NED = transform_in_E.change_frame(reference_frame)
transform_in_NED = transform_in_NED.pvector.ravel()
'''get transformation in ENU frame (ROS compatible)'''
transform_in_ENU = [
transform_in_NED[1], transform_in_NED[0], transform_in_NED[2]
]
h = Header()
h.stamp = current_stamped_geopoint.stamp
h.frame_id = earth_frame_id
p_stamped = PointStamped()
p_stamped.header = h
p_stamped.point.x = transform_in_ENU[0]
p_stamped.point.y = transform_in_ENU[1]
p_stamped.point.z = transform_in_ENU[2]
local_euclidean_position_pub.publish(p_stamped)
if (publish_tf):
t_stamped = TransformStamped()
t_stamped.header = h
t_stamped.child_frame_id = gps_no_orientation_frame_id
t_stamped.transform = Transform()
t_stamped.transform.translation = Vector3(*transform_in_ENU)
t_stamped.transform.rotation = Quaternion(0, 0, 0, 1)
tf_broadcaster.sendTransform(t_stamped)
