Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044 import rospy
00045 import roslib.message
00046 import applanix_msgs.msg
00047 import applanix_generated_msgs.srv
00048
00049 response_codes = dict([(val, name) for name, val in applanix_msgs.msg.Ack.__dict__.items() if name.startswith("RESPONSE_")])
00050
00051
00052 def main():
00053 rospy.init_node("applanix_params")
00054
00055 com_ports = rospy.get_param('com_ports', None)
00056 if com_ports != None:
00057 req_msg = applanix_msgs.msg.COMPortSetup()
00058 for port_num, port_params in enumerate(com_ports):
00059
00060 port_msg = applanix_msgs.msg.COMPortParams()
00061 port_msg.parity = port_msg.PARITY_NONE
00062 port_msg.data_stop = port_msg.DATA_8_STOP_1
00063 port_msg.flow = port_msg.FLOW_NONE
00064 port_msg.baud = getattr(port_msg, "BAUD_%s" % port_params['baud'])
00065 port_msg.input_select = getattr(port_msg, "INPUT_%s" % port_params['input'])
00066 port_msg.output_select = getattr(port_msg, "OUTPUT_%s" % port_params['output'])
00067 req_msg.port_mask |= 2 << port_num
00068 req_msg.ports.append(port_msg)
00069
00070 call_applanix_service("com_port_setup", req_msg)
00071 rospy.loginfo("Configured COM ports.")
00072
00073 base_gnss = rospy.get_param('base_gnss', None)
00074 if base_gnss != None:
00075 for base_num, base_params in enumerate(base_gnss):
00076 base_msg = applanix_msgs.msg.BaseGNSSSetup()
00077 base_msg.base_gnss_input_type = getattr(base_msg, "TYPE_%s" % base_params['type'])
00078 base_msg.datum = getattr(base_msg, "DATUM_%s" % base_params['datum'])
00079 call_applanix_service("base_gnss_%i_setup" % (base_num + 1), base_msg)
00080 rospy.loginfo("Configured base GNSS #%i." % (base_num + 1))
00081
00082 geometry = rospy.get_param('geometry', None)
00083 if geometry != None:
00084 req_msg = applanix_msgs.msg.GeneralParams()
00085 for vector_name in ['imu_lever_arm', 'primary_gnss_lever_arm', 'imu_mounting_angle', 'ref_mounting_angle']:
00086 if vector_name in geometry:
00087 vector = geometry[vector_name]
00088 getattr(req_msg, vector_name).x = vector['x']
00089 getattr(req_msg, vector_name).y = vector['y']
00090 getattr(req_msg, vector_name).z = vector['z']
00091 req_msg.time_types = 0x1
00092 req_msg.distance_type = 1
00093 req_msg.autostart = 1
00094 req_msg.multipath = 0
00095 call_applanix_service('general', req_msg)
00096 rospy.loginfo("Configured geometry.")
00097
00098
00099 rate = rospy.get_param('rate', 10)
00100 rospy.Subscriber("subscribed_groups", applanix_msgs.msg.Groups, groups_callback)
00101
00102
00103 sensor_overrides = rospy.get_param('sensor_overrides', None)
00104 if sensor_overrides != None:
00105 override_msg = applanix_msgs.msg.AidingSensorIntegrationControl()
00106 for override_str in sensor_overrides:
00107 override_msg.override |= getattr(override_msg, "OVERRIDE_%s" % override_str)
00108 rospy.loginfo("Waiting on Fine Align before configuring sensor overrides.")
00109 proceed = [False]
00110 def _cb(msg):
00111 if msg.status_a & applanix_msgs.msg.GeneralStatus.STATUS_A_FINE_ALIGN_ACTIVE != 0:
00112 proceed[0] = True
00113 sub = rospy.Subscriber("status/general", applanix_msgs.msg.GeneralStatus, _cb)
00114 while not proceed[0]:
00115 rospy.sleep(1.)
00116 sub.unregister()
00117 call_applanix_service("aiding_sensor_integration", override_msg)
00118 rospy.loginfo("Configured sensor overrides.")
00119
00120 rospy.spin()
00121
00122
00123
00124 def call_applanix_service(name, req):
00125 service_defn = getattr(applanix_generated_msgs.srv, req.__class__.__name__)
00126 rospy.wait_for_service(name)
00127 ack = rospy.ServiceProxy(name, service_defn)(req).ack
00128 if ack.response_code != applanix_msgs.msg.Ack.RESPONSE_ACCEPTED:
00129 rospy.logwarn("Parameter change call to %s resulted in error code %d (%s)." %
00130 (name, ack.response_code, response_codes[ack.response_code]))
00131 return ack
00132
00133
00134 def groups_callback(message):
00135 req_msg = applanix_msgs.msg.PortControl()
00136 req_msg.rate = rospy.get_param('rate', 10)
00137 groups = set(message.groups)
00138 groups.add(10)
00139 for group_num in groups:
00140 req_msg.groups.append(applanix_msgs.msg.OutputGroup(group=group_num))
00141 call_applanix_service("primary_data_port", req_msg)
00142