def __init__(self):
# Fetch constants
config_path = os.getenv('MROVER_CONFIG')
sim_config_path = config_path + "/config_filter/simConfig.json"
with open(sim_config_path, "r") as configJson:
config = json.load(configJson)
self.GPS_UPDATE_RATE_MILLIS = config['gps_update_rate_millis']
self.IMU_UPDATE_RATE_MILLIS = config['imu_update_rate_millis']
self.DT_MILLIS = config['dt_s'] * 1000
self.CSV_MODE = config['csv_mode']
config_path += "/config_filter/config.json"
with open(config_path, "r") as configJson:
config = json.load(configJson)
self.SEND_RATE_MILLIS = config['UpdateRate'] * 1000
self.REF_LAT = config["RefCoords"]["lat"]
self.REF_LONG = config["RefCoords"]["long"]
self.GPS_DTS = int(self.GPS_UPDATE_RATE_MILLIS / self.DT_MILLIS)
self.IMU_DTS = int(self.IMU_UPDATE_RATE_MILLIS / self.DT_MILLIS)
self.path_generator = PathGenerator(self.REF_LAT, self.REF_LONG)
path_out = self.path_generator.run()
self.truth = path_out['truth']
self.noisy = path_out['noisy']
self.lcm = aiolcm.AsyncLCM()
self.millis = time.time() * 1000
self.timesteps = 0
def __init__(self):
config_path = getenv('MROVER_CONFIG')
config_path += "/config_filter/config.json"
with open(config_path, "r") as config:
self.config = json.load(config)
self.gps = Gps()
self.imu = Imu(self.config["IMU_accel_filter_bias"],
self.config["IMU_accel_threshold"])
self.nav_state = None
self.static_nav_states = {
None, "Off", "Done", "Search Spin Wait", "Turned to Target Wait",
"Gate Spin Wait", "Turn", "Search Turn", "Turn to Target",
"Turn Around Obstacle", "Search Turn Around Obstacle", "Gate Turn",
"Gate Turn to Center Point", "Radio Repeater Turn"
}
self.filter = None
self.state_estimate = StateEstimate(
ref_lat=self.config["RefCoords"]["lat"],
ref_long=self.config["RefCoords"]["long"])
self.lcm = aiolcm.AsyncLCM()
self.lcm.subscribe("/gps", self._gpsCallback)
self.lcm.subscribe("/imu_data", self._imuCallback)
self.lcm.subscribe("/nav_status", self._navStatusCallback)
def main():
# how you get lcm messages
lcm = aiolcm.AsyncLCM()
# instantiates Simulator class
Simulator = SimulatorMetaClass()
# constantly queries lcm server
lcm.subscribe("/autonstate", Simulator.autonstate_cb)
lcm.subscribe("/course", Simulator.course_cb)
lcm.subscribe("/gps", Simulator.gps_cb)
lcm.subscribe("/joystick", Simulator.joystick_cb)
lcm.subscribe("/navstatus", Simulator.navstatus_cb)
lcm.subscribe("/obstacle", Simulator.obstacle_cb)
lcm.subscribe("/obstacles", Simulator.obstacles_cb)
lcm.subscribe("/odometry", Simulator.odometry_cb)
lcm.subscribe("/target", Simulator.target_cb)
lcm.subscribe("/targetlist", Simulator.targetlist_cb)
lcm.subscribe("/waypoint", Simulator.waypoint_cb)
# creates loop to execute this code repeatedly with the lcm
run_coroutines(lcm.loop(), Simulator.publish_autonstate(lcm),
Simulator.publish_course(lcm), Simulator.publish_gps(lcm),
Simulator.publish_joystick(lcm),
Simulator.publish_navstatus(lcm),
Simulator.publish_obstacle(lcm),
Simulator.publish_obstacles(lcm),
Simulator.publish_odometry(lcm),
Simulator.publish_target(lcm),
Simulator.publish_targetlist(lcm),
Simulator.publish_waypoint(lcm)
# runSimulator(Simulator)
)
def main():
config_path = os.environ['MROVER_CONFIG']
geom_file = config_path + '/config_kinematics/mrover_arm_geom.json'
args = {
'geom_file': geom_file,
}
lcm_ = aiolcm.AsyncLCM()
arm = MRoverArm(args, lcm_)
config = ConfigurationSpaceTest(arm)
config.straight_up_torque_test()
# config.write_file()
# config.read_file()
# tester = KinematicsTester(arm)
# tester.kinematics_test()
lcm_.subscribe("/arm_position", arm.arm_position_callback)
lcm_.subscribe("/target_orientation", arm.target_orientation_callback)
lcm_.subscribe("/target_angles", arm.target_angles_callback)
lcm_.subscribe("/confirmation", arm.arm_position_callback)
lcm_.subscribe("/motion_execute", arm.motion_execute_callback)
lcm_.subscribe("/simulation_mode", arm.simulation_mode_callback)
lcm_.subscribe("/ik_arm_control", arm.cartesian_control_callback)
lcm_.subscribe("/lock_joint_e", arm.lock_e_callback)
lcm_.subscribe("/ik_enabled", arm.ik_enabled_callback)
run_coroutines(lcm_.loop(), arm.execute_spline())
def main():
lcm = aiolcm.AsyncLCM()
settings_path = os.environ['MROVER_CONFIG'] + '/config_imu/config.ini'
config = ConfigParser()
config.read(settings_path)
calibration_readings = int(config['calibration']['readings'])
serial_port = Serial(config['serial']['port'])
serial_port.baudrate = int(config['serial']['baud'])
calibrated = False
calibrate_start = 0
calibrate_end = 0
drift = 0
num_readings = 0
print('Calibrating...')
while (True):
accel_x = read_value(serial_port)
accel_y = read_value(serial_port)
accel_z = read_value(serial_port)
gyro_x = read_value(serial_port)
gyro_y = read_value(serial_port)
gyro_z = read_value(serial_port)
mag_x = read_value(serial_port)
mag_y = read_value(serial_port)
mag_z = read_value(serial_port)
read_value(serial_port) # angle_x
read_value(serial_port) # angle_y
angle_z = read_value(serial_port, b'\r')
if num_readings == 0:
calibrate_start = angle_z
elif num_readings == calibration_readings:
calibrate_end = angle_z
drift = (calibrate_end - calibrate_start) / calibration_readings
calibrated = True
print('Done.')
num_readings += 1
if calibrated:
imu_msg = IMU()
imu_msg.accel_x = accel_x
imu_msg.accel_y = accel_y
imu_msg.accel_z = accel_z
imu_msg.gyro_x = gyro_x
imu_msg.gyro_y = gyro_y
imu_msg.gyro_z = gyro_z
imu_msg.mag_x = mag_x
imu_msg.mag_y = mag_y
imu_msg.mag_z = mag_z
imu_msg.bearing = (angle_z - (drift * num_readings)) % 360
lcm.publish('/imu', imu_msg.encode())
def __init__(self, path_name):
self.start_time = time.time()
self.lcm = aiolcm.AsyncLCM()
self.sensor_history = None
self.odom_history = None
self.trajectory = None
config_path = getenv('MROVER_CONFIG')
sim_config_path = config_path + "/config_loc_sim/config.json"
with open(sim_config_path, "r") as sim_config_file:
self.sim_config = json.load(sim_config_file)
filter_config_path = config_path + "/config_filter/config.json"
with open(filter_config_path, "r") as filter_config_file:
self.filter_config = json.load(filter_config_file)
self.ref_coords = self.filter_config["ref_coords"]
self.lcm.subscribe(self.filter_config["odom_channel"],
self.odomCallback)
path_name = os.path.split(path_name)[1]
path_path = config_path + "/config_loc_sim/paths/" + path_name
if os.path.exists(path_path):
path = np.genfromtxt(path_path, delimiter=",")
print("[LocSim] Path loaded!")
else:
print("[LocSim] Error: chosen path doesn't exist")
exit()
self.trajectory = generateTrajectory(
path, self.sim_config["max_vel"], self.sim_config["max_accel"],
self.sim_config["max_ang_vel_pi"] * np.pi, self.sim_config["dt"])
plt.ion()
def __init__(self):
# Read in options from logConfig
config_path = os.getenv('MROVER_CONFIG')
config_path += "/config_filter/logConfig.json"
with open(config_path, "r") as config:
self.logConfig = json.load(config)
config_path = os.getenv('MROVER_CONFIG')
config_path += "/config_filter/config.json"
with open(config_path, "r") as config:
self.filterConfig = json.load(config)
# Make logs if they don't already exist
os.makedirs(os.path.join(os.getcwd(), 'onboard', 'filter', 'logs'),
exist_ok=True)
# Create files and write headers
self.write(
['lat_deg', 'lat_min', 'long_deg', 'long_min', 'bearing', 'speed'],
'gps')
self.write([
'accel_x', 'accel_y', 'accel_z', 'gyro_x', 'gyro_y', 'gyro_z',
'mag_x', 'mag_y', 'mag_z', 'bearing'
], 'imu')
self.write([
'nav_state', 'nav_state_name', 'completed_wps', 'missed_wps',
'total_wps', 'found_tbs', 'total_tbs'
], 'navStatus')
self.write(
['lat_deg', 'lat_min', 'long_deg', 'long_min', 'bearing', 'speed'],
'phone')
self.write(
['lat_deg', 'lat_min', 'long_deg', 'long_min', 'bearing', 'speed'],
'odom')
self.write(
['lat_deg', 'lat_min', 'long_deg', 'long_min', 'bearing', 'speed'],
'movAvg')
self.write(['Q', 'FilterType', 'P_initial', 'R', 'dt', 'UpdateRate'],
'config')
P_initial_str = str(self.filterConfig['P_initial']).replace(',', ' ')
R_str = str(self.filterConfig['R']).replace(',', ' ')
self.write([
self.filterConfig['Q'], self.filterConfig['FilterType'],
P_initial_str, R_str, self.filterConfig['dt'],
self.filterConfig['UpdateRate']
], 'config')
# Subscribe to LCM channels
self.lcm = aiolcm.AsyncLCM()
self.lcm.subscribe("/gps", self.gps_callback)
self.lcm.subscribe("/imu", self.imu_callback)
self.lcm.subscribe("/odometry", self.odom_callback)
# Initialize sensor timestamps
self.gps_millis = time.time() * 1000
self.imu_millis = time.time() * 1000
self.odom_millis = time.time() * 1000
def main():
if len(sys.argv) < 3:
usage()
sys.exit(1)
channel = sys.argv[1]
msg = json.loads(" ".join(sys.argv[2:]).replace("\'", "\""))
lcm_ = aiolcm.AsyncLCM()
lcm_.publish(channel, lcmutil.dict_to_lcm(msg).encode())
def main():
# Uses a context manager to ensure serial port released
with ScienceBridge() as bridge:
_lcm = aiolcm.AsyncLCM()
_lcm.subscribe("/mosfet_cmd", bridge.mosfet_transmit)
_lcm.subscribe("/auton_led", bridge.auton_led)
_lcm.subscribe("/servo_cmd", bridge.servo_transmit)
_lcm.subscribe("/heater_cmd", bridge.heater_transmit)
_lcm.subscribe("/heater_auto_shutdown_cmd", bridge.heater_auto_transmit)
print("properly started")
run_coroutines(_lcm.loop(), bridge.receive(_lcm))
def __init__(self):
"""
Creates a Bridge.
A Bridge consists of a Heartbeater and an AsyncLCM instance.
"""
self.hb = heartbeatlib.BaseStationHeartbeater(
self.connection_state_changed)
self.is_connected = False
self.lcm_ = aiolcm.AsyncLCM()
self.subscriptions = {}
def main():
if len(sys.argv) < 3:
usage()
sys.exit(1)
type_name = sys.argv[1]
channel = sys.argv[2]
if not hasattr(rover_msgs, type_name):
print('error: no such type {}'.format(type_name))
sys.exit(1)
lcm_ = aiolcm.AsyncLCM()
lcm_.subscribe(channel, functools.partial(recv_message, type_name))
run_coroutines(lcm_.loop())
def __init__(self):
"""
Creates a Bridge.
A Bridge consists of a Heartbeater and an AsyncLCM instance.
"""
self.num_hbs = 7
self.hbs = []
for x in range(0, self.num_hbs):
self.hbs.append(
heartbeatlib.BaseStationHeartbeater(
self.connection_state_changed, x))
self.connections = [False] * self.num_hbs
self.lcm_ = aiolcm.AsyncLCM()
self.subscriptions = {}
def __init__(self):
config_path = getenv('MROVER_CONFIG')
config_path += "/config_filter/config.json"
with open(config_path, "r") as config:
self.config = json.load(config)
self.gps = Zed_F9P()
self.imu = Icm_20948(
accel_filter_bias=self.config["IMU_accel_filter_bias"],
accel_threshold=self.config["IMU_accel_threshold"])
self.filter = None
self.lcm = aiolcm.AsyncLCM()
self.lcm.subscribe(self.config["gps_channel"], self._gpsCallback)
self.lcm.subscribe(self.config["imu_channel"], self._imuCallback)
def __init__(self):
config_path = getenv('MROVER_CONFIG')
config_path += "/config_filter/config.json"
with open(config_path, "r") as config:
self.config = json.load(config)
self.gps = Gps()
self.imu = Imu(self.config["IMU_accel_filter_bias"],
self.config["IMU_accel_threshold"])
self.filter = None
self.state_estimate = StateEstimate(
ref_lat=self.config["RefCoords"]["lat"],
ref_long=self.config["RefCoords"]["long"])
self.lcm = aiolcm.AsyncLCM()
self.lcm.subscribe("/gps", self._gpsCallback)
self.lcm.subscribe("/imu_data", self._imuCallback)
def main():
# how you get lcm messages
lcm = aiolcm.AsyncLCM()
# instantiates Simulator class
Simulator = SimulatorMetaClass()
# constantly queries lcm server
lcm.subscribe("/ping", Simulator.ping_cb)
lcm.subscribe("/obstacle", Simulator.obstacle_cb)
# lcm.subscribe("\nav_state", Simulator.nav_state_cb)
# lcm.subscribe("\drive_control", Simulator.joystick_cb)
# creates loop to execute this code repeatedly with the lcm
run_coroutines(lcm.loop(), Simulator.publish_ping(lcm),
Simulator.publish_obstacle(lcm)
# runSimulator(Simulator)
)
def main():
lcm = aiolcm.AsyncLCM()
serialPort = serial.Serial('/dev/ttyUSB0')
serialPort.baudrate = 4800
serialPort.bytesize = serial.EIGHTBITS
serialPort.parity = serial.PARITY_NONE
serialPort.stopbits = serial.STOPBITS_ONE
while (True):
oneByte = serialPort.read()
if oneByte != b'$':
continue
fiveBytes = serialPort.read(5)
if fiveBytes != b'GPRMC':
continue
gps = GPS()
serialPort.read_until(b',')[:-1]
serialPort.read_until(b',')[:-1]
hasFix = serialPort.read_until(b',')[:-1]
if hasFix != b'A':
continue
latitude = float(serialPort.read_until(b',')[:-1])
ns = serialPort.read_until(b',')[:-1]
latitude = latitude * -1 if ns is b'S' else latitude
longitude = float(serialPort.read_until(b',')[:-1])
ew = serialPort.read_until(b',')[:-1]
longitude = longitude * -1 if ew is b'W' else longitude
speed = float(serialPort.read_until(b',')[:-1])
bearing = float(serialPort.read_until(b',')[:-1])
gps.latitude_deg = int(latitude/100)
gps.longitude_deg = int(longitude/100)
gps.latitude_min = latitude - (
gps.latitude_deg * 100)
gps.longitude_min = longitude - (
gps.longitude_deg * 100)
gps.bearing_deg = bearing
gps.speed = speed
lcm.publish('/gps', gps.encode())
def run(self):
'''
Reads a chunk of serial data from the base station GPS
and sends it over LCM to the rover GPS. The RTCM messages
are not being interpreted, merely chunked into bytes so they
can be packaged up.
'''
lcm = aiolcm.AsyncLCM()
rtcm = RTCM()
while (self.ser.is_open):
try:
byte_string = self.ser.read(self.buffsize)
print(byte_string)
except Exception as e:
print(e)
time.sleep(1)
continue
rtcm.size = len(byte_string)
rtcm.data = list(byte_string)
lcm.publish('/rtcm', rtcm.encode())
def main():
config_path = os.environ['MROVER_CONFIG']
geom_file = config_path + '/config_kinematics/mrover_arm_geom.json'
args = {
'geom_file': geom_file,
}
lcm_ = aiolcm.AsyncLCM()
arm = MRoverArm(args, lcm_)
lcm_.subscribe("/arm_position", arm.arm_position_callback)
lcm_.subscribe("/target_point", arm.target_point_callback)
lcm_.subscribe("/target_angles", arm.target_angles_callback)
lcm_.subscribe("/confirmation", arm.arm_position_callback)
lcm_.subscribe("/motion_execute", arm.motion_execute_callback)
lcm_.subscribe("/simulation_mode", arm.simulation_mode_callback)
lcm_.subscribe("/ik_arm_control", arm.cartesian_control_callback)
lcm_.subscribe("/lock_joint_e", arm.lock_e_callback)
lcm_.subscribe("/ik_enabled", arm.ik_enabled_callback)
run_coroutines(lcm_.loop(), arm.execute_spline())
def main():
lcm = aiolcm.AsyncLCM()
serialPort = serial.Serial('/dev/ttyUSB0')
serialPort.baudrate = 4800
serialPort.bytesize = serial.EIGHTBITS
serialPort.parity = serial.PARITY_NONE
serialPort.stopbits = serial.STOPBITS_ONE
while (True):
oneByte = serialPort.read()
if oneByte != b'$':
continue
fiveBytes = serialPort.read(5)
if fiveBytes != b'GPRMC':
continue
odometry = Odometry()
serialPort.read_until(b',')[:-1]
serialPort.read_until(b',')[:-1]
hasFix = serialPort.read_until(b',')[:-1]
if hasFix != b'A':
continue
latitude = float(serialPort.read_until(b',')[:-1])
serialPort.read_until(b',')[:-1]
longitude = float(serialPort.read_until(b',')[:-1])
serialPort.read_until(b',')[:-1]
speed = float(serialPort.read_until(b',')[:-1])
bearing = float(serialPort.read_until(b',')[:-1])
odometry.latitude_deg = int(latitude / 100)
odometry.longitude_deg = int(longitude / 100)
odometry.latitude_min = latitude - (odometry.latitude_deg * 100)
odometry.longitude_min = longitude - (odometry.longitude_deg * 100)
odometry.bearing_deg = bearing
odometry.speed = speed
lcm.publish('/odometry', odometry.encode())
def main():
lcm = aiolcm.AsyncLCM()
settings_path = os.environ['MROVER_CONFIG'] + '/config_imu/config.ini'
config = ConfigParser()
config.read(settings_path)
calibration_readings = int(config['calibration']['readings'])
serial_port = Serial(config['serial']['port'])
serial_port.baudrate = int(config['serial']['baud'])
calibrated = False
calibrate_start = 0
calibrate_end = 0
drift = 0
num_readings = 0
print('Calibrating...')
while (True):
serial_port.read_until(b',') # angle_x
serial_port.read_until(b',') # angle_y
angle_z = float(serial_port.read_until(b'\r'))
if num_readings == 0:
calibrate_start = angle_z
elif num_readings == calibration_readings:
calibrate_end = angle_z
drift = (calibrate_end - calibrate_start) / calibration_readings
calibrated = True
print('Done.')
num_readings += 1
if calibrated:
bearing_msg = Bearing()
bearing_msg.bearing = (angle_z - (drift * num_readings)) % 360
lcm.publish('/bearing', bearing_msg.encode())
def main():
# how you get lcm messages
lcm = aiolcm.AsyncLCM()
# instantiates Simulator class
Simulator = SimulatorMetaClass()
# constantly queries lcm server
lcm.subscribe("\nav_state", Simulator.nav_state_cb)
lcm.subscribe("\drive_control", Simulator.joystick_cb)
# creates loop to execute this code repeatedly with the lcm
run_coroutines(lcm.loop(), Simulator.publish_auton_state(lcm),
Simulator.publish_course(lcm), Simulator.publish_GPS(lcm),
Simulator.publish_obstacle(lcm),
Simulator.publish_tennis_ball(lcm), runSimulator(Simulator))
# as a general improvement, it may be worth threading all of the
# lcm-related bruhaha to offload the worst of the performance hits
# as the sim becomes more complex and computationally intensive
# time to run this mf'er
runSimulator(Simulator)
ESC_RAMP_RATE = 25
ESC_EXECUTE_DELAY = 0.1
ESC_RAMP_PERCENT = ESC_RAMP_RATE * ESC_EXECUTE_DELAY
SERVO_AMMONIA_1 = "P9_14"
SERVO_AMMONIA_2 = "P9_16"
servos = [SERVO_AMMONIA_1, SERVO_AMMONIA_2]
VACUUM_1 = "P8_13"
VACUUM_2 = "P8_19"
escs = [VACUUM_1, VACUUM_2]
escs_on = [False, False]
escs_percent = [0, 0]
lcm_ = aiolcm.AsyncLCM()
def angle_to_dc(degrees):
percent = degrees / 120.0
dc = SERVO_MIN_DC + (percent * (SERVO_MAX_DC - SERVO_MIN_DC))
return dc
def run_servo(pin, degrees):
dc = angle_to_dc(degrees)
PWM.set_duty_cycle(pin, dc)
def percent_to_dc(percent):
percent = percent / 100.0
def main():
# Uses a context manager to ensure serial port released
with GPS_Manager() as manager:
lcm = aiolcm.AsyncLCM()
lcm.subscribe("/rtcm", manager.transmit)
run_coroutines(lcm.loop(), manager.recieve(lcm))
