webots python e-puck 集群通信案例

导入模块

from controller import Robot, Supervisor, Receiver, Emitter

实例化e-puck机器人对象，开启传感器，设定参数

# 初始化e-puck机器人模块
spvr = Supervisor()
robot_name = "e-puck_666"
robot_spr = spvr.getFromDef(robot_name)
timestep = int(spvr.getBasicTimeStep())		# 时间粒度默认为32毫秒
its_name = robot_spr.getField("name").getSFString()

# 初始化通信模块
# emt
emt = Emitter(name="emitter")
# rcv
rcv = Receiver(name="receiver")
rcv.enable(samplingPeriod=timestep)

# 电机
# motor
maxMotorVelocity = 6.28
initialVelocity = 0.5 * maxMotorVelocity
# left motor
leftMotor = spvr.getDevice("left wheel motor")
leftMotor.setPosition(float('inf'))
leftMotor.setVelocity(initialVelocity)
# right motor
rightMotor = spvr.getDevice("right wheel motor")
rightMotor.setPosition(float('inf'))
rightMotor.setVelocity(initialVelocity)

主循环（默认）

def cal_polarization_(phi):
    """
    计算极化指数
    """
    polarization = 0
    import math
    # vecs_x, vecs_y = [], []
    vec_ = []
    for i in range(len(phi)):
        # vecs_x.append(np.cos(phi[i,:][0]))
        # vecs_y.append(np.sin(phi[i,:][0]))
        vec_.append( [math.cos(phi[i]), math.sin(phi[i])] )

    # p_vec = np.array([ np.sum(vecs_x), np.sum(vecs_y) ])
    p_vec = [ sum([i[0] for i in vec_]), sum([i[1] for i in vec_]) ]

    # polarization = p_vec / np.linalg.norm( p_vec )
    # polarization = np.linalg.norm(p_vec)

    polarization = math.sqrt(pow(p_vec[0],2)+pow(p_vec[1],2))

    polarization /= len(phi)

    return polarization

def correct_hd(hd):
    """
    修正hd
    """
    if (hd>6.28):
        hd -= 6.28
    elif (hd<0):
        hd += 6.28
    else:
        assert ValueError("heading is wrong")

    return hd

# Main loop:
# - perform simulation steps until Webots is stopping the controller
while spvr.step(timestep) != -1:
    # Read the sensors:
    # Enter here functions to read sensor data, like:
    #  val = ds.getValue()

    # Process sensor data here.
    trans_field = robot_spr.getField("translation").getSFVec3f()
    rota_field = robot_spr.getField("rotation").getSFRotation()


    # ====================================
    # vicsek
    # 获取自己航向
    my_hd = rota_field[-1]

    my_hd = correct_hd(my_hd)
    # if (my_hd<0):
    #     my_hd += 6.28

    # 发送航向
    my_msg = [its_name, my_hd]
    send_data = str(my_msg).encode()
    emt.send(data=send_data)
    # 接收航向
    nei_msg = [ my_hd ]
    while ( rcv.getQueueLength() ):
        msg = rcv.getData()
        nei_msg += [list(eval(msg.decode()))[-1]]
        next_msg = rcv.nextPacket()
    # 平均处理
    act = "GO"
    if (nei_msg):
        ave_hd = sum(nei_msg)/len(nei_msg)

        # if (ave_hd<0):
        #     ave_hd += 6.28
        ave_hd = correct_hd(ave_hd)

        min_speed = maxMotorVelocity*0.001
        max_speed = maxMotorVelocity-min_speed
        # 求出转向
        error_of_hd = ave_hd-my_hd

        error_of_hd = correct_hd(error_of_hd)

        # 近似值
        lim = 0.001
        if (abs(error_of_hd)<=lim) or (abs(6.28-error_of_hd)<=lim):   # 直行
            leftMotor.setVelocity(initialVelocity)
            rightMotor.setVelocity(initialVelocity)
        elif (abs(error_of_hd-3.14)<=lim):    # 反了180度，应逆向，右转
            leftMotor.setVelocity(max_speed)
            rightMotor.setVelocity(min_speed)
            act = "R"
        # elif (error_of_hd+6.28<lim and 3.14+error_of_hd>-lim ) or (error_of_hd>lim and 3.14-error_of_hd>lim):  # 右转
        elif (error_of_hd<3.14):  # 右转
            leftMotor.setVelocity(max_speed)
            rightMotor.setVelocity(min_speed)
            act = "R"
        elif (error_of_hd>3.14):     # 左转
            leftMotor.setVelocity(min_speed)
            rightMotor.setVelocity(max_speed)
            act = "L"
        else:
            assert ValueError("heading compare error exist!")
    else:
        leftMotor.setVelocity(initialVelocity)
        rightMotor.setVelocity(initialVelocity)
    # 设定左轮/右轮转速，为：左转/右转/不变


    if (its_name=="e-puck_666(1)"):
        print("="*120)


        print("p_val:",cal_polarization_(phi=nei_msg))


        # print("name {}, rota: {}, trans: {}, emitter: {}".format(its_name,rota_field,trans_field,emt==None))


        # print("name {}, trans: {}, heading: {}, ave heading: {}, action: {}".format(
        #     its_name, trans_field, my_hd, ave_hd, act
        # ))
        # print("nei-lst:",nei_msg)

    # leftMotor.setVelocity(initialVelocity)
    # rightMotor.setVelocity(initialVelocity)

    # send_data = bytes('caonima' + its_name, encoding='UTF-8')

    # emt.send(data=send_data)


    # 收发信息
    # get_data_lst = []
    # while (rcv.getQueueLength()!=0):
    #
    #     msg = rcv.getData()
    #     get_data_lst.append(msg)
    #
    #     pass
    # print("####### data-lst:",get_data_lst)

    # print("queue len:", rcv.getQueueLength() )
    #
    # if (rcv.getQueueLength()):
    #     # print("######################## data-len:",rcv.getQueueLength(),"data:",msg)
    #     # msg = rcv.nextPacket()
    #
    #     msg = rcv.getData()
    #     next_msg = rcv.nextPacket()
    #     print("######################## data-len:",rcv.getQueueLength(),"data:",msg)
    #
    #     msg = rcv.getData()
    #     next_msg = rcv.nextPacket()
    #     print("######################## data-len:",rcv.getQueueLength(),"data:",msg)
    #
    #     # msg = rcv.getData()
    #     # next_msg = rcv.nextPacket()
    #     # print("######################## data-len:",rcv.getQueueLength(),"data:",msg)
    #
    #
    #     # next_msg = rcv.nextPacket()
    #     # print("######################## data-len:",rcv.getQueueLength(),"data:",next_msg)
    #     # next_msg = rcv.nextPacket()
    #     # print("######################## data-len:",rcv.getQueueLength(),"data:",next_msg)
    #
    # Enter here functions to send actuator commands, like:
    #  motor.setPosition(10.0)
    pass