def test_fromTf(self): transformer = Transformer(True, rospy.Duration(10.0)) m = TransformStamped() m.header.frame_id = 'wim' m.child_frame_id = 'james' m.transform.translation.x = 2.71828183 m.transform.rotation.w = 1.0 transformer.setTransform(m) b = pm.fromTf(transformer.lookupTransform('wim', 'james', rospy.Time(0)))
def test_fromTf(self): transformer = Transformer(True, rospy.Duration(10.0)) m = TransformStamped() m.header.frame_id = 'wim' m.child_frame_id = 'james' m.transform.translation.x = 2.71828183 m.transform.rotation.w = 1.0 transformer.setTransform(m) b = pm.fromTf( transformer.lookupTransform('wim', 'james', rospy.Time(0)))
def __init__(self): # specify topic and data type self.sub_bbox_1 = rospy.Subscriber("camera1/detection/vision_objects", DetectedObjectArray, self.bbox_array_callback) self.sub_bbox_6 = rospy.Subscriber("camera6/detection/vision_objects", DetectedObjectArray, self.bbox_array_callback) self.sub_pcd = rospy.Subscriber("/points_raw", PointCloud2, self.pcd_callback) # self.sub_pose = rospy.Subscriber("/current_pose", PoseStamped, self.pose_callback) # Publisher self.pub_intersect_markers = rospy.Publisher( "/vision_objects_position_rviz", MarkerArray, queue_size=10) self.pub_plane_markers = rospy.Publisher("/estimated_plane_rviz", MarkerArray, queue_size=10) self.pub_plane = rospy.Publisher("/estimated_plane", Plane, queue_size=10) self.pub_pcd_inlier = rospy.Publisher("/points_inlier", PointCloud2, queue_size=10) self.pub_pcd_outlier = rospy.Publisher("/points_outlier", PointCloud2, queue_size=10) self.cam1 = camera_setup_1() self.cam6 = camera_setup_6() self.plane = Plane3D(-0.157, 0, 0.988, 1.9) self.plane_world = None self.plane_tracker = None self.sac = RANSAC(Plane3D, min_sample_num=3, threshold=0.22, iteration=200, method="MSAC") self.tf_listener = TransformListener() self.tfmr = Transformer() self.tf_ros = TransformerROS()
def __init__(self, params): self.transform = TransformListener() self.transformer = Transformer(True, rospy.Duration(10.0)) self.params = params self.cvbridge = CvBridge() self.OBJ_TOPIC = self.params['obj_topic'] self.POSE_TOPIC = self.params['pose_topic'] self.sub_obj = rospy.Subscriber(self.OBJ_TOPIC, objs_array, self.callback_objs, queue_size=1) self.pose_pub = rospy.Publisher(self.POSE_TOPIC, objs_array, queue_size=1) self.flag = 0 self.last_detect = time.time() return None
def get_vertices(pose, size, shape_type): tf = Transformer(True, rospy.Duration(10.0)) m = TransformStamped() m.header.frame_id = 'object' m.transform.rotation.w = 1 keys = [] if shape_type == 'box': for mx in POM: for my in POM: for mz in POM: key = 'frame%s%s%s' % (F[mx], F[my], F[mz]) m.child_frame_id = key keys.append(key) m.transform.translation.x = size[0] / 2.0 * mx m.transform.translation.y = size[1] / 2.0 * my m.transform.translation.z = size[2] / 2.0 * mz tf.setTransform(m) else: return None m.transform.translation.x = pose.position.x m.transform.translation.y = pose.position.y m.transform.translation.z = pose.position.z m.transform.rotation.x = pose.orientation.x m.transform.rotation.y = pose.orientation.y m.transform.rotation.z = pose.orientation.z m.transform.rotation.w = pose.orientation.w m.header.frame_id = 'map' m.child_frame_id = 'object' tf.setTransform(m) vertices = [] for key in keys: (off, rpy) = tf.lookupTransform('map', key, rospy.Time(0)) vertices.append(off) return vertices
def get_vertices(pose, size, shape_type): tf = Transformer(True, rospy.Duration(10.0)) m = TransformStamped() m.header.frame_id = 'object' m.transform.rotation.w = 1 keys = [] if shape_type=='box': for mx in POM: for my in POM: for mz in POM: key = 'frame%s%s%s'%(F[mx], F[my], F[mz]) m.child_frame_id = key keys.append(key) m.transform.translation.x = size[0]/2.0*mx m.transform.translation.y = size[1]/2.0*my m.transform.translation.z = size[2]/2.0*mz tf.setTransform(m) else: return None m.transform.translation.x = pose.position.x m.transform.translation.y = pose.position.y m.transform.translation.z = pose.position.z m.transform.rotation.x = pose.orientation.x m.transform.rotation.y = pose.orientation.y m.transform.rotation.z = pose.orientation.z m.transform.rotation.w = pose.orientation.w m.header.frame_id = 'map' m.child_frame_id = 'object' tf.setTransform(m) vertices = [] for key in keys: (off,rpy) = tf.lookupTransform('map', key, rospy.Time(0)) vertices.append(off) return vertices
def __init__(self, *args): print("init") self.tf = TransformListener() self.tt = Transformer() rospy.Subscriber("/trackedHumans", TrackedHumans, self.pos_callback) self.publisher = rospy.Publisher("directionmarker_array", MarkerArray)
def __init__(self, params): #NaoQI self.ip = params['ip'] self.port = params['port'] self.session = qi.Session() self.match_images = {} ld = os.listdir('./object_images/') for fn in ld: if fn.split('.')[-1] != 'png' or len(fn.split('_')) != 3: continue on = fn.split('.')[0] img = cv2.imread('./object_images/' + fn) self.match_images[on] = img print 'matching objects loaded : ', self.match_images.keys() try: self.session.connect("tcp://" + self.ip + ":" + str(self.port)) except RuntimeError: print("Connection Error!") sys.exit(1) self.video = self.session.service("ALVideoDevice") video_subs_list = [ 'detector_rgb_t_0', 'detector_pc_0', 'detector_dep_0' ] print self.video.getSubscribers() for sub_name in video_subs_list: print sub_name, self.video.unsubscribe(sub_name) self.rgb_top = self.video.subscribeCamera( 'detector_rgb_t', 0, 2, 11, 20) #name, idx, resolution, colorspace, fps self.pc = self.video.subscribeCamera('detector_pc', 2, 1, 19, 20) self.depth = self.video.subscribeCamera('detector_dep', 2, 1, 17, 20) self.point_cloud = np.zeros((240, 320, 3)) offset_x = -120 offset_y = -160 self.x_temp = -(np.tile(np.arange(240).reshape(240, 1), (1, 320)).reshape(240, 320, 1) + offset_x) self.y_temp = -(np.tile(np.arange(320).reshape(1, 320), (240, 1)).reshape(240, 320, 1) + offset_y) #self.thread_cloud = Thread(target=self.get_point_cloud, args=(None,)) #self.thread_cloud.daemon = True #self.thread_cloud.start() time.sleep(1) print self.video.getSubscribers() #Darknet self.gg = tensorflow.Graph() with self.gg.as_default() as g: self.tfnet = TFNet(self.options) self.classes = open('cfg/coco.names', 'r').readlines() #ROS self.cvbridge = CvBridge() self.transform = TransformListener() self.transformer = Transformer(True, rospy.Duration(10.0)) self.RGB_TOPIC = params['rgb_topic'] self.OBJ_TOPIC = params['obj_topic'] self.rgb_sub = rospy.Subscriber(self.RGB_TOPIC, Image, self.callback_image, queue_size=1) self.obj_pub = rospy.Publisher(self.OBJ_TOPIC, objs_array, queue_size=1) self.show = params['od_show'] self.object_id = 0 self.object_id_max = 999999 self.msg_idx = 0 self.msg_idx_max = 9999999 if self.show: cv2.startWindowThread() cv2.namedWindow('objs') self.tttt = time.time() time.sleep(1)
def process(bagfile): # Load bag print "\nOpening bag file [%s]" % bagfile bag = rosbag.Bag(bagfile) hz = 100 # Get dictionary of topics # topics = list(set([c.topic for c in bag._get_connections() if c.topic.startswith("/cf") or c.topic.])) topics = list(set([c.topic for c in bag._get_connections() if c.topic != "/diagnostics" and c.topic.rfind("rosout")+c.topic.rfind("parameter_") == -2])) if topics == []: rospy.logwarn('[%s] has no topics, skipping') return start, finish = printInfo(bag, topics=topics) dur = finish-start rostf = Transformer(True, dur) #buffer duration = dur.to_sec() # deal with TF # treat each tf frame as a topic # tfdict = {} # for tfs in [m[1] for m in bag.read_messages("/tf")]: # #from->to # for t in tfs.transforms: # tFrom = t.header.frame_id # tTo = t.child_frame_id # if not tfdict.has_key(tFrom): # tfdict[tFrom] = {} # if not tfdict[tFrom].has_key(tTo): # tfdict[tFrom][tTo] = [] # tfdict[tFrom][tTo].append(t) for tfs in bag.read_messages("/tf"): for t in tfs[1].transforms: t.header.stamp -= start if (t.header.stamp < rospy.Duration(0)): rospy.logwarn('%s - %s [%f]' % (t.header.frame_id, t.child_frame_id, t.header.stamp.to_sec())) continue rostf.setTransform(t) #cache all tf info into messages to a transformer object # tFrom = t.header.frame_id # tTo = t.child_frame_id # if not tfdict.has_key(tTo): # tfdict[tTo] = {} # if not tfdict[tTo].has_key(tFrom): # tfdict[tTo][tFrom] = [] # tfdict[tTo][tFrom].append(t) # # Check # for f in tfdict.keys(): # fs = tfdict[f].keys() # if (len(fs)>1): # rospy.logerr("Warning: Frame [%s] has multiple parent frames: %s", f, fs) # for f in sorted(tfdict.keys()): # for fs in sorted(tfdict[f].keys()): # print fs, "->", f # print "___" # Print existing frames # get [(child_frame, parent_frame), ...] for all known transforms in bag framestr = [(k[k.rfind(' ')+1:],k[0:k.find(' ')]) for k in rostf.allFramesAsString().replace('Frame ','').split('.\n') if k] for parent, child in framestr: if parent != "NO_PARENT": print parent, "->", child # List of transforms we are interested in using FRAMES = [("/world", "/cf_gt"), ("/world", "/goal"), ("/cf_gt2d", "/goal")] # from->to pairs # For TF we interpolate at equal intervals timelist = np.arange(0, duration, 1./hz) # container for all transforms frames_pq = {"time": timelist} for parent, child in FRAMES: # Get TF info at given HZ pq = [] for ti in timelist: t = rospy.Time(secs=ti) if rostf.canTransform(parent, child, t): p, q = rostf.lookupTransform(parent, child, t) pq.append([p[0],p[1],p[2],q[0],q[1],q[2],q[3]]) else: #rospy.logwarn("Could not get transform %s -> %s at time %fs", parent, child, t.to_sec()) pq.append([0,0,0, 0,0,0,1]) frames_pq[parent[1:].replace('/','_')+"__"+child[1:].replace('/','_')] = np.array(pq) # matlab friendly # # PLot xyz # import matplotlib.pyplot as plt # # Create the plot # plt.plot(timelist, frames_pq["world-cf_gt"][:, 0:3]) # FRAME, time step, dimension # plt.plot(timelist, frames_pq["world-goal"][ :, 0:3]) # FRAME, time step, dimension # plt.show() # Collect all data data = {t: {} for t in topics if t != '/tf' and not t.endswith('image_raw') and not t.endswith('camera_info')} # Prepare all the dictionaries for t in data.keys(): print t name, msg, t = bag.read_messages(topics=t).next() data[name] = {slot: [] for slot in msg.__slots__ if slot != "header"} data[name]["time"] = [] # print "USED DATA: \n", [k +": "+str(data[k].keys()) for k in data.keys()] # Fill all dictionaries for name, msg, t in bag.read_messages(topics=data.keys()): # They all have a time stamp data[name]["time"].append((t-start).to_sec()) # Append data for each slot for slot in data[name].keys(): if slot == "time": continue contents = msg.__getattribute__(slot) data[name][slot].append(contents) # Convert all to NP arrays mdata = {} for name in data.keys(): mname = name[name.rfind('/')+1:] # matlab friendly name mdata[mname] = {} for slot in data[name].keys(): mdata[mname][slot] = np.array(data[name][slot]) # add TF data mdata['tf'] = frames_pq # Save as MAT sio.savemat(bagfile[:-4]+'.mat', {'bag'+bagfile[:-4].replace('-','_'):mdata}, oned_as='column') return
def __init__(self, params): self.params = params self.match_images = {} ld = os.listdir('./object_images/') for fn in ld: if fn.split('.')[-1] != 'png' or len(fn.split('_')) != 3: continue on = fn.split('.')[0] img = cv2.imread('./object_images/' + fn) self.match_images[on] = img print 'matching objects loaded : ', self.match_images.keys() self.point_cloud = None self.object_id = 0 self.object_id_max = 999999 self.msg_idx = 0 self.msg_idx_max = 9999999 #Darknet self.gg = tensorflow.Graph() with self.gg.as_default() as g: self.tfnet = TFNet(self.options) self.classes = open('cfg/coco.names', 'r').readlines() if self.params['show_od']: cv2.startWindowThread() cv2.namedWindow('Object_detector') #ROS self.cvbridge = CvBridge() self.transform = TransformListener() self.transformer = Transformer(True, rospy.Duration(10.0)) self.RGB_TOPIC = params['rgb_topic'] self.Depth_TOPIC = params['depth_topic'] self.OBJ_TOPIC = params['obj_topic'] self.depth_sub = rospy.Subscriber(self.Depth_TOPIC, Image, self.callback_depth, queue_size=1) time.sleep(1) self.rgb_sub = rospy.Subscriber(self.RGB_TOPIC, Image, self.callback_image, queue_size=1) self.obj_pub = rospy.Publisher(self.OBJ_TOPIC, objs_array, queue_size=1) self.tttt = time.time() time.sleep(1) print('[DIP] Object Detector Module Ready!')
def __init__(self, params): # 初始化ROS节点 rospy.init_node("pepper_test") # 程序退出时的回调函数 atexit.register(self.__del__) self.ip = params['ip'] self.port = params['port'] self.session = qi.Session() try: self.session.connect("tcp://" + self.ip + ":" + str(self.port)) except RuntimeError: print("[Kamerider W] : connection Error!!") sys.exit(1) # 订阅需要的服务 self.VideoDev = self.session.service("ALVideoDevice") self.FaceCha = self.session.service("ALFaceCharacteristics") self.Memory = self.session.service("ALMemory") self.Dialog = self.session.service("ALDialog") self.AnimatedSpe = self.session.service("ALAnimatedSpeech") self.AudioDev = self.session.service("ALAudioDevice") self.BasicAwa = self.session.service("ALBasicAwareness") self.AutonomousLife = self.session.service("ALAutonomousLife") self.TabletSer = self.session.service("ALTabletService") self.TextToSpe = self.session.service("ALTextToSpeech") self.Motion = self.session.service("ALMotion") self.RobotPos = self.session.service("ALRobotPosture") self.Tracker = self.session.service("ALTracker") # 关闭 basic_awareness self.BasicAwa.setEnabled(False) # 设置tracker模式为头追踪 self.Tracker.setMode("Head") # 初始化平板 self.TabletSer.cleanWebview() # 初始化位置点 self.PepperPosition = PoseStamped() # 推荐的商品的位置 self.Point_A = MoveBaseGoal() self.Point_A.target_pose.header.frame_id = '/map' self.Point_A.target_pose.header.stamp = self.PepperPosition.header.stamp self.Point_A.target_pose.header.seq = self.PepperPosition.header.seq self.Point_A.target_pose.pose.position.x = 1.57589215035 self.Point_A.target_pose.pose.position.y = -4.95822132707 self.Point_A.target_pose.pose.position.z = .0 self.Point_A.target_pose.pose.orientation.x = .0 self.Point_A.target_pose.pose.orientation.y = .0 self.Point_A.target_pose.pose.orientation.z = -0.996909852153 self.Point_A.target_pose.pose.orientation.w = 0.0785541003461 # 起始位置 self.Point_B = MoveBaseGoal() self.Point_B.target_pose.header.frame_id = '/map' self.Point_B.target_pose.header.stamp = self.PepperPosition.header.stamp self.Point_B.target_pose.header.seq = self.PepperPosition.header.seq self.Point_B.target_pose.pose.position.x = 3.48176515207 self.Point_B.target_pose.pose.position.y = -4.7906732889 self.Point_B.target_pose.pose.position.z = .0 self.Point_B.target_pose.pose.orientation.x = .0 self.Point_B.target_pose.pose.orientation.y = .0 self.Point_B.target_pose.pose.orientation.z = -0.99958660438 self.Point_B.target_pose.pose.orientation.w = 0.0287510059817 # 设置输出音量 # 相机 video_subs_list = ['rgb_t_0', 'rgb_b_0', 'dep_0'] for sub_name in video_subs_list: print sub_name, self.VideoDev.unsubscribe(sub_name) self.rgb_top = self.VideoDev.subscribeCamera('rgb_t', 0, 2, 11, 40) self.depth = self.VideoDev.subscribeCamera('dep', 2, 1, 17, 20) # 语言 self.Dialog.setLanguage("English") # 载入topic self.Topic_path = '/home/nao/top/WRS_pepper_enu.top' self.Topic_path = self.Topic_path.decode('utf-8') # topic回调函数 self.go_to_pointA_sub = self.Memory.subscriber("go_to_pointA") self.go_to_pointA_sub.signal.connect(self.callback_pointA) self.Bar_code_sub = self.Memory.subscriber("Bar_code") self.Bar_code_sub.signal.connect(self.callback_Bar_code) self.change_img_language_sub = self.Memory.subscriber("Info_Chinese") self.change_img_language_sub.signal.connect(self.callback_img_lan_CHN) self.record_item_sub = self.Memory.subscriber("record") self.record_item_sub.signal.connect(self.callback_record_item) self.pay_bill_sub = self.Memory.subscriber("pay_bill") self.pay_bill_sub.signal.connect(self.callback_pay_bill) self.finish_sub = self.Memory.subscriber("finish") self.finish_sub.signal.connect(self.callback_finish) self.recommend_sub = self.Memory.subscriber("recommend_start") self.recommend_sub.signal.connect(self.callback_recommend) self.hide_image_sub = self.Memory.subscriber("hide_image") self.hide_image_sub.signal.connect(self.callback_hide_image) self.half_price_sub = self.Memory.subscriber("half_price") self.half_price_sub.signal.connect(self.callback_half_price) self.face_start_sub = self.Memory.subscriber("start_face") self.face_start_sub.signal.connect(self.start_face_recog) self.japan_sub = self.Memory.subscriber("japan") self.japan_sub.signal.connect(self.callback_japan) # 自主说话模式 self.configure = {"bodyLanguageMode": "contextual"} # ROS节点 self.nav_as = actionlib.SimpleActionClient("/move_base", MoveBaseAction) self.cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1) self.goal_cancel_pub = rospy.Publisher('/move_base/cancel', GoalID, queue_size=1) self.nav_as.wait_for_server() self.transform = TransformListener() self.transformer = Transformer(True, rospy.Duration(10)) self.goal_pub = rospy.Publisher('/move_base/current_goal', PoseStamped, queue_size=1) # 清除costmap self.map_clear_srv = rospy.ServiceProxy('/move_base/clear_costmaps', Empty) self.map_clear_srv() # 连接数据库 self.db = MySQLdb.connect("localhost", "root", "'", "WRS_PEPPER") # 商品条形码对应的价格 self.price = { "jam": 8, "tea": 3, "bread": 5, "juice": 6, "potato chips": 7, "beef": 10, "instant noodles": 5 } self.price_sum = 0 # 变量值 self.filename = "/home/jiashi/Desktop/1538483080.png" # 当前记录的单号 self.order_id = 6 self.month = 10 # 记录是否停止对话 self.if_stop_dia = False self.if_start_dia = False self.Pause_dia = False # 初始化头的位置 self.Motion.setStiffnesses("Head", 1.0) self.Motion.setAngles("Head", [0., -0.25], .05) # 要执行的javascript self.java_script = "" # 设置说话速度 self.TextToSpe.setParameter("speed", 80.0) # pepper头的角度 self.angle = -.33 # 第一个商品 self.first_item = 1 self.state = True self.start_head = True # 调用成员函数 self.set_volume(1) self.cal_user_similarity() self.close_auto_life() self.Topic_name = self.Dialog.loadTopic( self.Topic_path.encode('utf-8')) self.Dialog.activateTopic(self.Topic_name) self.show_image("welcome.jpg") self.start_head_fix() self.start_dialog() self.activate_keyboard_control()