def search(self, signal): direction = random.randint(0, 1) # slowly rotate for fixed degree and wait for few msec, repeat for x times sig = "" for i in range(0, 10): if signal == 'light': sig = self.lightSignal else: sig = self.irSignal if self.leftTouchSignal == 1 or self.rightTouchSignal == 1: self.avoid() if sig == 1: # no detection action = Int16MultiArray() action.data = [3, 24 * direction - 12] self.pub.publish(action) time.sleep(0.1) action = Int16MultiArray() action.data = [0, 0] self.pub.publish(action) time.sleep(0.23) else: break #if no detection after rotating for 10 times, walk forward a little action = Int16MultiArray() action.data = [8, 0] self.pub.publish(action) time.sleep(0.7)
def avoid(self): # move backward a little action = Int16MultiArray() action.data = [-10, 0] self.pub.publish(action) time.sleep(0.15) # rotate either clockwise or counterclockwise, randomly direction = random.randint(0, 1) action = Int16MultiArray() action.data = [0, 20 * direction - 10] self.pub.publish(action) time.sleep(0.15)
def publish_instructions(self, power1=0, power2=0, angle1=0, angle2=0, rotation1=0, rotation2=0): """publishing movement parameters for hexapod""" hex1_array = Int16MultiArray( data=[self.power1, self.angle1, self.rotation1]) hex2_array = Int16MultiArray( data=[self.power2, self.angle2, self.rotation2]) self.pub1.publish(hex1_array) self.pub2.publish(hex1_array) time.sleep(0.2)
def main(): # parse arguments inputURI = rospy.get_param('~inputURI', '/dev/video0') outputURI = rospy.get_param('~outputURI', '') network = rospy.get_param('~network', 'facenet-120') overlay = rospy.get_param('~overlay', 'box,labels,conf') threshold = rospy.get_param('~threshold', 0.5) # prepare publisher box_publisher = rospy.Publisher("bounding_box", Int16MultiArray, queue_size=10) # load the object detection network net = jetson.inference.detectNet(network, threshold) # create video sources & outputs input = jetson.utils.videoSource(inputURI) output = jetson.utils.videoOutput(outputURI) # process frames until the user exits rate = rospy.Rate(10) while not rospy.is_shutdown(): # capture the next image img = input.Capture() # detect objects in the image (with overlay) detections = net.Detect(img, overlay=overlay) # print the detections # rospy.loginfo("detected {:d} objects in image".format(len(detections))) if (len(detections) == 0): continue target_detection = detections[0] rospy.loginfo(target_detection) # render the image output.Render(img) # update the title bar output.SetStatus("{:s} | Network {:.0f} FPS".format( network, net.GetNetworkFPS())) # Publish bounding box rospy.loginfo("Publishing detection box") box = Int16MultiArray() box.data.append(target_detection.ClassID) box.data.append(target_detection.Left) box.data.append(target_detection.Top) box.data.append(target_detection.Right) box.data.append(target_detection.Bottom) box_publisher.publish(box) rate.sleep()
def __init__(self): print("init") rospy.init_node('detector', anonymous=True) self.bridge = CvBridge() self.pose_pub = rospy.Publisher("detector/pose", Float32MultiArray, queue_size=1) self.pspace_pub = rospy.Publisher("detector/p_space", Int16MultiArray, queue_size=1) self.pspace_id_pub = rospy.Publisher("p_space_id", Int16, queue_size=1) self.distance = rospy.Publisher("car_dis", Int16, queue_size=1) self.parking_distance = rospy.Publisher("goal_dis", Int16, queue_size=1) self.image_sub = rospy.Subscriber("/ipm0", Image, self.imageCB) self.pspace_info = Int16MultiArray() self.pspace_info.data = [] self.pose_info = Float32MultiArray() self.pose_info.data = [] self.ego_x = 0.0 self.ego_y = 0.0 self.ego_heading = 0.0 self.Det = Detector() self.img_w, self.img_h = 1300, 1200 self.center_rescale = np.float32( [self.img_w / 512., self.img_h / 512.]) self.box_rescale = np.tile(self.center_rescale, (4, 1)) self.prev_center = None self.pprev_center = None self.prev_heading = None print("init end")
def _movement_controller_state_callback(self, req): ''' Callback function for the movement controller enable/disable service. If true, the movement controller will make corrections to meet the desired speed and steering direction with PID controller. Else if disabled, the controller will simply send PWM values of [0, 0, 0, 0] to the motor driver. Parameters: req: The request message (bool) to enable/disable the motor. Type std_srvs/SetBool. Returns: response: A response with a string & bool notifiying if the request was valid. Type std_srvs/SetBoolResponse ''' self.movement_controller_enabled = req.data response = SetBoolResponse() response.success = True if (self.movement_controller_enabled): response.message = "Movement Controller Enabled" else: response.message = "Movement Controller Disabled" pwm_msg = Int16MultiArray() pwm_msg.data = [0, 0, 0, 0] self.pwm_pub.publish(pwm_msg) return response
def publishData(leftPower, rightPower): global stampId stampId += 1 #print stampId, " Published from steering at: ", int(round(time.time() * 1000)) data = Int16MultiArray() data.data = [leftPower, rightPower] pub.publish(data)
def set_servos(servo_positions, publisher): ''' Set servo positions (0.0-180.0/closed-open). ([a, b], pub) ''' servo_msg = Int16MultiArray() servo_msg.data = [int(servo_positions[0]), int(servo_positions[1])] # [Thumb, Fingers] publisher.publish(servo_msg)
def talker(): flag = 1 data_list = Int16MultiArray() pub = rospy.Publisher('contl_data', Int16MultiArray, queue_size=2) rospy.init_node('talker', anonymous=True) while not rospy.is_shutdown(): if (flag == 1): data_list.data = [25, -60, -60] flag = 0 pub.publish(data_list) time.sleep(2) elif (flag == 2): data_list.data = [-15, 0, 0] flag = 0 pub.publish(data_list) time.sleep(1.0) elif (flag == 3): data_list.data = [20, 0, 0] flag = 0 pub.publish(data_list) time.sleep(1) else: data_list.data = [0, 0, 0] pub.publish(data_list) time.sleep(1.0)
def __init__(self, id, NofUWBs, pos): self.ID = id self.NofUWBs = NofUWBs self.pos = pos self.pub_list = Int16MultiArray() self.pub_list.data = [] print("Initializing UWB " + str(self.ID) + " with position: " + str(pos)) rospy.init_node('UWB' + str(self.ID), anonymous=True) self.UWB_pub = rospy.Publisher("UWBdata", Int16MultiArray, queue_size=10) self.UWB_sub_list = [] for i in range(int(self.NofUWBs)): if i != self.ID: self.UWB_sub_list.append( rospy.Subscriber("UWBdata", Int16MultiArray, self.callback)) self.pub_list.data.append(self.ID) for i in range(3): self.pub_list.data.append(pos[i]) rospy.Timer(rospy.Duration(1. / 0.5), self.timer_callback)
def callback(msg, prevMarkers): detectedMarkers = msg.markers # The current time in seconds now = rospy.get_time() for detectedMarker in detectedMarkers: measuredTime = detectedMarker.header.stamp.secs markerID = detectedMarker.id prevMarkers[markerID] = measuredTime detected_markers = list() for marker in prevMarkers.keys(): if abs(prevMarkers[marker] - now) > 5: del prevMarkers[marker] else: detected_markers.append(marker) array = MultiArrayDimension() array.label = 'numMarkers' array.size = len(detected_markers) array.size = len(detected_markers) layout = MultiArrayLayout() layout.dim = [ array, ] layout.data_offset = 0 msg = Int16MultiArray() msg.layout = layout msg.data = detected_markers pub.publish(msg)
def talker(): rospy.init_node('control_motor', anonymous=False) pub = rospy.Publisher('py_control', Int16MultiArray, queue_size=10) rate = rospy.Rate(10) b = [0, 0, 0, 0, 0] while not rospy.is_shutdown(): msg = Int16MultiArray() i = raw_input() if (i == "w"): b = [1, 0, 0, 0, 1] elif (i == "s"): b = [0, 1, 0, 0, 1] elif (i == "a"): b = [0, 0, 1, 0, 1] elif (i == "d"): b = [0, 0, 0, 1, 1] elif (i == "p"): if b[4] <= 3: b[4] += 1 elif (i == "o"): if b[4] >= 2: b[4] -= 1 else: b = [0, 0, 0, 0, 0] msg.data = b pub.publish(msg) rate.sleep()
def execute(self, userdata): rospy.loginfo('Executing state AtTable') rospy.Subscriber("serve_status", String, self.callback) global attable_count global reached global ready_to_go global cur_path cur_state = robot_state() cur_state.state = "AtTable" if len(cur_path) >1: cur_state.next_goal = cur_path[0] else: cur_state.next_goal = 0 pub.publish(cur_state) attable_count = 0 if ready_to_go == False: time.sleep(0.1) return 'wait' attable_count = 1 ready_to_go = False print(cur_path[0]) if cur_path[0]==11: return 'finished' else: nav_data = [cur_path[0],cur_path[1]] nav = Int16MultiArray(data = nav_data) nav_pub.publish(nav) nav_pub.publish(nav) nav_pub.publish(nav) nav_pub.publish(nav) cur_path.pop(0) return 'Done'
def ultrasonic_sensor(): distance_pub = rospy.Publisher('isObstacle', Int16MultiArray, queue_size=1) flag_pub = rospy.Publisher('isFlagSet', Int16, queue_size=1) rospy.init_node('ultrasonic_sensor', anonymous=True) # Initializing the node sensor_1 = Distance(2, 3) sensor_2 = Distance(4, 17) sensor_3 = Distance(27, 22) sensor_4 = Distance(10, 9) rate = rospy.Rate(10) while not rospy.is_shutdown(): reading_1 = sensor_1.distance_from_obj() reading_2 = sensor_2.distance_from_obj() reading_3 = sensor_3.distance_from_obj() reading_4 = sensor_4.distance_from_obj() threshold_flag = get_threshold_flag(reading_1, reading_2, reading_3, reading_4) sensor_reading_array = Int16MultiArray() sensor_reading_array.data = [ reading_1, reading_2, reading_3, reading_4 ] distance_pub.publish(sensor_reading_array) flag_pub.publish(threshold_flag) print("sensor:", sensor_reading_array.data) print("flags:", threshold_flag) rate.sleep()
def publishData(power, objectPosition, objectDimension): global stampId stampId += 1 #print stampId, " Published from power at: ", int(round(time.time() * 1000)) data = Int16MultiArray() data.data = [objectPosition, objectDimension, power] pub.publish(data)
def callback1(self, data): # Recieve the image try: self.cv_image1 = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) cv2.imshow("", self.cv_image1) cv2.waitKey(1) # Uncomment if you want to save the image #cv2.imwrite('image_copy_1.png', self.cv_image1) #im1=cv2.imshow('window1', self.cv_image1) # Publish the results try: self.image_pub1.publish( self.bridge.cv2_to_imgmsg(self.cv_image1, "bgr8")) except CvBridgeError as e: print(e) self.red = self.detect_red(self.cv_image1) self.green = self.detect_green(self.cv_image1) self.blue = self.detect_blue(self.cv_image1) self.target = self.detect_target(self.cv_image1) self.pub = Int16MultiArray() self.pub.data = np.array([ self.red[0], self.red[1], self.green[0], self.green[1], self.blue[0], self.blue[1], self.target[0], self.target[1] ]) #Publish the y and z coordinates self.y_z_pub.publish(self.pub)
def __init__(self): self.twist_pub = rospy.Publisher('transform', Twist, queue_size=5) self.light_pub = rospy.Publisher('light', Bool, queue_size=5) self.motor_values_pub = rospy.Publisher('motor_values', Int16MultiArray, queue_size=5) self.e_stop_pub = rospy.Publisher('e_stop', Bool, queue_size=5) self.prev_light_button_state = self.light_toggle = self.e_stop = False self.twist_msg = Twist() self.motor_values_msg = Int16MultiArray() self.motor_values_msg.layout.dim = [MultiArrayDimension('data', 1, 9)] self.light_toggle_msg = Bool() self.light_toggle_msg = False self.e_stop_msg = Bool() self.e_stop_msg = False # direction vectors for each of the motors self.front_left_dir = [math.sqrt(2) / 2, math.sqrt(2) / 2] self.front_right_dir = [-math.sqrt(2) / 2, math.sqrt(2) / 2] self.back_left_dir = [math.sqrt(2) / 2, -math.sqrt(2) / 2] self.back_right_dir = [-math.sqrt(2) / 2, -math.sqrt(2) / 2] self.vt = [0.0, 0.0] self.w = 0.0 self.front_left_vel = self.front_right_vel = self.back_left_vel = self.back_right_vel = 0.0 self.front_up_vel = self.back_up_vel = 0.0 self.claw_grab = self.claw_rotation = self.claw_bottom = 0.0 self.motor_values = [0, 0, 0, 0, 0, 0, 0, 0, 0]
def rec_thread(quit_event): global chunk_lock, chunk_buffer, azimuth_global conn, addr = s.accept() pubFrameRaw = rospy.Publisher('sound_raw', Int16MultiArray, queue_size=1) while not rospy.is_shutdown(): # con_start = rospy.Time.now() # It only takes 0.00011 secs to transfer # 32768 bits from respeaker to robots through Ethernet chunk_buffer = conn.recv(32768, socket.MSG_WAITALL) # con_end = rospy.Time.now() chunk_time = rospy.Time.now() # print("Receive") # print("con time: ", (con_end-con_start).to_sec()) # print("==========================") xx = np.frombuffer(chunk_buffer, 'int16') # print("The xx: ", xx) # print("xx len: ", xx.size) s_raw_msg = Int16MultiArray() s_raw_msg.data = xx pubFrameRaw.publish(s_raw_msg) chunk_lock = False conn.sendall(str(int(np.floor(azimuth_global))).encode('utf-8')) conn.close()
def watcher(data): if data is None: return msg = Int16MultiArray() msg.data = data msg.layout = gen_layout(data.shape) self.publisher.publish(msg)
def callback1(data): global cur_x global cur_y global flag max_pwm = 100 # read current angles for i in range(0, num_of_joints): cur_x[i] = data.data[2 * i] cur_y[i] = data.data[2 * i + 1] # only start publishing after the first desired angles publish if flag: max_angle = 35 # the main calculation w_len = PID() # if one of the angles exceeds maximum angle, zero all for i in range(0, num_of_joints): if (abs(cur_x[i]) > max_angle) or (abs(cur_y[i]) > max_angle): w_len = np.zeros(num_of_joints * 3) # if one of the pwm values exceeds a max val, reduce it to max val and all other values in ratio for i in range(0, 9): pwm_val = abs(w_len[i]) if pwm_val > max_pwm: for j in range(0, 9): w_len[j] = w_len[j] * max_pwm / pwm_val w_len = np.around(w_len) msg = Int16MultiArray() msg.data = w_len pub.publish(msg)
def run(self): start = time.time() image = cv2.resize(self.cv_image, (self.input_shape[0], self.input_shape[1])) tmp_inp = np.array(image, dtype=np.uint8) tmp_inp = np.expand_dims(tmp_inp, 0) preds = model.predict(tmp_inp)[0] top_preds = np.argsort(preds)[::-1][0:predictions_to_return] # print('\n' + '--PREDICTED SCENE CATEGORIES:') # output the prediction pub_num = Int16MultiArray() prob = Float32MultiArray() for i in range(0, 3): # if preds[top_preds[i]] > 0.1: print classes[top_preds[i]], prob.data.append( Decimal(str(preds[top_preds[i]])).quantize( Decimal('0.000001'), rounding=ROUND_HALF_UP)) pub_num.data.append(top_preds[i]) print '' # print 'processing time:', time.time() - start pub_num.layout.data_offset = 5 self.pub_labels.publish(pub_num) self.pub_prob.publish(prob)
def onButton7(event): print "7" pub_array = np.array([7]) msg = Int16MultiArray() msg.data = pub_array pub.publish(msg) rate.sleep()
def callback2(self,data): # Recieve the image try: self.cv_image2 = self.bridge.imgmsg_to_cv2(data, "bgr8") except CvBridgeError as e: print(e) # Uncomment if you want to save the image #cv2.imwrite('image_copy_2.png', self.cv_image2) im2=cv2.imshow('window2', self.cv_image2) cv2.waitKey(1) # Publish the results try: self.image_pub2.publish(self.bridge.cv2_to_imgmsg(self.cv_image2, "bgr8")) except CvBridgeError as e: print(e) red = self.detect_red(self.cv_image2) green = self.detect_green(self.cv_image2) blue = self.detect_blue(self.cv_image2) target = self.detect_target(self.cv_image2) yellow = self.detect_yellow(self.cv_image2) self.pub = Int16MultiArray() self.pub.data = np.array([red[0], red[1], green[0],green[1], blue[0], blue[1],yellow[0], yellow[1], target[0],target[1]]) # Publish the x and z coordinates self.cam2_pub.publish(self.pub)
def talker(): pub = rospy.Publisher('get_speed_from_random', Int16MultiArray, queue_size=10) rospy.init_node('get_speed_from_random', anonymous=True) rate = rospy.Rate(0.1) while not rospy.is_shutdown(): speed1 = random.randint(200, 250) speed2 = random.randint(200, 250) speed1_boolean = random.randint(0, 1) speed2_boolean = random.randint(0, 1) if speed1_boolean == 1: speed1 = -speed1 if speed2_boolean == 1: speed2 = -speed2 var_from_the_phone = Int16MultiArray() var_from_the_phone.data = [speed1, speed2] rospy.loginfo(var_from_the_phone) pub.publish(var_from_the_phone) rate.sleep()
def convert_np_vector_to_int16_multi_array(vector): assert len(vector.shape) == 1 N = vector.shape[0] msg = Int16MultiArray(layout=MultiArrayLayout( dim=[MultiArrayDimension(label="", size=N, stride=1)], data_offset=0), data=vector.astype(int).tolist()) return msg
def loop(self, args): state_file = None if len(args): state_file = args[0] # periodic reports count = 0 # safety dropout if receiver not present dropout_data_r = -1 dropout_count = 3 # loop while not rospy.core.is_shutdown(): # check state_file if not state_file is None: if not os.path.isfile(state_file): break # if we've received a report if self.buffer_total > 0: # compute amount to send buffer_rem = self.buffer_total - self.buffer_space n_bytes = BUFFER_STUFF_BYTES - buffer_rem n_bytes = max(n_bytes, 0) n_bytes = min(n_bytes, MAX_STREAM_MSG_SIZE) # if amount to send is non-zero if n_bytes > 0: msg = Int16MultiArray( data=self.data[self.data_r:self.data_r + n_bytes]) self.pub_stream.publish(msg) self.data_r += n_bytes # break if self.data_r >= len(self.data): break # report once per second if count == 0: count = 10 print "streaming:", self.data_r, "/", len(self.data), "bytes" # check at those moments if we are making progress, also if dropout_data_r == self.data_r: if dropout_count == 0: print "dropping out because of no progress..." break print "dropping out in", str(dropout_count) + "..." dropout_count -= 1 else: dropout_data_r = self.data_r # count tenths count -= 1 time.sleep(0.1)
def execute(self, action): speed_wheel = Int16MultiArray() rospy.loginfo("%s", action.direction) # rospy.loginfo("Server get action %s", action.angle) if action.direction == "backward": speed_wheel.data = [-100, -100] if action.direction == "forward": speed_wheel.data = [100, 100] elif action.direction == "stop": speed_wheel.data = [0, 0] elif action.direction == "backward": speed_wheel.data = [-100, -100] elif action.direction == "rotate": if action.side == "right": speed_wheel.data = [100, -100] elif action.side == "left": speed_wheel.data = [-100, 100] speed_wheel = self.modificator_direction(speed_wheel, action) # rospy.loginfo(speed_wheel) self.change_directionToArduino(speed_wheel, action) self.result = setDirectionRobotResult(done=True) self.server.set_succeeded(self.result)
def microphone_listener_node(): global flag rospy.init_node('microphone_listener_node') pub = rospy.Publisher('/audio/mic_samples', Int16MultiArray, queue_size=800) rospy.Subscriber('/audio/detection_flag', Bool, flag_callback) #Block samples / second rate = rospy.Rate(int(RATE)) #Start audio stream from microphone p = pyaudio.PyAudio() stream = p.open(format=p.get_format_from_width(WIDTH), channels=CHANNELS, rate=RATE, input=True, output=False) sample = Int16MultiArray() while not rospy.is_shutdown(): if flag: #Samples microphone input input_string = stream.read(BLOCKSIZE, exception_on_overflow=False) input_value = struct.unpack('h' * BLOCKSIZE, input_string) sample.data = input_value pub.publish(sample) else: rate.sleep() #Closes input stream stream.stop_stream() stream.close() p.terminate()
def publish_interpolation_pos(): msg = Int16MultiArray() try: msg.data = np.array(next(control.interpolation), dtype=np.int16) except StopIteration: msg.data = [0] control.reset() interpolation_pub.publish(msg)
def talker(): data_list = Int16MultiArray() pub = rospy.Publisher('Testmessage', Int16MultiArray, queue_size=1) rospy.init_node('talker', anonymous=True) while not rospy.is_shutdown(): data_list.data = [3, 10] pub.publish(data_list) time.sleep(5)