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()