[-0.017 ,0.046 , 0.017 ,0.999 ], [-0.017 ,0.045 , 0.016 ,0.999 ], [-0.016 ,0.045 , 0.017 ,0.999 ], [-0.016 ,0.043 , 0.017 ,0.999 ], [-0.016 ,0.042 , 0.017 ,0.999 ], [-0.016 ,0.042 , 0.017 ,0.999 ], [-0.016 ,0.041 , 0.018 ,0.999 ], [-0.015 ,0.042 , 0.019 ,0.999 ], [-0.015 ,0.043 , 0.019 ,0.999 ], [-0.014 ,0.043 , 0.019 ,0.999 ], [-0.014 ,0.044 , 0.019 ,0.999 ], [-0.013 ,0.043 , 0.018 ,0.999 ], [-0.012 ,0.043 , 0.018 ,0.999 ], [-0.011 ,0.043 , 0.017 ,0.999 ], [-0.010 ,0.042 , 0.016 ,0.999 ] ] last_data = Float32MultiArray() last_data.data.extend([0.0, 0.0, 0.0, 1.0]) started = False pub = rospy.Publisher('kumara/head/pose/mbed', Float32MultiArray, queue_size=1000) def listener(): global started, pub, last_data rospy.init_node('fuvkyou', anonymous=False) for i in pop: last_data.data = i pub.publish(last_data) print(last_data)
def move_arm(self, position): rospy.loginfo("[Decon] Starting to position arm...") request = Float32MultiArray() request.data = position self.arm_command_topic.publish(request)
def __init__(self): parser = argparse.ArgumentParser() parser.add_argument( '--cfg', type=str, default='/module/src/rectifier/rectifier/cfg/yolov3.cfg', help='*.cfg path') parser.add_argument('--names', type=str, default='data/coco.names', help='*.names path') parser.add_argument( '--weights', type=str, default='/module/src/rectifier/rectifier/weights/yolov3.pt', help='weights path') parser.add_argument('--source', type=str, default='data/samples', help='source') parser.add_argument('--output', type=str, default='output', help='output folder') # output folder parser.add_argument('--img-size', type=int, default=416, help='inference size (pixels)') parser.add_argument('--conf-thres', type=float, default=0.3, help='object confidence threshold') parser.add_argument('--iou-thres', type=float, default=0.6, help='IOU threshold for NMS') parser.add_argument('--fourcc', type=str, default='mp4v', help='output video codec (verify ffmpeg support)') parser.add_argument('--half', action='store_true', help='half precision FP16 inference') parser.add_argument('--device', default='0', help='device id (i.e. 0 or 0,1) or cpu') parser.add_argument('--view-img', action='store_true', help='display results') parser.add_argument('--save-txt', action='store_true', help='save results to *.txt') parser.add_argument('--classes', nargs='+', type=int, help='filter by class') parser.add_argument('--agnostic-nms', action='store_true', help='class-agnostic NMS') self.opt = parser.parse_args() # self.opt = args_ print(self.opt) # (320, 192) or (416, 256) or (608, 352) for (height, width) img_size = (320, 192) if ONNX_EXPORT else self.opt.img_size out, source, weights, self.half, view_img, save_txt = self.opt.output, self.opt.source, self.opt.weights, self.opt.half, self.opt.view_img, self.opt.save_txt self.device = select_device( device='cpu' if ONNX_EXPORT else self.opt.device) self.model = Darknet(self.opt.cfg, img_size) self.model.load_state_dict( torch.load(weights, map_location=self.device)['model']) # Second-stage classifier self.classify = False if self.classify: modelc = load_classifier(name='resnet101', n=2) # initialize modelc.load_state_dict( torch.load('weights/resnet101.pt', map_location=device)['model']) # load weights modelc.to(self.device).eval() # Eval mode self.model.to(self.device).eval() self.tl_bbox = Float32MultiArray() self.arry_size = 6 super().__init__('talker') self._pub = self.create_publisher(Float32MultiArray, '/tl_bbox_info', 10) self._cv_bridge = CvBridge() super().__init__('rectifier') self._sub = self.create_subscription( Image, '/snowball/perception/traffic_light/cropped_roi', self.img_callback, 10) print( "ready to process rectifier----------------------------------------------------------" )
def callback(datas): global inn global dest_x_global global dest_y_global global dest_y_stair global total_g global fake_void if (inn >= 6): inn = 0 dist = np.empty((480, 640), dtype=np.uint16) imagem = np.empty((480, 640), dtype=np.uint8) k = 0 valor_medio = 0 dist_media = 0 dist_esc = 0 total = 0 begin = 0 void = 0 is_void = False stair = False initial_h = 0 valor_medio_e = 0 desv = 0 msg = rospy.Publisher('/object_detection', Float32MultiArray, queue_size=1) pub = Float32MultiArray() # Matrix of the kinect image for i in range(480): for j in range(640): dist[i, j] = ord( datas.data[2 * k]) + (ord(datas.data[2 * k + 1]) << 8) imagem[i, j] = dist[i, j] // 13.6718751 k += 1 # Method to find one end of the obstacle for k in range(480): if imagem[k][10] != 255: k -= 30 break for l in range(380): if imagem[k - 10][l] != 255 and begin == 0: begin = l if imagem[k - 10][l] == 255 and begin != 0 and void == 0: void = l + 10 is_void = True break # Identification of the restrict area if begin != 0: if is_void: for p in range(k, 100, -1): if imagem[p][void] != 255: is_void = False break # Calculate the the average distance of the region of pixels for m in range(k - 10, k): for n in range(begin, begin + 10): total += 1 valor_medio += imagem[m][n + 30] dist_media += dist[m][n + 30] dist_esc += dist[m][n] imagem[m][n] = 0 # Identification of the stair if valor_medio / total > 240: for o in range(k, 480): if imagem[o][begin] != 255: stair = True break if stair: for q in range(begin + 50, 630): if imagem[k][q] != 255: initial_h = q break for m in range(k - 10, k): for r in range(initial_h, initial_h + 10): valor_medio_e += dist[m][r] imagem[m][r] = 0 desv = (((320 - begin) * 0.00173667 * (dist_esc / total)) + ((320 - initial_h) * 0.00173667 * (valor_medio_e / total))) / 2 # Calculates the destination spot dest_x = -dist_esc / (total * 1000) + gps_x_global + 1 dest_y = desv / 1000 + gps_y_global else: if is_void: # Calculates the destination spot if gps_y_global < 0: dest_x = dist_media / (total * 1000) + gps_x_global - 0.4248 dest_y = (void - 320) * 0.00173667 * ( dist_esc / (total * 1000)) + 0.2 + gps_y_global print dest_y if dest_y > -1.8: fake_void = True if gps_y_global > 0: dest_x = -dist_media / (total * 1000) + gps_x_global + 0.4248 dest_y = (void - 320) * 0.00173667 * ( dist_esc / (total * 1000)) * (-1) - 0.2 + gps_y_global print dest_y if dest_y < 1.7: fake_void = True if dist_media / total < 2300 and dist_media / total > 1850 and not fake_void: dest_x_global += dest_x dest_y_global += dest_y total_g += 1 elif total_g != 0 and dist_media / total < 1850 and not fake_void: dest_x_global /= total_g dest_y_global /= total_g # void identificator, X destnation coordinate, Y destnation coordinate arg = (1, dest_x_global, dest_y_global) pub.data = arg msg.publish(pub) total_g = 0 dest_x_global = 0 dest_y_global = 0 if not is_void or fake_void: # Calculates the destination spot if gps_y_global < 0: dest_x = dist_media / (total * 1000) + gps_x_global - 0.3248 dest_y = (begin - 320) * 0.00173667 * ( dist_esc / (total * 1000)) - 1.0 + gps_y_global if gps_y_global > 0: dest_x = -dist_media / (total * 1000) + gps_x_global + 0.3248 dest_y = ((begin - 320) * 0.00173667 * (dist_esc / (total * 1000)) - 1.2) * (-1) + gps_y_global if dist_media / total < 2500 and dist_media / total > 2300: dest_x_global += dest_x dest_y_global += dest_y total_g += 1 elif total_g != 0 and dist_media / total < 2300: dest_x_global /= total_g dest_y_global /= total_g # obstacle identificator, X destnation coordinate, Y destnation coordinate arg = (2, dest_x_global, dest_y_global) pub.data = arg msg.publish(pub) total_g = 0 dest_x_global = 0 dest_y_global = 0
self.state = "EXIT" self.rate.sleep() #Home coordinates: [2.7955,-0.41917,0,-24.27] if __name__ == '__main__': try: home_coord = [3.2278,-0.94701,0,62] #dcontroller = DeconController([[0.4,-1.06,0,0], [0.4,-0.532,0], [0.5,-1.5,0,-90]]) #dcontroller = DeconController([[3.49093,-1.12527,0,-26.5],[2.883,-0.72133,0,65.91]]) dcontroller = DeconController([[3.2278,-0.94701,0,62]]) #[2.883,-0.72133,0,65.91]]) dcontroller.start() dcontroller.move_arm([0.0, 0.0]) ret = dcontroller.wait_for(ARM_FLAG) if ret is True: ret = False request = Float32MultiArray() request.data = home_coord dcontroller.move2next_location_command_topic.publish(request) ret = dcontroller.wait_for(LOCATION_FLAG) if ret is not True: rospy.loginfo("[Decon] Error Occured going back to home position...") rospy.signal_shutdown("[Decon] Good Bye...") except rospy.ROSInterruptException: log_string = "[Decon] Decon Controller exiting at %s!" % rospy.get_time() rospy.loginfo(log_string)
#!/usr/bin/python import rospy import numpy as np from geometry_msgs.msg import Pose from std_msgs.msg import Float32MultiArray from std_msgs.msg import Float64MultiArray # Node rospy.init_node('matlab_path', anonymous=True) path_publisher = rospy.Publisher('/matlab_topic', Float32MultiArray, queue_size=10) path_msg = Float32MultiArray() #path_msg. path_msg.data = [0.0, 3.0] """ path_msg.data = [[2.75414116625081, 2.04761128963112], [2.89518236041524, 3.61823207641913], [5.03080150303697, 6.30976383559748], [4.93753765416243, 7.30776336301881], [5.86937727776949, 8.33166766049375], [11.8932359182601, 10.0929765780663], [13.2824577304770, 12.3308383615348], [16.3616564164733, 14.8233350983323], [16.2075912367160, 17.2198548623449], [17, 17]] """
def camera_fix_publisher(): # ROS节点初始化 rospy.init_node('camera_fix_publisher', anonymous=True) cap = cv2.VideoCapture(1) # 创建一个Publisher,发布名为/camera_fix_info的topic,消息类型为std_msgs::Float32MultiArray,队列长度1 camera_fix_info_pub = rospy.Publisher('/camera_fix_info', Float32MultiArray, queue_size=1) rospy.Subscriber("/robot_motion_info", Bool, robotCallback) #设置循环的频率 rate = rospy.Rate(5) speed = 0 color = 0 dx = 0.406 # image space to robot space 60.5/149 xc = 0 last_xc = 0 yc = 0 num = 0 while not rospy.is_shutdown() & cap.isOpened(): color, xc, yc = ca.Detect(cap) # print(xc, yc, speed) if (color > 0) & (xc > 180) & (num < 5): if last_xc > 180 & num <= 3: if num == 0: speed = ((xc - last_xc) * dx / 0.2) else: speed = (speed + ((xc - last_xc) * dx / 0.2)) / 2 num = num + 1 print(num, last_xc, xc, yc, speed) if last_xc > xc: # 如果物体已经检测过,就清零 speed = 0 color = 0 xc = 0 last_xc = 0 yc = 0 num = 0 last_xc = xc if num == 4: num += 1 global Motion if (speed > 0.01) & (xc > 0) & (not Motion): array = [xc, yc, speed, color] camera_fix_info = Float32MultiArray(data=array) rospy.loginfo(camera_fix_info.data) # 发布消息 camera_fix_info_pub.publish(camera_fix_info) if (num == 5) & (xc == 0): speed = 0 color = 0 xc = 0 last_xc = 0 yc = 0 num = 0 # 按照循环频率延时 rate.sleep() cap.release() cv2.destroyAllWindows()
def update_state(msg): ''' for simulation ''' global pub_gps, gps_message, pub_imu, x, y, v, omega, theta, pub_odom #uR = -uR/ 100 #uL = -uL/ 100 update_lidar() setup_walls() uR = -msg.strboard / 100.0 uL = -msg.port / 100.0 rudder = msg.servo current_v = rospy.get_param('v_current', 0.5) current_ang = rospy.get_param('current_ang', math.pi / 2) current = Float32MultiArray() current.data = [current_v, current_ang] # Rudder Angle rudder_rate = (1894.0 - 1195.0) / 90.0 rudder_ang = (rudder - 1600.0) / rudder_rate rudder_ang = -rudder_ang / 180.0 * math.pi b_l = 4.0 # sim linear drag b_r = 2.5 # sim rotational drag I_zz = 1.0 # sim moment of inertia m = 5.0 # sim mass robot_radius = 0.5 x_cg = 0.9 w = 20.0 dt = 0.2 # update state a = (uR + uL) / m - b_l / m * v # print(a) ang_acc = -b_r / I_zz * omega + (uR + uL) * math.sin(rudder_ang) * x_cg / I_zz v = v + a * dt omega = omega + ang_acc * dt robot_vx = v * math.cos(theta) - current_v * math.cos(current_ang) robot_vy = v * math.sin(theta) - current_v * math.sin(current_ang) v_robot = np.array([robot_vx, robot_vy]) ang_course = math.atan2(v_robot[1], v_robot[0]) v_course = math.sqrt(np.dot(v_robot, v_robot)) omega = min(max(omega, -1.0), 1.0) # update position # x = x + v*math.cos(angleDiff(theta)) * dt + current_v * math.cos(angleDiff(current_ang)) * dt # y = y + v*math.sin(angleDiff(theta)) * dt + current_v * math.sin(angleDiff(current_ang))* dt x = x + robot_vx * dt y = y + robot_vy * dt theta = angleDiff(theta + omega * dt) gps_message.x = x gps_message.y = y gps_message.x_vel = robot_vx gps_message.y_vel = robot_vy gps_message.ang_course = ang_course orig_lat = rospy.get_param('originLat', 0.0) orig_lng = rospy.get_param('originLon', 0.0) latlng = XYtoGPS(orig_lat, orig_lng, x, y) gps_message.latitude = latlng[0] gps_message.longitude = latlng[1] # quat = qfe(0,0,theta) # pose.pose.position.x = x # pose.pose.position.y = y # pose.pose.position.z = 0.0 # pose.pose.orientation.x = quat[0] # pose.pose.orientation.y = quat[1] # pose.pose.orientation.z = quat[2] # pose.pose.orientation.w = quat[3] # pose.header.frame_id = "/map" pub_gps.publish(gps_message) pub_imu.publish(theta) pub_adcp.publish(current) # pub_pose.publish(pose) # print("x: ", x) # print("y: ", y) # print("theta: ", theta) # print("xvel: ", robot_vx) # print("yvel: ", robot_vy) # print("ang_course: ", ang_course) # print("current_v", current_v) # print("current_ang", current_ang) # odom message odom_msg = Odometry() odom_msg.pose.pose.position.x = x odom_msg.pose.pose.position.y = y quat = tf.transformations.quaternion_from_euler(0, 0, theta) odom_msg.pose.pose.orientation.x = quat[0] odom_msg.pose.pose.orientation.y = quat[1] odom_msg.pose.pose.orientation.z = quat[2] odom_msg.pose.pose.orientation.w = quat[3] odom_msg.header.frame_id = 'map' pub_odom.publish(odom_msg) # print(state.v) # print(state.theta) print("x, y:", x, y) print('current: ', current.data[1])
#! /usr/bin/env python import rospy import math import time from std_msgs.msg import Float32MultiArray, MultiArrayLayout # Create node rospy.init_node('test_tigrillo', anonymous=True) pub = rospy.Publisher('tigrillo/legs_cmd', Float32MultiArray, queue_size=1) # In a loop, send a actuation signal t = 0 while True: val = [-math.pi/8 * math.sin(0.01*t), math.pi/8 * math.sin(0.01*t), \ math.pi/8 * math.sin(0.01*t), -math.pi/8 * math.sin(0.01*t)] rospy.loginfo("Publishing in tigrillo/legs_cmd topic: " + str(val)) pub.publish(Float32MultiArray(layout=MultiArrayLayout([], 1), data=val)) time.sleep(0.001) t += 1 if t > 20000: exit()
def servo_to_tag(self, pose_goal, goal_error=[0.03, 0.03, 0.1], initial_ar_pose=None): lost_tag_thresh = 0.6 #0.4 # TODO REMOVE err_pub = rospy.Publisher("servo_err", Float32MultiArray) if False: self.test_move() return "aborted" ####################### goal_ar_pose = homo_mat_from_2d(*pose_goal) rate = 20. kf_x = ServoKalmanFilter(delta_t=1. / rate) kf_y = ServoKalmanFilter(delta_t=1. / rate) kf_r = ServoKalmanFilter(delta_t=1. / rate) if initial_ar_pose is not None: ar_err = homo_mat_to_2d( homo_mat_from_2d(*initial_ar_pose) * goal_ar_pose**-1) kf_x.update(ar_err[0]) kf_y.update(ar_err[1]) kf_r.update(ar_err[2]) print "initial_ar_pose", initial_ar_pose pid_x = PIDController(k_p=0.5, rate=rate, saturation=0.05) pid_y = PIDController(k_p=0.5, rate=rate, saturation=0.05) pid_r = PIDController(k_p=0.5, rate=rate, saturation=0.08) r = rospy.Rate(rate) while True: if rospy.is_shutdown(): self.base_pub.publish(Twist()) return 'aborted' if self.preempt_requested: self.preempt_requested = False self.base_pub.publish(Twist()) return 'preempted' goal_mkr = create_base_marker(goal_ar_pose, 0, (0., 1., 0.)) self.mkr_pub.publish(goal_mkr) if self.cur_ar_pose is not None: # make sure we have a new observation new_obs = self.ar_pose_updated self.ar_pose_updated = False # find the error between the AR tag and goal pose print "self.cur_ar_pose", self.cur_ar_pose cur_ar_pose_2d = homo_mat_from_2d( *homo_mat_to_2d(self.cur_ar_pose)) print "cur_ar_pose_2d", cur_ar_pose_2d ar_mkr = create_base_marker(cur_ar_pose_2d, 1, (1., 0., 0.)) self.mkr_pub.publish(ar_mkr) ar_err = homo_mat_to_2d(cur_ar_pose_2d * goal_ar_pose**-1) print "ar_err", ar_err print "goal_ar_pose", goal_ar_pose # filter this error using a Kalman filter x_filt_err, x_filt_cov, x_unreli = kf_x.update(ar_err[0], new_obs=new_obs) y_filt_err, y_filt_cov, y_unreli = kf_y.update(ar_err[1], new_obs=new_obs) r_filt_err, r_filt_cov, r_unreli = kf_r.update(ar_err[2], new_obs=new_obs) if np.any( np.array([x_unreli, y_unreli, r_unreli]) > [lost_tag_thresh] * 3): self.base_pub.publish(Twist()) return 'lost_tag' print "Noise:", x_unreli, y_unreli, r_unreli # TODO REMOVE ma = Float32MultiArray() ma.data = [ x_filt_err[0, 0], x_filt_err[1, 0], ar_err[0], x_unreli, y_unreli, r_unreli ] err_pub.publish(ma) print "xerr" print x_filt_err print x_filt_cov print "Cov", x_filt_cov[0, 0], y_filt_cov[0, 0], r_filt_cov[0, 0] x_ctrl = pid_x.update_state(x_filt_err[0, 0]) y_ctrl = pid_y.update_state(y_filt_err[0, 0]) r_ctrl = pid_r.update_state(r_filt_err[0, 0]) base_twist = Twist() base_twist.linear.x = x_ctrl base_twist.linear.y = y_ctrl base_twist.angular.z = r_ctrl cur_filt_err = np.array( [x_filt_err[0, 0], y_filt_err[0, 0], r_filt_err[0, 0]]) print "err", ar_err print "Err filt", cur_filt_err print "Twist:", base_twist if np.all(np.fabs(cur_filt_err) < goal_error): self.base_pub.publish(Twist()) return 'succeeded' self.base_pub.publish(base_twist) r.sleep()
def detect(input_image, save_img=True): out, source, weights, view_img, save_txt, imgsz = \ opt.output, opt.source, opt.weights, opt.view_img, opt.save_txt, opt.img_size global frame_num, model global pubCentBlem, pubCentUnBlem # Initialize #device = torch_utils.select_device(opt.device) #if os.path.exists(out): # shutil.rmtree(out) # delete output folder #os.makedirs(out) # make new output folder half = device.type != 'cpu' # half precision only supported on CUDA if half: model.half() # to FP16 # Get names and colors names = model.module.names if hasattr(model, 'module') else model.names print("class names array ", names) # names = ['blemished', 'unblemished', 'glove', 'belt', 'bin', 'head'] colors = [[random.randint(0, 255) for _ in range(3)] for _ in range(len(names))] # Run inference t0 = time.time() img = torch.zeros((1, 3, imgsz, imgsz), device=device) # init img _ = model(img.half() if half else img ) if device.type != 'cpu' else None # run once bounding_boxes_all_images = [] img0 = input_image.astype('float32') #img0 = cv2.resize(img0, (640,480), interpolation = cv2.INTER_AREA) img = letterbox(img0, new_shape=imgsz)[0] # Convert img = img[:, :, ::-1].transpose(2, 0, 1) # BGR to RGB, to 3x416x416 img = np.ascontiguousarray(img) dataset = [("frame_num" + str(frame_num) + '.jpg', img, img0, None)] for path, img, im0s, vid_cap in dataset: img = torch.from_numpy(img).to(device) img = img.half() if half else img.float() # uint8 to fp16/32 img /= 255.0 # 0 - 255 to 0.0 - 1.0 if img.ndimension() == 3: img = img.unsqueeze(0) # Inference t1 = torch_utils.time_synchronized() pred = model(img, augment=opt.augment)[0] # pred = model(img)[0] t2 = torch_utils.time_synchronized() # Apply NMS pred = non_max_suppression(pred, opt.conf_thres, opt.iou_thres, classes=opt.classes, agnostic=opt.agnostic_nms) t3 = torch_utils.time_synchronized() # needed specific to each image Loc2arrCent = {} arrCentBlem = [] arrCentUnBlem = [] Loc2Cls = {} # Process detections for i, det in enumerate(pred): # detections per image p, s, im0 = path, '', im0s save_path = str(Path(out) / Path(p).name) s += '%gx%g ' % img.shape[2:] # print string gn = torch.tensor(im0.shape)[[1, 0, 1, 0]] # normalization gain whwh bounding_boxes = {} if det is not None and len(det): # Rescale boxes from img_size to im0 size det[:, :4] = scale_coords(img.shape[2:], det[:, :4], im0.shape).round() # Print results for c in det[:, -1].unique(): n = (det[:, -1] == c).sum() # detections per class s += '%g %ss, ' % (n, names[int(c)]) # add to string # Write results minx = 5000 miny = 5000 maxx = 0 maxy = 0 box_num = 0 bounding_boxes = {} for *xyxy, conf, cls in det: box_num += 1 xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist() # normalized xywh #print("Box num:", box_num, " label: ",names[int(cls)], " xyxy: ", int(xyxy[0]), int(xyxy[1]), int(xyxy[2]), int(xyxy[3]), "xywh: ", float(xywh[0]), float(xywh[1]), float(xywh[2]), float(xywh[3])) if (bounding_boxes.get(names[int(cls)], None) == None): bounding_boxes[names[int(cls)]] = [[ int(xyxy[0]), int(xyxy[1]), abs(int(xyxy[2]) - int(xyxy[0])), abs(int(xyxy[3]) - int(xyxy[1])) ]] else: bounding_boxes[names[int(cls)]].append([ int(xyxy[0]), int(xyxy[1]), abs(int(xyxy[2]) - int(xyxy[0])), abs(int(xyxy[3]) - int(xyxy[1])) ]) tlx, tly, brx, bry = int(xyxy[0]), int(xyxy[1]), int( xyxy[2]), int(xyxy[3]) if tlx < minx: minx = tlx if tly < miny: miny = tly if bry > maxy: maxy = bry if brx > maxx: maxx = brx #crop_img = img[y:y+h, x:x+w] if save_txt: # Write to file xywh = (xyxy2xywh(torch.tensor(xyxy).view(1, 4)) / gn).view(-1).tolist() # normalized xywh with open(save_path[:save_path.rfind('.')] + '.txt', 'a') as file: file.write(('%g ' * 5 + '\n') % (cls, *xywh)) # label format if save_img or view_img: # Add bbox to image label = '%s %.2f' % (names[int(cls)], conf) # print("label input to plot_one_box ",label) # plot_one_box(xyxy, im0, label=label, color=colors[int(cls)], line_thickness=3) plot_one_box(xyxy, im0, label=label, color=colors[int(cls)], line_thickness=1) # append onion centroid to list of centroids if names[int(cls)] == 'blemished' or names[int( cls)] == 'unblemished': centx = (tlx + brx) / 2 centy = (tly + bry) / 2 if centx > 250: # FOR CAMERA LOOKING FROM FRONT # if center is on the belt if names[int(cls)] == 'blemished': Loc2Cls[centx] = 0 else: Loc2Cls[centx] = 1 Loc2arrCent[centx] = (centx, centy) if names[int( cls )] == 'blemished' and centx not in arrCentBlem: arrCentBlem.append(centx) arrCentBlem.append(centy) else: if centx not in arrCentUnBlem: arrCentUnBlem.append(centx) arrCentUnBlem.append(centy) ############################################### all bounding boxes for this frames: "bounding_boxes" ######################################################################################################## bounding_boxes_all_images.append(bounding_boxes) print("Frame number = ", frame_num, "Bounding boxes: ", bounding_boxes) frame_num += 1 # Print time (inference + NMS) print('%s Inference. (%.3fs)' % (s, t2 - t1)) print('%s NMS. (%.3fs)' % (s, t3 - t2)) #print('im0.shape before',im0.shape) ############################################### cropping #im0 = im0[miny:maxy, minx:maxx] #print('im0.shape after',im0.shape) # Stream results view_img = True if view_img: #cv2_imshow( im0) not_showing = True #cv2.imshow(p, im0) if cv2.waitKey(1) == ord('q'): # q to quit raise StopIteration # Save results (image with detections) save_img = True if save_img: #'src/beginner_tutorials/scripts/yolov5/inference/output' #save_path = '/home/psuresh/src/beginner_tutorials/scripts/yolov5/inference/output/frame1.jpg' cv2.imwrite(save_path, im0) #if dataset.mode == 'images': ########################################################## comment this line to avoid saving images ######################################################################################################## # publish centroids for current image print("publishing arrCentBlem ", arrCentBlem) print("publishing arrCentUnBlem ", arrCentUnBlem) pubCentBlem.publish(Float32MultiArray(data=arrCentBlem)) pubCentUnBlem.publish(Float32MultiArray(data=arrCentUnBlem)) # make array LocOrderedPreds LocOrderedPreds = list( OrderedDict(sorted(Loc2Cls.items(), key=lambda t: t[0])).values()) print("publishing LocOrderedPreds ", LocOrderedPreds) pubLocOrderedPreds.publish(Int32MultiArray(data=LocOrderedPreds)) arrCentxyOrd = list( OrderedDict(sorted(Loc2arrCent.items(), key=lambda t: t[0])).values()) print("arrCentxyOrd ", arrCentxyOrd) arrCentLocOrd = [] for i in range(len(arrCentxyOrd)): arrCentLocOrd.append(arrCentxyOrd[i][0]) arrCentLocOrd.append(arrCentxyOrd[i][1]) print("publishing arrCentLocOrd ", arrCentLocOrd) pubCentLocOrd.publish(Float32MultiArray(data=arrCentLocOrd)) #global img_processed #img_processed = True if save_txt or save_img: print('Results saved to %s' % os.getcwd() + os.sep + out) if platform == 'darwin': # MacOS os.system('open ' + save_path) print('Done. (%.3fs)' % (time.time() - t0)) return bounding_boxes_all_images
def __init__(self, dim=2, udp_ip='127.0.0.1', udp_recv_port=8026, udp_send_port=6001, buffer_size=8192 * 4): RosProcessingComm.__init__(self, udp_ip=udp_ip, udp_recv_port=udp_recv_port, udp_send_port=udp_send_port, buffer_size=buffer_size) if rospy.has_param('framerate'): self.frame_rate = rospy.get_param('framerate') else: self.frame_rate = 60.0 self.period = rospy.Duration(1.0 / self.frame_rate) self.running = True self.runningCV = threading.Condition() self.rate = rospy.Rate(self.frame_rate) self.lock = threading.Lock() rospy.Subscriber('joy', Joy, self.joyCB) self.human_control_pub = None self.human_robot_pose_pub = None self.initializePublishers() self.dim = dim self.num_goals = 2 assert (self.num_goals > 0) self.goal_positions = npa([[0] * self.dim] * self.num_goals, dtype='f') if rospy.has_param('max_cart_vel'): self._max_cart_vel = np.array(rospy.get_param('max_cart_vel')) else: self._max_cart_vel = 0.25 * np.ones(self.dim) rospy.logwarn( 'No rosparam for max_cart_vel found...Defaulting to max linear velocity of 50 cm/s and max rotational velocity of 50 degrees/s' ) if rospy.has_param('width'): self.width = rospy.get_param('width') else: self.width = 1200 if rospy.has_param('height'): self.height = rospy.get_param('height') else: self.height = 900 self.user_vel = CartVelCmd() _dim = [MultiArrayDimension()] _dim[0].label = 'cartesian_velocity' _dim[0].size = 2 _dim[0].stride = 2 self.user_vel.velocity.layout.dim = _dim self.user_vel.velocity.data = np.zeros(self.dim) self.user_vel.header.stamp = rospy.Time.now() self.user_vel.header.frame_id = 'human_control' self.human_robot_pose = np.zeros(self.dim) #UNUSED for the time being. self.human_robot_pose_msg = Point() self.getRobotPosition() self.human_goal_pose = Float32MultiArray() self.human_goal_pose.data = [ list(x) for x in list(self.goal_positions) ] self.human_goal_pose_pub.publish(self.human_goal_pose) self.data = CartVelCmd() self._msg_dim = [MultiArrayDimension()] self._msg_dim[0].label = 'cartesian_velocity' self._msg_dim[0].size = 2 self._msg_dim[0].stride = 2 self.data.velocity.layout.dim = _dim self.data.velocity.data = np.zeros(self.dim) self.data.header.stamp = rospy.Time.now() self.data.header.frame_id = 'human_control' rospy.Service("point_robot_human_control/set_human_goals", GoalPoses, self.set_human_goals) self.send_thread = threading.Thread(target=self._publish_command, args=(self.period, )) self.send_thread.start() rospy.loginfo("END OF CONSTRUCTOR - point_robot_human_control_node")
kiA = 25 kdA = 0.004 kpB = 0.42 #Antes 0.2 kiB = 25 kdB = 0.004 # Acumulacion de error para integrador integrandA = [] integrandB = [] # Velocidades de referencia de las ruedas desiredSpeedA = 0 desiredSpeedB = 0 # Velocidades actual de las ruedas currentSpeedA = 0 currentSpeedB = 0 # Instancia para reutilizar de msnaje a topico de velocidad actual messageCurrentSpeedPublisher = Float32MultiArray() # Variable de saturacion maxima de ciclo util dutyCycleSaturation = 70 # Ultimo error de la señal de velocidad lastErrorA = 0 lastErrorB = 0 def setPins(): global pwmDriverA1, pwmDriverA2, pwmDriverB1, pwmDriverB2 # Configurandp estructura de pins de raspberry GPIO.setmode(GPIO.BOARD) # Configurando los pines de salida para el driver GPIO.setup(pinDriverA1, GPIO.OUT) GPIO.setup(pinDriverA2, GPIO.OUT) GPIO.setup(pinDriverB1, GPIO.OUT)
def __init__(self, args): pubs = { "/homeostasis_motor": [ Float32MultiArray, ], "/lpzros/x": [Float32MultiArray], "/lpzros/xsi": [ Float32MultiArray, ], "/lpzros/EE": [ Float32, ], } subs = { "/tinyImu": [tinyIMU, self.cb_imu], } # gather arguments and transfer modes to integer representation self.mode = LPZRos.modes[args.mode] self.control_mode = LPZRos.control_modes[args.control_mode] self.numtimesteps = args.numtimesteps self.loop_time = args.looptime self.automaticMode = args.automaticMode self.verbose = args.verbose self.save_pickle = args.save_pickle self.period = args.period smp_thread_ros.__init__(self, loop_time=self.loop_time, pubs=pubs, subs=subs) # initialize counters self.cnt = 0 self.cb_imu_cnt = 0 ############################################################ # model + meta params # self.numsen_raw = 11 # 5 # 2 self.numsen = 6 # 5 # 2 self.nummot = 4 # introduce 2 ghost variables self.imu_lin_acc_gain = 0 # 1e-3 self.imu_gyrosco_gain = 1 / 5000. self.imu_orienta_gain = 0 # 1e-1 self.linear_gain = 0.0 # 1e-1 self.output_gain = 32000 # 5000 # velocitycontrol self.msg_inputs = Float32MultiArray() self.msg_xsi = Float32MultiArray() self.msg_xsi.data = [0] * self.numsen self.msg_motors = Float32MultiArray() self.msg_motors.data = [0] * self.nummot #self.msg_sensor_exp = Float64MultiArray() # sphero lag is 4 timesteps self.lag = 4 # 2 tested on puppy not so much influence # buffer size accomodates causal minimum 1 + lag time steps self.bufsize = 1 + self.lag # 2 self.creativity = args.creativity # self.epsA = 0.1 # self.epsA = 0.02 self.epsA = args.epsA # self.epsC = 0.001 #self.epsC = 0.01 #self.epsC = 0.9 self.epsC = args.epsC # sampling scale for automaticMode self.automaticModeScaleEpsA = 0.5 self.automaticModeScaleEpsC = 0.5 # initialize for logging self.hz = 0 if self.automaticMode: if self.control_mode == 2: self.period = np.random.randint(1, 20) * 2 elif self.control_mode == 3: self.period = 0 else: self.epsA = np.random.exponential( scale=self.automaticModeScaleEpsA, size=1)[0] self.epsC = np.random.exponential( scale=self.automaticModeScaleEpsC, size=1)[0] print "EpsA:\t%f\nEpsC:\t%f\nCreativity:\t%f\nEpisodelength:\t%d\nLag:\t%d" % ( self.epsA, self.epsC, self.creativity, self.numtimesteps, self.lag) ############################################################ # forward model self.A = np.random.uniform(-1e-1, 1e-1, (self.numsen, self.nummot)) self.b = np.zeros((self.numsen, 1)) print "initial A" print self.A # controller self.C = np.random.uniform(-1e-1, 1e-1, (self.nummot, self.numsen)) print "initial C" print self.C self.h = np.zeros((self.nummot, 1)) self.g = np.tanh # sigmoidal activation function self.g_ = dtanh # derivative of sigmoidal activation function # state self.x = np.ones((self.numsen, self.bufsize)) self.y = np.zeros((self.nummot, self.bufsize)) self.z = np.zeros((self.numsen, 1)) # auxiliary variables self.L = np.zeros((self.numsen, self.nummot)) self.v_avg = np.zeros((self.numsen, 1)) self.xsi = np.zeros((self.numsen, 1)) self.xsiAvg = 0 self.xsiAvgSmooth = 0.01 self.imu = tinyIMU() self.imu_vec = np.zeros((3 + 3, 1)) self.imu_smooth = 0.5 # coef self.motorSmooth = 0.5 warnings.filterwarnings("error") np.seterr(all='print') self.exceptionCounter = 0 self.maxExceptionCounter = 10 if self.control_mode == 3: self.epsA = 0 self.epsC = 0 self.sin_phase = 0 self.sin_speed = 0.1 # If something is changed in the variableDict style or content, change dataversion! # Version History: # 1: Initial Commit # 2: xsi was not saved correctly before. # 3: lag and automaticMode + scales included # 4: exceptionCounter added # 5: exceptionCounter working :) forgot to increase versionnumber and wasn't working good # 6: added file id (when there are multiple results with the same eps and c) and filename # 7: added weight_in_body should be used to indicate if the battery is built in or not. For all measurements before, it was true # 8: added control_mode variable 1 = position control, 2 = velocity control, 3= step, also period for step period # 9: added hz of swinging motion in new control mode 4 = sin_sweep self.variableDict = { "dataversion": 9, "control_mode": self.control_mode, "timesteps": self.numtimesteps, "looptime": self.loop_time, "startTime": time.time(), "endTime": 0, "numsen": self.numsen, "nummot": self.nummot, "epsA": self.epsA, "epsC": self.epsC, "creativity": self.creativity, "imuSmooth": self.imu_smooth, "motorSmooth": self.motorSmooth, "lag": self.lag, "period": self.period, "hz": self.hz, "automaticMode": self.automaticMode, "automaticModeScaleEpsA": self.automaticModeScaleEpsA, "automaticModeScaleEpsC": self.automaticModeScaleEpsC, "exceptionCounter": 0, "id": 0, "filename": 0, "weight_in_body": False, "C": np.zeros((self.numtimesteps, ) + self.C.shape), "A": np.zeros((self.numtimesteps, ) + self.A.shape), "h": np.zeros((self.numtimesteps, ) + self.h.shape), "b": np.zeros((self.numtimesteps, ) + self.b.shape), "x": np.zeros((self.numtimesteps, ) + self.x.shape), "y": np.zeros((self.numtimesteps, ) + self.y.shape), "xsi": np.zeros((self.numtimesteps, ) + self.xsi.shape), }
def draw(self): self.screen.fill(pygame.Color('grey')) xAndYArray = [] prev_distance = 0 max_point_difference = 0 noise_index_array = [] temp_index_array = [] angle_started = False temp_angle = self.angle_min for i in range(len(self.ranges)): #if i in noise_index_array: #color = pygame.Color('red') #else: #color = pygame.Color('blue') self.current_angle = i*self.angle_increment + self.angle_min if(not math.isnan(self.ranges[i])): x = -int(self.ranges[i]*math.sin(self.current_angle)*self.scaling) y = -int(self.ranges[i]*math.cos(self.current_angle)*self.scaling) if(abs(x) > + self.tolerance and abs(y) > self.tolerance): xAndYArray.append([x + self.model.width/2, y + self.model.height/2]) pygame.draw.circle(self.screen, pygame.Color('blue'), (x + self.model.width/2,y + self.model.height/2), 2) #print self.heading_x if(len(xAndYArray) > 0): rdp_array = dpa.rdp(xAndYArray, 2) #print rdp_array line_array = [] for i in range(len(rdp_array)): #print rdp_array[i] x = rdp_array[i][0] y = rdp_array[i][1] pygame.draw.circle(self.screen, pygame.Color('green'), (x,y), 2) for j in range(i, len(rdp_array)): x1 = rdp_array[i][0] y1 = rdp_array[i][1] x2 = rdp_array[j][0] y2 = rdp_array[j][1] length = dpa.distance((x1, y1),(x2, y2)) if((x2 -x1) != 0 and length < (10*self.scaling)and length > (.5*self.scaling)): slope = (y2-y1)/(x2-x1) line_array.append(Line(x1, y1, x2, y2, self.get_angle((x1, y1), (x2, y2)))) #for i in renage(len(slope_array)): for i, line in enumerate(line_array): #line.reset_dots() for j, point in enumerate(xAndYArray): distance_to_line = dpa.point_line_distance((point[0], point[1]), (line.x1, line.y1), (line.x2, line.y2)) distance_to_start = dpa.distance((point[0], point[1]),(line.x1, line.y1)) if (distance_to_line < .14*self.scaling and distance_to_line > 0): #print distance_to_line line.number_of_dots += 1 line.total_distance_to_start += distance_to_start #print line.number_of_dots line_distance = dpa.distance((line.x1, line.y1), (line.x2, line.y2)) line.line_distance = line_distance if(line.number_of_dots > 5 and line_distance/line.number_of_dots < 1.5): #(line.number_of_dots > 30 and line.total_distance_to_start/line.number_of_dots < 1.5*self.scaling): pygame.draw.line(self.screen, pygame.Color('red'), (line.x1, line.y1), (line.x2, line.y2), 2) line.is_good_line = True number_of_lines = 0 total = 0 for i, line, in enumerate(line_array): if line.is_good_line: if line.number_of_dots > 0: angle = line.angle while (angle < 0): angle += math.pi while (angle > math.pi): angle -= math.pi total += angle*line.line_distance#/line.number_of_dots #print angle*180.0/math.pi, line.line_distance number_of_lines += 1*line.line_distance#/line.number_of_dots #print angle #print 'start' average = 0 if (number_of_lines != 0): average = total/number_of_lines point3 = ((self.center[0] - math.cos(average)*100), (self.center[1] - math.sin(average)*100)) pygame.draw.line(self.screen, pygame.Color('green'), self.center, point3, 5) if (average != 0): if(self.cycle > 5): total_average = self.total_cycle/6 if total_average < 0: total_average += math.pi self.turn_direction = total_average - math.pi/2 self.current_go_heading = self.turn_direction - self.heading self.turn_array = [0]*11 #print self.turn_direction*7/(math.pi/6) index = int(self.turn_direction*7/(math.pi/6)) if index < -4: index = -5 if index > 4: index = 5 self.turn_array[5 + index] = .01 msg = Float32MultiArray() self.vel_array = [0]*11 total_array = self.vel_array total_array.extend(self.turn_array) msg.data = total_array self.pub.publish(msg) print self.turn_array self.heading_point = ((self.center[0] - math.cos(total_average)*100), (self.center[1] - math.sin(total_average)*100)) self.cycle = 0 self.total_cycle = 0 else: self.cycle += 1 self.total_cycle += average pygame.draw.line(self.screen, pygame.Color('black'), self.center, self.heading_point, 5) pygame.display.update()
#!/usr/bin/env python import rospy from dynamic_reconfigure.server import Server from arm_mobility.cfg import armGainConfig from std_msgs.msg import Float32MultiArray outMsg = Float32MultiArray() def callback(config, level): rospy.loginfo("""Reconfigure Request: Kp {Kp} Ki {Ki} Kd {Kd} Kii {Kii} """.format(**config)) data =[config.Kp,config.Ki, config.Kd, config.Kii] rospy.loginfo("Gains Changed") rospy.loginfo(data) outMsg.data = data; pub.publish(outMsg) return config if __name__ == "__main__": rospy.init_node("arm_dyn_server", anonymous = True) pub = rospy.Publisher('arm_conf_mssg', Float32MultiArray, queue_size=10) srv = Server(armGainConfig, callback) rospy.spin()
global open_paths open_paths = paths.data def scan_callback(msg): global scan scan = msg.data def handle_path_data(req): # [self.aim_steer, self.aim_dist, self.steer_ang, self.speed] global open_window try: window = get_boundaries(req.data[0]) except IndexError: window = (0,0,0,0,0,0) open_window.data = window pub.publish(open_window) return PathDataResponse(window) if __name__ == '__main__': open_paths = [] scan = [] edge = 12 # minimum sudden change in distance to be considered an edge open_window = Float32MultiArray() rospy.init_node('window_publisher') rospy.Subscriber('/open_paths', Float32MultiArray, open_paths_callback) rospy.Subscriber('/scan_transformed', Float32MultiArray, scan_callback) pub = rospy.Publisher('/open_window', Float32MultiArray, queue_size=1) s = rospy.Service('open_window', PathData, handle_path_data) rospy.spin()
epsilon_decay = 0.99 epsilon_min = 0.04 rospy.init_node('ddpg_classes') pub_result = rospy.Publisher('result', Float32MultiArray, queue_size=5) pub_get_action = rospy.Publisher('get_action', Float32MultiArray, queue_size=5) train_indicator = False sess = tf.Session() backend.set_session(sess) actor = Actor(sess, state_dim, TAU, LRA) critic = Critic(sess, state_dim, TAU, LRC) result = Float32MultiArray() get_action = Float32MultiArray() past_action = np.zeros((1, 2)) if load_weight == True: print("Now we load the weight") try: actor.model.load_weights("actormodel.h5") critic.model.load_weights("criticmodel.h5") actor.target_model.load_weights("actormodel.h5") critic.target_model.load_weights("criticmodel.h5") print("Weight load successfully") except: print("Cannot find the weight") env = Env(2)
def array_publish(data): # fonksiyon içinde fonksiyon tanımlandı array = Float32MultiArray(data=data) cmd.publish(array) # dizi elemanları tek tek paylaşılır
def __init__(self, conf={}): """SpheroSys.__init__ Arguments: - conf: configuration dictionary -- mass: point _mass_ -- sysdim: dimension of system, usually 1,2, or 3D -- statedim: 1d pointmass has 3 variables (pos, vel, acc) in this model, so sysdim * 3 -- dt: integration time step -- x0: initial state -- order: NOT IMPLEMENT (control mode of the system, order = 0 kinematic, 1 velocity, 2 force) """ SMPSys.__init__(self, conf) # state is (pos, vel, acc).T # self.state_dim if not hasattr(self, 'x0'): self.x0 = np.zeros((self.dim_s_proprio, 1)) self.x = self.x0.copy() self.cnt = 0 if not self.ros: import sys print( "ROS not configured but this robot (%s) requires ROS, exiting" % (self.__class__.__name__)) sys.exit(1) # self.pubs = { # 'motors': rospy.Publisher( # "/lpzbarrel/motors", Float64MultiArray, queue_size = 2), # 'sensors': rospy.Publisher( # "/lpzbarrel/x", Float32MultiArray, queue_size = 2) # } # self.subs = { # 'sensors': rospy.Subscriber("/lpzbarrel/sensors", Float64MultiArray, self.cb_sensors), # } self.pubs = { '_cmd_vel': rospy.Publisher("/cmd_vel", Twist, queue_size=2), '_cmd_vel_raw': rospy.Publisher("/cmd_vel_raw", Twist, queue_size=2), '_cmd_raw_motors': rospy.Publisher("/cmd_raw_motors", Float32MultiArray, queue_size=2), '_set_color': rospy.Publisher("/set_color", ColorRGBA, queue_size=2), '_lpzros_x': rospy.Publisher("/lpzros/x", Float32MultiArray, queue_size=2), '_set_stab': rospy.Publisher('disable_stabilization', Bool, queue_size=2), } # self.cb = { # "/imu": get_cb_dict(self.cb_imu), # "/odom": get_cb_dict(self.cb_odom), # } self.subs = { 'imu': rospy.Subscriber("/imu", Imu, self.cb_imu), 'odom': rospy.Subscriber("/odom", Odometry, self.cb_odom), } # timing self.rate = rospy.Rate(1 / self.dt) self.cb_imu_cnt = 0 self.cb_odom_cnt = 0 # custom self.numsen_raw = 10 # 8 # 5 # 2 self.numsen = 10 # 8 # 5 # 2 self.imu = Imu() self.odom = Odometry() # sphero color self.color = ColorRGBA() self.motors = Twist() self.raw_motors = Float32MultiArray() self.raw_motors.data = [0.0 for i in range(4)] self.msg_inputs = Float32MultiArray() self.msg_motors = Float64MultiArray() self.msg_sensor_exp = Float64MultiArray() self.imu_vec = np.zeros((3 + 3 + 3, 1)) self.imu_smooth = 0.8 # coef self.imu_lin_acc_gain = 0 # 1e-1 self.imu_gyrosco_gain = 1e-1 self.imu_orienta_gain = 0 # 1e-1 self.linear_gain = 1.0 # 1e-1 self.pos_gain = 0 # 1e-2 self.angular_gain = 360.0 # 1e-1 self.output_gain = 255 # 120 # 120 # sphero lag is 4 timesteps self.lag = 1 # 2 # enable stabilization """$ rostopic pub /disable_stabilization std_msgs/Bool False""" stab = Bool() stab.data = False print("stab", stab, stab.data) for i in range(5): self.pubs['_set_stab'].publish(stab)
def callback(data): list_obj = listobj() list_obj.single_obj_info = [] brocili_list = broclist() brocili_list.broc_uv_list = [] temp_list_broc = [] global temp_single_obj #print(data.objects_vector[0].id) # print(data.objects_vector[0].classname) # print(data.objects_vector[0].score) #print("---"*20) single_obj = sglobj() #print("single_obj",single_obj) single_obj.element_data_temp = [] single_obj.classname = [] single_obj.score = [] get_msg = data.objects_vector[0] bridge = CvBridge() cv_image = bridge.imgmsg_to_cv2(data.rgb_img, "bgr8") cv_depth_image = bridge.imgmsg_to_cv2(data.depth_img, "passthrough") rgb_img_to_pos = cv_image.copy() depth_img_to_pos = cv_depth_image.copy() plate_score = [0, 0] pan_score = [0] vegetablebowl_score = [0] broccoli_score = [0, 0, 0] souppothandle_score = [0] panhandle_score = [0] beef_score = [0] nethandle_score = [0] seasoningbottle_score = [0, 0] seasoningbowl_score = [0] for i in range(len(get_msg.id)): if (get_msg.classname[i] == "pan"): pan_score.append(get_msg.score[i]) if (get_msg.classname[i] == "beef"): beef_score.append(get_msg.score[i]) if (get_msg.classname[i] == "plate"): plate_score.append(get_msg.score[i]) if (get_msg.classname[i] == "vegetablebowl"): vegetablebowl_score.append(get_msg.score[i]) if (get_msg.classname[i] == "broccoli"): broccoli_score.append(get_msg.score[i]) if (get_msg.classname[i] == "souppothandle"): souppothandle_score.append(get_msg.score[i]) if (get_msg.classname[i] == "panhandle"): panhandle_score.append(get_msg.score[i]) if (get_msg.classname[i] == "seasoningbowl"): beef_score.append(get_msg.score[i]) if (get_msg.classname[i] == "nethandle"): nethandle_score.append(get_msg.score[i]) if (get_msg.classname[i] == "seasoningbottle"): seasoningbottle_score.append(get_msg.score[i]) plate_score = sorted(plate_score, reverse=True) pan_score = sorted(pan_score, reverse=True) vegetablebowl_score = sorted(vegetablebowl_score, reverse=True) broccoli_score = sorted(broccoli_score, reverse=True) souppothandle_score = sorted(souppothandle_score, reverse=True) panhandle_score = sorted(panhandle_score, reverse=True) beef_score = sorted(beef_score, reverse=True) nethandle_score = sorted(nethandle_score, reverse=True) seasoningbottle_score = sorted(seasoningbottle_score, reverse=True) seasoningbowl_score = sorted(seasoningbowl_score, reverse=True) det_bar_pos = detectionbar.model_detection(cv_image) bro_list = broccolidetection.broccoli_detection(cv_image) global temp_list_broc if len(bro_list) > 0: brocili_list.broc_uv_list = [] brocili_list.broc_uv_list = bro_list temp_list_broc = bro_list if len(bro_list) <= 0: brocili_list.broc_uv_list = temp_list_broc #print("wo cao :",temp_list_broc) #print("xi lan hua: ",brocili_list.broc_uv_list) ''' print("plate score : ",plate_score) print("pan score : ",pan_score) print("vegetablebowl score : ",vegetablebowl_score) print("broccoli score : ",broccoli_score) print("souppothandle score : ",souppothandle_score) print("panhandle score : ",panhandle_score) print("beef score : ",beef_score) print("nethandle score : ",nethandle_score) print("seasoningbottle score : ",seasoningbottle_score) print("seasoningbowl score : ",seasoningbowl_score) ''' single_obj.element_data_temp = [] single_obj.classname = [] single_obj.score = [] for i in range(len(get_msg.id)): element_data = element_info() element_data.id_info = [] mask_area = get_msg.roi[i].height * get_msg.roi[i].width if mask_area > 0 and get_msg.score[i] > 0.19: mask_image = bridge.imgmsg_to_cv2(get_msg.masks[i], "passthrough") if get_msg.classname[i] == "plate": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > plate1[0] and center_x < plate1[ 1] and center_y > plate1[2] and center_y < plate1[3]: temp_list = [] temp_list = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list != []: #element_data.id_info = [] element_data.id_info = temp_list single_obj.element_data_temp.append(element_data) #print("aaa:",single_obj.element_data_temp) single_obj.score.append(get_msg.score[i]) single_obj.classname.append("plate1") if get_msg.classname[i] == "plate": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > plate2[0] and center_x < plate2[ 1] and center_y > plate2[2] and center_y < plate2[3]: temp_list1 = [] temp_list1 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list1 != []: #element_data.id_info = [] element_data.id_info = temp_list1 single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) single_obj.classname.append("plate2") if get_msg.classname[i] == "plate": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > plate3[0] and center_x < plate3[ 1] and center_y > plate3[2] and center_y < plate3[3]: temp_list2 = [] temp_list2 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list2 != []: element_data.id_info = temp_list2 #print("wo-2-cia",temp_list) single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) single_obj.classname.append("plate3") if get_msg.classname[i] == "beef" and get_msg.score[ i] == beef_score[0]: temp_list3 = [] temp_list3 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list3 != []: element_data.id_info = temp_list3 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "pan" and get_msg.score[i] == pan_score[ 0]: temp_list4 = [] temp_list4 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list4 != []: element_data.id_info = temp_list4 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[0]: temp_list5 =[] temp_list = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list5!=[]: element_data.id_info = temp_list5 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[1]: temp_list6 = [] temp_list6 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list6!=[]: element_data.id_info = temp_list6 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "broccoli" and get_msg.score[i] == broccoli_score[2]: temp_list7 = [] temp_list7 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list7!=[]: element_data.id_info = temp_list7 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "souppothandle" and get_msg.score[i] == souppothandle_score[0]: temp_list8 = [] temp_list8 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list8!=[]: element_data.id_info = temp_list8 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' ''' if get_msg.classname[i] == "nethandle" and get_msg.score[i] == nethandle_score[0]: temp_list9 = [] temp_list9 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list9!=[]: element_data.id_info = temp_list9 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "vegetablebowl" and get_msg.score[i] == vegetablebowl_score[0]: temp_list10 = [] temp_list10 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list10!=[]: element_data.id_info = temp_list10 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' if get_msg.classname[i] == "seasoningbowl" and get_msg.score[ i] == seasoningbowl_score[0]: temp_list11 = [] temp_list11 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list11 != []: element_data.id_info = temp_list11 single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "seasoningbottle": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > bottle1[0] and center_x < bottle1[ 1] and center_y > bottle1[2] and center_y < bottle1[3]: temp_list12 = [] temp_list12 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list12 != []: single_obj.classname.append("seasoningbottle1") element_data.id_info = temp_list12 single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) if get_msg.classname[i] == "seasoningbottle": center_x = int(get_msg.roi[i].x_offset + get_msg.roi[i].height / 2) # rect area center center_y = int(get_msg.roi[i].y_offset + get_msg.roi[i].width / 2) # rect area center if center_x > bottle2[0] and center_x < bottle2[ 1] and center_y > bottle2[2] and center_y < bottle2[3]: temp_list13 = [] temp_list13 = prcess_contours( cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list13 != []: single_obj.classname.append("seasoningbottle2") element_data.id_info = temp_list13 single_obj.element_data_temp.append(element_data) #single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' if get_msg.classname[i] == "panhandle" and get_msg.score[i] == panhandle_score[0]: temp_list14 = [] temp_list14 = prcess_contours(cv_image, get_msg.classname[i], mask_image, get_msg.id[i], get_msg.roi[i].x_offset, get_msg.roi[i].y_offset, get_msg.roi[i].height, get_msg.roi[i].width, get_msg.score[i]) if temp_list14!=[]: element_data.id_info = temp_list14 single_obj.element_data_temp.append(element_data) single_obj.classname.append(get_msg.classname[i]) single_obj.score.append(get_msg.score[i]) ''' #print("88888",single_obj) if len(single_obj.score) > 0: #print("1111:",single_obj) temp_single_obj = single_obj #print("****",temp_single_obj) list_obj.single_obj_info.append(single_obj) else: list_obj.single_obj_info.append(temp_single_obj) #print("1111:",list_obj.single_obj_info) list_obj.header.stamp = data.header.stamp list_obj.header.frame_id = data.header.frame_id print("---" * 20) print("--ww--", list_obj.single_obj_info) list_obj.rgb_img_to_pos = bridge.cv2_to_imgmsg(rgb_img_to_pos, encoding="bgr8") list_obj.depth_img_to_pos = bridge.cv2_to_imgmsg(depth_img_to_pos, "passthrough") temp_det_bar_pos = Float32MultiArray(data=det_bar_pos) #print("1111:",list_obj) list_obj_pub = rospy.Publisher("process_uv/listobjs", listobj, queue_size=1) temp_det_bar_pos_pub = rospy.Publisher("process_uv/bar_pos", Float32MultiArray, queue_size=1) list_broc_pub = rospy.Publisher("process_uv/listbroc", broclist, queue_size=1) list_broc_pub.publish(brocili_list) list_obj_pub.publish(list_obj) temp_det_bar_pos_pub.publish(temp_det_bar_pos) cv2.rectangle(cv_image, (plate2[0], plate2[2]), (plate2[0] + (plate2[1] - plate2[0]), plate2[2] + (plate2[3] - plate2[2])), (0, 0, 255), 2) cv2.rectangle(cv_image, (plate1[0], plate1[2]), (plate1[0] + (plate1[1] - plate1[0]), plate1[2] + (plate1[3] - plate1[2])), (0, 0, 255), 2) cv2.rectangle(cv_image, (plate3[0], plate3[2]), (plate3[0] + (plate3[1] - plate3[0]), plate3[2] + (plate3[3] - plate3[2])), (0, 0, 255), 2) #cv2.rectangle(cv_image,(bottle1[0], bottle1[2]),(bottle1[0]+(bottle1[1]-bottle1[0])/2, bottle1[2]+(bottle1[3]-bottle1[2])/2),(0,0,255),2) #cv2.rectangle(cv_image,(bottle2[0], bottle2[2]),(bottle2[0]+(bottle2[1]-bottle2[0])/2, bottle2[2]+(bottle2[3]-bottle2[2])/2),(0,0,255),2) cv2.imshow("img2", cv_image) cv2.waitKey(1)
def RadarPub(data): Radar_Dist = Float32MultiArray() Radar_Dist.data = [0, 0, 0] if (data.id != 1343): a = UInt8MultiArray a = [0, 0, 0, 0, 0, 0, 0, 0] # data bytes print("data", data) for i in range(8): a[i] = ord(data.data[i]) print("a", a) byte_L5 = '{0:08b}'.format(a[5]) byte_H6 = '{0:08b}'.format(a[6]) Radial_angle = byte_H6[3:8] + byte_L5[0:6] Radial_angle = int(Radial_angle, 2) Radial_angle = Radial_angle * 0.1 # scaled value if (Radial_angle > 102.3): Radial_angle = Radial_angle - 204.8 else: Radial_angle = Radial_angle angle = round(Radial_angle, 2) print "Scaled_Radial_angle = ", angle, "Degree" byte_L0 = '{0:08b}'.format(a[0]) byte_H1 = '{0:08b}'.format(a[1]) Radial_range = byte_H1[1:8] + byte_L0 Radial_range = int(Radial_range, 2) Radial_range = Radial_range * 0.01 # scaled value R_range = (round(Radial_range, 2)) print "Scaled_Radial_range = ", R_range, "m" if data.id < 1343: if (angle >= -15 and angle <= 15): # Center Object distance print("Center", angle) string_range = str(R_range) All_Rdis_Center.append(R_range) print("Centre dis", min(All_Rdis_Center)) elif (angle < -15 and angle >= -45): # Left Object distance print(" Left ", angle) string_range = str(R_range) All_Rdis_Left.append(R_range) print("Left dis", min(All_Rdis_Left)) elif (angle > 15 and angle <= 45): # Right Object distance print(" Right ", angle) string_range = str(R_range) All_Rdis_Right.append(R_range) print("Right dis", min(All_Rdis_Right)) elif data.id >= 1343: Rdis_Center = min(All_Rdis_Center) Rdis_Left = min(All_Rdis_Left) Rdis_Right = min(All_Rdis_Right) Radar_Dist = [Rdis_Left, Rdis_Center, Rdis_Right] print(Radar_Dist) global cond cond = String '''if Rdis_Center <= 2 and Rdis_Left <= 2 and Rdis_Right <= 2: cond = "A" print("stop") if Rdis_Center <= 2: cond = "A" print("stop") elif Rdis_Center <= 2 and Rdis_Left <= 2 and Rdis_Right >= 2: cond = "B" print("go right") elif Rdis_Center <= 2 and Rdis_Left >= 2 and Rdis_Right <= 2: cond = "C" print("go left") elif Rdis_Center <= 2 and Rdis_Left >= 2 and Rdis_Right >= 2: if Rdis_Left > Rdis_Right: cond = "C" print("go left") else: print("go right") cond = "B"''' if Rdis_Center <= 1.5: cond = "A" print("Stop") elif Rdis_Center <= 1.5 and Rdis_Left <= 1.5 and Rdis_Right <= 1.5: cond = "B" print("stop") elif Rdis_Left <= 1.5: cond = "C" print("go right") elif Rdis_Right <= 1.5: cond = "D" print("go left") else: cond = "E" print("goes straight") ''' #print (" Radar_Center", Rdis_Center) #print (" Radar_Left", Rdis_Left) #print (" Radar_Right", Rdis_Right) print(Radar_Dist) #key = input() ''' pub = rospy.Publisher("Radar", String, queue_size=10) pub.publish(cond) #rospy.loginfo(Radar_Dist) del All_Rdis_Center[:] del All_Rdis_Left[:] del All_Rdis_Right[:]
# get path of pkg rospy.init_node("map_setup") # load parameters max_prm_size = rospy.get_param('~max_prm_size', 1000) # wait for services rospy.wait_for_service('/rosplan_roadmap_server/create_prm') wp_pub = rospy.Publisher('/uav01/add_waypoint', Float32MultiArray, queue_size=100) # generate dense PRM rospy.loginfo("KCL: (%s) Creating PRM of size %i" % (rospy.get_name(), max_prm_size)) prm = rospy.ServiceProxy('/rosplan_roadmap_server/create_prm', CreatePRM) if not prm(max_prm_size, 2, 3, 4, 50, 100000): rospy.logerr("KCL: (%s) No PRM was made" % rospy.get_name()) # load ground_wps from parameter ground_wps = rospy.get_param("/ground_wp") # add each as a new waypoint to the PMR (usin sensing interface) for wp in ground_wps: if type(ground_wps[wp]) is list: rospy.loginfo("KCL: (%s) Adding wp: %s" % (rospy.get_name(), wp)) wpmsg = Float32MultiArray() for coord in ground_wps[wp]: wpmsg.data.append(coord) wp_pub.publish(wpmsg)
from Environment import Environment import pdb EPISODES = 6000 if __name__ == '__main__': rospy.init_node('turtlebot2i_deep_qlearning') publish_result = rospy.Publisher('result', Float32MultiArray, queue_size=5) publish_action = rospy.Publisher('get_action', Float32MultiArray, queue_size=5) # store the action taken and the result action_data = Float32MultiArray() result = Float32MultiArray() state_space_size = 28 num_of_actions = 5 parameter_dictionary = None environment = Environment(num_of_actions) neural_network = NeuralNetwork(state_space_size, num_of_actions) scores, episodes = [], [] global_step = 0 start_time = time.time() for episode in range(neural_network.load_episode + 1, EPISODES):
def move_closer(self): rospy.loginfo("[Decon] Starting to move...") request = Float32MultiArray() request.data = self.move_position self.move_closer_command_topic.publish(request)
def callback(scan): #print(scan) # if scan.intensities[0] > 0: # intensities 360 length 0 or 47 # scan_time 7.96710082795e-05 # ranges tuple float32 # angle_min -3.12413907051 # 3.14159274101 # angle_increment 0.0157 global ranges_filtered global num_accumulated ranges_np = np.array(scan.ranges) ranges_np[ranges_np == inf] = 0 ranges_np[ranges_np > 1] = 0 ranges_angle = 80 ranges_np[ranges_angle + 1:360 - ranges_angle] = 0 num_accumulated = 8 center = [] center_index = 0 if 1 > 0: temp = ranges_filtered[:, 1:num_accumulated] ranges_filtered[:, 0:num_accumulated - 1] = temp ranges_filtered[:, num_accumulated - 1] = ranges_np ranges_median = np.median(ranges_filtered, axis=1) ranges_scan = np.zeros(2 * ranges_angle) ranges_scan[0:ranges_angle] = ranges_median[360 - ranges_angle:360] ranges_scan[ranges_angle:2 * ranges_angle] = ranges_median[0:ranges_angle] ranges_scan = ranges_scan[::-1] i = 0 index = 0 center_index = 0 center = [] while i < 2 * ranges_angle: if ranges_scan[i] > 0: index = int(np.ceil(0.2 / ranges_scan[i] / 0.0175)) + 2 flag = 0 for j in range(index): if i + j > 2 * ranges_angle - 1: break if abs(ranges_scan[i] - ranges_scan[i + j]) < 0.25 and ranges_scan[i + j] > 0: flag = j if flag > 0 and flag > index / 2: ranges_scan[i:i + index] = np.sqrt(ranges_scan[i]**2 - 0.1**2) center_ranges = np.sqrt(ranges_scan[i]**2 - 0.1**2) center_index = np.floor(i + flag / 2) print(i) print(index) center_index = np.mod((360 + center_index - ranges_angle), 360).astype('int') center = ranges_index_to_point(center_ranges, center_index) i = i + index else: i += 1 else: i += 1 range_median = np.zeros(360) ranges_scan = ranges_scan[::-1] ranges_median[360 - ranges_angle:360] = ranges_scan[0:ranges_angle] ranges_median[0:ranges_angle] = ranges_scan[ranges_angle:2 * ranges_angle] location = Float32MultiArray() location.data = center print(center) print(center_index) publisher_single_location(location) scan_median = scan scan_median.ranges = tuple(ranges_median) publisher(scan_median)
def pub_test(self): print('Printing test') test = Float32MultiArray() self.pubPose.publish(test)
import tornado.websocket import tornado.ioloop import tornado.web # FLAG for fully manual control (TRUE) or shared control (FALSE) #Tonado server port try: os.nice(-10) except PermissionError: # Need to run via sudo for high priority rospy.logerr("Cannot set niceness for the process...") rospy.logerr("Run the script as sudo...") pub_remote = rospy.Publisher('qolo/user_commands', Float32MultiArray, queue_size=1) data_remote = Float32MultiArray() ### ---------- GLOBAL VARIABLES ---------- #### PORT = 8080 Max_V = 0.4 Max_W = 0.4 level_relations = { # 'debug':logging.DEBUG, 'info': logging.INFO, # 'warning':logging.WARNING, # 'error':logging.ERROR, # 'crit':logging.CRITICAL } # Tornado Folder Paths
#!/usr/bin/env python # -*- coding: utf-8 -*- # # FileName: sample # CreatedDate: 2019-06-05 18:25:22 # import rospy from std_msgs.msg import Float32MultiArray if __name__ == "__main__": rospy.init_node("sample", anonymous=True) pub = rospy.Publisher("/maxon_bringup/all_position", Float32MultiArray, queue_size=1) pub.publish(Float32MultiArray(data=[10000, -20000]))
def joint_states_received(self, msg): self.encoders_pub.publish( Float32MultiArray(data=[ msg.position[0] * self.wheel_radius, msg.position[1] * self.wheel_radius ]))