def __init__(self, config):
MOOSCommClient.__init__(self)
self.SetOnConnectCallBack(self.onConnect)
self.SetOnMailCallBack(self.onMail)
self.get_config(config)
self.set_publishers()
self.holder = {}
self.create_cap_freq_holders()
self.navpy_gps = GPS()
def __init__(self):
object.__init__(self)
self.gps = GPS()
self.get_config()
self.set_publishers()
self.create_map() # create marker arrays of the stripes and lane centers
print('mapBridge: Map markers published - you should see the lane/stripe markers once running')
self.create_map_mesh()
print('mapBridge: Map mesh has been published - you should see it once running')
class MAP2RVIZ(object):
def __init__(self):
object.__init__(self)
self.gps = GPS()
self.get_config()
self.set_publishers()
self.create_map() # create marker arrays of the stripes and lane centers
print('mapBridge: Map markers published - you should see the lane/stripe markers once running')
self.create_map_mesh()
print('mapBridge: Map mesh has been published - you should see it once running')
def get_config(self):
"""startup function"""
self.prefix = rospy.get_param('~prefix')
# self.UTMdatum = {'E': float(rospy.get_param("~UTMdatum_E")),
# 'N': float(rospy.get_param("~UTMdatum_N"))}
self.UTMdatum = rospy.get_param('/UTMdatum')
self.survey_stripe_locs = rospy.get_param("~survey_stripe_locs").split(', ')
self.survey_center_locs = rospy.get_param("~survey_center_locs").split(', ')
self.track_mesh_resource = rospy.get_param("~track_mesh_resource")
print('\ntrack_mesh_resource: %s' % self.track_mesh_resource)
print('track_mesh_resource: %s' % str(bool(self.track_mesh_resource)))
self.marking_mesh_resource = rospy.get_param("~marking_mesh_resource")
self.sign_mesh_resource = rospy.get_param('~sign_mesh_resource')
# self.sign_file_loc = rospy.get_param('~survey_sign_locs')
self.stop_mesh_resource = rospy.get_param('~stop_mesh_resource')
def set_publishers(self):
"""startup function"""
self.map_stripe_publisher = rospy.Publisher('/map/survey_stripes', MarkerArray, latch=True)
self.map_lane_publisher = rospy.Publisher('/map/survey_lanes', MarkerArray, latch=True)
self.track_mesh_publisher = rospy.Publisher('/map/mesh', MarkerArray, latch=True)
# self.sign_publisher = rospy.Publisher('/map/signs', MarkerArray, latch=True)
def survey(self):
"""
reads "DecLat DecLon \n" text file and groups into 2 lists
e.g., stripes = [[lat1, lon1], [lat2, lon2], ...]
"""
stripes = []
centers = []
for stripe_file_loc in self.survey_stripe_locs:
stripe_file = open(os.path.join(self.prefix, stripe_file_loc), 'rU')
for line in stripe_file:
line = line[0:-2] # remove the '\n' at the end
pt_list = line.split(' ')
stripes.append(pt_list)
for center_file_loc in self.survey_center_locs:
center_file = open(os.path.join(self.prefix, center_file_loc), 'rU')
for line in center_file:
line = line[0:-2] # remove the '\n' at the end
pt_list = line.split(' ')
centers.append(pt_list)
return stripes, centers
def create_map(self):
"""
This function collects the survey data and puts it into marker arrays ready to be published in rviz
"""
stripes, centers = self.survey()
self.map_stripe_array = MarkerArray()
self.map_lane_array = MarkerArray()
NCAT_id = 0
### Stripes ################################################################
for pt in stripes:
lat = float(pt[0])
lon = float(pt[1])
(east, nrth, _), info = self.gps.lla2utm((lat, lon, 0)) # convert to UTM
marker = Marker()
marker.header.frame_id = 'odom'
marker.id = NCAT_id # enumerate subsequent markers here
marker.action = Marker.ADD # can be ADD, REMOVE, or MODIFY
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = "stripes"
marker.type = Marker.CUBE
marker.pose.position.x = east - float(self.UTMdatum['E'])
marker.pose.position.y = nrth - float(self.UTMdatum['N'])
marker.pose.position.z = 0 # zero is a novatel mount level
marker.color.r = 255
marker.color.g = 255
marker.color.b = 0
marker.color.a = 1.0
marker.scale.x = 0.1
marker.scale.y = 0.1
marker.scale.z = 0.4
marker.mesh_use_embedded_materials = False
self.map_stripe_array.markers.append(marker)
NCAT_id += 1
self.map_stripe_publisher.publish(self.map_stripe_array) # publish stripes as markers
print('mapBridge: Stripe Markers have been printed')
del marker
### Lane Centers ###########################################################
for pt in centers:
lat = float(pt[0])
lon = float(pt[1])
(east, nrth, _), _ = self.gps.lla2utm((lat, lon, 0)) # convert to UTM
marker = Marker()
marker.header.frame_id = 'odom'
marker.id = NCAT_id # enumerate subsequent markers here
marker.action = Marker.ADD # can be ADD, REMOVE, or MODIFY
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = "lane_centers"
marker.type = Marker.SPHERE
marker.pose.position.x = east - float(self.UTMdatum['E'])
marker.pose.position.y = nrth - float(self.UTMdatum['N'])
marker.pose.position.z = 0 # zero is a novatel mount level
marker.color.r = 255
marker.color.g = 255
marker.color.b = 255
marker.color.a = 0.75
marker.scale.x = 0.2
marker.scale.y = 0.2
marker.scale.z = 0.2
marker.mesh_use_embedded_materials = False
self.map_lane_array.markers.append(marker)
NCAT_id += 1
self.map_lane_publisher.publish(self.map_lane_array) # publish lane centers as markers
print('mapBridge: Lane Center Markers have been printed')
def create_map_mesh(self):
"""Puts a blender mesh of the paement and stripes (continuous) into rviz"""
marker_array = MarkerArray()
### Track Pavement Publisher ###########################################
if self.track_mesh_resource:
marker = Marker()
marker.header.frame_id = 'odom' # publish in static frame
marker.id = 0
marker.action = Marker.ADD
marker.lifetime = rospy.Duration() # immortal unless changed
marker.ns = "track_mesh_pavement"
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = False
marker.mesh_resource = '//'.join(['file:', self.track_mesh_resource])
marker.pose.position.x = 0# - float(self.UTMdatum['E'])
marker.pose.position.y = 0# - float(self.UTMdatum['N'])
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 0.3
marker.color.g = 0.3
marker.color.b = 0.3
marker.color.a = 1.0
marker_array.markers.append(marker)
### Lane Marking Publisher ##############################################
if self.marking_mesh_resource:
marker = Marker()
marker.header.frame_id = 'odom' # publish in static frame
marker.id = 1
marker.action = Marker.ADD
marker.lifetime = rospy.Duration() # immortal unless changed
marker.ns = "track_mesh_lane_markings"
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = False
marker.mesh_resource = self.marking_mesh_resource
marker.pose.position.x = 0# - float(self.UTMdatum['E'])
marker.pose.position.y = 0# - float(self.UTMdatum['N'])
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 255
marker.color.g = 255
marker.color.b = 0
marker.color.a = 1.0
marker_array.markers.append(marker)
if self.sign_mesh_resource:
marker = Marker()
marker.header.frame_id = 'odom' # publish in static frame
marker.id = 1
marker.action = Marker.ADD
marker.lifetime = rospy.Duration() # immortal unless changed
marker.ns = "track_mesh_section_signs"
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = False
marker.mesh_resource = '//'.join(['file:',self.sign_mesh_resource])
marker.pose.position.x = 0# - float(self.UTMdatum['E'])
marker.pose.position.y = 0# - float(self.UTMdatum['N'])
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 0.1
marker.color.g = 0.6
marker.color.b = 0.1
marker.color.a = 1.0
marker_array.markers.append(marker)
if self.stop_mesh_resource:
marker = Marker()
marker.header.frame_id = 'odom' # publish in static frame
marker.id = 1
marker.action = Marker.ADD
marker.lifetime = rospy.Duration() # immortal unless changed
marker.ns = "track_mesh_stop_signs"
marker.type = Marker.MESH_RESOURCE
marker.mesh_use_embedded_materials = False
marker.mesh_resource = '//'.join(['file:',self.stop_mesh_resource])
marker.pose.position.x = 0# - float(self.UTMdatum['E'])
marker.pose.position.y = 0# - float(self.UTMdatum['N'])
marker.pose.position.z = 0
marker.scale.x = 1
marker.scale.y = 1
marker.scale.z = 1
marker.color.r = 0.7
marker.color.g = 0.1
marker.color.b = 0.1
marker.color.a = 1.0
marker_array.markers.append(marker)
self.track_mesh_publisher.publish(marker_array)
class MOOS2RVIZ(MOOSCommClient):
"""Takes moos messages from an onboard moos db and displays the data in rViz"""
def __init__(self, config):
MOOSCommClient.__init__(self)
self.SetOnConnectCallBack(self.onConnect)
self.SetOnMailCallBack(self.onMail)
self.get_config(config)
self.set_publishers()
self.holder = {}
self.create_cap_freq_holders()
self.navpy_gps = GPS()
### Init-related Functions #################################################
def get_config(self, config):
"""saves the yaml config file info as instance attributes, utilizes
values from the ROS parameter server
"""
self.ip = str(rospy.get_param('~sender_ip'))
self.port = int(rospy.get_param('~port'))
self.freq_max = config["freq_max"]
self.UTMdatum = config["UTMdatum"]
self.coord_sys = config["coord_sys"]
self.sensor_name = config["sensor_name"]
self.desired_variables = config["desired_variables"]
self.color = config["color"]
self.legend_text_height = config["legend_text_height"]
self.legend_text = config["display_name"]
self.ismaster = config["dictate_pos"] # this instance will govern the current position
if self.ismaster:
self.veh_mesh_resource = config["veh_mesh_resource"]
else:
self.veh_mesh_resource = None
def set_publishers(self):
odom_topic = '/' + self.sensor_name + '/odom'
self.odom_publisher = rospy.Publisher(odom_topic, Odometry)
ell_topic = '/' + self.sensor_name + '/error_ellipse'
self.ell_publisher = rospy.Publisher(ell_topic, Marker)
legend_topic = '/' + self.sensor_name + '/legend'
self.legend_publisher = rospy.Publisher(legend_topic, Marker)
if self.ismaster: # this instance dictates accepted position (veh disp mesh)
curpos_topic = '/' + self.sensor_name + ''
self.curpos_publisher = rospy.Publisher('/novatel/current_position', Marker) # even though this is at the same position as the novatel error ellipse, we want it to have a different name in case the integrated solution is different
rospy.Subscriber(odom_topic, Odometry, self.pub_at_position) # put the vehicle model at the accepted position solution
def create_cap_freq_holders(self):
"""makes none value holders to keep track of when the last of each
measurement was published -- to limit frequency
last_gNovatel_zLat = None --> will have last published timestamp
-used in mailroom.cap_freq function
"""
self.cap_freq_holders = {}
for meas in range(len(self.desired_variables)):
name = ''.join(['last_', self.desired_variables[meas]])
self.cap_freq_holders[name] = time() # wall time
############################################################################
def cameraFollow_tf(self, odom_msg):
msg = odom_msg
br = tf.TransformBroadcaster()
br.sendTransform((msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z),\
(0, 0, 0, 1), # this is a unit quaternion\
msg.header.stamp,"base_footprint", "odom")
#time, #child frame , #parent frame
# def FPV_tf(self, odom_msg):
# msg = odom_msg
# br = tf.TransformBroadcaster()
# br.sendTransform((msg.pose.pose.position.x,
# msg.pose.pose.position.y,
# msg.pose.pose.position.z),
# (odom_msg.pose.pose.orientation.x,
# odom_msg.pose.pose.orientation.y,
# odom_msg.pose.pose.orientation.z,
# odom_msg.pose.pose.orientation.w),
# msg.header.stamp, "fpv", "odom")
# #time, #child frame , #parent frame
##### Mailroom Functions ###################################################
def onConnect(self):
"""Function required in every pyMOOS App"""
# print('liveBridge.py :: In onConnect')
for var in self.desired_variables: # expand later
self.Register(var) #defined in MOOSCommClient.py
def onMail(self):
"""Function required in every pyMOOS App"""
# print('In onMail')
messages = self.FetchRecentMail()
# print('Recent Mail Fetched')
for message in messages:
self.handle_msg(message)
# print('Messages Handled')
return True
def handle_msg(self, msg):
"""
takes a msg and passes usable info to gather_odom_var
"""
if msg.IsDouble():
var_type = "Double"
value = str(msg.GetDouble()) #store all values as strings until handled specifically
elif msg.IsString():
var_type = "String"
value = msg.GetString()
else:
rospy.logwarn('wtf? Unknown variable type')
# obtain info from msg via functions defined in pyMOOSMsg.cpp
time = msg.GetTime() # this may be grabbing the wrong time (not from the msg)
name = msg.GetKey() # type of measurement "z______" string
sens = msg.GetSource() # will yield "g______" string
# print('Here')
if (sens == self.sensor_name) and (name in self.desired_variables): # where desired messages are scooped
if not self.cap_freq(name): # frequency capping
self.gather_odom_var(name, var_type, value, time)
# else:
# rospy.logwarn("handle_msg :: Unhandled msg: %(name)s of type %(var_type)s carries value %(value)s" % locals())
def cap_freq(self, name):
"""enfores the maximum frequency by emitting boolean to send only messages
arriving after the prescribed period following the previous message of
the same type
"""
# minimum amount of time between msgs for each sensor
this_time = time()
tmin = 1/self.freq_max
## reconstruct name of last time holder to find its contents (last time)
last_one_name = ''.join(['last_', name])
last_one = self.cap_freq_holders[last_one_name]
# publish if first time or enough time elapsed
if self.freq_max == 0: # disable freq capping
catch = False
elif (last_one is None) or ((this_time - last_one) >= tmin):
catch = False
else:
catch = True
# update the holder since we're now publishing
if not catch:
last_one = time()
return catch
def gather_odom_var(self, name, var_type, value, time):
"""
This function will only invoke publishing when it can send off all necessary info for a single source at once
"""
# print("In gather_odom_var")
time = int(time*1000.0)/1000.0 #rounding to 3 decimal places
# TIME STAMPS ARE CRITICAL HERE!!
# may build up unused messages if they aren't complete enough to publish
if time not in self.holder: # no messages from this timestep yet, initialize a dictionary for this time
self.holder[time] = {}
# stick the message in a holder specific to its sensor/source, according to msg time
self.holder[time][name] = dict([['var_type', var_type], ['value', value]]) # assume no multiple messages
# the latest addition may have made something publishable - check to see if the HOLDER WE JUST ADDED TO now meets the req's at ANY OF ITS TIMESTAMPS
tstamps_sent = []
for tstamp in self.holder: # loop through all times (may be async)
holder_attime_publishable = [False] *len(self.desired_variables) # assume that it isn't publishable until proven otherwise
for des_var_ind in range(len(self.desired_variables)):
if self.desired_variables[des_var_ind] in self.holder[tstamp]:
holder_attime_publishable[des_var_ind] = True
if all(holder_attime_publishable): # ka-ching
# this will just roll the publishable group of msgs from sensor "holder" at time "tstamp" over to convert_odom_var; note that it doesn't contain the sensor/source "g_______"
skateboard = self.holder[tstamp]
# send it off
self.convert_odom_var(skateboard, tstamp)
# remember what we've sent so it can be cleaned
tstamps_sent.append(tstamp)
for tstamp in tstamps_sent:
del self.holder[tstamp] # clean up the holder of anything we already printed
def convert_odom_var(self, skateboard, time):
"""
Once all the information for a single odom msg from a single sensor(source) is built together, this function converts it to UTM in a special UTM holder and sends it to the publishing function: mailroom.package_odom_var()
Covariances - Lat/Lon Std Dev are output in meters already
converts course to radians for ROS; Will orient odom arrows to velocity direction
"""
# print("In convert_odom_var")
## FIXME here an assumption is made about the order of variables - robustify later
UTMtoPub = dict([['time', time]]) # is time really necessary? Not using as dictionary index in self...
if self.coord_sys == 'ECEF':
(Easting, Northing, _), info = self.navpy_gps.ecef2utm((float(skateboard[self.desired_variables[0]]),
float(skateboard[self.desired_variables[1]]),
float(skateboard[self.desired_variables[2]])))
elif self.coord_sys == 'LLA':
(Easting, Northing, _), info = self.navpy_gps.lla2utm((float(skateboard['zLat']['value']),
float(skateboard['zLong']['value'])),
0)
# print("{} - {} = {}".format(Northing, self.UTMdatum['N'], Northing - self.UTMdatum['N']))
UTMtoPub['N'] = Northing - self.UTMdatum['N']
UTMtoPub['E'] = Easting - self.UTMdatum['E']
UTMtoPub['Nsd'] = float(skateboard['zLatStdDev']['value'])
UTMtoPub['Esd'] = float(skateboard['zLongStdDev']['value'])
UTMtoPub['crs'] = radians(float(skateboard['zCourse']['value']))
self.package_odom_var(UTMtoPub)
def package_odom_var(self, UTMtoPub):
"""
Puts odom var (from UTM N & E) into a ros message and sends to the publishing decider.
-does all the ROS packaging common to all sources
-each individual legend is now
"""
### Odometry Arrows ####################################################
odom_msg = Odometry()
odom_msg.header.stamp = rospy.Time(UTMtoPub['time'])
odom_msg.header.frame_id = "odom" # may need to expand?
odom_msg.pose.pose.position.x = UTMtoPub['E']
odom_msg.pose.pose.position.y = UTMtoPub['N']
odom_msg.pose.pose.position.z = 1.55
# make a quaternion :: came from trial-and-error...
quat = qfe(-pi, -pi, -UTMtoPub['crs']-pi/2)
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
odom_msg.pose.covariance[0] = UTMtoPub['Nsd']
odom_msg.pose.covariance[7] = UTMtoPub['Esd']
odom_msg.pose.covariance[14] = 0
self.odom_publisher.publish(odom_msg)
### Error Ellipses #####################################################
## pulls from the odom msg --
ell_marker = Marker()
ell_marker.header = odom_msg.header # clarify?
ell_marker.id = 0 # enumerate subsequent markers here
ell_marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
ell_marker.pose = odom_msg.pose.pose # put at same place as its odom arrow
ell_marker.lifetime = rospy.Duration() # will last forever unless modified
ell_marker.ns = ''.join(["Error_Ellipses", '___', self.sensor_name])
ell_marker.type = Marker.CYLINDER
ell_marker.scale.x = sqrt(odom_msg.pose.covariance[0]) *10 # not visible unless scaled up
ell_marker.scale.y = sqrt(odom_msg.pose.covariance[7]) *10 # not visible unless scaled up
ell_marker.scale.z = 0.000001 # We just want a disk
ell_marker.color.r = self.color['r']
ell_marker.color.g = self.color['g']
ell_marker.color.b = self.color['b']
ell_marker.color.a = 0.95
self.ell_publisher.publish(ell_marker)
### Legend #############################################################
marker = Marker()
marker.header = odom_msg.header
marker.id = 0
marker.ns = self.sensor_name + '_legend'
marker.type = Marker.TEXT_VIEW_FACING
marker.text = self.legend_text
marker.action = Marker.MODIFY
marker.pose = odom_msg.pose.pose
marker.pose.position.z = self.legend_text_height # elevate to spread
marker.scale.x = 0.5
marker.scale.y = 0.5
marker.scale.z = 0.6
marker.color.r = self.color['r']
marker.color.g = self.color['g']
marker.color.b = self.color['b']
marker.color.a = 1.0
self.legend_publisher.publish(marker)
# tell camera tf where the look if master ##############################
if self.ismaster: # update the vehicle mesh position
self.pub_at_position(odom_msg)
self.cameraFollow_tf(odom_msg)
# self.FPV_tf(odom_msg) # add a view
def pub_at_position(self, odom_msg):
"""
Handles necessary information for displaying things at
ACCEPTED (NOVATEL) POSITION SOLUTION:
-vehicle mesh
!!!this should only be invoked when the accepted (novatel) position
is updated
"""
### G35 Mesh #############################################################
marker = Marker()
pub = self.curpos_publisher
marker.header = odom_msg.header
marker.id = 0 # enumerate subsequent markers here
marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
marker.pose = odom_msg.pose.pose
marker.pose.position.z = 1.55
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = "vehicle_model"
marker.type = Marker.MESH_RESOURCE
marker.scale.x = 0.0254 # artifact of sketchup export
marker.scale.y = 0.0254 # artifact of sketchup export
marker.scale.z = 0.0254 # artifact of sketchup export
marker.color.r = .05
marker.color.g = .05
marker.color.b = .05
marker.color.a = .2
marker.mesh_resource = self.veh_mesh_resource
marker.mesh_use_embedded_materials = False
pub.publish(marker)
def __init__(self):
self.get_config()
self.set_publishers()
self.navpy_gps = GPS()
self.holder = {}
class MOOS2RVIZ:
"""Takes moos messages from an onboard moos db and displays the data in rViz"""
def __init__(self):
self.get_config()
self.set_publishers()
self.navpy_gps = GPS()
self.holder = {}
### Init-related Functions #################################################
def get_config(self):
"""saves the yaml config file info as instance attributes, utilizes
values from the ROS parameter server
"""
self.DEBUG = bool(rospy.get_param('~DEBUG'))
self.tag = rospy.get_param('~tag')
self.UTMdatum = rospy.get_param('/UTMdatum')
self.coord_sys = rospy.get_param("~coord_sys")
self.color = {'r': float(rospy.get_param("/"+self.tag+"_color").split(',')[0]),
'g': float(rospy.get_param("/"+self.tag+"_color").split(',')[1]),
'b': float(rospy.get_param("/"+self.tag+"_color").split(',')[2])}
self.legend_text_height = rospy.get_param("~legend_text_height")
self.legend_text = rospy.get_param("/"+self.tag+"_text")
self.veh_mesh_resource = rospy.get_param("~veh_mesh_resource")
self.err_ell_opacity = float(rospy.get_param('/err_ell_opacity'))
def set_publishers(self):
rospy.Subscriber("/moos/solutions/"+self.tag, ECEFpos, self.handle_msg)
odom_topic = '/'.join(['moos', self.tag, 'odom'])
ell_topic = '/'.join(['moos',self.tag, 'error_ellipse'])
legend_topic = '/'.join(['moos', self.tag, 'legend'])
self.odom_publisher = rospy.Publisher(odom_topic, Odometry)
self.ell_publisher = rospy.Publisher(ell_topic, Marker)
self.legend_publisher = rospy.Publisher(legend_topic, Marker)
curpos_topic = '/'.join(['moos', self.tag, 'current_position'])
self.curpos_publisher = rospy.Publisher(curpos_topic, Marker) # even though this is at the same position as the novatel error ellipse, we want it to have a different name in case the integrated solution is different
rospy.Subscriber(odom_topic, Odometry, self.pub_at_position) # put the vehicle model at the accepted position solution
if self.DEBUG:
print('rvizBridge '+self.tag+': Publishers and Subscribers set')
############################################################################
def cameraFollow_tf(self, odom_msg):
"""
reexamine if using urdf (base_footprint)
"""
msg = odom_msg
br = tf.TransformBroadcaster()
br.sendTransform((msg.pose.pose.position.x,
msg.pose.pose.position.y,
msg.pose.pose.position.z),
(0, 0, 0, 1), # this is a unit quaternion\
msg.header.stamp,
''.join([self.tag,"_base_link"]), "odom")
def handle_msg(self, msg):
"""
message callback
"""
skateboard = [msg.x, msg.y, msg.z, \
msg.x_covar, msg.y_covar, msg.z_covar, \
msg.orient, msg.header.stamp]
self.convert_odom_var(skateboard)
def convert_odom_var(self, skateboard):
"""
Once all the information for a single odom msg from a single sensor
(source) is built together, this function converts it to UTM in a
special UTM holder and sends it to the publishing function: mailroom.package_odom_var()
Covariances - Lat/Lon Std Dev are output in meters already
converts course to radians for ROS; Will orient odom arrows to velocity direction
skateboard = [X, Y, Z, XStdDev, YStdDev, ZStdDev, Course, MOOStime]
# ASSUME Y ECEF IS NEGATIVE
"""
## FIXME here an assumption is made about the order of variables - robustify later
if self.DEBUG:
print('rvizBridge: '+self.tag+': convert_odom_var: SKATEBOARD RECIEVED --')
pp(skateboard)
if self.coord_sys == 'ECEF':
(E, N, _), _ = self.navpy_gps.ecef2utm((float(skateboard[0]),
float(skateboard[1]),
float(skateboard[2])))
Xstray = float(skateboard[0])+float(skateboard[3])
Ystray = float(skateboard[1])-float(skateboard[4]) # ASSUME Y ECEF IS NEGATIVE
Zstray = float(skateboard[2])+float(skateboard[5])
(Estray, Nstray, _), _ = self.navpy_gps.ecef2utm((Xstray, Ystray, Zstray))
Esd = Estray - E
Nsd = Nstray - N
UTMtoPub = {}
UTMtoPub['N'] = N - float(self.UTMdatum['N'])
UTMtoPub['E'] = E - float(self.UTMdatum['E'])
UTMtoPub['Nsd'] = abs(float(Nsd))
UTMtoPub['Esd'] = abs(float(Esd))
UTMtoPub['crs'] = radians(float(skateboard[6]))
UTMtoPub['time'] = skateboard[7]
self.package_odom_var(UTMtoPub)
if self.DEBUG:
print('rvizBridge: '+self.tag+': convert_odom_var: UTMtoPub --')
pp(UTMtoPub)
else:
raise Exception('Only ECEF implemented thus far')
def package_odom_var(self, UTMtoPub):
"""
Puts odom var (from UTM N & E) into a ros message and sends to the publishing decider.
-does all the ROS packaging common to all sources
-each individual legend is now
"""
if self.DEBUG:
print('rvizBridge: '+self.tag+': In package_odom_var')
combined_array = MarkerArray()
### Odometry Arrows ####################################################
odom_msg = Odometry()
# odom_msg.header.stamp = rospy.Time(UTMtoPub['time'])
odom_msg.header.stamp = rospy.Time.now()
odom_msg.header.frame_id = "odom" # may need to expand?
odom_msg.pose.pose.position.x = UTMtoPub['E']
odom_msg.pose.pose.position.y = UTMtoPub['N']
odom_msg.pose.pose.position.z = 1.55
# make a quaternion :: came from trial-and-error...
quat = qfe(-pi, -pi, -UTMtoPub['crs']-pi/2)
odom_msg.pose.pose.orientation.x = quat[0]
odom_msg.pose.pose.orientation.y = quat[1]
odom_msg.pose.pose.orientation.z = quat[2]
odom_msg.pose.pose.orientation.w = quat[3]
odom_msg.pose.covariance[0] = UTMtoPub['Nsd']
odom_msg.pose.covariance[7] = UTMtoPub['Esd']
odom_msg.pose.covariance[14] = 0
self.odom_publisher.publish(odom_msg)
### Error Ellipses #####################################################
ell_marker = Marker()
ell_marker.header = odom_msg.header # clarify?
ell_marker.id = 0 # enumerate subsequent markers here
ell_marker.action = Marker.MODIFY # can be ADD, REMOVE, or MODIFY
ell_marker.pose = odom_msg.pose.pose # put at same place as its odom arrow
ell_marker.lifetime = rospy.Duration() # will last forever unless modified
ell_marker.ns = "Error_Ellipses__"+ self.tag
ell_marker.type = Marker.CYLINDER
ell_marker.scale.x = abs(odom_msg.pose.covariance[0]) # not visible unless scaled up
ell_marker.scale.y = abs(odom_msg.pose.covariance[7]) # not visible unless scaled up
ell_marker.scale.z = 0.000001 # We just want a disk
ell_marker.color.r = self.color['r']
ell_marker.color.g = self.color['g']
ell_marker.color.b = self.color['b']
ell_marker.color.a = self.err_ell_opacity
ell_marker.color.a = 0.5
self.ell_publisher.publish(ell_marker)
### Legend #############################################################
legend_marker = Marker()
legend_marker.header = odom_msg.header
legend_marker.id = 0
legend_marker.ns = ''.join(["Error_Ellipses", '__', self.tag, '__', self.tag])
legend_marker.type = Marker.TEXT_VIEW_FACING
legend_marker.text = self.legend_text
legend_marker.action = Marker.MODIFY
legend_marker.pose = odom_msg.pose.pose
legend_marker.pose.position.z = self.legend_text_height # elevate to spread
legend_marker.scale.x = .7
legend_marker.scale.y = .7
legend_marker.scale.z = .7
legend_marker.color.r = self.color['r']
legend_marker.color.g = self.color['g']
legend_marker.color.b = self.color['b']
legend_marker.color.a = .9
self.legend_publisher.publish(legend_marker)
self.cameraFollow_tf(odom_msg)
self.pub_at_position(odom_msg)
def pub_at_position(self, odom_msg):
"""
Handles necessary information for displaying things at
ACCEPTED (NOVATEL) POSITION SOLUTION:
-vehicle mesh
!!!this should only be invoked when the accepted (novatel) position
is updated
"""
### G35 Mesh ###########################################################
marker = Marker()
marker.header = odom_msg.header
marker.id = 0 # enumerate subsequent markers here
marker.action = Marker.MODIFY
marker.pose = odom_msg.pose.pose
marker.pose.position.z = 1.55
marker.lifetime = rospy.Duration() # will last forever unless modified
marker.ns = ''.join(["vehicle_model", '__', self.tag, '__', self.tag])
marker.type = Marker.MESH_RESOURCE
marker.scale.x = 0.0254 # artifact of sketchup export
marker.scale.y = 0.0254 # artifact of sketchup export
marker.scale.z = 0.0254 # artifact of sketchup export
marker.color.r = self.color['r']
marker.color.g = self.color['g']
marker.color.b = self.color['b']
marker.color.a = .1
marker.mesh_resource = '//'.join(['file:',self.veh_mesh_resource])
marker.mesh_use_embedded_materials = False
self.curpos_publisher.publish(marker)
#!/usr/bin/env python
import sys, os
from util import GPS
in_loc = '/home/rgcofield/devel/fhwa2_ws/fhwa2_viz/fhwa2_survey/survey/signs/sections_ecef.txt'
out_loc = '/home/rgcofield/devel/fhwa2_ws/fhwa2_viz/fhwa2_survey/survey/signs/sections_utm.txt'
out_file = open(out_loc, 'w')
gps = GPS()
datum_e = 659300
datum_n = 3607850
for line in open(in_loc, 'rU'):
pt = line[0:-1].split(' ')
print(pt)
if pt[0] == '':
out_file.write('\n')
continue
if pt[-1][0] not in ['n','s','e','w']: # exclude stop/yield/etc
out_file.write('\n')
continue
(E, N, _), _ = gps.ecef2utm((float(pt[0]), float(pt[1]), float(pt[2])))
line = ', '.join([str(E-datum_e), str(N-datum_n), '0'])
out_file.write(line+'\n')
out_file.close()
