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()
