def metric_dist(a, b):
pA = fromLatLong(a[0], a[1], a[2]).toPoint()
pB = fromLatLong(b[0], b[1], b[2]).toPoint()
dx2 = (pA.x - pB.x) * (pA.x - pB.x)
dy2 = (pA.y - pB.y) * (pA.y - pB.y)
dz2 = (pA.z - pB.z) * (pA.z - pB.z)
return sqrt(dx2 + dy2 + dz2)
def execute(self, userdata):
move_base_client = SimpleActionClient('move_base', MoveBaseAction)
move_base_client.wait_for_server(rospy.Duration(5))
goal_pose = MoveBaseGoal()
goal_pose.target_pose.header.frame_id = 'utm'
goal_pose.target_pose.header.stamp = rospy.Time.now()
# First point specified is latitude, second is longitude
utm_waypoint = utm.fromLatLong(userdata.waypoint_in.position.x, userdata.waypoint_in.position.y, 250)
goal_pose.target_pose.pose.position.x = utm_waypoint.easting
goal_pose.target_pose.pose.position.y = utm_waypoint.northing
goal_pose.target_pose.pose.position.z = 246
goal_pose.target_pose.pose.orientation.x = 0
goal_pose.target_pose.pose.orientation.y = 0
goal_pose.target_pose.pose.orientation.z = 0
goal_pose.target_pose.pose.orientation.w = 1
rospy.loginfo(goal_pose)
move_base_client.send_goal(goal_pose)
rospy.loginfo("Sent goal")
move_base_client.wait_for_result()
result = move_base_client.get_state()
if (result == GoalStatus.SUCCEEDED):
return 'succeeded'
else:
return 'failed'
def _parseFromNavSatFix(randomBag,
eastingShift=0.0,
northingShift=0.0,
heightShift=0.0,
startTime=_startTime0,
stopTime=_stopTime_1):
assert (randomBag.type() == 'sensor_msgs/NavSatFix')
timestamps = []
i = 0
msgSamples = GeographicTrajectory._createTimeRange(
randomBag, startTime, stopTime)
parsedCoordinates = np.zeros((len(msgSamples), 7), dtype=np.float)
for s in tqdm(msgSamples):
rawmsg = randomBag[s]
coord = utm.fromLatLong(rawmsg.latitude, rawmsg.longitude,
rawmsg.altitude)
parsedCoordinates[i, 0:3] = [
coord.easting + eastingShift, coord.northing + northingShift,
coord.altitude + heightShift
]
if i >= 1:
quat = GeographicTrajectory.orientationFromPositionOnlyYaw(
parsedCoordinates[i], parsedCoordinates[i - 1])
parsedCoordinates[i, 3:7] = quat
if i == 1:
parsedCoordinates[0, 3:7] = quat
timestamps.append(rawmsg.header.stamp)
i += 1
return timestamps, parsedCoordinates
def __init__(self):
self.node_name = rospy.get_name()
self.pose = Pose()
self.prior_pose = Pose()
self.prior_roll = 0
self.prior_pitch = 0
self.prior_yaw = 0
self.start = False
self.covariance = np.zeros((36, ), dtype=float)
self.odometry = Odometry()
self.br = tf.TransformBroadcaster()
# param
self.imu_offset = 0
self.lat_orig = rospy.get_param('~latitude', 0.0)
self.long_orig = rospy.get_param('~longitude', 0.0)
self.yaw_offset = rospy.get_param("~yaw_offset", 0.0)
self.visual = rospy.get_param("~visual", False)
self.utm_orig = fromLatLong(self.lat_orig, self.long_orig)
# Publisher
self.pub_odm = rospy.Publisher("~odometry", Odometry, queue_size=1)
# Subscriber
sub_imu = message_filters.Subscriber("imu/data", Imu)
sub_gps = message_filters.Subscriber("fix", NavSatFix)
ats = ApproximateTimeSynchronizer((sub_imu, sub_gps),
queue_size=1,
slop=0.1)
ats.registerCallback(self.cb_gps_imu)
def gps_callback(self, gps_msg):
if gps_msg.status == -1:
self.positions = self.positions[1:]
return
utm_point = utm.fromLatLong(gps_msg.latitude, gps_msg.longitude)
easting = utm_point.easting
northing = utm_point.northing
print "Easting, northing: ", easting, northing
self.pub.publish(self.imu_msg)
if len(self.positions) >= self.window:
self.positions = self.positions[1:] + [[northing, easting]]
else:
self.positions = self.positions + [[northing, easting]]
pos = np.array(self.positions)
if pos.shape[0] < 2:
return
diffs = pos[1:, :] - pos[:-1, :]
mean_diff = np.mean(diffs, axis=0)
print diffs
print mean_diff
print self.positions
yaw = math.atan2(mean_diff[0], mean_diff[1])
print yaw
roll = 0.
pitch = 0.
self.imu_msg.orientation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(self.roll, self.pitch, yaw))
print self.imu_msg.orientation
self.imu_msg.header.stamp = rospy.Time.now()
self.pub.publish(self.imu_msg)
def _save_plan_lat_long_cb(self, feedback):
#This is the object that we are pressing (feedback) so
#that we can get the marker name etc..
pre, ext = os.path.splitext(self.mission_file)
lat_lon_file = pre + ".lolo"
rospy.loginfo("Saving the plan to file: %s", lat_lon_file)
gps_msg = rospy.wait_for_message('/gps/fix', NavSatFix)
lon = gps_msg.longitude
lat = gps_msg.latitude
utm_obj = utm.fromLatLong(lat, lon)
with open(lat_lon_file, 'w') as csvfile:
csvfile.write("ts\n")
spamwriter = csv.writer(csvfile,
delimiter=' ',
quotechar='"',
quoting=csv.QUOTE_MINIMAL)
for waypoint_index, pose in enumerate(self.waypoints):
new_obj = copy.deepcopy(utm_obj)
new_obj.northing += pose.position.y
new_obj.easting += pose.position.x
geo_obj = new_obj.toMsg()
lat_rounded = round(geo_obj.latitude, 5)
lon_rounded = round(geo_obj.longitude, 5)
spamwriter.writerow([
"ADD", "GOTOWP", lat_rounded, lon_rounded,
self.goal_tolerance, self.default_rpm
])
csvfile.write("start\n..\n")
def __init__(self):
self.node_name = rospy.get_name()
self.pose = Pose()
self.prior_pose = Pose()
self.prior_roll = 0
self.prior_pitch = 0
self.prior_yaw = 0
self.start = False
self.covariance = np.zeros((36, ), dtype=float)
self.odometry = Odometry()
self.br = tf.TransformBroadcaster()
# param
self.imu_offset = 0
self.lat_orig = rospy.get_param('~latitude', 0.0)
self.long_orig = rospy.get_param('~longitude', 0.0)
self.utm_orig = fromLatLong(self.lat_orig, self.long_orig)
# Service
self.srv_imu_offset = rospy.Service('~imu_offset', SetValue,
self.cb_srv_imu_offest)
# Publisher
self.pub_odm = rospy.Publisher("~odometry", Odometry, queue_size=1)
# Subscriber
sub_imu = rospy.Subscriber("~imu/data", Imu, self.cb_imu, queue_size=1)
sub_gps = rospy.Subscriber("~fix",
NavSatFix,
self.cb_gps,
queue_size=1)
def estimate_position(self, fixes, covars):
try:
now = rospy.Time(0)
(world_trans, world_rot) = self.listener.lookupTransform(
"world_utm", "world_local", now)
except (tf.LookupException, tf.ConnectivityException):
self._feedback.status = "Could not get transform between %s and %s" % (
"world_utm", "world_local")
rospy.loginfo("Could not get transform between %s and %s" %
("world_utm", "world_local"))
self._as.publish_feedback(self._feedback)
# easting, northing is in world_utm coordinate system,
# we need to transform it to world or world_local
pos = np.zeros((len(fixes), 3))
for i, fix in enumerate(fixes):
utm_point = utm.fromLatLong(fix[0], fix[1])
easting = utm_point.easting
northing = utm_point.northing
utm_zone = utm_point.zone
pos[i, :] = np.array(
[easting - world_trans[0], northing - world_trans[1], 0.])
# use the cov to weight the means in the future
estimate = np.mean(pos, axis=0)
return estimate
def __init__(self):
# Initialize class variables.
self.pose_estimate = None
self.move_goal_status = 'None'
self.utm_origin = utm.fromLatLong(*rospy.get_param('/MISSION_CTRL/arena_params/GPS_origin')[0:3])
# TODO: make it parametric
self.pick_success_rate = 1
self.place_success_rate = 1
# Create subscribers.
rospy.Subscriber('odometry', Odometry, self.odometry_cb, queue_size=1)
rospy.Subscriber('move_base/result', MoveBaseActionResult, self.move_base_cb, queue_size=1)
# Create publishers.
self.goal_pose_pub = rospy.Publisher('move_base_simple/goal', PoseStamped, queue_size=1)
self.hardware_pub = rospy.Publisher('hardware_status', HWStatusUGV, queue_size=1)
# Create services.
rospy.Service('go_to_gps', GoGPSPosition, self.go_to_GPS_position_srv)
rospy.Service('go_to_position', GoPosition, self.go_to_local_position_srv)
rospy.Service('brick_pickup', PickupUGV, self.pick_srv)
rospy.Service('brick_place', PlaceUGV, self.place_srv)
self.hardware_pub.publish(HWStatusUGV.READY, '')
rospy.spin()
def waypoint_pub(self, lat, lon):
utm_conversion = utm.fromLatLong(lat, lon)
print(utm_conversion)
goal = MoveBaseGoal()
goal.target_pose.header.frame_id = "utm"
goal.target_pose.header.stamp = rospy.Time.now()
goal.target_pose.pose.position.x = utm_conversion.easting
goal.target_pose.pose.position.y = utm_conversion.northing
goal.target_pose.pose.position.z = 0
goal.target_pose.pose.orientation.w = 1.0
try:
transform = self.tf_buffer.lookup_transform(
"map",
goal.target_pose.header.frame_id, #source frame
rospy.Time(0), #get the tf at first available time
rospy.Duration(2.0)) #wait for 1 second
goal.target_pose = tf2_geometry_msgs.do_transform_pose(
goal.target_pose, transform)
#goal.target_pose.pose.position.x = -goal.target_pose.pose.position.x
goal.target_pose.header.stamp = rospy.Time.now()
print "goal transformed\n", goal
self.mb_client.send_goal(goal)
wait = self.mb_client.wait_for_result()
return wait
except:
print "Couldnt find transform from utm to map"
return False
def __init__(self):
rospy.init_node("utm_odom")
self.tfBuffer = tf2_ros.Buffer()
self.listener = tf2_ros.TransformListener(self.tfBuffer)
datum = rospy.get_param("~datum", False)
mag_dec = rospy.get_param('~magnetic_declination_radians', 0.0)
yaw_offset = rospy.get_param('~yaw_offset', 0.0)
self.do_tf = rospy.get_param('~broadcast_odom_base', False)
pub_gps = rospy.get_param('~pub_gps', False)
rotation_offset = yaw_offset + mag_dec
rospy.loginfo(
"Magnetic Declination is {}\n IMU Reference Offset is:{}\nTotal Yaw Offset for UTM->Odom is {}"
.format(mag_dec, yaw_offset, rotation_offset))
self.odom_tf = tf2_ros.TransformBroadcaster()
# Transform odom into UTM frame
if datum:
rospy.loginfo(
"Setting datum to Latitude: {}, Longitude: {}".format(
datum[0], datum[1]))
datumPoint = utm.fromLatLong(datum[0], datum[1], 0.0) # get the
(zone, band) = datumPoint.gridZone()
rospy.loginfo("UTM Zone: {}{}".format(zone, band))
self.datum = datumPoint.toPoint()
rospy.loginfo("UTM Translation is: (X,Y,Z)=({},{},{})".format(
self.datum.x, self.datum.y, self.datum.z))
else:
self.datum = utm.UTMPoint(easting=0.0,
northing=0.0,
altitude=0.0,
zone=zone,
band=band).toPoint()
rospy.logwarn(
"Setting datum to current zone origin. odom frame will have large values!"
)
T = TransformStamped()
T.header.frame_id = "utm"
T.header.stamp = rospy.Time.now()
T.child_frame_id = "odom"
quat = tf.quaternion_from_euler(0, 0, rotation_offset, axes='sxyz')
T.transform.rotation.x = quat[0]
T.transform.rotation.y = quat[1]
T.transform.rotation.z = quat[2]
T.transform.rotation.w = quat[3]
T.transform.translation.x = self.datum.x # Northing
T.transform.translation.y = self.datum.y # Easting
T.transform.translation.z = self.datum.z # Altitude
br = tf2_ros.StaticTransformBroadcaster()
br.sendTransform(T)
self.pub = rospy.Publisher("odometry/gps", Odometry, queue_size=10)
imu_sub = message_filters.Subscriber('imu/data', Imu)
nav_sub = message_filters.Subscriber("gps/fix", NavSatFix)
ts = message_filters.ApproximateTimeSynchronizer([imu_sub, nav_sub],
10, 0.25)
ts.registerCallback(self.publish)
if pub_gps:
filt_sub = rospy.Subscriber("odometry/filtered", Odometry,
self.filt_cb)
rospy.spin()
def nextMessageCallback(self, msg):
if hasattr(msg, 'latitude') and hasattr(msg, 'longitude'):
xycoord = utm.fromLatLong(msg.latitude, msg.longitude,
msg.altitude)
elif hasattr(msg, 'sentence'):
try:
mgeo = pynmea2.parse(msg.sentence)
xycoord = utm.fromLatLong(float(mgeo.latitude),
float(mgeo.longitude),
float(mgeo.altitude))
except:
return
pxy = PointStamped()
pxy.header = msg.header
pxy.point.x = xycoord.easting
pxy.point.y = xycoord.northing
pxy.point.z = xycoord.altitude
publisher.publish(pxy)
def utm_transform(self):
self.rcv_goals = []
for data in self.path_list:
p = Pose()
utm_data = fromLatLong(data[0], data[1])
x = utm_data.easting - self.robot_orig[0]
y = utm_data.northing - self.robot_orig[1]
p.position.x, p.position.y = x, y
self.rcv_goals.append(p)
def __init__(self):
self.node_name = rospy.get_name()
self.debug = False
self.pose = Pose()
self.prior_pose = Pose()
self.prior_roll = 0
self.prior_pitch = 0
self.prior_yaw = 0
self.flush_count = 0
self.start = False
self.robot_origin = Point()
self.covariance = np.zeros((36, ), dtype=float)
self.odometry = Odometry()
self.br = tf.TransformBroadcaster()
self.IMU_MSG = None
self.GPS_MSG = None
# param
self.imu_offset = 0
self.lat_orig = rospy.get_param('~latitude', 0.0)
self.long_orig = rospy.get_param('~longitude', 0.0)
self.utm_orig = fromLatLong(self.lat_orig, self.long_orig)
self.imu_rotate = rospy.get_param('~imu_rotate', np.pi / 2)
rospack = rospkg.RosPack()
self.imu_param_path = os.path.join(rospack.get_path('asv_config'),
"calibration/imu.yaml")
self.imu_param = None
with open(self.imu_param_path, 'r') as file:
self.imu_param = yaml.safe_load(file)
# Service
self.srv_imu_offset = rospy.Service('~imu_offset', SetValue,
self.cb_srv_imu_offest)
# Publisher
self.pub_odm = rospy.Publisher("odometry", Odometry, queue_size=1)
self.pub_orig = rospy.Publisher("robot_origin", Point, queue_size=1)
# Subscriber
sub_imu_only = rospy.Subscriber("imu/data",
Imu,
self.cb_imu,
queue_size=1)
sub_gps_only = rospy.Subscriber("fix",
NavSatFix,
self.cb_gps,
queue_size=1)
# sub_imu = message_filters.Subscriber("imu/data", Imu)
# sub_gps = message_filters.Subscriber("fix", NavSatFix)
# ats = ApproximateTimeSynchronizer((sub_imu, sub_gps), queue_size = 1, slop = 0.1)
# ats.registerCallback(self.cb_gps_imu)
self.timer = rospy.Timer(rospy.Duration(0.2), self.cb_timer)
def vehgps_cb(msg):
global vehUTMx, vehUTMy, cb_flag
#NavSatFix
#temp = utm.from_latlon(msg.latitude, msg.longitude) #from utm module
temp = utm.fromLatLong(msg.latitude, msg.longitude) #from geodesy.utm
vehUTMx = temp.easting #temp[0]
vehUTMy = temp.northing #temp[1]
cb_flag[3] += 1
cb_flag[3] = clamp(cb_flag[3], 5, 0)
def _recv_map_origin(self, data):
map_origin_utm = utm.fromLatLong(data.pose.position.y,
data.pose.position.x,
data.pose.position.z)
self.map_origin_point = np.array(
(map_origin_utm.toPoint().x, map_origin_utm.toPoint().y))
self.subscribers['map_origin'].unregister()
def mtigps_cb(msg):
global mtiUTMx, mtiUTMy, cb_flag
#positionEstimate
#temp = utm.from_latlon(msg.latitude, msg.longitude) #using utm module
temp = utm.fromLatLong(msg.latitude, msg.longitude) #using geodesy.utm
mtiUTMx = temp.easting #temp[0]
mtiUTMy = temp.northing #temp[1]
cb_flag[4] += 1
cb_flag[4] = clamp(cb_flag[4], 5, 0)
def capture_calibration_pose(self):
utm_pose = utm.fromLatLong(self.global_position.latitude,
self.global_position.longitude,
self.global_position.altitude)
pose = ((self.local_pose.pose.position.x,
self.local_pose.pose.position.y), (utm_pose.toPoint().x,
utm_pose.toPoint().y))
rospy.loginfo(pose)
self.calibration_poses.append(pose)
def read_plan(self):
""" read .plan file from QGroundControl and build W member """
with open(PLAN_FILE, 'r') as f:
d = json.load(f)
if 'mission' in d:
d = d['mission']
if 'plannedHomePosition' in d:
home_lat = d['plannedHomePosition'][0]
home_lon = d['plannedHomePosition'][1]
utm_home = fromLatLong(home_lat, home_lon, 0)
geo_home = utm_home.toPoint()
else:
raise KeyError("No home position in .plan file")
if 'items' in d:
for wp in d['items']:
lat = float(wp['params'][4])
lon = float(wp['params'][5])
alt = float(wp['params'][6])
utm_wp = fromLatLong(lat, lon, alt)
geo_wp = utm_wp.toPoint()
# make local by subtracting home position
N = geo_wp.y - geo_home.y
E = geo_wp.x - geo_home.x
D = -(geo_wp.z - geo_home.z)
if self.W is None:
self.W = mat([N, E, D]).T
else:
self.W = mat(np.hstack((self.W, mat([N, E, D]).T)))
else:
raise KeyError("No waypoints in .plan file")
self.waypoints.header.stamp = rospy.Time.now()
self.waypoints.x = Int32MultiArray(data=self.W[0])
self.waypoints.y = Int32MultiArray(data=self.W[1])
self.waypoints.z = Int32MultiArray(data=self.W[2])
waypoint_rate = rospy.Rate(0.5)
for i in range(0, 3):
self.waypoint_pub.publish(self.waypoints)
waypoint_rate.sleep()
def gps_callback(self, gps_msg):
if gps_msg.status == -1:
return
try:
utm_point = utm.fromLatLong(gps_msg.latitude, gps_msg.longitude)
except ValueError:
return
print utm_point.gridZone()
easting = utm_point.easting
northing = utm_point.northing
self.process(easting, northing)
def CreateBag(matfile, bagname, frame_id, navsat_topic="/fix"):
'''Creates the actual bag file by successively adding images'''
bag = rosbag.Bag(bagname, 'w', compression=rosbag.Compression.BZ2, chunk_threshold=32 * 1024 * 1024)
data = loadmat(matfile)['rnv']
rnv = {}
for i, col in enumerate(data.dtype.names):
rnv[col] = data.item()[i]
try:
ref = None
x, y = [], []
for i, t in enumerate(tqdm(rnv['t'])):
# print("Adding %s" % image_name)
stamp = rospy.Time.from_sec(t)
nav_msg = NavSatFix()
nav_msg.header.stamp = stamp
nav_msg.header.frame_id = "map"
nav_msg.header.seq = i
nav_msg.latitude = rnv['lat'][i][0]
nav_msg.longitude = rnv['lon'][i][0]
nav_msg.altitude = -rnv['depth'][i][0]
nav_msg.position_covariance_type = nav_msg.COVARIANCE_TYPE_UNKNOWN
transform_msg = TransformStamped()
transform_msg.header = copy(nav_msg.header)
utm = fromLatLong(nav_msg.latitude, nav_msg.longitude, nav_msg.altitude)
if ref is None:
ref = utm.toPoint()
x.append(utm.toPoint().x - ref.x)
y.append(utm.toPoint().y - ref.y)
dx = x[-1] - sum(x[-min(20, len(x)):]) / min(20, len(x))
dy = y[-1] - sum(y[-min(20, len(y)):]) / min(20, len(y))
transform_msg.transform.translation.x = x[-1]
transform_msg.transform.translation.y = y[-1]
transform_msg.transform.translation.z = nav_msg.altitude
transform_msg.transform.rotation = Quaternion(*tf_conversions.transformations.quaternion_from_euler(0, 0,
math.atan2(dy, dx)))
transform_msg.child_frame_id = frame_id
tf_msg = tfMessage(transforms=[transform_msg])
odometry_msg = Odometry()
odometry_msg.header = copy(transform_msg.header)
odometry_msg.child_frame_id = frame_id
odometry_msg.pose.pose.position = transform_msg.transform.translation
odometry_msg.pose.pose.orientation = transform_msg.transform.rotation
bag.write(navsat_topic, nav_msg, stamp)
bag.write("/tf", tf_msg, stamp)
bag.write("/transform", transform_msg, stamp)
bag.write("/odom", odometry_msg, stamp)
finally:
bag.close()
def process(self, args, odometry_iter):
with open(args.out, 'w') as f:
fmt = '{0:.' + str(args.precision) + 'f}'
# if the input is a geo pose it is UTM-projected, and we need to detect
# (zone, band) changes from message to message
previous_utm_zone_band = None
for topic_name, msg, _ in odometry_iter:
if isinstance(msg, Odometry):
pos = msg.pose.pose.position
ori = msg.pose.pose.orientation
elif isinstance(msg, NavSatFix):
utm_point = fromLatLong(msg.latitude, msg.longitude,
msg.altitude)
pos = Vector3()
pos.x = utm_point.easting
pos.y = utm_point.northing
pos.z = utm_point.altitude
utm_lattice_coordinate = (utm_point.zone, utm_point.band)
if previous_utm_zone_band and previous_utm_zone_band != utm_lattice_coordinate:
msg = f'UTM (zone, band) changes between messages in topic {topic_name}'
raise ExporterError(msg)
previous_utm_zone_band = utm_lattice_coordinate
# orientation is unit for NavSatFix
ori = Quaternion()
else:
exp_clz = self.__class__.__name__
msg_clz = msg.__class__.__name__
raise TypeError(
f'{exp_clz} can not export messages of type {msg_clz}')
t_ros = Time.from_msg(msg.header.stamp)
t_sec = t_ros.nanoseconds / 1e9
f.write(str(t_sec))
f.write(' ')
f.write(fmt.format(pos.x))
f.write(' ')
f.write(fmt.format(pos.y))
f.write(' ')
f.write(fmt.format(pos.z))
f.write(' ')
f.write(fmt.format(ori.x))
f.write(' ')
f.write(fmt.format(ori.y))
f.write(' ')
f.write(fmt.format(ori.z))
f.write(' ')
f.write(fmt.format(ori.w))
f.write('\n')
def cb_timer(self, event):
if self.flush_count < 10:
# rospy.loginfo("[%s] Flush Data" %self.node_name)
if self.GPS_MSG is None or self.IMU_MSG is None:
return
self.flush_count = self.flush_count + 1
self.utm_orig = fromLatLong(self.GPS_MSG.latitude,
self.GPS_MSG.longitude)
self.robot_origin.x = self.utm_orig.easting
self.robot_origin.y = self.utm_orig.northing
return
self.pub_orig.publish(self.robot_origin)
self.process_gps(self.GPS_MSG)
self.process_imu(self.IMU_MSG)
self.kalman_filter()
def cb_gps(self, msg_gps):
if not self.isRecording:
return
self.gps = msg_gps
if self.flush_count < 5:
# rospy.loginfo("[%s] Flush Data" %self.node_name)
self.flush_count = self.flush_count + 1
self.utm_orig = fromLatLong(msg_gps.latitude, msg_gps.longitude)
return
utm_point = fromLatLong(msg_gps.latitude, msg_gps.longitude)
self.pose.position.x = utm_point.easting - self.utm_orig.easting
self.pose.position.y = utm_point.northing - self.utm_orig.northing
self.pose.position.z = 0
if self.first:
self.first = False
self.save_point()
return
if self.distance(self.pre_pose, self.pose) > self.dis_threshold:
self.save_point()
elif (rospy.get_time() - self.pre_time) > self.time_threshold:
self.save_point()
def read_plandb(plandb, plan_frame, local_frame):
"""
planddb message is a bunch of nested objects,
we want a list of waypoints in the local frame,
"""
tf_listener = tf.TransformListener()
try:
tf_listener.waitForTransform(plan_frame, local_frame, rospy.Time(), rospy.Duration(4.0))
except:
rospy.logerr_throttle(5, "Could not find tf from:"+plan_frame+" to:"+local_frame)
waypoints = []
waypoint_man_ids = []
request_id = plandb.request_id
plan_id = plandb.plan_id
plan_spec = plandb.plan_spec
for plan_man in plan_spec.maneuvers:
man_id = plan_man.maneuver_id
man_name = plan_man.maneuver.maneuver_name
man_imc_id = plan_man.maneuver.maneuver_imc_id
maneuver = plan_man.maneuver
# probably every maneuver has lat lon z in them, but just in case...
if man_imc_id == imc_enums.MANEUVER_GOTO:
lat = maneuver.lat
lon = maneuver.lon
depth = maneuver.z
utm_point = fromLatLong(np.degrees(lat), np.degrees(lon)).toPoint()
stamped_utm_point = PointStamped()
stamped_utm_point.header.frame_id = plan_frame
stamped_utm_point.header.stamp = rospy.Time(0)
stamped_utm_point.point.x = utm_point.x
stamped_utm_point.point.y = utm_point.y
stamped_utm_point.point.z = depth
try:
waypoint_local = tf_listener.transformPoint(local_frame, stamped_utm_point)
# because the frame changes changes depth, we really want the original depth
waypoint = (waypoint_local.point.x, waypoint_local.point.y, depth)
waypoints.append(waypoint)
waypoint_man_ids.append(man_id)
except:
rospy.logwarn_throttle_identical(10, "Can not transform plan point to local point!")
else:
rospy.logwarn("SKIPPING UNIMPLEMENTED MANEUVER:", man_imc_id, man_name)
return waypoints, waypoint_man_ids
def update(self):
data = self.bb.get(bb_enums.GPS_FIX)
if (data is None or data.latitude is None or data.latitude == 0.0
or data.longitude is None or data.latitude == 0.0
or data.status.status == -1):
rospy.loginfo_throttle_identical(
self._spam_period,
"GPS lat/lon are 0s or Nones, cant set utm zone/band from these >:( "
)
# shitty gps
self._spam_period = min(self._spam_period * 2,
self._max_spam_period)
return pt.Status.SUCCESS
self.gps_zone, self.gps_band = fromLatLong(data.latitude,
data.longitude).gridZone()
if self.gps_zone is None or self.gps_band is None:
rospy.logwarn_throttle_identical(
10, "gps zone and band from fromLatLong was None")
return pt.Status.SUCCESS
# first read the UTMs given by ros params
prev_band = self.bb.get(bb_enums.UTM_BAND)
prev_zone = self.bb.get(bb_enums.UTM_ZONE)
if prev_zone != self.gps_zone or prev_band != self.gps_band:
rospy.logwarn_once(
"PREVIOUS UTM AND GPS_FIX UTM ARE DIFFERENT!\n Prev:" +
str((prev_zone, prev_band)) + " gps:" +
str((self.gps_zone, self.gps_band)))
if common_globals.TRUST_GPS:
rospy.logwarn_once("USING GPS UTM!")
self.bb.set(bb_enums.UTM_ZONE, self.gps_zone)
self.bb.set(bb_enums.UTM_BAND, self.gps_band)
else:
rospy.logwarn_once("USING PREVIOUS UTM!")
self.bb.set(bb_enums.UTM_ZONE, prev_zone)
self.bb.set(bb_enums.UTM_BAND, prev_band)
return pt.Status.SUCCESS
def __init__(self):
self.node_name = rospy.get_name()
rospy.loginfo("[%s] Initializing " % (self.node_name))
# file_name = ""
# if len(sys.argv) != 2:
# file_name = "path"
# else:
# file_name = sys.argv[1]
# self.file = open(file_name + ".txt" , 'w')
self.file = None
self.flush_count = 0
self.pose = Pose()
self.pre_pose = Pose()
self.pre_time = rospy.get_time()
self.time_threshold = 3.5
self.dis_threshold = 5.
self.first = True
self.gps = NavSatFix()
self.isRecording = False
self.record_srv = rospy.Service("/ASV/record_path", SetString,
self.record_cb)
self.save_srv = rospy.Service("/ASV/save_path", SetBool, self.save_cb)
self.lat_orig = rospy.get_param('~latitude', 0.0)
self.long_orig = rospy.get_param('~longitude', 0.0)
self.utm_orig = fromLatLong(self.lat_orig, self.long_orig)
self.gps_sub = rospy.Subscriber("/mavros/global_position/raw/fix",
NavSatFix,
self.cb_gps,
queue_size=1,
buff_size=2**24)
def _parseFromNmea(randomBag,
eastingShift=0.0,
northingShift=0.0,
heightShift=0.0,
startTime=_startTime0,
stopTime=_stopTime_1):
try:
import pynmea2
except ImportError:
raise RuntimeError(
"NMEA support is not available; install pynmea2")
parsedCoordinates = []
timestamps = []
msgSamples = GeographicTrajectory._createTimeRange(
randomBag, startTime, stopTime)
i = 0
for s in tqdm(msgSamples):
rawmsg = randomBag[s]
i += 1
try:
m = pynmea2.parse(rawmsg.sentence)
coord = utm.fromLatLong(float(m.latitude), float(m.longitude),
float(m.altitude))
gcoord = [
coord.easting, coord.northing, coord.altitude, 0, 0, 0, 0
]
if len(parsedCoordinates) > 1:
quat = GeographicTrajectory.orientationFromPositionOnlyYaw(
parsedCoordinates[-1], parsedCoordinates[-2])
gcoord[3:7] = quat
parsedCoordinates.append(gcoord)
timestamps.append(rawmsg.header.stamp)
except (AttributeError, TypeError, pynmea2.SentenceTypeError):
continue
parsedCoordinates = np.array(parsedCoordinates)
return timestamps, parsedCoordinates
def export(output_dir, bag_file):
data = rosbag_pandas.bag_to_dataframe(bag_file)
bag_name = os.path.splitext(os.path.basename(bag_file))[0]
output_dir = os.path.abspath(output_dir)
#data = data.truncate(before="20150319 13:38:30", after="20150319 13:39:00")
#data = data.truncate(before="20150319 14:53:30", after="20150319 14:54:30")
# for c in data.columns:
# print c
for chart in CHARTS:
# Filter series
# Drop rows where all values are missing
# Resample:
# - reduces amount of data
# - reduces gaps in data (gives a nicer chart)
chart_df = data.filter(items=chart["series"]).dropna(axis=0, how="all").resample('100L', how="first")
if chart_df.columns.size > 0:
path = os.path.join(output_dir, bag_name, chart["name"])
if not os.path.exists(path):
os.makedirs(path)
print "Exporting chart %s to %s" % (chart["name"], path)
os.chdir(path)
vis = bearcart.Chart(chart_df)
vis.create_chart(html_path=HTML_FILE, data_path=DATA_FILE,
js_path=JS_FILE, css_path=CSS_FILE)
#
# KML export
#
base = utm.fromLatLong(47.388650, 8.033269, 368)
chart_df = data.filter(items=KML_SERIES).dropna(axis=0, how="all").resample('100L', how="first")
if chart_df.columns.size > 0:
mavros = KML_SERIES[0] in chart_df.columns
path = os.path.join(output_dir, bag_name + '.kml')
print "Exporting KML to %s" % (path)
fid = open(path, 'w')
fid.write(kmlhead(bag_name))
for row_index, row in chart_df.iterrows():
x = base.easting
y = base.northing
a = base.altitude
# if (math.isnan(row[KML_SERIES[0]]) or math.isnan(row[KML_SERIES[1]]) or math.isnan(row[KML_SERIES[2]])):
# print "found nan"
# print row
if mavros and not (math.isnan(row[KML_SERIES[0]]) or math.isnan(row[KML_SERIES[1]]) or math.isnan(row[KML_SERIES[2]])):
x = row[KML_SERIES[0]]
y = row[KML_SERIES[1]]
a = row[KML_SERIES[2]]
elif not mavros and not (math.isnan(row[KML_SERIES[3]]) or math.isnan(row[KML_SERIES[4]]) or math.isnan(row[KML_SERIES[5]])):
x = x + row[KML_SERIES[3]]
y = y + row[KML_SERIES[4]]
a = a - row[KML_SERIES[5]]
else:
continue
p = utm.UTMPoint(x, y, zone=base.zone, band=base.band).toMsg()
fid.write("%f,%f,%f " % (p.longitude, p.latitude, a))
fid.write(kmltail("absolute"))
fid.close()
import math
if __name__ == '__main__':
if len(sys.argv) != 2:
print 'Usage: <bagfile>'
exit()
bag = rosbag.Bag(sys.argv[1], 'r')
topics = ['/gps_data']
data = []
for topic, msg, t, in bag.read_messages(topics=topics):
if topic == "/gps_data":
if msg.fix_status:
utm_point = utm.fromLatLong(msg.latitude, msg.longitude)
data.append([utm_point.easting, utm_point.northing])
data = np.array(data)
print data
x1 = data[0][0]
x2 = data[len(data)-1][0]
y1 = data[0][1]
y2 = data[len(data)-1][1]
dist = math.sqrt( (x1-x2)**2 + (y1-y2)**2 )
print "Start to end distance: " + str(dist) + "\n"
def piksigps_cb(msg):
global piksiUTMx, piksiUTMy
temp = utm.fromLatLong(msg.latitude, msg.longitude)
piksiUTMx = temp.easting
piksiUTMy = temp.northing