Example #1
0
def main():
    arduinoSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)

    arduinoSocket.connect((host, port))
    # arduinoSocket.setblocking(0)
    t = PeriodTimer(0.1)
    t.start()
    count = 0
    num1 = 105
    num2 = 50
    while True:
        try:
            with t:
                count += 1
                num1 += 1
                num2 += 1
                if num1 > 120:
                    num1 = 100
                if num2 > 130:
                    num2 = 50
                print count
                data = [num1, num2]
                arduinoSocket.send(dataSend(data))
                print data
                print dataRead(arduinoSocket)
        except (Exception, KeyboardInterrupt):
            arduinoSocket.close()
Example #2
0
def main():
    mio = MIO_TRANS()
    t = PeriodTimer(T)
    t.start()
    try:
        while True:
            with t:
                mio.update()
    except (Exception, KeyboardInterrupt):
        mio.dev.close()
        mio.jsdev.close()
        mio.tmdev.close()
        if Serial_on:
            mio.Arduino_ser.close()
        raise
Example #3
0
def main():
    Arduino_ser = serial.Serial(port='COM3',
                                baudrate=9600,
                                bytesize=8,
                                parity='N',
                                stopbits=1,
                                timeout=1)
    t = PeriodTimer(0.1)
    t.start()
    data = 0
    while True:
        with t:
            data += 1
            if data > 360:
                data = 0
            Arduino_ser.write(send_data(data))
            print data, data + 1, data + 2, data + 3
            read_data(Arduino_ser)
def main():
    arduinoSer = serial.serial_for_url(arduinoUrl,
                                       baudrate=115200,
                                       do_not_open=True,
                                       timeout=1)
    dev = MsgDevice()
    dev.open()
    dev.pub_bind(msgPubBind)
    dev.sub_connect(msgSubConnect)
    dev.sub_add_url('motorSpeed')
    dev.sub_add_url('maxPower')
    for i in range(7):
        dev.sub_add_url('rudder' + str(i) + 'Ang')

    t = PeriodTimer(0.1)
    t.start()
    try:
        try:
            arduinoSer.open()
        except serial.serialutil.SerialException, e:
            arduinoSer.close()
            print e, ", trying reconnet..."
        while True:
            with t:
                # data = subFromVeristand(dev)
                data = [140, 10, 130, 50, 50, 130, 50, 130, 50]
                print data
                if not arduinoSer._isOpen:
                    try:
                        arduinoSer.open()
                    except serial.serialutil.SerialException, e:
                        print e, ", trying reconnect..."
                try:
                    if arduinoSer._isOpen:
                        arduinoSer.write(dataSend(data))
                        dataFromArduino = dataRead(arduinoSer)
                        print dataFromArduino
                except serial.serialutil.SerialException, e:
                    arduinoSer.close()
                try:
                    if dataFromArduino:
                        pubToVeristand(dev, dataFromArduino)
                except UnboundLocalError:
                    pass
def main():
    Arduino_ser = serial.Serial(port='COM9',
                                baudrate=9600,
                                bytesize=8,
                                parity='N',
                                stopbits=1,
                                timeout=1)
    t = PeriodTimer(0.1)
    t.start()
    data = 0
    while True:
        with t:
            # data += 1
            # if data > 254:
            #     data = 0
            data = [10, 130]
            Arduino_ser.write(send_data(data))
            print data
            read_data(Arduino_ser)
def main():
    # arduinoSocket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    arduinoSer = serial.serial_for_url(
        arduinoUrl, baudrate=115200, do_not_open=True, timeout=1)
    dev = MsgDevice()
    dev.open()
    dev.pub_bind(msgPubBind)
    dev.sub_connect(msgSubConnect)
    dev.sub_add_url('motorSpeed')
    dev.sub_add_url('rudderAng')
    dev.sub_add_url('sailAng')

    t = PeriodTimer(0.1)
    t.start()
    try:
        try:
            arduinoSer.open()
        except serial.serialutil.SerialException, e:
            arduinoSer.close()
            print e, ", trying reconnet..."
        while True:
            with t:
                data = subFromVeristand(dev)
                # data = [100, 90, 0]
                print "SEND: ", data
                if not arduinoSer._isOpen:
                    try:
                        arduinoSer.open()
                    except serial.serialutil.SerialException, e:
                        print e, ", trying reconnect..."
                try:
                    if arduinoSer._isOpen:
                        arduinoSer.write(dataSend(data))
                        dataFromArduino = dataRead(arduinoSer)
                        print "RECV: ", dataFromArduino
                except serial.serialutil.SerialException, e:
                    arduinoSer.close()
                try:
                    if dataFromArduino:
                        pubToVeristand(dev, dataFromArduino)
                except UnboundLocalError:
                    pass
Example #7
0
 def thread(self, points, costfunc):
     self.animation_initialize(points)
     timer = PeriodTimer(0.1)
     i = 0
     cost = []
     while True:
         try:
             with timer:
                 state = self.c.getNEData()
                 cost.append(costfunc(state))
                 adata = [
                     state['x'], state['y'], state['u'], state['v'],
                     state['phi'], state['alpha'], state['lm'], state['rm']
                 ]
                 self.animate(adata, i, np.mean(cost))
                 i += 1
         except:
             print('Error happened in drawing!!!!')
             break
def main(args):
    USE_PZH = args.use_pzh
    PP = args.pp
    DWA = args.dwa

    # initialize
    interface001, interface002, pzhdata = ship_initialize(True, True, USE_PZH)
    pid = PID(kp=kp, ki=ki, kd=kd, minout=-1000, maxout=1000, sampleTime=0.1)

    u = np.array([0.0, 0.0])  # speed, yaw speed
    config = Config()

    # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    # 需要根据实际出发点赋值
    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])

    # goal position [x(m), y(m)]
    goal = np.array([-90, 3])

    # obstacles
    obstacles = []
    csv_file = csv.writer(
        open(
            "log-{:s}.txt".format(
                time.strftime("%m-%d-%H-%M", time.localtime())), 'w'))

    csv_file.writerow([
        "timestamp", 'gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed',
        'gps.hspeed', 'gps.stdx', 'gps.stdy', 'gps.track', 'target.posx',
        'target.posy', 'target.yaw', 'target.yaw_speed', 'target.hspeed',
        'target.stdx', 'target.stdy', 'target.track', 'distance', 'left_motor',
        'right_motor'
    ])

    t = PeriodTimer(0.5)
    t.start()
    try:
        while True:
            with t:
                self_state = interface001.receive('gps.posx', 'gps.posy',
                                                  'ahrs.yaw', 'ahrs.yaw_speed',
                                                  'gps.hspeed', 'gps.stdx',
                                                  'gps.stdy', 'gps.track')
                target_state = interface002.receive('gps.posx', 'gps.posy',
                                                    'ahrs.yaw',
                                                    'ahrs.yaw_speed',
                                                    'gps.hspeed', 'gps.stdx',
                                                    'gps.stdy', 'gps.track')

                obstacles.append([target_state[POS_X],
                                  target_state[POS_Y]])  # [num_obstacles, 2]
                #  TODO 为什么把对手船也当作obstacle来看待?
                #  TODO 而且如果你用我的数据,这个地方不用做处理吗?

                if USE_PZH:
                    assert pzhdata is not None
                    lidar_data = pzhdata.receive()

                    if lidar_data["terminated"]:
                        print(
                            "Peng Zhenghao's program is terminated. For safety we close this program."
                        )
                        break

                    target = lidar_data["target"]
                    if not target:
                        # TODO 你的代码是完全不支持发呆状态的。所以如果target没有指定会发生什么事情?
                        print("No Target Specified!")
                        # continue TODO 也许可以用continue跳过后面的所有东西,但这样可能有意外后果。
                    else:
                        goal = lidar_data[target]  # goal = [x, y]
                        for key, centroid in lidar_data.items():
                            if key.startswith(
                                    "Object") and centroid and key != target:
                                obstacles.append(centroid)
                        obstacles = trans_coord(
                            [self_state[POS_X], self_state[POS_Y]],
                            self_state[YAW], np.array(obstacles))

                if PP:
                    # TODO 所以如果你要用我的target坐标来做pure pursuit呢?
                    target_angle, dist = pure_pursuit(self_state, target_state)
                    output = pid.compute(self_state[YAW], target_angle)
                    # TODO 你确定 yaw 和 target angle的零点与单位(rad or degree)一样?
                elif DWA:
                    x = [
                        self_state[POS_X], self_state[POS_Y], self_state[YAW],
                        self_state[SPD], self_state[YAW_SPEED]
                    ]
                    u, ltraj = dwa_control(x, u, config, goal, obstacles)
                    ideal_angle = self_state[YAW] + u[1] * 0.1
                    output = pid.compute(self_state[YAW], ideal_angle)
                else:
                    output = 0

                ################## 此处往下没有问题 #######################

                # dead band
                output = 0 if abs(output) < 5 else output
                # 本船距离目标的距离
                dist = math.sqrt((self_state[POS_X] - goal[0])**2 +
                                 (self_state[POS_Y] - goal[1])**2)
                if dist <= 3:
                    average = baseline[0]
                elif dist <= 10:
                    average = baseline[1]
                else:
                    average = baseline[2]
                left_motor, right_motor = (average +
                                           output / 2), average - output / 2
                left_motor = -rpm_limit(left_motor)
                right_motor = rpm_limit(right_motor)

                print(
                    'self ship state: ',
                    'posx:{},posy:{},yaw:{},speed:{},left:{},right:{}'.format(
                        self_state[POS_X], self_state[POS_Y], self_state[YAW],
                        self_state[SPD], left_motor, right_motor))
                print(
                    'target ship state: ',
                    'posx:{}, posy: {},distance:{}'.format(
                        target_state[POS_X], target_state[POS_Y], dist))

                interface001.Motor_send(left_motor, right_motor)

                csv_file.writerow([time.time()] + self_state + target_state +
                                  [dist, left_motor, right_motor])

    finally:
        interface001.Motor_send(0, 0)
        time.sleep(0.1)
        interface001.dev.close()
        interface002.dev.close()
        csv_file.close()
        print('everything closed')
# def b2a_little(bytes):
# 	return array.array('d', bytes)


# value=a2b_little(default_values)

# print(b2a_little(value)[0])
# print(len(17.0))

from msgdev import MsgDevice,PeriodTimer
from math import sin,cos

if __name__=="__main__":
	dwa = MsgDevice()
	dwa.open()
	dwa.pub_bing('tcp://0.0.0.0:55010')
	interval = 0.3
	t = PeriodTimer(interval)
	t.start()
	try:
		while True:
			with t:


	except KeyboardInterrupt:
		print('close')
		dwa.dev.close()


Example #10
0
def main():
    # auv = auv_initialize()
    auv = MsgDevice()
    auv.open()
    auv.pub_bind('tcp://0.0.0.0:55004')

    # Pure Pursuit initialize
    target_angle = 0
    dist = 0
    # DWA initialize
    configDWA = ConfigDWA()
    dynamic_window = DWA(configDWA)
    # initial state [x(m), y(m), yaw(rad), speed(m/s), yaw_speed(rad/s)]
    # init_state = np.array([0, 10, 45 * pi / 180, 0.0, 0.0])
    init_state = np.array([0, 0, 0, 0.0, 0.0])
    # goal position [x(m), y(m)]
    predefined_goal = np.array([90.0, 80.0])
    goal = predefined_goal
    # obstacles [x(m) y(m), spd(m/s), yaw]
    # static_obstacles = np.array([[100.0, 70.0, 0.0, 0.0],
    #                [30.0, 39.0, 0, 0.0],
    #                [30.0, 70.0, 0.0, 0.0],
    #                [50.0, 70.0, 0.0, 0.0],
    #                [80, 70, 0, 0],
    #                [60, 40, 0, 0],
    #                [40, 40, 0, 0],
    #                [70, 80, 0, 0],
    #                [20.0, 20.0, 0.0, 0.0],
    #                ])
    static_obstacles = np.array([
        [90.0, 60.0, 0.0, 0.0],
        [60.0, 70.0, 0, 0.0],
        [80.0, 60.0, 0.0, 0.0],
        [50.0, 70.0, 0.0, 0.0],
        [80, 70, 0, 0],
        [60, 40, 0, 0],
        [40, 50, 0, 0],
        [30, 60, 0, 0],
        [20.0, 30.0, 0.0, 0.0],
    ])
    moving_obstacles = np.array([[20, 10, 0.7, 2], [60, 30, 0.2, 2]])
    u = np.array([0.0, 0.0])
    # RRT* initialize
    rrt_star = RRTStar(start=[init_state[0], init_state[1]],
                       goal=goal,
                       rand_area=[0, 100],
                       obstacle_list=static_obstacles,
                       expand_dis=5.0,
                       path_resolution=1.0,
                       goal_sample_rate=10,
                       max_iter=100,
                       connect_circle_dist=50.0)
    # path = rrt_star.planning(animation=True, search_until_max_iter=True)
    # path_index = -1
    # localgoal = path[path_index]
    # time.sleep(3)
    traj = [init_state]
    best_traj = None
    state = init_state

    interval = 0.1  # Simulation time step
    pid_spd = PID(kp=2500.0,
                  ki=100.0,
                  kd=0,
                  minout=-1000,
                  maxout=1000,
                  sampleTime=interval)
    pid_yawspd = PID(kp=15000,
                     ki=500.0,
                     kd=10,
                     minout=-1500,
                     maxout=1500,
                     sampleTime=interval)
    pid_yaw = PID_angle(kp=800,
                        ki=3,
                        kd=10,
                        minout=-1500,
                        maxout=1500,
                        sampleTime=interval)
    t = PeriodTimer(interval)
    t.start()
    i = 0

    try:
        while True:
            with t:
                start_time = time.perf_counter()
                if i < 10000:
                    i += 1
                else:
                    i = 0

                goal = predefined_goal
                obstacle = np.vstack((moving_obstacles, static_obstacles))
                min_dist = 100**2
                for ob_single in moving_obstacles:
                    dist = (state[0] - ob_single[0])**2 + (state[1] -
                                                           ob_single[1])**2
                    if dist < min_dist:
                        min_dist = dist

                # to_localgoal_dist_square = (state[POSX] - localgoal[0]) ** 2 + (state[POSY] - localgoal[1]) ** 2
                # if to_localgoal_dist_square <= configDWA.robot_radius_square:
                #     path_index -= 1
                # if abs(path_index) < len(path):
                #     localgoal = path[path_index]
                # else:
                #     localgoal = goal

                # if min_dist > 8 ** 2:
                if 0:
                    best_traj = None
                    target_angle, dist = pure_pursuit(state, localgoal)
                    if target_angle > pi:
                        target_angle -= 2 * pi
                    elif target_angle < -pi:
                        target_angle += 2 * pi
                    output = pid_yaw.compute(
                        state[2], target_angle)  # yaw, target_angle unit: rad
                    # dead band
                    diff = 0 if abs(output) < 5 else output
                    # use different speed based on distance
                    if dist <= 3:
                        average = 800
                    elif dist <= 10:
                        average = 1000
                    else:
                        average = 1200

                    print('Pure Pursuit, Current path point: ', -path_index,
                          "/", len(path))
                else:
                    u, best_traj = dynamic_window.update(
                        state, u, goal, obstacle)
                    u = [1, 0.1]
                    # Forward velocity
                    average = pid_spd.compute(state[3], u[0])
                    average = 0 if abs(average) < 5 else average
                    # Yaw angle velocity
                    diff = pid_yawspd.compute(state[4], u[1])
                    diff = 0 if abs(diff) < 5 else diff
                    print('Dynamic Window Approach')
                    # which path point to follow when switch to the pure pursuit
                    # path_index = get_nearest_path_index(path, state)-len(path)
                    if min_dist < 3**2:
                        print('Collision!!!!!!!!!!!!!!!!!!!!!!!!!!!!!!')

                print(
                    info_format.format(i=i,
                                       real_spd=state[3],
                                       target_spd=u[0],
                                       real_yaw=state[2],
                                       target_yaw=target_angle,
                                       real_yawspd=state[4],
                                       target_yawspd=u[1],
                                       average_output=average,
                                       output_diff=diff,
                                       calc_time=time.perf_counter() -
                                       start_time))

                if show_animation:
                    traj.append(state.copy())
                    # plot(best_traj, state, goal, obstacle, path, traj)
                    plot2(best_traj, state, goal, obstacle, traj)

                to_goal_dist_square = (state[POSX] -
                                       goal[0])**2 + (state[POSY] - goal[1])**2
                if to_goal_dist_square <= configDWA.robot_radius_square:
                    time.sleep(3)
                    print("Goal!!")
                    break

                # Simulation
                # state = trimaran_model(state, left, right, interval)
                state = AUV_model(state, average, -diff, interval)
                state = apply_noise(state)
                moving_obstacles = update_obstacle(moving_obstacles, interval)

                auv.pub_set1('posx', state[0])
                auv.pub_set1('posy', state[1])
                auv.pub_set1('speed', state[3])
                auv.pub_set1('yaw', state[2])
                auv.pub_set1('yawspd', state[4])
                auv.pub_set1('speed_tar', u[0])
                auv.pub_set1('yaw_tar', target_angle)
                auv.pub_set1('yawspd_tar', u[1])

    finally:
        time.sleep(interval)
        print('everything closed')
        # self.capture()
        # self.parse()
        self.publish()

    def close(self):
        self.s.close()
        print('VLP-16 Disconnected.')


if __name__ == "__main__":
    try:
        dev = MsgDevice()
        dev.open()
        devinitial(dev)
        vlp = VLP(dev)
        t = PeriodTimer(t_refresh)
        t.start()
        while True:
            with t:
                print(time.strftime('%Y-%m-%d-%H-%M-%S', time.localtime()))
                vlp.update()

    except KeyboardInterrupt:
        vlp.close()
        dev.close()
    except Exception:
        vlp.close()
        dev.close()
        raise
    else:
        vlp.close()
Example #12
0
    ret["target"] = target
    # return ret
    return pickle.dumps(ret)


def build_push_detection_dev(push_detection_dev):
    push_detection_dev.open()
    # push_detection_dev.pub_bind('tcp://0.0.0.0:55010')  # 上传端口
    push_detection_dev.sub_connect("tcp://192.168.0.8:55019")
    push_detection_dev.sub_add_url("det.data", [-100] * 7)


if __name__ == "__main__":
    push_detection_dev = MsgDevice()
    build_push_detection_dev(push_detection_dev)

    from msgdev import PeriodTimer

    t = PeriodTimer(0.1)
    try:
        while True:
            with t:
                data = push_detection_dev.sub_get("det.data")
                print(data)
                # print(data, pickle.loads(data))

    finally:
        push_detection_dev.pub_set("data", pickle.dumps(None))
        push_detection_dev.close()
        logging.info("Everything Closed!")
        except ValueError:
            self.logger.warning('invalid novatel cksum: ' + ps[1])
            return False
        if cksum != novatel_crc32(self.header + ';' + self.content):
            self.logger.warning('invalid novatel cksum: ' + ps[1])
            return False

        return True


if __name__ == "__main__":
    gps_url = URL_BaseStation
    try:
        gps = GNSS(gps_url)
        gps.write(INIT_COMMANDS)
        t = PeriodTimer(0.2)
        t.start()
        aver_x = 0
        aver_y = 0
        aver_z = 0
        n = 1
        while n < 300:
            with t:
                gps.update()
                if gps.posx != 0:
                    aver_x = 1.0 * (n - 1) / n * aver_x + 1.0 / n * gps.posx
                    print(aver_x)
                    aver_y = 1.0 * (n - 1) / n * aver_y + 1.0 / n * gps.posy
                    aver_z = 1.0 * (n - 1) / n * aver_z + 1.0 / n * gps.posz
                    n += 1
        print('Fixed Position is:', aver_x, aver_y, aver_z)
from PID import PID
from communicater import Communicater
from decisionMaker import Maker
# from drawer import Drawer
from msgdev import PeriodTimer

kp = 800
ki = 3
kd = 10

points = [[-42, 46], [-17, -34], [-85, -24], [-85, 46], [-42, 46]]

if __name__ == '__main__':
    c = Communicater()
    pid = PID(kp=kp, ki=ki, kd=kd, minout=-2500, maxout=500, sampleTime=0.1)
    timer = PeriodTimer(0.1)

    maker = Maker(points)
    cost = 0
    data = []
    i = 0

    while True:
        try:
            with timer:
                state = c.getNEData()
                ideal_angle = maker.getDecision(state)
                cost += maker.getCost(state)
                if ideal_angle == -1000:
                    c.upload(0, 0)
                    print('试验成功!')
from msgdev import MsgDevice, PeriodTimer
import numpy as np


if __name__=="__main__":
	dev=MsgDevice()
	dev.open()
	dev.pub_bind('tcp://0.0.0.0:55005')

	data = np.loadtxt('log-05-05-17-07.txt', skiprows=1, usecols=(3,4), delimiter=',')
	i = 0
	t=PeriodTimer(0.1)
	t.start()
	try:
		while True:
			with t:
				dev.pub_set1('ahrs.yaw', data[i,0])
				dev.pub_set1('ahrs.yaw_speed', data[i,1])
				print('ahrs.yaw', data[i,0], 'ahrs.yaw_speed', data[i,1])
				i += 1
				if i == len(data): i = 0
	except (KeyboardInterrupt, Exception) as e:
		print('closed')
	finally:
		pass
def main():
    USE_PZH = True

    # initialize
    interface001, interface002, pzhdata = ship_initialize(True, True, True)

    t = PeriodTimer(0.2)

    diff_x_average_gps = MovingAverage(100)
    diff_y_average_gps = MovingAverage(100)

    diff_x_average_lidar = MovingAverage(100)
    diff_y_average_lidar = MovingAverage(100)

    # t.start()

    cnt = 0
    end = 200

    try:
        while True:
            with t:
                self_state = interface001.receive('gps.posx', 'gps.posy',
                                                  'ahrs.yaw', 'ahrs.yaw_speed',
                                                  'gps.hspeed', 'gps.stdx',
                                                  'gps.stdy', 'gps.track')

                target_state = interface002.receive('gps.posx', 'gps.posy',
                                                    'ahrs.yaw',
                                                    'ahrs.yaw_speed',
                                                    'gps.hspeed', 'gps.stdx',
                                                    'gps.stdy', 'gps.track')

                assert pzhdata is not None
                lidar_data = pzhdata.receive()

                if lidar_data["terminated"]:
                    print(
                        "Peng Zhenghao's program is terminated. For safety we close this program."
                    )
                    break

                target = lidar_data["target"]
                if not target:
                    print("No Target Specified!")
                    continue
                else:
                    cnt += 1
                    # print("Current CNT")
                    goal = lidar_data[target]  # goal = [x, y]
                    diff_x = target_state[POS_X] - self_state[POS_X]
                    diff_y = target_state[POS_Y] - self_state[POS_Y]
                    diff_x_average_gps.update(diff_x)
                    diff_y_average_gps.update(diff_y)
                    diff_x_average_lidar.update(goal[0])
                    diff_y_average_lidar.update(goal[1])

                    phi2 = -atan2(diff_y_average_gps.avg,
                                  diff_x_average_gps.avg) - pi / 2
                    phi1 = atan2(diff_y_average_lidar.avg,
                                 diff_x_average_lidar.avg)
                    out = phi1 + phi2 - pi / 2

                    # offset = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg) - \
                    #          atan2(diff_y_average_gps.avg, diff_x_average_gps.avg)

                    print("[CNT {}] Current GPS ({}, {}), LiDAR ({}, {}). \
                    ph1{}, ph2 {}, out {} ({} deg).".format(
                        cnt, diff_x_average_gps.avg, diff_y_average_gps.avg,
                        diff_x_average_lidar.avg, diff_y_average_lidar.avg,
                        phi1, phi2, out, out * 180 / pi))
                    if cnt >= end:
                        break

    finally:
        import pickle
        import time

        def get_formatted_time(timestamp=None):
            if not timestamp:
                return time.strftime('%Y-%m-%d_%H-%M-%S', time.localtime())
            else:
                return time.strftime('%Y-%m-%d_%H-%M-%S',
                                     time.localtime(timestamp))

        if diff_y_average_lidar.avg is not None:
            phi2 = -atan2(diff_y_average_gps.avg,
                          diff_x_average_gps.avg) - pi / 2
            phi1 = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg)
            out = phi1 + phi2 - pi / 2

            out = atan2(diff_y_average_lidar.avg, diff_x_average_lidar.avg) - \
                  atan2(diff_y_average_gps.avg, diff_x_average_gps.avg)
            pickle.dump(
                {
                    "offset": out,
                    "timestamp": time.time(),
                    "time": get_formatted_time()
                }, open("offset.pkl", "wb"))
            print("Data have saved to offset.pkl")
        else:
            print("Data is not received.")

        time.sleep(0.5)
        interface001.dev.close()
        interface002.dev.close()
        pzhdata.dev.close()
        print('dev closed')
Example #17
0
def main():
    # USE_PZH = args.use_pzh
    # PP = args.pp
    # DWA = args.dwa

    # initialize
    interface001, interface002, pzhdata = ship_initialize(True, True, USE_PZH)
    pid = PID(kp=kp, ki=ki, kd=kd, minout=-1000, maxout=1000, sampleTime=0.1)

    u = np.array([0.0, 0.0])  # speed, yaw speed
    config = Config()

    # initial state [x(m), y(m), yaw(rad), v(m/s), omega(rad/s)]
    # 需要根据实际出发点赋值
    x = np.array([0.0, 0.0, math.pi / 8.0, 0.0, 0.0])

    # goal position [x(m), y(m)]
    goal = np.array([-90, 3])
    predefiend_goal = goal.copy()

    # obstacles
    obstacles = []

    csv_source = open("log-{:s}.txt".format(time.strftime("%m-%d-%H-%M", time.localtime())), 'w')
    csv_file = csv.writer(csv_source)
    csv_file.writerow(
        ["timestamp", 'gps.posx', 'gps.posy', 'ahrs.yaw', 'ahrs.yaw_speed', 'gps.hspeed',
         'gps.stdx',
         'gps.stdy', 'gps.track', 'target.posx', 'target.posy', 'target.yaw', 'target.yaw_speed',
         'target.hspeed', 'target.stdx', 'target.stdy', 'target.track', 'distance', 'left_motor',
         'right_motor'])
    # 计算频率
    interval = 0.5
    t = PeriodTimer(interval)
    t.start()
    try:
        while True:
            with t:
                self_state = interface001.receive('gps.posx', 'gps.posy', 'ahrs.yaw',
                                                  'ahrs.yaw_speed', 'gps.hspeed',
                                                  'gps.stdx', 'gps.stdy', 'gps.track')
                target_state = interface002.receive('gps.posx', 'gps.posy', 'ahrs.yaw',
                                                    'ahrs.yaw_speed', 'gps.hspeed',
                                                    'gps.stdx', 'gps.stdy', 'gps.track')
                # target_state = interface001.receive('gps.posx', 'gps.posy', 'ahrs.yaw',
                #                                   'ahrs.yaw_speed', 'gps.hspeed',
                #                                   'gps.stdx', 'gps.stdy', 'gps.track')
                # self_state = interface002.receive('gps.posx', 'gps.posy', 'ahrs.yaw',
                #                                     'ahrs.yaw_speed', 'gps.hspeed',
                #                                     'gps.stdx', 'gps.stdy', 'gps.track')

                if USE_PZH:
                    assert pzhdata is not None
                    lidar_data = pzhdata.receive()

                    if lidar_data["terminated"]:
                        print(
                            "Peng Zhenghao's program is terminated. For safety we close this program.")
                        break

                    #### FOR DEBUG
                    # print("Peng Zehgnhao data: ", lidar_data)
                    # continue

                    target = lidar_data["target"]
                    if not target:  # 如果没有搜索到target, 则用初始化的goal作为目标点
                        # TODO 所以如果target没有指定会发生什么事情?
                        print("No Target Specified! Heading to predefiend_goal")
                        goal = predefiend_goal
                    else:
                        goal = target  # goal = [x, y]
                        for key, centroid in lidar_data.items():
                            if key.startswith("Object") and centroid and key != target:
                                obstacles.append(centroid)
                        obstacles = trans_coord([self_state[POS_X], self_state[POS_Y]],
                                                self_state[YAW],
                                                np.array(obstacles))
                        assert obstacles.shape[1] == 2
                        obstacles = obstacles.tolist()
                else:  # 如果不用PZH, 则使用避障目标船的GPS
                    obstacles = [[target_state[POS_X], target_state[POS_Y]]]  # [num_obstacles, 2]

                if PP:
                    if USE_PZH:
                        target_angle, dist = pure_pursuit(self_state, goal)
                    else:
                        target_angle, dist = pure_pursuit(self_state, target_state)
                    output = pid.compute(self_state[YAW], target_angle)  # yaw, target_angle unit: rad
                elif DWA:
                    x = [self_state[POS_X], self_state[POS_Y], self_state[SPD_DIR], self_state[SPD],
                         self_state[YAW_SPEED]]
                    u, ltraj = dwa_control(x, u, config, goal, obstacles)
                    ideal_angle = self_state[SPD_DIR] + u[1] * interval
                    output = pid.compute(self_state[SPD_DIR], ideal_angle) * 50  # 输出太小???
                    print('output', output, 'self_state[SPD_DIR]', self_state[SPD_DIR], 'ideal_angle', ideal_angle)
                    # 本船距离目标的距离
                    dist = math.sqrt((self_state[POS_X] - goal[0]) ** 2 +
                                     (self_state[POS_Y] - goal[1]) ** 2)
                else:
                    output, dist = 0, 0

                # dead band
                diff = 0 if abs(output) < 5 else output
                # 根据距离给速度分级 # TODO 改为速度控制器
                if dist <= 3:
                    average = baseline[0]
                elif dist <= 10:
                    average = baseline[1]
                else:
                    average = baseline[2]
                left_motor, right_motor = (average + diff / 2), (average - diff / 2)  # 除以2, 转弯时超调小
                left_motor = -rpm_limit(left_motor)
                right_motor = rpm_limit(right_motor)

                print('self ship state: ',
                      'posx:{},posy:{},yaw:{},speed:{},left:{},right:{}'.format(self_state[POS_X],
                                                                                self_state[POS_Y],
                                                                                self_state[SPD_DIR],
                                                                                self_state[SPD],
                                                                                left_motor,
                                                                                right_motor))
                print('target ship state: ',
                      'posx:{}, posy: {},distance:{}'.format(target_state[POS_X],
                                                             target_state[POS_Y], dist))

                print('goal', goal, 'obstacles', obstacles)
                interface001.Motor_send(left_motor, right_motor)

                csv_file.writerow(
                    [time.time()] + self_state + target_state + [dist, left_motor,
                                                                 right_motor])

    finally:
        interface001.Motor_send(0, 0)
        time.sleep(interval)
        interface001.dev.close()
        interface002.dev.close()
        pzhdata.dev.close()
        csv_source.close()
        print('everything closed')