nbv.py
Go to the documentation of this file.
1 #from control import *
2 import csv
3 import rospy
4 from asr_next_best_view.srv import *
5 import imp
6 from configuration import *
7 import pickle
8 import numpy as np
9 from tmux import *
10 from configuration import *
11 from move import *
12 from general_ros import *
13 from visualization_msgs.msg import *
14 from geometry_msgs.msg import *
15 import copy
16 import math
17 import __builtin__
18 
19 state_acquisition = imp.load_source("*", scene_exploration_path + "/src/common/state_acquisition.py")
20 nbvSettingsFilePath = next_best_view_path + "/param/next_best_view_settings_sim.yaml"
21 
23  restartPanes([nbvPane])
24  waitForAllModules(True)
25 
27  dataMap = getYaml(nbvSettingsFilePath)
28  dataMap["minIterationSteps"] = 15
29  dataMap["epsilon"] = 0.000000001
30  dataMap["spaceSamplerId"] = 1
31  dataMap["radius"] = 0.05
32  dataMap["enableGA"] = False
33  dataMap["cacheResults"] = False
34  dataMap["sampleSizeUnitSphereSampler"] = 2000
35  saveYaml(nbvSettingsFilePath, dataMap, True)
36 
38  dataMap = getYaml(nbvSettingsFilePath)
39  dataMap["minIterationSteps"] = 15
40  dataMap["epsilon"] = 0.000000001
41  dataMap["spaceSamplerId"] = 1
42  dataMap["radius"] = 0.05
43  dataMap["enableGA"] = False
44  dataMap["cacheResults"] = True
45  dataMap["sampleSizeUnitSphereSampler"] = 2000
46  saveYaml(nbvSettingsFilePath, dataMap, True)
47 
49  dataMap = getYaml(nbvSettingsFilePath)
50  dataMap["minIterationSteps"] = 1
51  dataMap["epsilon"] = 0.01
52  dataMap["spaceSamplerId"] = 1
53  dataMap["radius"] = 0.2
54  dataMap["enableGA"] = False
55  dataMap["cacheResults"] = False
56  dataMap["sampleSizeUnitSphereSampler"] = 384
57  saveYaml(nbvSettingsFilePath, dataMap, True)
58 
60  dataMap = getYaml(nbvSettingsFilePath)
61  dataMap["minIterationSteps"] = 1
62  dataMap["epsilon"] = 0.01
63  dataMap["spaceSamplerId"] = 1
64  dataMap["radius"] = 0.2
65  dataMap["enableGA"] = False
66  dataMap["cacheResults"] = True
67  dataMap["sampleSizeUnitSphereSampler"] = 384
68  saveYaml(nbvSettingsFilePath, dataMap, True)
69 
71  dataMap = getYaml(nbvSettingsFilePath)
72  dataMap["minIterationSteps"] = 5
73  dataMap["epsilon"] = 0.01
74  dataMap["spaceSamplerId"] = 5
75  dataMap["radius"] = 0.2
76  dataMap["enableGA"] = True
77  dataMap["cacheResults"] = True
78  dataMap["sampleSizeUnitSphereSampler"] = 384
79  saveYaml(nbvSettingsFilePath, dataMap, True)
80 
81 def getOptimalNBV(logDir, idx, positionAndOrientation):
82  bkp = getYaml(nbvSettingsFilePath)
83  setOptimalParameters()
85  setPositionAndOrientation(positionAndOrientation)
86  setPCAndViewportsFromLog(logDir, idx)
87  nbv = getNBV()
88  saveYaml(nbvSettingsFilePath, bkp, True)
90  return nbv
91 
93  return state_acquisition.get_camera_pose_cpp()
94 
96  # try to get current robot config and set it in the nbv
97  getRobotState = state_acquisition.GetRobotState()
98  robotState = getRobotState.get_robot_state()
99 
100  rospy.wait_for_service('/nbv/set_init_robot_state')
101  set_init_state = rospy.ServiceProxy('/nbv/set_init_robot_state', SetInitRobotState)
102  set_init_state(robotState)
103 
104 def getNBV():
105  get_nbv = rospy.ServiceProxy('/nbv/next_best_view', GetNextBestView)
106  setRobotState()
107  current_camera_pose = getCurrentCameraPose()
108  return get_nbv(current_camera_pose)
109 
111  return nbvUpdate(getPoseFromNBV(nbv), nbv.object_type_name_list)
112 
113 def nbvUpdate(pose, obj_types):
114  nbv_update = rospy.ServiceProxy('/nbv/update_point_cloud', UpdatePointCloud)
115  return nbv_update(pose, obj_types)
116 
117 def getPC():
118  get_pc = rospy.ServiceProxy("/nbv/get_point_cloud", GetAttributedPointCloud)
119  return get_pc()
120 
121 def getPCAndViewportsFromLog(logDir, idx):
122  pointCloudFile = open(logDir + "/PointCloud/PointCloud_" + str(idx), "rb")
123  viewportsFile = open(logDir + "/PointCloud/Viewports_" + str(idx), "rb")
124  return (pickle.load(pointCloudFile), pickle.load(viewportsFile))
125 
126 def setPCAndViewportsFromLog(logDir, idx):
127  setPC(*getPCAndViewportsFromLog(logDir, idx))
128 
129 def setPC(pointcloud, viewports):
130  set_pc = rospy.ServiceProxy("/nbv/set_point_cloud", SetAttributedPointCloud)
131  return set_pc(pointcloud, viewports)
132 
133 def getNumberOfNBVs(logDir):
134  fileContent = getFile(logDir + "/nbv_results.csv")
135  if fileContent is None:
136  return -1
137  count = fileContent.count("utility:")
138  return count
139 
140 def getNBVs(logDir):
141  try:
142  fileName = logDir + "/nbv_results.csv"
143  f = open(fileName)
144  csvReader = csv.reader(f, delimiter = " ", quotechar = "|")
145  nbvsTuples = list(csvReader)
146  nbvs = map(lambda x: yaml.load(x[1]), nbvsTuples)
147  return nbvs
148  except IOError:
149  print("file does not exist")
150  return None
151 
152 # camera pose
153 def getPoseFromNBV(nbv):
154  if type(nbv) is not dict:
155  point = copy.deepcopy(nbv.resulting_pose.position)
156  orientation = copy.deepcopy(nbv.resulting_pose.orientation)
157  else:
158  pointDict = nbv["resulting_pose"]["position"]
159  point = Point(pointDict["x"], pointDict["y"], pointDict["z"])
160  orientationDict = nbv["resulting_pose"]["orientation"]
161  orientation = Quaternion(orientationDict["x"], orientationDict["y"], orientationDict["z"], orientationDict["w"])
162  return Pose(point, orientation)
163 
164 # robot state
166  if type(nbv) is dict:
167  robotState = nbv["robot_state"]
168  return ((robotState["x"], robotState["y"], math.degrees(robotState["rotation"])), (math.degrees(robotState["pan"]), math.degrees(robotState["tilt"])))
169  else:
170  robotState = nbv.robot_state
171  return ((robotState.x, robotState.y, math.degrees(robotState.rotation)), (math.degrees(robotState.pan), math.degrees(robotState.tilt)))
172 
174  nbvPositions = map(lambda nbvResult: nbvResult["resulting_pose"]["position"], nbvs)
175  nbvNPPos = map(lambda p: np.array([p["x"], p["y"], p["z"]]), nbvPositions)
176  sum = 0
177  for i in range(0, len(nbvNPPos) - 1):
178  sum += np.linalg.norm(nbvNPPos[i] - nbvNPPos[i+1])
179  return sum
180 
181 def rateViewport(pose):
182  if type(pose) is not list:
183  pose = [pose]
184  rate_viewports = rospy.ServiceProxy("/nbv/rate_viewports", RateViewports)
185  return rate_viewports(getCurrentCameraPose(), pose, ["Cup", "PlateDeep"], False)
186 
187 def getActualNBV(nbv):
188  oldPos = (basePos, ptuPos)
189  position, orientation = getPositionAndOrientationFromNBV(nbv)
190  moveBase(*position)
191  setPTU(*orientation)
192  nbvCameraPose = getCurrentCameraPose()
193  print(getCurrentCameraPose())
194  print(str((basePos, ptuPos)))
196  ratedMovedNbv = rateViewport(nbvCameraPose)
197  return ratedMovedNbv
198 
199 # this actually takes nbvs as parameters
200 pub = rospy.Publisher("/nbv/viewportsVisualization", MarkerArray, queue_size=10)
201 
202 def showPoses(poses):
203  if type(poses) is not list:
204  poses = [poses]
205  viewportMarkers = MarkerArray()
206  i = 0
207  for pose in poses:
208  # straight vertical line
209  newMarker = Marker()
210  newMarker.color.r = 1.0
211  newMarker.color.a = 1.0
212  newMarker.scale.x = 0.05
213  newMarker.scale.y = 0.05
214  newMarker.scale.z = 0.05
215  lowerPoint = copy.deepcopy(pose.position)
216  lowerPoint.z = 0
217  upperPoint = copy.deepcopy(pose.position)
218  newMarker.points.append(lowerPoint)
219  newMarker.points.append(upperPoint)# perfect = getNBVs(mayFolder)[0]
220  newMarker.header.frame_id = "/map"
221  newMarker.ns = "viewports"
222  newMarker.id = 0 + i * 2
223  newMarker.type = Marker.LINE_LIST
224  newMarker.action = Marker.ADD
225  viewportMarkers.markers.append(newMarker)
226  # arrow
227  newMarker = Marker()
228  newMarker.color.r = 1.0
229  newMarker.color.a = 1.0
230  newMarker.scale.x = 0.5
231  newMarker.scale.y = 0.05
232  newMarker.scale.z = 0.05
233  newMarker.pose.position = pose.position
234 
235  newMarker.pose.orientation = pose.orientation
236  newMarker.header.frame_id = "/map"
237  newMarker.ns = "viewports"
238  newMarker.id = 1 + i * 2
239  newMarker.type = Marker.ARROW
240  newMarker.action = Marker.ADD
241  viewportMarkers.markers.append(newMarker)
242  i += 1
243 
244  pub.publish(viewportMarkers)
245 
246 def showNBVs(nbvs):
247  if type(nbvs) is not list:
248  nbvs = [nbvs]
249  poses = []
250  for nbv in nbvs:
251  poses.append(getPoseFromNBV(nbv))
252  showPoses(poses)
def restartPanes(windowAndPanes)
Definition: tmux.py:130
def setOldcacheParameters()
Definition: nbv.py:59
def setRobotState()
Definition: nbv.py:95
def getNBV()
Definition: nbv.py:104
def nbvUpdateFromNBV(nbv)
Definition: nbv.py:110
def moveBase(x, y, rotation)
Definition: move.py:69
def setGAParameters()
Definition: nbv.py:70
def getPCAndViewportsFromLog(logDir, idx)
Definition: nbv.py:121
def getNBVs(logDir)
Definition: nbv.py:140
def setOptcacheParameters()
Definition: nbv.py:37
def getNumberOfNBVs(logDir)
Definition: nbv.py:133
def getPC()
Definition: nbv.py:117
def showPoses(poses)
Definition: nbv.py:202
def setOldParameters()
Definition: nbv.py:48
def setPositionAndOrientation(positionAndOrientation)
Definition: move.py:93
def getPositionAndOrientationFromNBV(nbv)
Definition: nbv.py:165
def getCurrentCameraPose()
Definition: nbv.py:92
def restartAndWaitForNBV()
Definition: nbv.py:22
def setPCAndViewportsFromLog(logDir, idx)
Definition: nbv.py:126
def getFile(fileLoc)
def getActualNBV(nbv)
Definition: nbv.py:187
def getPoseFromNBV(nbv)
Definition: nbv.py:153
def setOptParameters()
Definition: nbv.py:26
def waitForAllModules(silence=False)
Definition: general_ros.py:11
def getTotalDistance(nbvs)
Definition: nbv.py:173
def getYaml(file)
def getOptimalNBV(logDir, idx, positionAndOrientation)
Definition: nbv.py:81
def saveYaml(file, dataMap, flow_style=False)
def rateViewport(pose)
Definition: nbv.py:181
def setPTU(pan, tilt)
Definition: move.py:50
def nbvUpdate(pose, obj_types)
Definition: nbv.py:113
def setPC(pointcloud, viewports)
Definition: nbv.py:129
def showNBVs(nbvs)
Definition: nbv.py:246


asr_resources_for_active_scene_recognition
Author(s): Allgeyer Tobias, Aumann Florian, Borella Jocelyn, Karrenbauer Oliver, Marek Felix, Mei├čner Pascal, Stroh Daniel, Trautmann Jeremias
autogenerated on Tue Feb 18 2020 03:14:15