def update_sensors(self):
# print "light:", self._bridge.get_light_sensor()
# print "floor:", self._bridge.get_floor_sensors()
## Send image
if self.enabled_sensors['camera']:
# Get Image
image = self._bridge.get_image()
if image is not None:
nimage = np.asarray(image)
image_msg = CvBridge().cv2_to_imgmsg(nimage, "rgb8")
self.image_publisher.publish(image_msg)
# Send accelerometer sensor values
if self.enabled_sensors['accelerometer']:
accel = self._bridge.get_accelerometer()
self.accelerometer_publisher.publish(Vector3(*accel))
# Send the motor positions
if self.enabled_sensors["motor_position"]:
motor_position = self._bridge.get_motor_position()
self.motor_position_publisher.publish(
UInt32MultiArray(data=list(motor_position)))
# Send the proximity sensor values
if self.enabled_sensors["proximity"]:
proximity = self._bridge.get_proximity()
self.proximity_publisher.publish(
UInt32MultiArray(data=list(proximity)))
def __init__(self):
#Initialize
super().__init__("simulator_node")
self.get_logger().info("INIT")
self.pub_raw = self.create_publisher(Image, "/mopat/testbed/raw_image",
2)
self.pub_starts = self.create_publisher(UInt32MultiArray,
"/mopat/robot/robot_starts", 2)
self.pub_goals = self.create_publisher(UInt32MultiArray,
"/mopat/robot/robot_goals", 2)
self.pub_num = self.create_publisher(UInt32, "/mopat/robot/robot_num",
2)
#Class variables
self.goals_multiarray = UInt32MultiArray(
) #Robot goals - UInt32MultiArray type rosmsg
self.positions_multiarray = UInt32MultiArray(
) #Robot positions - UInt32MultiArray type rosmsg
self.starts_multiarray = UInt32MultiArray(
) #Robot starts - UInt32MultiArray type rosmsg
self.robot_num = UInt32()
self.robot_starts = {} #Dict - robot_index : robot start
self.robot_goals = {} #Dict - robot_index : robot goal
self.steps = 50 #Simulation steps
self.robot_index = 0 #Number of robots and indexing variable
self.got_starts = False #Flag - True if user inputs all starts
self.got_goals = False #Flag - True if user inputs all goals
self.started = False #Flag - True if simulation started
self.got_mouse_click = False #Flag - True if mouse clicked
self.bridge = CvBridge() #Required for rosmsg-cv conversion
self.robot_list = {} #Dict - robot_index : ith Robot object
self.screen_size = (500, 500) #Default screen_size - (int, int)
self.end_sim = False #Flag - Default ros param to end sim
#Game initialization
os.environ['SDL_VIDEO_WINDOW_POS'] = "+0,+50"
pygame.init()
pygame.display.set_caption("MoPAT Multi-Robot Simulator MkV")
self.screen = pygame.display.set_mode(self.screen_size)
self.draw_options = pymunk.pygame_util.DrawOptions(self.screen)
self.clock = pygame.time.Clock()
self.space = pymunk.Space()
#Create map
self.generate_random_map()
#Get the simulator running in a way that the node can still receive messages
#Run the simulator on a different thread while this nodes gets locked in spin
run_thread = Thread(target=self.run)
run_thread.daemon = True
run_thread.start()
def setDevSatus(self,pub,dataBuff):
data = []
data.append(0x04)
data.append(dataBuff[0])
data.append(dataBuff[1])
msg = UInt32MultiArray(data = data)
pub.publish(msg)
def __init__(self):
# TODO: Use super() when moving to Python 3
MiRo.__init__(self)
# Topics
self.cmd_vel = rospy.Publisher(self.tr + 'control/cmd_vel',
TwistStamped,
queue_size=self.qs)
self.kinematic_joints = rospy.Publisher(self.tr +
'control/kinematic_joints',
JointState,
queue_size=self.qs)
self.cosmetic_joints = rospy.Publisher(self.tr +
'control/cosmetic_joints',
Float32MultiArray,
queue_size=self.qs)
self.illum = rospy.Publisher(self.tr + 'control/illum',
UInt32MultiArray,
queue_size=self.qs)
self.tone = rospy.Publisher(self.tr + 'control/tone',
UInt16MultiArray,
queue_size=self.qs)
# Initialise messages
self.cmd_vel_msg = TwistStamped()
self.kinematic_joints_msg = JointState()
self.cosmetic_joints_msg = Float32MultiArray()
self.illum_msg = UInt32MultiArray()
self.tone_msg = UInt16MultiArray()
# Sleep for ROS initialisation
self.ros_sleep(self.sleep_time)
def __init__(self, num_of_wheels):
self.ser = serial.Serial('/dev/ttyACM0', 57600, timeout=1.0)
self.line = []
self.jointstate = JointState()
self.num_of_wheels = num_of_wheels
self.jointstate.header.frame_id = 'base_link'
for i in range(0, self.num_of_wheels):
self.jointstate.name.append("motor_" + str(i + 1))
self.jointstate.position.append(0)
self.jointstate.effort.append(0)
self.jointstate.velocity.append(0)
self.array_msg = UInt32MultiArray()
self.imu = Imu()
self.imu.header.frame_id = "imu"
self.imu.angular_velocity_covariance = [
0.02, 0, 0, 0, 0.02, 0, 0, 0, 0.02
]
self.imu.linear_acceleration_covariance = [
0.04, 0, 0, 0, 0.04, 0, 0, 0, 0.04
]
self.imu.orientation_covariance = [
0.0025, 0, 0, 0, 0.0025, 0, 0, 0, 0.0025
]
self.voltage = Float32()
self.previous_cmd_time = time()
self.sub_cmd = rospy.Subscriber("cmd_vel", Twist, self.cmd_cb)
self.js_pub = rospy.Publisher('joint_state', JointState, queue_size=1)
self.ir_pub = rospy.Publisher('ir_array',
UInt32MultiArray,
queue_size=1)
self.imu_pub = rospy.Publisher('imu', Imu, queue_size=1)
self.bat_pub = rospy.Publisher('battery_voltage',
Float32,
queue_size=1)
def callback_mics(self, msg):
data = np.array(msg.data).astype('float32') * (1.0 / 32768.0)
rms_l = np.sqrt(np.mean(data[0:500]**2))
rms_r = np.sqrt(np.mean(data[500:1000]**2))
new_audio_level = np.max((self.audio_level, np.mean([rms_l, rms_r])))
if new_audio_level > self.audio_level:
self.illum = UInt32MultiArray()
self.illum.data = [
0xFF00CC, 0xFF00CC, 0xFF00CC, 0xFF00CC, 0xFF00CCFF, 0x00FF00CC
]
self.audio_level = new_audio_level
self.pub_illum.publish(self.illum)
# if recording
if not self.micbuf is None and self.audio_level > 0.5:
# append mic data to store
data = np.array(msg.data, 'int16')
x = np.reshape(data, (-1, 500))
self.micbuf = np.concatenate((self.micbuf, x.T))
# report
sys.stdout.write(".")
sys.stdout.flush()
# finished recording?
if self.micbuf.shape[0] >= SAMPLE_COUNT:
self.micbuf = np.delete(self.micbuf, range(0, 9000), 0)
# end recording
self.outbuf = self.micbuf
print(self.micbuf.shape)
self.micbuf = None
print(" OK!")
self.audio_level = 0
def __init__(self):
print "Initializing the controller"
self.actions = [self.earWiggle, self.tailWag, self.rotate, self.shine]
self.Q = [0] * 4
self.N = [0] * 4
self.r = 0
# Set robot name
topic_root = "/" + os.getenv("MIRO_ROBOT_NAME")
# Python needs to initialise a ROS node for publishing data
rospy.init_node("kbandit", anonymous=True)
# Define ROS publishers
self.pub_cmd_vel = rospy.Publisher(topic_root + "/control/cmd_vel",
TwistStamped,
queue_size=0)
self.pub_cos = rospy.Publisher(topic_root + "/control/cosmetic_joints",
Float32MultiArray,
queue_size=0)
self.pub_illum = rospy.Publisher(topic_root + "/control/illum",
UInt32MultiArray,
queue_size=0)
# Subscribers
rospy.Subscriber(topic_root + '/sensors/package',
miro.msg.sensors_package, self.touchListener)
# Initializing object for data publishing
self.velocity = TwistStamped()
self.cos_joints = Float32MultiArray()
self.cos_joints.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.illum = UInt32MultiArray()
self.illum.data = [
0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
0xFFFFFFFF
]
def publishError(errData):
global errorPub
errNum = []
errNum.append(errData[4:6])
error_int16 = []
error_int16.append(int.from_bytes(errNum[0], byteorder="little"))
left_top = UInt32MultiArray(data=error_int16)
errorPub.publish(left_top)
def __init__(self):
# Set robot name
topic_root = "/" + os.getenv("MIRO_ROBOT_NAME")
rospy.init_node("sign_stimuli", anonymous=True)
# Define ROS publishers
self.pub_cmd_vel = rospy.Publisher(topic_root + "/control/cmd_vel",
TwistStamped,
queue_size=0)
self.pub_cos = rospy.Publisher(topic_root + "/control/cosmetic_joints",
Float32MultiArray,
queue_size=0)
self.pub_illum = rospy.Publisher(topic_root + "/control/illum",
UInt32MultiArray,
queue_size=0)
self.pub_kin = rospy.Publisher(topic_root +
"/control/kinematic_joints",
JointState,
queue_size=0)
# Subscribers
#rospy.Subscriber(topic_root + '/sensors/package', miro.msg.sensors_package, self.touchListener)
self.sub_caml = rospy.Subscriber(topic_root +
"/sensors/caml/compressed",
CompressedImage,
self.callback_caml,
queue_size=1,
tcp_nodelay=True)
self.sub_camr = rospy.Subscriber(topic_root +
"/sensors/camr/compressed",
CompressedImage,
self.callback_camr,
queue_size=1,
tcp_nodelay=True)
self.sub_mics = rospy.Subscriber(topic_root + "/sensors/mics",
Int16MultiArray, self.callback_mics)
# Initializing object for data publishing
self.velocity = TwistStamped()
self.cos_joints = Float32MultiArray()
self.cos_joints.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.kin_joints = JointState()
self.kin_joints.name = ["tilt", "lift", "yaw", "pitch"]
self.kin_joints.position = [0.0, math.radians(34.0), 0.0, 0.0]
self.illum = UInt32MultiArray()
self.illum.data = [
0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
0xFFFFFFFF
]
self.image = [None, None]
self.image_converter = CvBridge()
self.controller = HypothalamusController(self)
self.running = True
self.wag_t = 0.0
self.v = [0.0, 0.0]
self.audio = None
def robuster_demo():
global robuster_mode_pub
rospy.Subscriber("Dev_status", UInt32MultiArray, getdevStatus)
rospy.Subscriber("Dev_error", UInt32MultiArray, getdevError)
robuster_mode_pub = rospy.Publisher('DEV_CTL',
UInt32MultiArray,
queue_size=1000)
rospy.init_node('robuster_sdk_demo', anonymous=True)
rate = rospy.Rate(10) # 10hz
ctl_data = [1, 1]
ctl_send_data = UInt32MultiArray(data=ctl_data)
robuster_mode_pub.publish(ctl_send_data)
ctl_data = [2, 0]
ctl_send_data = UInt32MultiArray(data=ctl_data)
robuster_mode_pub.publish(ctl_send_data)
light_lux = 0
while not rospy.is_shutdown():
key = getKey()
if key == 'h':
ctl_data = [1, 1]
ctl_send_data = UInt32MultiArray(data=ctl_data)
robuster_mode_pub.publish(ctl_send_data)
rospy.loginfo(ctl_send_data)
if key == 'd':
print("get dev ctl")
ctl_data = [2, 0]
ctl_send_data = UInt32MultiArray(data=ctl_data)
robuster_mode_pub.publish(ctl_send_data)
rospy.loginfo(ctl_send_data)
if key == 'l':
print("control light")
light_lux = 5 - light_lux
ctl_data = [4, light_lux, light_lux]
ctl_send_data = UInt32MultiArray(data=ctl_data)
robuster_mode_pub.publish(ctl_send_data)
rospy.loginfo(ctl_send_data)
if key == 'x' or key == ' ':
print("stop dev")
return
if (key == '\x03'):
return
rate.sleep()
def publishImu(imu):
global imuPub
imuNum = []
imuNum.append(imu[0:4])
imuNum.append(imu[4:8])
imu_int32 = []
imu_int32.append(int.from_bytes(imuNum[0], byteorder="little"))
imu_int32.append(int.from_bytes(imuNum[1], byteorder="little"))
left_top = UInt32MultiArray(data=imu_int32)
imuPub.publish(left_top)
def conv2multiarray(self, list):
'''
Convert the robot list to a Multiarray
Arguments:
list : robot_list
'''
robot_list = UInt32MultiArray()
#Flatten out list as the data for multiarray
for robot in list:
for loc in robot:
robot_list.data.append(int(loc))
return robot_list
def __init__(self, pi, n, channel=0, aux=0):
self.pi = pi
self.n = n
self.buf = bytearray(self.n * 24)
self.colors = [0] * self.n
flag = 0
if aux == 1:
flag = 256
self.h = self.pi.spi_open(channel, 6400000, flag)
self.subscriber = rospy.Subscriber('/neopixels', UInt32MultiArray,
self.callback)
self.message = UInt32MultiArray()
def convplan2multiarray(self, robot_index):
'''
Function to convert motion_plan list to rosmsg type
Arguments:
robot_index : index of robot to save plans to
'''
self.motion_plans[robot_index] = UInt32MultiArray()
px = self.robot_planners[robot_index].px
py = self.robot_planners[robot_index].py
#Append data in x,y form
for i in range(len(px)):
self.motion_plans[robot_index].data.append(px[i]*self.samp_size)
self.motion_plans[robot_index].data.append(py[i]*self.samp_size)
def publishCSB(csbData):
global csbPub
csbNum = []
csbNum.append(csbData[0:2])
csbNum.append(csbData[2:4])
csbNum.append(csbData[4:6])
csbNum.append(csbData[6:8])
csb_int16 = []
csb_int16.append(int.from_bytes(csbNum[0], byteorder="little"))
csb_int16.append(int.from_bytes(csbNum[1], byteorder="little"))
csb_int16.append(int.from_bytes(csbNum[2], byteorder="little"))
csb_int16.append(int.from_bytes(csbNum[3], byteorder="little"))
left_top = UInt32MultiArray(data=csb_int16)
csbPub.publish(left_top)
def pub_distance():
# pub_tb3a = rospy.Publisher('/tb3a/distances',UInt32MultiArray, queue_size=100)
# pub_tb3b = rospy.Publisher('/tb3b/distances',UInt32MultiArray, queue_size=100)
# pub_tb3c = rospy.Publisher('/tb3c/distances',UInt32MultiArray, queue_size=100)
# pub_tb3d = rospy.Publisher('/tb3d/distances',UInt32MultiArray, queue_size=100)
# pub_tb3e = rospy.Publisher('/tb3e/distances',UInt32MultiArray, queue_size=100)
pub_distances = rospy.Publisher('distances',
UInt32MultiArray,
queue_size=100)
rospy.init_node('pub_distance', anonymous=True)
rate = rospy.Rate(1000)
# tb3a_distances = UInt32MultiArray()
# tb3a_distances.data = []
# tb3b_distances = UInt32MultiArray()
# tb3b_distances.data = []
# tb3c_distances = UInt32MultiArray()
# tb3c_distances.data = []
# tb3d_distances = UInt32MultiArray()
# tb3d_distances.data = []
# tb3e_distances = UInt32MultiArray()
# tb3e_distances.data = []
all_distances = UInt32MultiArray()
all_distances.data = []
while not rospy.is_shutdown():
text = ser.readline()
distnaces = list(map(int, text.split()))
# tb3a_distances.data = distnaces[0:4]
# tb3b_distances.data = distnaces[4:8]
# tb3c_distances.data = distnaces[8:11]
# tb3d_distances.data = distnaces[12:16]
# tb3e_distances.data = distnaces[16:20]
all_distances.data = distnaces
print(text)
#rospy.loginfo(anchors)
# pub_tb3a.publish(tb3a_distances)
# pub_tb3b.publish(tb3b_distances)
# pub_tb3c.publish(tb3c_distances)
# pub_tb3d.publish(tb3d_distances)
# pub_tb3e.publish(tb3e_distances)
pub_distances.publish(all_distances)
rate.sleep()
def publishDecode(decode):
global decodePub
decodeNum = []
decodeNum.append(decode[0:4])
decodeNum.append(decode[4:8])
decodeNum.append(decode[8:12])
decodeNum.append(decode[12:16])
decode_int32 = []
decode_int32.append(int.from_bytes(decodeNum[0], byteorder="little"))
decode_int32.append(int.from_bytes(decodeNum[1], byteorder="little"))
decode_int32.append(int.from_bytes(decodeNum[2], byteorder="little"))
decode_int32.append(int.from_bytes(decodeNum[3], byteorder="little"))
left_top = UInt32MultiArray(data=decode_int32)
decodePub.publish(left_top)
def pub_distance():
pub_anchors = rospy.Publisher('anchors', UInt32MultiArray, queue_size=100)
rospy.init_node('pub_distance', anonymous=True)
rate = rospy.Rate(100)
anchors = UInt32MultiArray()
anchors.data = []
while not rospy.is_shutdown():
text = ser.readline()
anchors.data = list(map(int, text.split()))
print(list(map(int, text.split())))
#rospy.loginfo(anchors)
pub_anchors.publish(anchors)
rate.sleep()
def convplan2multiarray(robot_index, px, py):
'''
Function to convert motion_plan list to rosmsg type
Arguments:
robot_index : index of robot to save plans to
px : list to convert x coordinates from
py : list to convert y coordinates from
'''
global motion_plans
#Set as UInt32MultiArray
motion_plans[robot_index] = UInt32MultiArray()
#Append data in x,y form
for i in range(len(px)):
motion_plans[robot_index].data.append(px[i] * samp_size)
motion_plans[robot_index].data.append(py[i] * samp_size)
def __init__(self):
model_path = get_model_path()
print(model_path)
data_path = get_data_path()
config = {
'hmm' : os.path.join(model_path, 'en-us'), # Hidden Markov Model, Speech Recongnition model - trained probability scoring system
'lm': os.path.join(model_path, 'en-us.lm.bin'), #language model
'dict' : os.path.join(model_path, 'testdict.dict')#, # language dictionary
}
#Start PocketSphinx Deocde
self.ps = Pocketsphinx(**config)
# Variables for Audio
self.micbuf = np.zeros((0, 4), 'uint16')
self.outbuf = None
self.buffer_stuff = 0
self.audio_level = 0
self.timeofclap = 0
self.playchan = 0
self.playsamp = 0
self.startTime = 0
self.TimeSinceLast = 0
self.DemoPause = False
self.PID = ''
self.velocity = TwistStamped()
# Variables for Illumination
self.illum = UInt32MultiArray()
self.illum.data = [0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF]
self.illumInt = 0
self.illumState = 0
# robot name
topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME")
#Publisher for Illum to control LED's while we are processing requests
topic = topic_base_name + "/control/illum"
self.pub_illum = rospy.Publisher(topic, UInt32MultiArray, queue_size=0)
self.velocity_pub = rospy.Publisher(topic_base_name + "/control/cmd_vel", TwistStamped, queue_size=0)
# subscribe
topic = topic_base_name + "/sensors/mics"
self.sub_mics = rospy.Subscriber(topic, Int16MultiArray, self.callback_mics, queue_size=1, tcp_nodelay=True)
def sweep_publisher():
sweep_data = UInt32MultiArray()
pub = rospy.Publisher('sweep_data', UInt32MultiArray, queue_size=10)
rospy.init_node('sweep_publisher', anonymous=True)
#rate = rospy.Rate(10) # 10hz
n = 0
with Sweep('/dev/ttyUSB1') as sweep:
sweep.start_scanning()
for scan in sweep.get_scans():
if rospy.is_shutdown() == True:
sweep.stop_scanning()
break
size = len(scan[0])
for i in range(size):
angle = scan[0][i].angle
distance = scan[0][i].distance
sweep_data.data = [angle, distance]
pub.publish(sweep_data)
def __init__(self, n, method):
# self.pi = pi
self.n = n
# self.buf = bytearray(self.n * 24)
# self.colors = [0] * self.n
# flag = 0
# if aux == 1:
# flag = 256
# self.h = self.pi.spi_open(channel, 6400000, flag)
if method < WAVES or method > SPIDEV:
method = WAVES
self.method = method
if method == PIGPIO or method == WAVES:
self.pi = pigpio.pi()
if not self.pi.connected:
exit()
if method == PIGPIO:
# 0xE0 says not to set chip enables
self.spi_handle = self.pi.spi_open(0, 2e6, 0xE0)
else:
self.oldDATmode = self.pi.get_mode(DAT)
self.oldCLKmode = self.pi.get_mode(CLK)
self.pi.set_mode(DAT, pigpio.OUTPUT)
self.pi.set_mode(CLK, pigpio.OUTPUT)
self.create_byte_waves()
else:
import spidev
self.spi = spidev.SpiDev()
self.spi.open(0, 0)
self.spi.max_speed_hz = int(2e6)
self.subscriber = rospy.Subscriber('/dotstars', UInt32MultiArray,
self.callback)
self.message = UInt32MultiArray()
def callback(event):
sub = rospy.Subscriber('/turtle1/image_sensor', Float64MultiArray, getdata)
#use wait_for_message handle this synchronization problem
rospy.wait_for_message('/turtle1/image_sensor',
Float64MultiArray,
timeout=None)
global imagedata
#transfer this data into 2D Array
multiarray = []
for i in range(0, imagedata.layout.dim[0].size):
new = []
for j in range(0, imagedata.layout.dim[1].size):
new.append(imagedata.data[i * imagedata.layout.dim[1].size + j])
multiarray.append(new)
onedimdata = []
for row in multiarray:
(start, end) = calMaxSubArray(row)
onedimdata += row[start:end]
(start, end) = calMaxSubArray(onedimdata)
arrayout = UInt32MultiArray()
arrayout.data = [start, end]
pub = rospy.Publisher('/hw1/subarray', UInt32MultiArray, queue_size=10)
pub.publish(arrayout)
def __init__(self):
# state
self.step = 0
#Load GUI from glade file
self.builder = Gtk.Builder()
self.builder.add_from_file("client_gui.glade")
# update constants in builder
update_range([
self.builder.get_object("lift_adjustment"),
self.builder.get_object("lift_meas_adjustment")
], miro.constants.LIFT_RAD_MIN, miro.constants.LIFT_RAD_MAX)
update_range([
self.builder.get_object("yaw_adjustment"),
self.builder.get_object("yaw_meas_adjustment")
], miro.constants.YAW_RAD_MIN, miro.constants.YAW_RAD_MAX)
update_range([
self.builder.get_object("pitch_adjustment"),
self.builder.get_object("pitch_meas_adjustment")
], miro.constants.PITCH_RAD_MIN, miro.constants.PITCH_RAD_MAX)
#Connect signals to handle callbacks from event
self.builder.connect_signals(self)
#Get Windows from GUI
self.MainWindow = self.builder.get_object("MainWindow")
self.CamWindow = self.builder.get_object("CamWindow")
self.MicWindow = self.builder.get_object("MicWindow")
self.MicScrolledWindow = self.builder.get_object("MicScrolledWindow")
#Get Cam and Mic Buttons from GUI
self.DisplayCamButton = self.builder.get_object("DisplayCamButton")
self.DisplayMicButton = self.builder.get_object("DisplayMicButton")
#Get Battery objects from GUI
self.BatteryText = self.builder.get_object("BatteryText")
self.BatteryBar = self.builder.get_object("BatteryBar")
self.BatteryBar.add_offset_value(Gtk.LEVEL_BAR_OFFSET_LOW, 4.6)
self.BatteryBar.add_offset_value(Gtk.LEVEL_BAR_OFFSET_HIGH, 4.8)
#self.BatteryBar.add_offset_value(Gtk.LEVEL_BAR_OFFSET_FULL, 5.0)
#Get Sonar objects from GUI
self.SonarText = self.builder.get_object("SonarText")
self.SonarBar = self.builder.get_object("SonarBar")
#Get Light objects from GUI
self.gui_LightBarFL = self.builder.get_object("LightBarFL")
self.gui_LightBarFR = self.builder.get_object("LightBarFR")
self.gui_LightBarRL = self.builder.get_object("LightBarRL")
self.gui_LightBarRR = self.builder.get_object("LightBarRR")
#Get Touch objects from GUI
self.BodyTouchText = self.builder.get_object("BodyTouchText")
self.HeadTouchText = self.builder.get_object("HeadTouchText")
self.BodyTouchBars = []
self.HeadTouchBars = []
for i in range(14):
temp = self.builder.get_object("BT" + str(i))
self.BodyTouchBars.append(temp)
temp = self.builder.get_object("HT" + str(i))
self.HeadTouchBars.append(temp)
#Get Accelerometer objects from GUI
self.gui_AccBarBX = self.builder.get_object("AccBarBX")
self.gui_AccBarBY = self.builder.get_object("AccBarBY")
self.gui_AccBarBZ = self.builder.get_object("AccBarBZ")
self.gui_AccBarBL2 = self.builder.get_object("AccBarBL2")
self.gui_AccBarHX = self.builder.get_object("AccBarHX")
self.gui_AccBarHY = self.builder.get_object("AccBarHY")
self.gui_AccBarHZ = self.builder.get_object("AccBarHZ")
self.gui_AccBarHL2 = self.builder.get_object("AccBarHL2")
#Get Cliff objects from GUI
self.gui_CliffBarL = self.builder.get_object("CliffBarL")
self.gui_CliffBarR = self.builder.get_object("CliffBarR")
#Get Wheel Speed objects from GUI
self.gui_WheelSpeedOptoL = self.builder.get_object("WheelSpeedOptoL")
self.gui_WheelSpeedOptoR = self.builder.get_object("WheelSpeedOptoR")
self.gui_WheelSpeedEMFL = self.builder.get_object("WheelSpeedEMFL")
self.gui_WheelSpeedEMFR = self.builder.get_object("WheelSpeedEMFR")
self.VelControl = self.builder.get_object("VelControl")
self.AngVelControl = self.builder.get_object("AngVelControl")
self.UserControlVel = self.builder.get_object("UserControlVelocity")
self.YawControl = self.builder.get_object("YawControl")
self.LiftControl = self.builder.get_object("LiftControl")
self.PitchControl = self.builder.get_object("PitchControl")
self.ResetKinButton = self.builder.get_object("ResetKinButton")
self.UserControlKin = self.builder.get_object("UserControlKin")
self.LeftEyeControl = self.builder.get_object("LeftEyeControl")
self.RightEyeControl = self.builder.get_object("RightEyeControl")
self.BlinkButton = self.builder.get_object("BlinkButton")
self.WiggleButton = self.builder.get_object("WiggleButton")
self.LeftEarControl = self.builder.get_object("LeftEarControl")
self.RightEarControl = self.builder.get_object("RightEarControl")
self.WagControl = self.builder.get_object("WagControl")
self.DroopControl = self.builder.get_object("DroopControl")
self.WagRateControl = self.builder.get_object("WagRateControl")
self.CosResetButton = self.builder.get_object("CosResetButton")
self.UserControlCos = self.builder.get_object("UserControlCos")
self.VelMeasured = self.builder.get_object("VelMeasured")
self.AngVelMeasured = self.builder.get_object("AngVelMeasured")
self.LiftMeasured = self.builder.get_object("LiftMeasured")
self.YawMeasured = self.builder.get_object("YawMeasured")
self.PitchMeasured = self.builder.get_object("PitchMeasured")
self.FreqControl = self.builder.get_object("FreqControl")
self.VolControl = self.builder.get_object("VolControl")
self.DurationControl = self.builder.get_object("DurationControl")
self.Tone1Button = self.builder.get_object("Tone1Button")
self.Tone2Button = self.builder.get_object("Tone2Button")
self.Tone3Button = self.builder.get_object("Tone3Button")
self.Tone4Button = self.builder.get_object("Tone4Button")
self.SendToneButton = self.builder.get_object("SendToneButton")
self.UserControlIllum = self.builder.get_object("UserControlIllum")
self.gui_LEDBrightF = self.builder.get_object("LEDBrightF")
self.gui_LEDBrightM = self.builder.get_object("LEDBrightM")
self.gui_LEDBrightR = self.builder.get_object("LEDBrightR")
self.gui_LEDColourF = self.builder.get_object("LEDColourF")
self.gui_LEDColourM = self.builder.get_object("LEDColourM")
self.gui_LEDColourR = self.builder.get_object("LEDColourR")
self.gui_Camera = [
self.builder.get_object("CameraLeft"),
self.builder.get_object("CameraRight")
]
self.gui_ResolutionSelection = self.builder.get_object("ResolutionSelection")
self.gui_CapResolutionSelection = self.builder.get_object("CapResolutionSelection")
self.gui_CapFPSSelection = self.builder.get_object("CapFPSSelection")
self.gui_MeasuredFPS = self.builder.get_object("MeasuredFPS")
#Generate variables to store user input
self.preset = False
self.user_blink = 0
self.user_waggle = 0
self.user_wiggle = 0
self.user_wag_rate = 0
self.wag_phase = 0
#Microphone Parameters
# Number of points to display
self.x_len = 40000
# number of microphones coming through on topic
self.no_of_mics = 4
#Generate figure for plotting mics
self.fig1 = plt.figure()
self.fig1.suptitle("Microphones") # Give figure title
#LEFT EAR
self.left_ear_plot = self.fig1.add_subplot(4,1,1)
self.left_ear_plot.set_ylim([-33000, 33000])
self.left_ear_plot.set_xlim([0, self.x_len])
self.left_ear_xs = np.arange(0, self.x_len)
self.left_ear_plot.set_xticklabels([])
self.left_ear_plot.set_yticks([])
self.left_ear_plot.grid(which="both", axis="x")
self.left_ear_plot.set_ylabel("Left Ear", rotation=0, ha="right")
self.left_ear_ys = np.zeros(self.x_len)
self.left_ear_line, = self.left_ear_plot.plot(self.left_ear_xs, self.left_ear_ys, linewidth=0.5, color="b")
#RIGHT EAR
self.right_ear_plot = self.fig1.add_subplot(4,1,2)
self.right_ear_plot.set_ylim([-33000, 33000])
self.right_ear_plot.set_xlim([0, self.x_len])
self.right_ear_xs = np.arange(0, self.x_len)
self.right_ear_plot.set_xticklabels([])
self.right_ear_plot.set_yticks([])
self.right_ear_plot.grid(which="both", axis="x")
self.right_ear_plot.set_ylabel("Right Ear", rotation=0, ha="right")
self.right_ear_ys = np.zeros(self.x_len)
self.right_ear_line, = self.right_ear_plot.plot(self.right_ear_xs, self.right_ear_ys, linewidth=0.5, color="r")
#HEAD
self.head_plot = self.fig1.add_subplot(4,1,3)
self.head_plot.set_ylim([-33000, 33000])
self.head_plot.set_xlim([0, self.x_len])
self.head_xs = np.arange(0, self.x_len)
self.head_plot.set_xticklabels([])
self.head_plot.set_yticks([])
self.head_plot.grid(which="both", axis="x")
self.head_plot.set_ylabel("Head", rotation=0, ha="right")
self.head_ys = np.zeros(self.x_len)
self.head_line, = self.head_plot.plot(self.head_xs, self.head_ys, linewidth=0.5, color="g")
#Tail
self.tail_plot = self.fig1.add_subplot(4,1,4)
self.tail_plot.set_ylim([-33000, 33000])
self.tail_plot.set_xlim([0, self.x_len])
self.tail_xs = np.arange(0, self.x_len)
self.tail_plot.set_yticks([])
self.tail_plot.set_xlabel("Samples")
self.tail_plot.grid(which="both", axis="x")
self.tail_plot.set_ylabel("Tail", rotation=0, ha="right")
self.tail_ys = np.zeros(self.x_len)
self.tail_line, = self.tail_plot.plot(self.tail_xs, self.tail_ys, linewidth=0.5, color="c")
self.canvas = FigureCanvas(self.fig1)
self.ani = animation.FuncAnimation(self.fig1, self.update_line, fargs=(self.left_ear_ys,self.right_ear_ys, self.head_ys, self.tail_ys,), init_func=self.animation_init, interval=10, blit=False)
self.fig1.subplots_adjust(hspace=0, wspace=0)
self.MicScrolledWindow.add_with_viewport(self.canvas)
# variables to store input data
self.input_package = None
self.input_camera = [None, None]
self.t_input_camera = [[], []]
self.input_mics = np.zeros((self.x_len, self.no_of_mics))
#Create object to convert ROS images to OpenCV format
self.image_converter = CvBridge()
#Start timer to call function which updates GUI
GObject.timeout_add(20, self.update_gui)
GObject.timeout_add(20, self.update_images)
#Create objects to hold published data
self.velocity = TwistStamped()
self.kin_joints = JointState()
self.kin_joints.name = ["tilt", "lift", "yaw", "pitch"]
self.kin_joints.position = [0.0, math.radians(34.0), 0.0, 0.0]
self.cos_joints = Float32MultiArray()
self.cos_joints.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.tone = UInt16MultiArray()
self.tone.data = [0, 0, 0]
self.illum = UInt32MultiArray()
self.illum.data = [0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF]
self.camera_zoom = None
self.auto_camera_zoom = [0, 0] # determine zoom from first received frame
self.frame_params = [180, '180w', 15]
self.meas_fps = ["", ""]
# set initial values
self.on_ResetCosButton_clicked()
self.adjustEyeInitialValues()
self.on_LEDReset_clicked()
# display GUI
self.MainWindow.show_all()
# robot name
topic_base_name = "/" + os.getenv("MIRO_ROBOT_NAME")
# publishers
self.pub_cmd_vel = rospy.Publisher(topic_base_name + "/control/cmd_vel", TwistStamped, queue_size=0)
self.pub_cos = rospy.Publisher(topic_base_name + "/control/cosmetic_joints", Float32MultiArray, queue_size=0)
self.pub_illum = rospy.Publisher(topic_base_name + "/control/illum", UInt32MultiArray, queue_size=0)
self.pub_kin = rospy.Publisher(topic_base_name + "/control/kinematic_joints", JointState, queue_size=0)
self.pub_tone = rospy.Publisher(topic_base_name + "/control/tone", UInt16MultiArray, queue_size=0)
self.pub_command = rospy.Publisher(topic_base_name + "/control/command", String, queue_size=0)
# subscribers
self.sub_package = rospy.Subscriber(topic_base_name + "/sensors/package",
miro.msg.sensors_package, self.callback_package, queue_size=1, tcp_nodelay=True)
self.sub_mics = rospy.Subscriber(topic_base_name + "/sensors/mics",
Int16MultiArray, self.callback_mics, queue_size=1, tcp_nodelay=True)
self.sub_caml = rospy.Subscriber(topic_base_name + "/sensors/caml/compressed",
CompressedImage, self.callback_caml, queue_size=1, tcp_nodelay=True)
self.sub_camr = rospy.Subscriber(topic_base_name + "/sensors/camr/compressed",
CompressedImage, self.callback_camr, queue_size=1, tcp_nodelay=True)
import threading
from aruco_msgs.msg import MarkerArray
from std_msgs.msg import UInt32MultiArray
import geometry_msgs.msg
from itertools import combinations
import pprint
import json
markers_map_file_path = "/home/majd/average_markers.json"
max_iterations = 600
total_number_of_markers = 5
freq = 30
# Dictionary visible markers
refrences_dict = {}
connections_dict = {}
markers_list_msg = UInt32MultiArray()
cancel_signal = False
t = tf.Transformer(True)
br = tf.TransformBroadcaster()
overall_markers_list = []
def Markers_list_Callback(msg):
global refrences_dict
for id, refrence in refrences_dict.items():
if int(id) not in msg.data:
del refrences_dict[id]
def MarkersCallback(msg):
def simulator_node():
'''
Function to run the simulation
'''
global robot_list
#Local variables
goals_multiarray = UInt32MultiArray(
) #Robot goals - UInt32MultiArray type rosmsg
positions_multiarray = UInt32MultiArray(
) #Robot positions - UInt32MultiArray type rosmsg
starts_multiarray = UInt32MultiArray(
) #Robot starts - UInt32MultiArray type rosmsg
robot_starts = {} #Dict - robot_index : robot start
robot_goals = {} #Dict - robot_index : robot goal
steps = 50 #Simulation steps
robot_index = 0 #Number of robots and indexing variable
got_starts = False #Flag - True if user inputs all starts
got_goals = False #Flag - True if user inputs all goals
started = False #Flag - True if simulation started
got_mouse_click = False #Flag - True if mouse clicked
bridge = CvBridge() #Required for rosmsg-cv conversion
#Initialize node
rospy.init_node("simulator_node")
print("LOG: Started MoPAT Multi-Robot Simulator MkII node")
#Game initialization
os.environ['SDL_VIDEO_WINDOW_POS'] = "+0,+50" #Set position
pygame.init()
pygame.display.set_caption("MoPAT Multi-Robot Simulator MkII")
screen = pygame.display.set_mode(screen_size)
draw_options = pymunk.pygame_util.DrawOptions(screen)
clock = pygame.time.Clock()
space = pymunk.Space() #Game space
#Subscriber and publishers
rospy.Subscriber("/mopat/control/mrc_output_flags", ByteMultiArray, mrc_cb)
pub_raw = rospy.Publisher("mopat/tracking/raw_image", Image, queue_size=5)
pub_starts = rospy.Publisher("mopat/robot/robot_starts",
UInt32MultiArray,
queue_size=5)
pub_goals = rospy.Publisher("mopat/robot/robot_goals",
UInt32MultiArray,
queue_size=5)
pub_positions = rospy.Publisher("mopat/robot/robot_positions",
UInt32MultiArray,
queue_size=5)
pub_num = rospy.Publisher("mopat/robot/robot_num", UInt32, queue_size=5)
#Create map
# generate_empty_map(space)
generate_test_map(space)
# generate_random_map(space)
print("USER: Enter initial positions now")
#Simulate!
while not rospy.is_shutdown():
for event in pygame.event.get():
#Exiting
if event.type == QUIT:
print("EXIT: Exiting simulation")
sys.exit(0)
elif event.type == KEYDOWN and (event.key in [K_ESCAPE, K_q]):
print("EXIT: Exiting simulation")
sys.exit(0)
#Get user input
if event.type == pygame.MOUSEBUTTONDOWN:
mouse_x, mouse_y = pygame.mouse.get_pos()
mouse_y = screen_size[1] - mouse_y #Pygame pymunk conversion
got_mouse_click = True #Flip flag
if (event.type == KEYDOWN) and (event.key
== K_w) and not got_goals:
print("USER: Enter goals now")
robot_index = 0 #Reset index for goals
goals_multiarray.data.clear() #Clear data for goals
got_starts = True #Flip flag
#Don't run if all goals not found yet
if not got_goals:
#If mouse click found
if got_mouse_click:
#If user inputs starting locations
if not got_starts:
print("LOG: Got Robot", robot_index, "Start:", mouse_x,
";", mouse_y)
#Create robot object with start
robot_list[robot_index] = Robot(robot_index, space,
(mouse_x, mouse_y))
starts_multiarray.data.append(mouse_x)
starts_multiarray.data.append(mouse_y)
#Start subscribers to motion plans for individual robots
rospy.Subscriber(
"mopat/control/motion_plan_{0}".format(robot_index),
UInt32MultiArray,
robot_list[robot_index].motion_plan_cb)
robot_index += 1 #Increment start index
#If user inputs goal locations
else:
#Goal obstacle overlap check
if raw_image[screen_size[1] - mouse_y, mouse_x][0] < 128:
#Set robot goal
robot_goals[robot_index] = (mouse_x, mouse_y)
robot_list[robot_index].set_goal((mouse_x, mouse_y))
goals_multiarray.data.append(mouse_x)
goals_multiarray.data.append(mouse_y)
print("LOG: Got Robot", robot_index, "Goal:", mouse_x,
";", mouse_y)
robot_index += 1 #Increment goal index
if robot_index >= len(robot_list):
got_goals = True #Flip flag to start simulation
continue
else:
print("USER: The point lies within an obstacle")
got_mouse_click = False
#Start simulation if all goals found
else:
#Start individual robot threads once
if not started:
print("LOG: Starting Simulation...")
for i in robot_list:
robot_list[i].start()
started = True
#Update screen
screen.fill((0, 0, 0))
space.step(1 / steps)
space.debug_draw(draw_options)
#After getting starting locations
if got_starts:
#Draw goal and update robot_reached_list
robot_reached_list = []
for i in range(robot_index):
robot_list[i].draw_goal(screen)
pos = robot_list[i].get_pos()
robot_reached_list.append(robot_list[i].robot_reached)
#Update robot positions
positions_multiarray.data.append(int(pos[0]))
positions_multiarray.data.append(int(pos[1]))
#Stop if all robots reached
if sum(robot_reached_list) == robot_index and robot_index != 0:
print("LOG: All Robots Reached, Simulation Completed!")
print("EXIT: Exiting simulation in 5s")
rospy.sleep(5)
sys.exit(0)
#Step up
pygame.display.flip()
#Publish raw image
raw_image = conv2matrix(screen, space, draw_options)
pub_raw.publish(bridge.cv2_to_imgmsg(raw_image,
encoding="passthrough"))
#Publish robot information
pub_starts.publish(starts_multiarray)
pub_goals.publish(goals_multiarray)
pub_positions.publish(positions_multiarray)
pub_num.publish(len(robot_list))
#Clear positions for next step
positions_multiarray.data.clear()
clock.tick(steps)
def __init__(self):
#Initialise
self.clap_detector = ClapDetector()
self.vision = Vision()
# state
self.m_ready = True
self.wagging = False
self.web_cmd = ""
# topic root
self.topic_root = '/' + os.getenv("MIRO_ROBOT_NAME") + '/'
#Configure ROS interface
#Publishers
self.velocity_pub = rospy.Publisher(self.topic_root +
"control/cmd_vel",
TwistStamped,
queue_size=0)
self.cosmetic_joints_pub = rospy.Publisher(self.topic_root +
"control/cosmetic_joints",
Float32MultiArray,
queue_size=0)
self.illum_pub = rospy.Publisher(self.topic_root + "control/illum",
UInt32MultiArray,
queue_size=0)
self.kinematic_joints_pub = rospy.Publisher(self.topic_root +
"control/kinematic_joints",
JointState,
queue_size=0)
self.audio_tone_pub = rospy.Publisher(self.topic_root + "control/tone",
UInt16MultiArray,
queue_size=0)
# self.param_pub = rospy.Publisher(self.topic_root + "control/params", Float32MultiArray, queue_size=0)
self.push_pub = rospy.Publisher(self.topic_root + "core/push",
miro.msg.push,
queue_size=0)
#Create objects to hold published data
self.velocity = TwistStamped()
self.kin_joints = JointState()
self.kin_joints.name = ["tilt", "lift", "yaw", "pitch"]
self.kin_joints.position = [
0.0, miro.constants.LIFT_RAD_CALIB, 0.0, 0.0
]
self.cos_joints = Float32MultiArray()
self.cos_joints.data = [0.0, 0.5, 0.0, 0.0, 0.0, 0.0]
self.tone = UInt16MultiArray()
self.tone.data = [0, 0, 0]
self.illum = UInt32MultiArray()
self.illum.data = [
0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF,
0xFFFFFFFF
]
# self.params = Float32MultiArray()
# self.params.data = [721.0, 15.0]
#Create Variables to store recieved data
self.sonar_range = None
self.light_array = [None, None, None, None]
self.cliff_array = None
self.head_touch = None
self.body_touch = None
#Arrays to hold image topics
self.cam_left_image = None
self.cam_right_image = None
#Create object to convert ROS images to OpenCV format
self.image_converter = CvBridge()
#Create resource for controlling body_node
self.pars = miro.utils.PlatformPars()
self.cam_model = miro.utils.CameraModel()
self.frame_w = 0
self.frame_h = 0
#Timer variables
self.pause_flag = False
self.timer_end_time = 0.0
self.time_now = 0.0
#Start thread
self.updated = False
self.update_thread = Thread(target=self.update)
self.update_thread.start()
self.thread_running = True
#Subscribe to sensors
self.touch_body_sub = rospy.Subscriber(self.topic_root +
"sensors/touch_body",
UInt16,
self.touch_body_callback,
tcp_nodelay=True)
self.touch_head_sub = rospy.Subscriber(self.topic_root +
"sensors/touch_head",
UInt16,
self.touch_head_callback,
tcp_nodelay=True)
self.mics_sub = rospy.Subscriber(self.topic_root + "sensors/mics",
Int16MultiArray,
self.mics_callback,
tcp_nodelay=True)
#Subscribe to Camera topics
self.cam_left_sub = rospy.Subscriber(self.topic_root +
"sensors/caml/compressed",
CompressedImage,
self.cam_left_callback,
tcp_nodelay=True)
self.cam_right_sub = rospy.Subscriber(self.topic_root +
"sensors/camr/compressed",
CompressedImage,
self.cam_right_callback,
tcp_nodelay=True)
# last subscription is to sensors_package because that drives the clock
self.sensors_sub = rospy.Subscriber(self.topic_root +
"sensors/package",
miro.msg.sensors_package,
self.sensors_callback,
tcp_nodelay=True)
UInt32MultiArray,
queue_size=0)
kinematic_joints_pub = rospy.Publisher(topic_root + "control/kinematic_joints",
JointState,
queue_size=0)
velocity = TwistStamped()
kin_joints = JointState()
kin_joints.name = ["tilt", "lift", "yaw", "pitch"]
kin_joints.position = [0.0, math.radians(34.0), 0.0, 0.0]
cos_joints = Float32MultiArray()
cos_joints.data = [0.0, 0.5, 0.0, 0.0, 0.0, 0.0]
illum = UInt32MultiArray()
illum.data = [
0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF, 0xFFFFFFFF
]
yaw_margin = 0.12
lift_current = None
pitch_current = None
yaw_current = None
update_flag = False
def update_kin(msg):
global lift_current
global pitch_current
#!/usr/bin/env python
import rospy
from std_msgs.msg import UInt32MultiArray
<<<<<<< HEAD
Id = UInt32MultiArray() #tableau des identifiant present sur l'image
def markers(msg)
for i in msg.data:
Id.data.append(i)
rospy.init_node("Piece_presente")
rospy.Subscriber("/aruco_marker_publisher/markers_list",UInt32MultiArray,markers,queue_size = 20)
=======
Id = UInt32MultiArray() #tableau des identifiant present sur l'image
def markers(msg):
Id.data = [] # effacer le tableau a chaque call
for i in msg.data:
Id.data.append(i)
rospy.init_node("Piece_presente")
rospy.Subscriber("/aruco_marker_publisher/markers_list",UInt32MultiArray,markers,queue_size = 10)
def __init__(self):
"""
Class initialisation
"""
print("Initialising the controller...")
# Get robot name
topic_root = "/" + os.getenv("MIRO_ROBOT_NAME")
# Initialise a new ROS node to communicate with MiRo
if not self.NODE_EXISTS:
rospy.init_node("kbandit", anonymous=True)
# Define ROS publishers
self.pub_cmd_vel = rospy.Publisher(topic_root + "/control/cmd_vel",
TwistStamped,
queue_size=0)
self.pub_cos = rospy.Publisher(topic_root + "/control/cosmetic_joints",
Float32MultiArray,
queue_size=0)
self.pub_illum = rospy.Publisher(topic_root + "/control/illum",
UInt32MultiArray,
queue_size=0)
# Define ROS subscribers
rospy.Subscriber(
topic_root + "/sensors/touch_head",
UInt16,
self.touchHeadListener,
)
rospy.Subscriber(
topic_root + "/sensors/touch_body",
UInt16,
self.touchBodyListener,
)
rospy.Subscriber(
topic_root + "/sensors/light",
Float32MultiArray,
self.lightCallback,
)
# List of action functions
##NOTE Yes, you can do such things
##TODO Try writing your own action functions and add them here
self.actions = [
self.earWiggle,
self.tailWag,
self.rotate,
self.shine,
self.braitenberg2a,
]
# Initialise objects for data storage and publishing
self.light_array = None
self.velocity = TwistStamped()
self.cos_joints = Float32MultiArray()
self.cos_joints.data = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
self.illum = UInt32MultiArray()
self.illum.data = [
0xFFFFFFFF,
0xFFFFFFFF,
0xFFFFFFFF,
0xFFFFFFFF,
0xFFFFFFFF,
0xFFFFFFFF,
]
# Utility enums
self.tilt, self.lift, self.yaw, self.pitch = range(4)
(
self.droop,
self.wag,
self.left_eye,
self.right_eye,
self.left_ear,
self.right_ear,
) = range(6)
# Variables for Q-learning algorithm
self.reward = 0
self.punishment = 0
self.Q = [0] * len(self.actions) # Highest Q value gets to run
self.N = [0] * len(self.actions) # Number of times an action was done
self.r = 0 # Current action index
self.alpha = 0.7 # learning rate
self.discount = 25 # discount factor (antidamping)
# Give it a sec to make sure everything is initialised
rospy.sleep(1.0)