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)