Skip to content

Commit

Permalink
return of the fact
Browse files Browse the repository at this point in the history
  • Loading branch information
@Alexandre-Bonneau
Alexandre-Bonneau committed Dec 18, 2020
1 parent 3d7e6c6 commit 799025f
Showing 1 changed file with 81 additions and 81 deletions.
162 changes: 81 additions & 81 deletions src/pyuwds3/reasoning/monitoring/graphic_monitor.py
View file
Original file line number Diff line number Diff line change
Expand Up @@ -246,87 +246,87 @@ def test_occlusion(self, object, tracks, occlusion_threshold=0.8):
#cansee
#getvisualshapedata [X][7] = color
#
# def can_reach(self,start_pose,obj):#,working_area,xy_n=10,z_n=5):
# end_id=self.simulator.entity_id_map[obj.id]
# # x_init = working_area[0][0]
# # y_init = working_area[0][1]
# # z_init = working_area[0][2]
# # x_step = (working_area[1][0] - x_init)/(xy_n*1.0)
# # y_step = (working_area[1][1] - y_init)/(xy_n*1.0)
# # z_step = (working_area[1][2] - z_init)/(z_n*1.0)
# # for x in range(xy_n):
# # for y in range(xy_n):
# # for z in range(z_n):
# # if can_reach_rot(self,
# # [x_init + x*x_step,
# # y_init + y*y_step,
# # z_init + z*z_step],end_id):
# # return True
# if self.can_reach_rot(start_pose,end_id):
# return True
#
# return False
#
#
# def can_reach_rot(self,start_pose,end_id):
# [xmin,ymin,zmin],[xmax,ymax,zmax] = p.getAABB(end_id)
# xlength = xmax - xmin
# ylength = ymax - ymin
# zlength = zmax - zmin
# pose_list = []
# for i in range(N_RAY):
# end_pose = [xmin +i*xlength/(N_RAY-1),
# ymin +i*xlength/(N_RAY-1),
# zmin +i*xlength/(N_RAY-1)]
# r = p.rayTest(start_pose,end_pose)
# if r[0][0]== end_id:
# return True
# return False
#
# def canSee(self,start_pose,obj):
# end_id=self.simulator.entity_id_map[obj.id]
# [xmin,ymin,zmin],[xmax,ymax,zmax] = p.getAABB(end_id)
# xlength = xmax - xmin
# ylength = ymax - ymin
# zlength = zmax - zmin
# pose_list = []
# # print start_pose
# for i in range(N_RAY):
# for j in range (N_RAY):
# for k in range(N_RAY):
# end_pose = [xmin +i*xlength/((N_RAY-1)*1.),
# ymin +j*xlength/((N_RAY-1)*1.),
# zmin +k*xlength/((N_RAY-1)*1.)]
# if self.canSeeRec(start_pose,end_pose,end_id,0):
# return True
# # pose_end=obj.pose.pos.to_array()
# # [pose_end[0],pose_end[1],pose_end[2]]
# start_pose=[start_pose[0][0],start_pose[1][0],start_pose[2][0]]
# if self.canSeeRec(start_pose,end_pose,end_id,0):
# return True
# return False
#
# def canSeeRec(self,start_pose,end_pose,end_id,hitnumber):
#
# r=p.rayTestBatch([start_pose],[end_pose],reportHitNumber = hitnumber)
# # print r[0]
# # print end_id
# if r[0][0] == end_id:
# return True
# if r[0][0]==-1:
# return False
# if not (r[0][0],r[0][1]) in self.alpha_dic:
# data = p.getVisualShapeData(r[0][0])
# if r[0][1] +1 >0 and r[0][1] +1 <len(data) and data[r[0][1] +1][1] == r[0][1]:
# self.alpha_dic[[0][0],r[0][1]] =data[ r[0][1] +1][7]
# else:
# for i in data:
# if i[1]== r[0][1]:
# self.alpha_dic[r[0][0],r[0][1]]=i[7]
#
# if self.alpha_dic[(r[0][0],r[0][1])] >ALPHA_THRESHOLD:
# return False
# return self.canSeeRec(r[0][3],end_pose,end_id,hitnumber+1)
def can_reach(self,start_pose,obj):#,working_area,xy_n=10,z_n=5):
end_id=self.simulator.entity_id_map[obj.id]
# x_init = working_area[0][0]
# y_init = working_area[0][1]
# z_init = working_area[0][2]
# x_step = (working_area[1][0] - x_init)/(xy_n*1.0)
# y_step = (working_area[1][1] - y_init)/(xy_n*1.0)
# z_step = (working_area[1][2] - z_init)/(z_n*1.0)
# for x in range(xy_n):
# for y in range(xy_n):
# for z in range(z_n):
# if can_reach_rot(self,
# [x_init + x*x_step,
# y_init + y*y_step,
# z_init + z*z_step],end_id):
# return True
if self.can_reach_rot(start_pose,end_id):
return True

return False


def can_reach_rot(self,start_pose,end_id):
[xmin,ymin,zmin],[xmax,ymax,zmax] = p.getAABB(end_id)
xlength = xmax - xmin
ylength = ymax - ymin
zlength = zmax - zmin
pose_list = []
for i in range(N_RAY):
end_pose = [xmin +i*xlength/(N_RAY-1),
ymin +i*xlength/(N_RAY-1),
zmin +i*xlength/(N_RAY-1)]
r = p.rayTest(start_pose,end_pose)
if r[0][0]== end_id:
return True
return False

def canSee(self,start_pose,obj):
end_id=self.simulator.entity_id_map[obj.id]
[xmin,ymin,zmin],[xmax,ymax,zmax] = p.getAABB(end_id)
xlength = xmax - xmin
ylength = ymax - ymin
zlength = zmax - zmin
pose_list = []
# print start_pose
for i in range(N_RAY):
for j in range (N_RAY):
for k in range(N_RAY):
end_pose = [xmin +i*xlength/((N_RAY-1)*1.),
ymin +j*xlength/((N_RAY-1)*1.),
zmin +k*xlength/((N_RAY-1)*1.)]
if self.canSeeRec(start_pose,end_pose,end_id,0):
return True
# pose_end=obj.pose.pos.to_array()
# [pose_end[0],pose_end[1],pose_end[2]]
start_pose=[start_pose[0][0],start_pose[1][0],start_pose[2][0]]
if self.canSeeRec(start_pose,end_pose,end_id,0):
return True
return False

def canSeeRec(self,start_pose,end_pose,end_id,hitnumber):

r=p.rayTestBatch([start_pose],[end_pose],reportHitNumber = hitnumber)
# print r[0]
# print end_id
if r[0][0] == end_id:
return True
if r[0][0]==-1:
return False
if not (r[0][0],r[0][1]) in self.alpha_dic:
data = p.getVisualShapeData(r[0][0])
if r[0][1] +1 >0 and r[0][1] +1 <len(data) and data[r[0][1] +1][1] == r[0][1]:
self.alpha_dic[[0][0],r[0][1]] =data[ r[0][1] +1][7]
else:
for i in data:
if i[1]== r[0][1]:
self.alpha_dic[r[0][0],r[0][1]]=i[7]

if self.alpha_dic[(r[0][0],r[0][1])] >ALPHA_THRESHOLD:
return False
return self.canSeeRec(r[0][3],end_pose,end_id,hitnumber+1)

# def hasInView(self,start_pose,end_id,camera):

Expand Down

0 comments on commit 799025f

Please sign in to comment.