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 callback(data):
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 size",
data.voxel_size, data.voxels_per_side)
rospy.loginfo("Layer type is: %s", data.layer_type)
rospy.loginfo("action is: %d", data.action)
rospy.loginfo("Number of blocks: %d", len(data.blocks))
esdf_layer = voxblox.EsdfLayer(data.voxel_size, data.voxels_per_side)
# layermsg = voxblox_ros.Layer
# layermsg = Layer
# import pdb; pdb.set_trace()
# Loop through each block in the message
for block_msg in data.blocks:
# Get the block from the index (function will vreate a new one if it doesn't exist yet)
block = esdf_layer.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())
# # Loop through the data in the block
# voxel_id = 0
# count = 0
# for packet in block_msg.data: # range(0,len(block.data)): # For each data packet
# if np.mod(count,2) == 0: # if even
# # Get the current voxel
# voxel = block.getVoxelByLinearIndex(voxel_id)
# voxel.distance = packet
# # voxel.distance[0:0+4] = packet # try something like memcpy? https://stackoverflow.com/questions/13689628/is-there-a-python-equivalent-to-memcpy
# # voxel.weight = (packet & 0x0000FFFF)*max_ntsdf_voxel_weight/0xFFFF
# # print("Voxel {} has distance {}".format(voxel_id,voxel.distance))
# voxel_id = voxel_id + 1
# else:
# pass # Second byte is color - don't care about this
# count = count + 1
# import pdb; pdb.set_trace()
esdf_layer.saveToFile(
"/home/bjm/TORQ/gcs_ws/src/torq/torq_gcs/mesh/test_unreal_esdf_new.proto"
)
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
#print(e)
# convert Tango TSDF to TSDF and serialize
#ntl = voxblox_tango_interfacepy.tangoLoadLayer("/Users/mereweth/Desktop/map.ntsdf.proto")
#ntl.saveToFile("/Users/mereweth/Desktop/map.tsdf.proto")
# make sure this throws an exception rather than segfaulting
try:
voxblox.loadTsdfLayer("/Users/mereweth/Desktop/map.ntsdf.proto")
except RuntimeError as e:
print(e)
tl = voxblox.loadTsdfLayer(
"/Users/mereweth/Desktop/environments/bldg_344_huge/map_cleared.tsdf.proto"
)
el = voxblox.EsdfLayer(tl.voxel_size, tl.voxels_per_side)
m = voxblox.EsdfMap(el)
ei = voxblox.EsdfIntegrator(voxblox.EsdfIntegratorConfig(), tl, el)
ei.updateFromTsdfLayerBatch()
b = tl.allocateBlockPtrByCoordinates(np.array([0, 0, 0.5], dtype='double'))
v = b.getVoxelByCoordinates(np.array([0, 0, 0.5], dtype='double'))
v.distance = 0.3
b.set_updated(True)
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")