def __init__(self):
# Global goal variable
self.goal = dict()
# Global start variable
self.start = dict()
self.app = QApplication( sys.argv )
self.global_dict = dict()
# Time for publishing
self.setpointCount = 0
self.time = 0.0
self.tmax = 100.0
self.averageVel = 0.25 # m/s
# Times for replanning
self.replanHz = 0.1
self.timeOfReplan = 0.0
self.startDeltaT = 0.5 # Time ahead of current time to use as the start location
self.firstPlan = True
# Initialise the map
self.esdfLayer = voxblox.EsdfLayer(0.2,16) # Start with default values
self.global_dict['fsp_out_map'] = voxblox.EsdfMap(self.esdfLayer)# simple implementation now
# Init planner GCS object
self.planner = torq_gcs.plan.astro_plan.QRPolyTrajGUI(self.global_dict,defer=True,curv_func=False)
def on_load_tsdf_button_click(self, checked=False, filename=None):
if filename is None:
filename = QFileDialog.getOpenFileName(
self,
'Import TSDF', #path,
"TSDF proto files (*tsdf.proto)")
if filename and len(filename) > 0:
filename = filename[0]
else:
print("Invalid file path")
return
try:
self.tsdf_layer = voxblox.loadTsdfLayer(filename)
self.tsdf_map = voxblox.TsdfMap(self.tsdf_layer)
self.esdf_layer = voxblox.EsdfLayer(
self.tsdf_layer.voxel_size, self.tsdf_layer.voxels_per_side)
self.global_dict['fsp_out_map'] = voxblox.EsdfMap(self.esdf_layer)
self.slice_value_spin_box.setSingleStep(
self.global_dict['fsp_out_map'].voxel_size)
# TODO([email protected]) - specify config
# how to deal with updating config?
esdf_integrator_config = voxblox.EsdfIntegratorConfig()
# TODO([email protected]) - hacky; how to get truncation distance from NTSDF dump?
esdf_integrator_config.min_distance_m = 0.22
self.esdf_integrator = voxblox.EsdfIntegrator(
esdf_integrator_config, self.tsdf_layer, self.esdf_layer)
except Exception as e:
print("Unknown error loading TSDF from {}".format(filename))
print(e)
return
def readESDFMapMessage(self,msg):
# rospy.loginfo(rospy.get_caller_id() + "In callback from esdf listening in python")
# Try to pull out parts of the message
rospy.loginfo("Voxel size is: %f, and there are %d voxels per side", msg.voxel_size,msg.voxels_per_side)
# rospy.loginfo("Layer type is: %s",msg.layer_type)
# rospy.loginfo("Action is: %d",msg.action)
rospy.loginfo("Number of blocks: %d", len(msg.blocks))
esdfLayer = voxblox.EsdfLayer(msg.voxel_size,msg.voxels_per_side)
# Loop through each block in the message
for block_msg in msg.blocks:
# Get the block from the index (function will vreate a new one if it doesn't exist yet)
block = esdfLayer.allocateBlockPtrByIndex(np.array([block_msg.x_index, block_msg.y_index, block_msg.z_index], dtype='int'))
# block.deserializeFromIntegers(block_msg.data)
block.deserializeFromIntegers(np.array(block_msg.data,dtype="uint32").tolist())
# update map TODO(TDBM) can this process be made more efficient to udpate the representation?
esdfMap = voxblox.EsdfMap(esdfLayer)
self.global_dict['fsp_out_map'] = esdfMap
# Run planner
if self.time > 1/self.replanHz or self.firstPlan: # If the time since the last replan is more than the desired period
self.resetStartFromTraj()
print("\n\nTime to replan ({}): Running ASTRO\n\n".format(self.time))
self.time = 0.0 # Starting at the start of the new trajectory
self.updateEsdfObstacle()
self.planTrajectory()
print("\n\n\t\t COMPLETED TRAJECTORY PLAN \n\n")
# Reset times:
# self.timeOfReplan = self.time
self.firstPlan = False
def on_load_esdf_button_click(self, checked=False, filename=None):
if filename is None:
filename = QFileDialog.getOpenFileName(
self,
'Import ESDF', #path,
"ESDF proto files (*esdf.proto)")
if filename and len(filename) > 0:
filename = filename[0]
else:
print("Invalid file path")
return
try:
#TODO([email protected]) - should we clear the TSDF?
self.esdf_layer = voxblox.loadEsdfLayer(filename)
self.global_dict['fsp_out_map'] = voxblox.EsdfMap(self.esdf_layer)
self.slice_value_spin_box.setSingleStep(
self.global_dict['fsp_out_map'].voxel_size)
except Exception as e:
print("Unknown error loading ESDF from {}".format(filename))
print(e)
return
v3 = b3.getVoxelByCoordinates(np.array([0, 0, 0.5], dtype='double'))
v4 = b3.getVoxelByLinearIndex(0)
import pdb
pdb.set_trace()
voxblox.clearSphereAroundPoint(el, np.array([0, 0, 0.5], dtype='double'), 0.5)
voxblox.fillSphereAroundPoint(el, np.array([0, 0, 0.5], dtype='double'), 0.5)
el.saveToFile("/Users/mereweth/Desktop/_test_cow_and_lady.esdf.proto")
layer = voxblox.loadEsdfLayer(
'/Users/mereweth/Desktop/environments/bldg_344_huge/map_cleared.esdf.proto'
)
assert (layer is not None)
map = voxblox.EsdfMap(layer)
print map.voxel_size
print map.block_size
import numpy as np
x_ = np.linspace(0.0, 0.2, 100)
y_ = np.linspace(0.0, 0.2, 100)
z_ = np.linspace(0.0, 0.2, 100)
x, y, z = np.meshgrid(x_, y_, z_)
query = np.matrix(np.c_[x.flatten(), y.flatten(), z.flatten()]).T
#query = np.matrix([[0,0,0.1],
# [0.1,0,0],
# [0.1,0.1,0],
#!/usr/bin/env python
import timeit
import sys
sys.path.append(
'/Users/mereweth/snappy/tsdf_catkin_ws/devel/.private/voxblox/lib/')
import voxblox
dir(voxblox.EsdfMap)
try:
map = voxblox.EsdfMap('THIS_DOES_NOT_EXIST')
except RuntimeError as e:
print(e)
map = voxblox.EsdfMap(
'/Users/mereweth/Desktop/cow_and_lady/cow_and_lady.esdf.proto')
import numpy as np
x_ = np.linspace(0.0, 0.2, 100)
y_ = np.linspace(0.0, 0.2, 100)
z_ = np.linspace(0.0, 0.2, 100)
x, y, z = np.meshgrid(x_, y_, z_)
query = np.matrix(np.c_[x.flatten(), y.flatten(), z.flatten()]).T
#query = np.matrix([[0,0,0.1],
# [0.1,0,0],
# [0.1,0.1,0],
# [0,0.1,0]], dtype='double').T