From f4cb234a1b3b60204fb1e946e9694b42b2d47d26 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Mon, 10 Feb 2025 20:45:21 -0600 Subject: [PATCH 01/27] Added an example of how to create a 3d matrix --- .../cuda/examples/CUDAExampleKernel3D.java | 102 ++++++++++++++++++ .../perception/cuda/examples/matrix_3d.cu | 27 +++++ 2 files changed, 129 insertions(+) create mode 100644 ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java create mode 100644 ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java new file mode 100644 index 00000000000..a5196feb92f --- /dev/null +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -0,0 +1,102 @@ +package us.ihmc.perception.cuda.examples; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.dim3; +import org.bytedeco.cuda.global.cudart; +import org.bytedeco.javacpp.ShortPointer; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; +import org.bytedeco.opencv.opencv_core.Scalar; +import us.ihmc.perception.cuda.CUDAKernel; +import us.ihmc.perception.cuda.CUDAProgram; +import us.ihmc.perception.cuda.CUDAStreamManager; +import us.ihmc.perception.cuda.CUDATools; + +import java.net.URL; +import java.util.ArrayList; +import java.util.List; + +import static org.bytedeco.cuda.global.cudart.*; + +public class CUDAExampleKernel3D +{ + public CUDAExampleKernel3D() throws Exception + { + int width = 2; + int height = 2; + int layers = 2; + + CUstream_st stream = CUDAStreamManager.getStream(); + + GpuMat gpuMatExample = new GpuMat(height, width, opencv_core.CV_16UC1); + long pitchA = gpuMatExample.step(); // CUDA pitch (in bytes) + + // Allocate enough memory for all layers + ShortPointer heightMapHistory = new ShortPointer(); + cudaMalloc(heightMapHistory, layers * pitchA * height); + int error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + URL programPath = getClass().getResource("matrix_3d.cu"); + String kernelName = "matrix_3d_example"; + + + Mat cpuData1 = new Mat(height, width, opencv_core.CV_16UC1); + cpuData1.ptr(0, 0, 1); + Mat cpuData2 = new Mat(height, width, opencv_core.CV_16UC1); + cpuData2.ptr(0, 0, 2); + + List heightMaps = new ArrayList<>(); + heightMaps.add(cpuData1); + heightMaps.add(cpuData2); + + List gpuHeightMaps = new ArrayList<>(); + + for (int i = 0; i < layers; i++) + { + Mat cpuData = new Mat(height, width, opencv_core.CV_16UC1, new Scalar(i + 1)); +// cpuData.setTo(new Scalar(i + 1)); // Example: Fill with 1 for layer 0, 2 for layer 1 + + GpuMat gpuData = new GpuMat(); + gpuData.upload(cpuData); + + // Copy this layer into the allocated memory + cudaMemcpy2D(heightMapHistory.position(i * pitchA * height), pitchA, + gpuData.data(), gpuData.step(), + width * Short.BYTES, height, + cudaMemcpyHostToDevice); + } + + CUDAProgram program = new CUDAProgram(programPath); + CUDAKernel kernel = program.loadKernel(kernelName); + + kernel.withPointer(heightMapHistory); + kernel.withLong(pitchA); + kernel.withLong(pitchA * height); // layerSize = pitchA * height + kernel.withInt(height); + kernel.withInt(width); + kernel.withInt(layers); + + dim3 gridDim = new dim3(); // The same thing as ( new dim3(1, 1, 1); ) + dim3 blockDim = new dim3(width, height, 1); + kernel.run(stream, gridDim, blockDim, 0); + + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + cudaStreamSynchronize(stream); + + kernel.close(); + program.close(); + gridDim.close(); + blockDim.close(); + + CUDAStreamManager.releaseStream(stream); + } + + public static void main(String[] args) throws Exception + { + new CUDAExampleKernel3D(); + } +} diff --git a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu new file mode 100644 index 00000000000..c287ba56ae0 --- /dev/null +++ b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu @@ -0,0 +1,27 @@ +extern "C" +__global__ +void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, size_t layerSize, int rows, int cols, int layers) +{ + int indexX = blockIdx.x * blockDim.x + threadIdx.x; // Column index + int indexY = blockIdx.y * blockDim.y + threadIdx.y; // Row index + + if (indexX >= cols || indexY >= rows) + return; // Prevent out-of-bounds access + + // Loop over layers + for (int layer = 0; layer < layers; layer++) + { + // Compute the base address of the current layer + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + + // Compute row offset using pitchA + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + // Fetch value from the current layer + int query_height_int = (int)(*matrixPointerRow); + + // Print out the value along with its position in the 3D matrix + printf("Layer %d, Value at (%d, %d): %d\n", layer, indexX, indexY, query_height_int); + } +} + From a8294a643418ed5abb7107f3472d83352bbacaea Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Mon, 10 Feb 2025 21:25:50 -0600 Subject: [PATCH 02/27] Remove unused code for testing --- .../cuda/examples/CUDAExampleKernel3D.java | 16 ---------------- 1 file changed, 16 deletions(-) diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java index a5196feb92f..a3d97582b5a 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -2,7 +2,6 @@ import org.bytedeco.cuda.cudart.CUstream_st; import org.bytedeco.cuda.cudart.dim3; -import org.bytedeco.cuda.global.cudart; import org.bytedeco.javacpp.ShortPointer; import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; @@ -14,8 +13,6 @@ import us.ihmc.perception.cuda.CUDATools; import java.net.URL; -import java.util.ArrayList; -import java.util.List; import static org.bytedeco.cuda.global.cudart.*; @@ -41,22 +38,9 @@ public CUDAExampleKernel3D() throws Exception URL programPath = getClass().getResource("matrix_3d.cu"); String kernelName = "matrix_3d_example"; - - Mat cpuData1 = new Mat(height, width, opencv_core.CV_16UC1); - cpuData1.ptr(0, 0, 1); - Mat cpuData2 = new Mat(height, width, opencv_core.CV_16UC1); - cpuData2.ptr(0, 0, 2); - - List heightMaps = new ArrayList<>(); - heightMaps.add(cpuData1); - heightMaps.add(cpuData2); - - List gpuHeightMaps = new ArrayList<>(); - for (int i = 0; i < layers; i++) { Mat cpuData = new Mat(height, width, opencv_core.CV_16UC1, new Scalar(i + 1)); -// cpuData.setTo(new Scalar(i + 1)); // Example: Fill with 1 for layer 0, 2 for layer 1 GpuMat gpuData = new GpuMat(); gpuData.upload(cpuData); From 7f03cdaca8b5c42e8b39184da0a69fd82ff1663d Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 11 Feb 2025 09:27:25 -0600 Subject: [PATCH 03/27] Fixed 3d cuda example and return the average to the cpu --- .../cuda/examples/CUDAExampleKernel3D.java | 79 +++++++++++++------ .../perception/cuda/examples/matrix_3d.cu | 25 +++--- 2 files changed, 70 insertions(+), 34 deletions(-) diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java index a3d97582b5a..e47f0120556 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -11,71 +11,102 @@ import us.ihmc.perception.cuda.CUDAProgram; import us.ihmc.perception.cuda.CUDAStreamManager; import us.ihmc.perception.cuda.CUDATools; +import us.ihmc.perception.tools.PerceptionDebugTools; import java.net.URL; +import java.util.ArrayList; +import java.util.List; import static org.bytedeco.cuda.global.cudart.*; public class CUDAExampleKernel3D { + /** + * This is an example of how to pass in a 3d array into the CUDA kernel. + * Each index will have values in each layer. + * This averages each index's layer and returns the result in a {@link GpuMat}. + */ public CUDAExampleKernel3D() throws Exception { - int width = 2; - int height = 2; - int layers = 2; + // Create a 2x2 with 5 layers + int rows = 2; + int cols = 2; + int layers = 4; CUstream_st stream = CUDAStreamManager.getStream(); - GpuMat gpuMatExample = new GpuMat(height, width, opencv_core.CV_16UC1); - long pitchA = gpuMatExample.step(); // CUDA pitch (in bytes) + URL programPath = getClass().getResource("matrix_3d.cu"); + String kernelName = "matrix_3d_example"; + CUDAProgram program = new CUDAProgram(programPath); + CUDAKernel kernel = program.loadKernel(kernelName); + + GpuMat gpuMatExample = new GpuMat(rows, cols, opencv_core.CV_16UC1); + long pitchForLayer = gpuMatExample.step(); // Allocate enough memory for all layers - ShortPointer heightMapHistory = new ShortPointer(); - cudaMalloc(heightMapHistory, layers * pitchA * height); + ShortPointer pointerTo3DArray = new ShortPointer(); + cudaMalloc(pointerTo3DArray, layers * pitchForLayer * rows); int error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); - URL programPath = getClass().getResource("matrix_3d.cu"); - String kernelName = "matrix_3d_example"; + List gpuLayers = new ArrayList<>(); for (int i = 0; i < layers; i++) { - Mat cpuData = new Mat(height, width, opencv_core.CV_16UC1, new Scalar(i + 1)); + // To get the average to be a whole number do (i * 2 + 2) + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(i * 2 + 2)); + // Upload that data to the gpu so we can allocate the memory for it GpuMat gpuData = new GpuMat(); gpuData.upload(cpuData); - // Copy this layer into the allocated memory - cudaMemcpy2D(heightMapHistory.position(i * pitchA * height), pitchA, - gpuData.data(), gpuData.step(), - width * Short.BYTES, height, + // Allocate memory for each layer, creating a 3d array + cudaMemcpy2D(pointerTo3DArray.position(i * pitchForLayer * rows), + pitchForLayer, + gpuData.data(), + gpuData.step(), + cols * Short.BYTES, + rows, cudaMemcpyHostToDevice); + + // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end + gpuLayers.add(gpuData); } - CUDAProgram program = new CUDAProgram(programPath); - CUDAKernel kernel = program.loadKernel(kernelName); + GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); - kernel.withPointer(heightMapHistory); - kernel.withLong(pitchA); - kernel.withLong(pitchA * height); // layerSize = pitchA * height - kernel.withInt(height); - kernel.withInt(width); + kernel.withPointer(pointerTo3DArray).withLong(gpuMatExample.step()); + kernel.withPointer(result.data()).withLong(result.step()); + kernel.withLong(pitchForLayer * rows); + kernel.withInt(rows); + kernel.withInt(cols); kernel.withInt(layers); - dim3 gridDim = new dim3(); // The same thing as ( new dim3(1, 1, 1); ) - dim3 blockDim = new dim3(width, height, 1); + dim3 gridDim = new dim3(); + dim3 blockDim = new dim3(cols, rows, 1); kernel.run(stream, gridDim, blockDim, 0); error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); - cudaStreamSynchronize(stream); + // Print the result to make sure we get what we expect + // In this example we go through 2,4,6,8 so we expect the average to be 5 + Mat cpuResult = new Mat(); + result.download(cpuResult); + PerceptionDebugTools.printMat("Result", cpuResult, 1); kernel.close(); program.close(); gridDim.close(); blockDim.close(); + // This is where we close the individual gpu layers + for (int i = 0; i < layers; i++) + { + gpuLayers.get(i).release(); + gpuLayers.get(i).close(); + } + CUDAStreamManager.releaseStream(stream); } diff --git a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu index c287ba56ae0..baf164f8a48 100644 --- a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu +++ b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu @@ -1,14 +1,17 @@ extern "C" __global__ -void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, size_t layerSize, int rows, int cols, int layers) +void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, + unsigned short * resultPointer, size_t pitchResult, + size_t layerSize, int rows, int cols, int layers) { - int indexX = blockIdx.x * blockDim.x + threadIdx.x; // Column index - int indexY = blockIdx.y * blockDim.y + threadIdx.y; // Row index + int indexX = blockIdx.x * blockDim.x + threadIdx.x; + int indexY = blockIdx.y * blockDim.y + threadIdx.y; if (indexX >= cols || indexY >= rows) - return; // Prevent out-of-bounds access + return; + + int sum = 0; - // Loop over layers for (int layer = 0; layer < layers; layer++) { // Compute the base address of the current layer @@ -17,11 +20,13 @@ void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, size_t lay // Compute row offset using pitchA unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - // Fetch value from the current layer - int query_height_int = (int)(*matrixPointerRow); - - // Print out the value along with its position in the 3D matrix - printf("Layer %d, Value at (%d, %d): %d\n", layer, indexX, indexY, query_height_int); + sum += (int) *matrixPointerRow; } + + unsigned short avg = sum / layers; + + unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; + *outputPointer = avg; + printf("GPU Average: %d, ", avg); } From c5b1f13002940d02d1f1bb53520a48e337a5d93c Mon Sep 17 00:00:00 2001 From: Tomasz Bialek Date: Tue, 11 Feb 2025 11:10:12 -0600 Subject: [PATCH 04/27] Using cudaMalloc3D --- .../cuda/examples/CUDAExampleKernel3D.java | 44 +++++++------------ .../perception/cuda/examples/matrix_3d.cu | 1 + 2 files changed, 16 insertions(+), 29 deletions(-) diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java index e47f0120556..0c22ac6dbf3 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -1,6 +1,8 @@ package us.ihmc.perception.cuda.examples; import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.cudaExtent; +import org.bytedeco.cuda.cudart.cudaPitchedPtr; import org.bytedeco.cuda.cudart.dim3; import org.bytedeco.javacpp.ShortPointer; import org.bytedeco.opencv.global.opencv_core; @@ -40,44 +42,35 @@ public CUDAExampleKernel3D() throws Exception CUDAProgram program = new CUDAProgram(programPath); CUDAKernel kernel = program.loadKernel(kernelName); - GpuMat gpuMatExample = new GpuMat(rows, cols, opencv_core.CV_16UC1); - long pitchForLayer = gpuMatExample.step(); - // Allocate enough memory for all layers - ShortPointer pointerTo3DArray = new ShortPointer(); - cudaMalloc(pointerTo3DArray, layers * pitchForLayer * rows); - int error = cudaStreamSynchronize(stream); + cudaPitchedPtr pointerTo3DArray = new cudaPitchedPtr(); + cudaExtent extent = make_cudaExtent(cols * Short.BYTES, rows, layers); + int error = cudaMalloc3D(pointerTo3DArray, extent); CUDATools.checkCUDAError(error); - List gpuLayers = new ArrayList<>(); - for (int i = 0; i < layers; i++) { // To get the average to be a whole number do (i * 2 + 2) Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(i * 2 + 2)); - // Upload that data to the gpu so we can allocate the memory for it - GpuMat gpuData = new GpuMat(); - gpuData.upload(cpuData); - // Allocate memory for each layer, creating a 3d array - cudaMemcpy2D(pointerTo3DArray.position(i * pitchForLayer * rows), - pitchForLayer, - gpuData.data(), - gpuData.step(), - cols * Short.BYTES, - rows, - cudaMemcpyHostToDevice); + cudaMemcpy2D(pointerTo3DArray.ptr().position(i * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + cpuData.data(), + cpuData.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end - gpuLayers.add(gpuData); + cpuData.close(); } GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); - kernel.withPointer(pointerTo3DArray).withLong(gpuMatExample.step()); + kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); - kernel.withLong(pitchForLayer * rows); + kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); kernel.withInt(layers); @@ -100,13 +93,6 @@ public CUDAExampleKernel3D() throws Exception gridDim.close(); blockDim.close(); - // This is where we close the individual gpu layers - for (int i = 0; i < layers; i++) - { - gpuLayers.get(i).release(); - gpuLayers.get(i).close(); - } - CUDAStreamManager.releaseStream(stream); } diff --git a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu index baf164f8a48..cf8a5bb23bb 100644 --- a/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu +++ b/ihmc-perception/src/test/resources/us/ihmc/perception/cuda/examples/matrix_3d.cu @@ -20,6 +20,7 @@ void matrix_3d_example(unsigned short * matrixPointer, size_t pitchA, // Compute row offset using pitchA unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); sum += (int) *matrixPointerRow; } From 06c1006a754a97b57d0bfb95feb02674ea758992 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 11 Feb 2025 13:05:39 -0600 Subject: [PATCH 05/27] Started writing a test to have this example be used in a class --- .../FilteredRapidHeightMapExtractor.java | 143 ++++++++++++++++++ .../FilteredRapidHeightMapExtractor.cu | 32 ++++ .../FilteredRapidHeightMapExtractorTest.java | 36 +++++ 3 files changed, 211 insertions(+) create mode 100644 ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java create mode 100644 ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu create mode 100644 ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java new file mode 100644 index 00000000000..7825ab424fe --- /dev/null +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -0,0 +1,143 @@ +package us.ihmc.perception.gpuHeightMap; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.dim3; +import org.bytedeco.javacpp.ShortPointer; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; +import org.bytedeco.opencv.opencv_core.Scalar; +import us.ihmc.perception.cuda.CUDAKernel; +import us.ihmc.perception.cuda.CUDAProgram; +import us.ihmc.perception.cuda.CUDATools; +import us.ihmc.perception.tools.PerceptionDebugTools; + +import java.net.URL; +import java.util.ArrayList; +import java.util.List; + +import static org.bytedeco.cuda.global.cudart.*; + +public class FilteredRapidHeightMapExtractor +{ + private final List gpuLayers; + private final ShortPointer pointerTo3DArray; + private final long pitchForLayer; + private final GpuMat gpuMatExample; + private int currentIndex; + int layers = 2; + + private final CUstream_st stream; + private final int rows; + private final int cols; + private final CUDAKernel kernel; + private final CUDAProgram program; + private int loopTracker = 0; + + public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) + { + this.stream = stream; + this.rows = rows; + this.cols = cols; + + // Load header and main file + URL kernelPath = getClass().getResource("FilteredRapidHeightMapExtractor.cu"); + try + { + program = new CUDAProgram(kernelPath); + kernel = program.loadKernel("filterRapidHeightMap"); + } + catch (Exception e) + { + throw new RuntimeException(e); + } + + gpuMatExample = new GpuMat(rows, cols, opencv_core.CV_16UC1); + pitchForLayer = gpuMatExample.step(); + + pointerTo3DArray = new ShortPointer(); + cudaMalloc(pointerTo3DArray, layers * pitchForLayer * rows); + int error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + gpuLayers = new ArrayList<>(layers); + currentIndex = 0; + + for (int i = 0; i < layers; i++) + { + // To get the average to be a whole number do (i * 2 + 2) + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(0)); + + // Upload that data to the gpu so we can allocate the memory for it + GpuMat gpuData = new GpuMat(); + gpuData.upload(cpuData); + + // Allocate memory for each layer, creating a 3d array + cudaMemcpy2D(pointerTo3DArray.position(i * pitchForLayer * rows), + pitchForLayer, + gpuData.data(), + gpuData.step(), + cols * Short.BYTES, + rows, + cudaMemcpyHostToDevice); + + // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end + gpuLayers.add(gpuData); + } + } + + public GpuMat update(GpuMat latestGlobalHeightMap) + { + // Only want to compute the average if we have the past values to use + if (loopTracker < layers) + { + loopTracker++; + latestGlobalHeightMap.convertTo(gpuLayers.get(currentIndex), latestGlobalHeightMap.type()); + Mat temp = new Mat(); + gpuLayers.get(currentIndex).download(temp); + PerceptionDebugTools.printMat("s", temp, 1); + currentIndex = (currentIndex + 1) % layers; + return latestGlobalHeightMap; + } + + int error; + + GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); + + kernel.withPointer(pointerTo3DArray).withLong(gpuMatExample.step()); + kernel.withPointer(result.data()).withLong(result.step()); + kernel.withLong(pitchForLayer * rows); + kernel.withInt(rows); + kernel.withInt(cols); + kernel.withInt(layers); + + dim3 gridDim = new dim3(); + dim3 blockDim = new dim3(cols, rows, 1); + kernel.run(stream, gridDim, blockDim, 0); + + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + // Print the result to make sure we get what we expect + // In this example we go through 2,4,6,8 so we expect the average to be 5 + Mat cpuResult = new Mat(); + result.download(cpuResult); + PerceptionDebugTools.printMat("Result", cpuResult, 1); + + gpuLayers.get(currentIndex).setTo(new Scalar(1), latestGlobalHeightMap); + currentIndex = (currentIndex + 1) % layers; + + return result; + } + + public void reset() + { + loopTracker = 0; + } + + public void close() + { + program.close(); + kernel.close(); + } +} diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu new file mode 100644 index 00000000000..7d4faba4595 --- /dev/null +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -0,0 +1,32 @@ +extern "C" +__global__ +void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, + unsigned short * resultPointer, size_t pitchResult, + size_t layerSize, int rows, int cols, int layers) +{ + int indexX = blockIdx.x * blockDim.x + threadIdx.x; + int indexY = blockIdx.y * blockDim.y + threadIdx.y; + + if (indexX >= cols || indexY >= rows) + return; + + int sum = 0; + + for (int layer = 0; layer < layers; layer++) + { + // Compute the base address of the current layer + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + + // Compute row offset using pitchA + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + printf("Layer: %d, Value: %d, %d, and: %d\n", layer, indexY, indexX, (int) *matrixPointerRow); + sum += (int) *matrixPointerRow; + } + + unsigned short avg = sum / layers; + + unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; + *outputPointer = avg; +} + diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java new file mode 100644 index 00000000000..e1c0cc47854 --- /dev/null +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java @@ -0,0 +1,36 @@ +package us.ihmc.perception.gpuHeightMap; + +import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.opencv.global.opencv_core; +import org.bytedeco.opencv.opencv_core.GpuMat; +import org.bytedeco.opencv.opencv_core.Mat; +import org.bytedeco.opencv.opencv_core.Scalar; +import org.junit.jupiter.api.Test; +import us.ihmc.perception.cuda.CUDAStreamManager; +import us.ihmc.perception.tools.PerceptionDebugTools; + +public class FilteredRapidHeightMapExtractorTest +{ + @Test + public void testGettingAverage() + { + int rows = 2; + int cols = 2; + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + + for (int i = 0; i < 8; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(i * 2 + 2)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("current", temp, 1); + } + + filteredRapidHeightMapExtractor.close(); + } +} From 94dac473e928601e5f25b31776ae8c2ff959c2bb Mon Sep 17 00:00:00 2001 From: nkitchel Date: Tue, 11 Feb 2025 13:58:20 -0600 Subject: [PATCH 06/27] Fixed FilteredRapidHeightMapExtractor to compute the average of the history of height maps --- .../FilteredRapidHeightMapExtractor.java | 76 ++++++++++--------- .../FilteredRapidHeightMapExtractor.cu | 3 +- 2 files changed, 43 insertions(+), 36 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 7825ab424fe..6349f6c5bc4 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -1,29 +1,26 @@ package us.ihmc.perception.gpuHeightMap; import org.bytedeco.cuda.cudart.CUstream_st; +import org.bytedeco.cuda.cudart.cudaExtent; +import org.bytedeco.cuda.cudart.cudaPitchedPtr; import org.bytedeco.cuda.cudart.dim3; -import org.bytedeco.javacpp.ShortPointer; import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; import org.bytedeco.opencv.opencv_core.Scalar; +import org.jetbrains.annotations.NotNull; import us.ihmc.perception.cuda.CUDAKernel; import us.ihmc.perception.cuda.CUDAProgram; import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.tools.PerceptionDebugTools; import java.net.URL; -import java.util.ArrayList; -import java.util.List; import static org.bytedeco.cuda.global.cudart.*; public class FilteredRapidHeightMapExtractor { - private final List gpuLayers; - private final ShortPointer pointerTo3DArray; - private final long pitchForLayer; - private final GpuMat gpuMatExample; + private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; int layers = 2; @@ -52,15 +49,11 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) throw new RuntimeException(e); } - gpuMatExample = new GpuMat(rows, cols, opencv_core.CV_16UC1); - pitchForLayer = gpuMatExample.step(); - - pointerTo3DArray = new ShortPointer(); - cudaMalloc(pointerTo3DArray, layers * pitchForLayer * rows); - int error = cudaStreamSynchronize(stream); + pointerTo3DArray = new cudaPitchedPtr(); + cudaExtent extent = make_cudaExtent((long) cols * Short.BYTES, rows, layers); + int error = cudaMalloc3D(pointerTo3DArray, extent); CUDATools.checkCUDAError(error); - gpuLayers = new ArrayList<>(layers); currentIndex = 0; for (int i = 0; i < layers; i++) @@ -68,34 +61,40 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) // To get the average to be a whole number do (i * 2 + 2) Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(0)); - // Upload that data to the gpu so we can allocate the memory for it - GpuMat gpuData = new GpuMat(); - gpuData.upload(cpuData); - // Allocate memory for each layer, creating a 3d array - cudaMemcpy2D(pointerTo3DArray.position(i * pitchForLayer * rows), - pitchForLayer, - gpuData.data(), - gpuData.step(), - cols * Short.BYTES, - rows, - cudaMemcpyHostToDevice); - - // Note we can't close the GpuMat here cause we need to access the data later in the program, so add it to a list, and close the list at the end - gpuLayers.add(gpuData); + cudaMemcpy2D(pointerTo3DArray.ptr().position(i * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + cpuData.data(), + cpuData.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); } } public GpuMat update(GpuMat latestGlobalHeightMap) + { + GpuMat heightMapAverage = computerHeightMapHistoryAverage(latestGlobalHeightMap); + + return latestGlobalHeightMap; + } + + @NotNull + private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) { // Only want to compute the average if we have the past values to use if (loopTracker < layers) { loopTracker++; - latestGlobalHeightMap.convertTo(gpuLayers.get(currentIndex), latestGlobalHeightMap.type()); - Mat temp = new Mat(); - gpuLayers.get(currentIndex).download(temp); - PerceptionDebugTools.printMat("s", temp, 1); + + cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + latestGlobalHeightMap.data(), + latestGlobalHeightMap.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); + currentIndex = (currentIndex + 1) % layers; return latestGlobalHeightMap; } @@ -104,9 +103,9 @@ public GpuMat update(GpuMat latestGlobalHeightMap) GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); - kernel.withPointer(pointerTo3DArray).withLong(gpuMatExample.step()); + kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); - kernel.withLong(pitchForLayer * rows); + kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); kernel.withInt(layers); @@ -124,7 +123,14 @@ public GpuMat update(GpuMat latestGlobalHeightMap) result.download(cpuResult); PerceptionDebugTools.printMat("Result", cpuResult, 1); - gpuLayers.get(currentIndex).setTo(new Scalar(1), latestGlobalHeightMap); + cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + latestGlobalHeightMap.data(), + latestGlobalHeightMap.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); + currentIndex = (currentIndex + 1) % layers; return result; diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 7d4faba4595..1a4496deb55 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -20,7 +20,7 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, // Compute row offset using pitchA unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - printf("Layer: %d, Value: %d, %d, and: %d\n", layer, indexY, indexX, (int) *matrixPointerRow); + printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); sum += (int) *matrixPointerRow; } @@ -28,5 +28,6 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; *outputPointer = avg; + printf("GPU Average: %d, ", avg); } From 29920f7e817528fbc8e94ad97aaa103bed3767c2 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Tue, 11 Feb 2025 20:59:49 -0600 Subject: [PATCH 07/27] Updated the kernel to compute the height estimate based on a moving average and variance --- .../FilteredRapidHeightMapExtractor.java | 8 ++- .../FilteredRapidHeightMapExtractor.cu | 55 ++++++++++++++++--- .../FilteredRapidHeightMapExtractorTest.java | 2 +- 3 files changed, 55 insertions(+), 10 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 6349f6c5bc4..1af1ed7297b 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -30,6 +30,7 @@ public class FilteredRapidHeightMapExtractor private final CUDAKernel kernel; private final CUDAProgram program; private int loopTracker = 0; + private int defaultValue; public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) { @@ -72,8 +73,10 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) } } - public GpuMat update(GpuMat latestGlobalHeightMap) + public GpuMat update(GpuMat latestGlobalHeightMap, int defaultValue) { + this.defaultValue = defaultValue; + GpuMat heightMapAverage = computerHeightMapHistoryAverage(latestGlobalHeightMap); return latestGlobalHeightMap; @@ -102,13 +105,16 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) int error; GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); + GpuMat variance = new GpuMat(rows, cols, opencv_core.CV_16UC1); kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); + kernel.withPointer(variance.data()).withLong(variance.step()); kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); kernel.withInt(layers); + kernel.withInt(defaultValue); dim3 gridDim = new dim3(); dim3 blockDim = new dim3(cols, rows, 1); diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 1a4496deb55..1b8457ee913 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -1,8 +1,11 @@ +#define LAYERS 2 // Set a fixed size + extern "C" __global__ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * resultPointer, size_t pitchResult, - size_t layerSize, int rows, int cols, int layers) + unsigned short * newestHeightMap, size_t pitchNewestHeightMap, + size_t layerSize, int rows, int cols, int defaultValue) { int indexX = blockIdx.x * blockDim.x + threadIdx.x; int indexY = blockIdx.y * blockDim.y + threadIdx.y; @@ -11,23 +14,59 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, return; int sum = 0; + float variance[LAYERS] = {0}; - for (int layer = 0; layer < layers; layer++) + // Compute the average height of the history in order to get the variance at each layer + for (int layer = 0; layer < LAYERS; layer++) { - // Compute the base address of the current layer unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); - - // Compute row offset using pitchA unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); sum += (int) *matrixPointerRow; } - unsigned short avg = sum / layers; + unsigned short avg = sum / LAYERS; + + // Compute the variance for each layer + for (int layer = 0; layer < LAYERS; layer++) + { + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + float diff = (float)(*matrixPointerRow) - avg; + variance[layer] = diff * diff; + } + + double heightSum = 0; + double varianceSum = 0; + + // Compute the height and variance sum + for (int layer = 0; layer < LAYERS; layer++) + { + unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); + unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + + heightSum += (float)(*matrixPointerRow) / variance[layer]; + varianceSum += 1.0 / variance[layer]; + } + + unsigned short newHeight = heightSum / varianceSum; + + + unsigned short * heightValue = (unsigned short*)((char*) newestHeightMap + indexY * pitchNewestHeightMap) + indexX; + float diff = (float) (*heightValue) - avg; + double newVariance = diff * diff; + + if (*heightValue == defaultValue) + return; + + unsigned short heightEstimate = (unsigned short) *heightValue + (newHeight * newVariance) / newVariance; + unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; - *outputPointer = avg; - printf("GPU Average: %d, ", avg); + *outputPointer = heightEstimate; + + printf("GPU Height Estimate: %d\n", heightEstimate); } diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java index e1c0cc47854..c633eeaf563 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java @@ -25,7 +25,7 @@ public void testGettingAverage() GpuMat latestDepthMat = new GpuMat(); latestDepthMat.upload(cpuData); - GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); Mat temp = new Mat(); currentAverage.download(temp); PerceptionDebugTools.printMat("current", temp, 1); From ab8ee1aa8f94f57bc017ffeecd75a40ad477aa31 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Wed, 12 Feb 2025 11:07:29 -0600 Subject: [PATCH 08/27] Alpha filter (ish) is working on the height map, UI button to turn the feature on and off was added as well. --- .../FilteredRapidHeightMapExtractor.java | 41 +++---- .../RapidHeightMapExtractorCUDA.java | 10 +- .../FilteredRapidHeightMapExtractor.cu | 30 +++-- .../FilteredRapidHeightMapExtractorTest.java | 107 +++++++++++++++++- .../heightMap/HeightMapParameters.java | 1 + .../heightMap/HeightMapParametersBasics.java | 5 + .../HeightMapParametersReadOnly.java | 5 + .../heightMap/HeightMapParameters.json | 1 + .../heightMap/HeightMapParametersGPU.json | 1 + 9 files changed, 159 insertions(+), 42 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 1af1ed7297b..34b3be4c08b 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -20,9 +20,11 @@ public class FilteredRapidHeightMapExtractor { + static final int BLOCK_SIZE_XY = 32; + private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; - int layers = 2; + int layers = 10; private final CUstream_st stream; private final int rows; @@ -56,30 +58,13 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) CUDATools.checkCUDAError(error); currentIndex = 0; - - for (int i = 0; i < layers; i++) - { - // To get the average to be a whole number do (i * 2 + 2) - Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(0)); - - // Allocate memory for each layer, creating a 3d array - cudaMemcpy2D(pointerTo3DArray.ptr().position(i * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), - pointerTo3DArray.pitch(), - cpuData.data(), - cpuData.step(), - pointerTo3DArray.xsize(), - pointerTo3DArray.ysize(), - cudaMemcpyDefault); - } } public GpuMat update(GpuMat latestGlobalHeightMap, int defaultValue) { this.defaultValue = defaultValue; - GpuMat heightMapAverage = computerHeightMapHistoryAverage(latestGlobalHeightMap); - - return latestGlobalHeightMap; + return computerHeightMapHistoryAverage(latestGlobalHeightMap); } @NotNull @@ -105,20 +90,24 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) int error; GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); - GpuMat variance = new GpuMat(rows, cols, opencv_core.CV_16UC1); kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); - kernel.withPointer(variance.data()).withLong(variance.step()); + kernel.withPointer(latestGlobalHeightMap.data()).withLong(latestGlobalHeightMap.step()); kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); kernel.withInt(layers); kernel.withInt(defaultValue); - dim3 gridDim = new dim3(); - dim3 blockDim = new dim3(cols, rows, 1); - kernel.run(stream, gridDim, blockDim, 0); + //Execute the CUDA kernels with the provided stream + //Each kernel performs a specific task related to the height map update, registration, and cropping + int registerKernelGridSizeXY = (rows + BLOCK_SIZE_XY - 1) / BLOCK_SIZE_XY; + + dim3 blockSize = new dim3(BLOCK_SIZE_XY, BLOCK_SIZE_XY, 1); + dim3 registerKernelGridDim = new dim3(registerKernelGridSizeXY, registerKernelGridSizeXY, 1); + + kernel.run(stream, registerKernelGridDim, blockSize, 0); error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); @@ -127,7 +116,7 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) // In this example we go through 2,4,6,8 so we expect the average to be 5 Mat cpuResult = new Mat(); result.download(cpuResult); - PerceptionDebugTools.printMat("Result", cpuResult, 1); +// PerceptionDebugTools.printMat("Result", cpuResult, 1); cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), pointerTo3DArray.pitch(), @@ -147,7 +136,7 @@ public void reset() loopTracker = 0; } - public void close() + public void destroy() { program.close(); kernel.close(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java index 2355840d2f0..fbdd329abd4 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java @@ -43,7 +43,7 @@ public class RapidHeightMapExtractorCUDA implements RapidHeightMapExtractorInter private final GpuMat inputDepthImage; private final GpuMat localHeightMapImage; - private final GpuMat globalHeightMapImage; + private GpuMat globalHeightMapImage; private final GpuMat terrainCostImage; private final GpuMat contactMapImage; private final GpuMat sensorCroppedHeightMapImage; @@ -70,6 +70,7 @@ public class RapidHeightMapExtractorCUDA implements RapidHeightMapExtractorInter private final FloatPointer worldToGroundTransformDevicePointer; private final FloatPointer parametersHostPointer; private final FloatPointer parametersDevicePointer; + private final FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor; public int sequenceNumber = 0; private float gridOffsetX; @@ -111,6 +112,8 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, terrainMapData = new TerrainMapData(heightMapParameters.getCropWindowSize(), heightMapParameters.getCropWindowSize()); recomputeDerivedParameters(); + // Need to initialize this after the parameters have been computed to get the right size + filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, globalCellsPerAxis, globalCellsPerAxis); try { @@ -206,6 +209,7 @@ public void reset() globalHeightMapImage.setTo(new Scalar(resetOffset)); emptyGlobalHeightMapImage.setTo(new Scalar(resetOffset)); + filteredRapidHeightMapExtractor.reset(); snappedFootstepsExtractor.reset(resetOffset); sequenceNumber = 0; @@ -296,6 +300,9 @@ public void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); + if (heightMapParameters.getEnableAlphaFilter()) + globalHeightMapImage = filteredRapidHeightMapExtractor.update(globalHeightMapImage, 0); + // Run the cropping kernel croppingKernel.withPointer(globalHeightMapImage.data()).withLong(globalHeightMapImage.step()); croppingKernel.withPointer(sensorCroppedHeightMapImage.data()).withLong(sensorCroppedHeightMapImage.step()); @@ -442,6 +449,7 @@ public void destroy() sensorCroppedHeightMapImage.close(); snappedFootstepsExtractor.destroy(); + filteredRapidHeightMapExtractor.destroy(); // At the end we have to destroy the stream to release the memory CUDAStreamManager.releaseStream(stream); diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 1b8457ee913..f05a52e9ea2 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -1,4 +1,4 @@ -#define LAYERS 2 // Set a fixed size +#define LAYERS 10 // Set a fixed size extern "C" __global__ @@ -22,7 +22,7 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); +// printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); sum += (int) *matrixPointerRow; } @@ -35,7 +35,7 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; float diff = (float)(*matrixPointerRow) - avg; - variance[layer] = diff * diff; + variance[layer] = abs(diff); } double heightSum = 0; @@ -47,26 +47,34 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - heightSum += (float)(*matrixPointerRow) / variance[layer]; - varianceSum += 1.0 / variance[layer]; + if (variance[layer] > 0.0f) // Prevent division by zero + { + heightSum += (double)(*matrixPointerRow) / (double)variance[layer]; + varianceSum += 1.0 / (double)variance[layer]; + } } +// printf("Height sum: %f\n", heightSum); +// printf("Variance sum: %f\n", varianceSum); unsigned short newHeight = heightSum / varianceSum; - unsigned short * heightValue = (unsigned short*)((char*) newestHeightMap + indexY * pitchNewestHeightMap) + indexX; - float diff = (float) (*heightValue) - avg; - double newVariance = diff * diff; + + int heightValueInt = (int) *heightValue; + float diff = (float) heightValueInt - avg; + float newVariance = abs(diff); if (*heightValue == defaultValue) return; - unsigned short heightEstimate = (unsigned short) *heightValue + (newHeight * newVariance) / newVariance; + float alpha = 0.2; +// printf("Equation parameters (heightValue %hu) (alpha %f) (avg %hu)\n", *heightValue, alpha, avg); + float heightEstimate = (float) *heightValue * alpha + (avg * (1.0 - alpha)); // (newHeight * newVariance) / newVariance; unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; - *outputPointer = heightEstimate; + *outputPointer = (unsigned short) heightEstimate; - printf("GPU Height Estimate: %d\n", heightEstimate); +// printf("GPU Height Estimate: %f\n", heightEstimate); } diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java index c633eeaf563..859faa73d8d 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java @@ -14,8 +14,8 @@ public class FilteredRapidHeightMapExtractorTest @Test public void testGettingAverage() { - int rows = 2; - int cols = 2; + int rows = 1501; + int cols = 1501; CUstream_st stream = CUDAStreamManager.getStream(); FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); @@ -28,9 +28,108 @@ public void testGettingAverage() GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); Mat temp = new Mat(); currentAverage.download(temp); - PerceptionDebugTools.printMat("current", temp, 1); +// PerceptionDebugTools.printMat("current", temp, 1); + } + + filteredRapidHeightMapExtractor.destroy(); + } + + @Test + public void testChangeAfterSteadyState() + { + int rows = 2; + int cols = 2; + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + + // This example fills the history with the same value, all 8's + // Lets see how this behaves when we later then introduce a change (noise) + for (int i = 0; i < 8; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(800)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + } + + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(1000)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("current", temp, 1); + + filteredRapidHeightMapExtractor.destroy(); + } + + @Test + public void testChangedAfterSteadyStateRealDepthValuesUnsignedShort() + { + int rows = 2; + int cols = 2; + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + + // This example fills the history with the same value, all 8's + // Lets see how this behaves when we later then introduce a change (noise) + for (int i = 0; i < 8; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(32768)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + } + + // 400 is about 20 centimeters? Ish depending on the parameters, this is hard coded could change it to be based on the parameters + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(33100)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("current", temp, 1); + + filteredRapidHeightMapExtractor.destroy(); + } + + + // TODO remove this guy heheeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee + @Test + public void testDefaultValueSkipping() + { + int rows = 3; + int cols = 3; + CUstream_st stream = CUDAStreamManager.getStream(); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + + // Set everything to 1, I'm gonna pass this in as the default value so these get skipped hopefully + for (int i = 0; i < 4; i++) + { + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(1)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + filteredRapidHeightMapExtractor.update(latestDepthMat, 0); } - filteredRapidHeightMapExtractor.close(); + //TODO maybe we can just skip this? Does it matter + // I guess the newest height map will have the same values, so we can skip it + + // 400 is about 20 centimeters? Ish depending on the parameters, this is hard coded could change it to be based on the parameters + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(33100)); + GpuMat latestDepthMat = new GpuMat(); + latestDepthMat.upload(cpuData); + + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + Mat temp = new Mat(); + currentAverage.download(temp); + PerceptionDebugTools.printMat("current", temp, 1); + + filteredRapidHeightMapExtractor.destroy(); } } diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java index 9a7cf87aa3d..ba610fc5b27 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.java @@ -19,6 +19,7 @@ public class HeightMapParameters extends StoredPropertySet implements HeightMapP public static final StoredPropertyKeyList keys = new StoredPropertyKeyList(); public static final BooleanStoredPropertyKey resetHeightMap = keys.addBooleanKey("Reset Height Map"); + public static final BooleanStoredPropertyKey enableAlphaFilter = keys.addBooleanKey("Enable alpha filter"); public static final IntegerStoredPropertyKey searchWindowHeight = keys.addIntegerKey("Search window height"); public static final IntegerStoredPropertyKey searchWindowWidth = keys.addIntegerKey("Search window width"); public static final DoubleStoredPropertyKey minHeightRegistration = keys.addDoubleKey("Min height registration"); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java index a8aafa6e6f1..2588aaaa070 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersBasics.java @@ -13,6 +13,11 @@ default void setResetHeightMap(boolean resetHeightMap) set(HeightMapParameters.resetHeightMap, resetHeightMap); } + default void setEnableAlphaFilter(boolean enableAlphaFilter) + { + set(HeightMapParameters.enableAlphaFilter, enableAlphaFilter); + } + default void setSearchWindowHeight(int searchWindowHeight) { set(HeightMapParameters.searchWindowHeight, searchWindowHeight); diff --git a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java index 736cc22e610..1c73cff5731 100644 --- a/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java +++ b/ihmc-sensor-processing/src/main/generated-java/us/ihmc/sensorProcessing/heightMap/HeightMapParametersReadOnly.java @@ -15,6 +15,11 @@ default boolean getResetHeightMap() return get(resetHeightMap); } + default boolean getEnableAlphaFilter() + { + return get(enableAlphaFilter); + } + default int getSearchWindowHeight() { return get(searchWindowHeight); diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json index e70622db6a2..4d038d2015c 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParameters.json @@ -1,6 +1,7 @@ { "title" : "HeightMapParameters", "Reset Height Map" : false, + "Enable alpha filter" : true, "Search window height": { "value": 20, "lowerBound": 10, diff --git a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json index 56461a4ea28..33c41d8d2d4 100644 --- a/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json +++ b/ihmc-sensor-processing/src/main/resources/us/ihmc/sensorProcessing/heightMap/HeightMapParametersGPU.json @@ -1,6 +1,7 @@ { "title" : "GPU Height Map Parameters", "Reset Height Map" : false, + "Enable alpha filter" : true, "Search window height" : { "value" : 350, "lowerBound" : 10, From 8c0cd8eb61380f0022ec65a6ec4b111211a7e060 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Thu, 13 Feb 2025 08:14:25 -0600 Subject: [PATCH 09/27] Tuned filter a little bit --- .../gpuHeightMap/FilteredRapidHeightMapExtractor.java | 2 +- .../perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 34b3be4c08b..e55e1d2f72b 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -24,7 +24,7 @@ public class FilteredRapidHeightMapExtractor private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; - int layers = 10; + int layers = 6; private final CUstream_st stream; private final int rows; diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index f05a52e9ea2..b1719835419 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -1,4 +1,4 @@ -#define LAYERS 10 // Set a fixed size +#define LAYERS 6 // Set a fixed size extern "C" __global__ From 582910a1508bd63a19333db08e03a8d1e9efce87 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Sat, 15 Feb 2025 14:10:08 -0600 Subject: [PATCH 10/27] Update environments for the UI --- .../environments/CinderBlockPallet.json | 122 ++++----- .../environments/Simple Rough Terrain | 220 +++++++++++++++ .../environments/Uneven Terrain 3 Levels | 256 ++++++++++++++++++ .../environments/ZZZRoughTerrainOne.json | 174 ++++++------ 4 files changed, 624 insertions(+), 148 deletions(-) create mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain create mode 100644 ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json b/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json index 52fcb505cb8..f1b6fe027bf 100644 --- a/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/CinderBlockPallet.json @@ -102,56 +102,56 @@ }, { "type" : "RDXPalletObject", "x" : 2.4930577278137207, - "y" : 0.06989191472530365, - "z" : 0.1110124127939346, - "qx" : 0.00856238873690151, + "y" : 0.06989191472530099, + "z" : 0.08732389168723786, + "qx" : 0.008562388736901518, "qy" : -0.005114153010419687, - "qz" : 0.7018977765776746, - "qs" : 0.7122078644422724 + "qz" : 0.7018977765776755, + "qs" : 0.7122078644422707 }, { "type" : "RDXPalletObject", "x" : 3.513807535171509, - "y" : 0.04207326173782348, - "z" : 0.1237301762215778, - "qx" : 0.7094953522455109, - "qy" : 0.7046140939793347, - "qz" : 0.0030468615931168426, - "qs" : -0.011226768997909059 + "y" : 0.04207326173781967, + "z" : 0.08234225453700866, + "qx" : 0.7094654357241527, + "qy" : 0.7046729978293471, + "qz" : 0.007336564946010361, + "qs" : -0.006398160495962346 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.8034467697143555, - "y" : -0.26781320571899414, - "z" : 0.3262125167995693, - "qx" : -0.10285762375856672, - "qy" : 0.09713503866937312, - "qz" : 0.7006268231279272, - "qs" : 0.6993619579381686 + "x" : 2.803446769714356, + "y" : -0.26781320571898903, + "z" : 0.2791487080525257, + "qx" : -0.10285762375856766, + "qy" : 0.097135038669374, + "qz" : 0.7006268231279288, + "qs" : 0.6993619579381668 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.6078357696533203, - "y" : -0.2656903862953186, - "z" : 0.32625531796365964, - "qx" : -0.10647845066123686, - "qy" : 0.10638572963805844, - "qz" : 0.6993553654921224, - "qs" : 0.6987463694585758 + "x" : 2.61168138422286, + "y" : -0.2656870360892501, + "z" : 0.2783433061531242, + "qx" : -0.10647845066123718, + "qy" : 0.10638572963805866, + "qz" : 0.6993553654921232, + "qs" : 0.6987463694585748 }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 2.6733238697052, - "y" : 0.23532751202583313, - "z" : 0.30945813106372944, + "y" : 0.2353275120258309, + "z" : 0.23830242759757592, "qx" : 0.0, "qy" : 0.0, - "qz" : 0.2976377221605589, + "qz" : 0.29763772216055895, "qs" : 0.954678891746892 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 2.5680363178253174, - "y" : 0.41301260069012646, - "z" : 0.3042421745136381, - "qx" : -4.259043794782986E-5, - "qy" : 1.3876773247399921E-4, - "qz" : 0.2934102624957087, + "x" : 2.5662500681208926, + "y" : 0.389346157233353, + "z" : 0.2386235305666895, + "qx" : -4.259043794782999E-5, + "qy" : 1.387677324739995E-4, + "qz" : 0.2934102624957088, "qs" : 0.9559866091069321 }, { "type" : "RDXMediumCinderBlockRoughed", @@ -264,47 +264,47 @@ }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 3.216216802597046, - "y" : 0.4195704162120819, - "z" : 0.3607892571017146, - "qx" : 0.09720753880522989, - "qy" : 0.09797572562251415, - "qz" : -0.6975779785814231, - "qs" : 0.7030906167674457 + "y" : 0.41957041621207125, + "z" : 0.2886322178312223, + "qx" : 0.09720753880522992, + "qy" : 0.09797572562251418, + "qz" : -0.6975779785814232, + "qs" : 0.7030906167674456 }, { "type" : "RDXMediumCinderBlockRoughed", "x" : 3.4069573879241943, - "y" : 0.41641926765441895, - "z" : 0.3601300859823824, - "qx" : 0.10611619192654026, - "qy" : 0.09930293387417374, - "qz" : -0.6917497248948593, - "qs" : 0.7073617173998588 + "y" : 0.41641926765440784, + "z" : 0.288369582091613, + "qx" : 0.10611619192654027, + "qy" : 0.09930293387417376, + "qz" : -0.6917497248948595, + "qs" : 0.7073617173998586 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.5369553565979004, - "y" : -0.34754684567451477, - "z" : 0.3093220075592398, - "qx" : 0.0, - "qy" : 0.0, - "qz" : -0.3582957156911485, - "qs" : 0.9336081512697754 + "x" : 3.5393702227098975, + "y" : -0.34539638462598843, + "z" : 0.22806068928635884, + "qx" : -0.0028667441390000164, + "qy" : 0.0019384903262589207, + "qz" : -0.3583054928379458, + "qs" : 0.9335979851282099 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.6725926399230957, - "y" : -0.20286306738853455, - "z" : 0.3029684722423553, + "x" : 3.672592639923095, + "y" : -0.20286306738852744, + "z" : 0.22763386235801902, "qx" : 0.0, "qy" : 0.0, - "qz" : -0.372481463577173, + "qz" : -0.37248146357717304, "qs" : 0.9280396323926081 }, { "type" : "RDXMediumCinderBlockRoughed", - "x" : 3.8084611892700195, - "y" : -0.056925807148218155, - "z" : 0.30668363943696014, + "x" : 3.8095721431824052, + "y" : -0.06435738580480432, + "z" : 0.22950602759147598, "qx" : 0.0, "qy" : 0.0, - "qz" : -0.3759256660029729, + "qz" : -0.37592566600297295, "qs" : 0.9266498225544648 }, { "type" : "RDXPointLightObject", diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain new file mode 100644 index 00000000000..f89bb16c72f --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Simple Rough Terrain @@ -0,0 +1,220 @@ +{ + "ambientLight" : 0.01, + "objects" : [ { + "type" : "RDXLabFloorObject", + "x" : 0.0, + "y" : 0.0, + "z" : 0.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0261326209306856, + "y" : -0.1312642229974892, + "z" : 0.04168685751514941, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0278165108023485, + "y" : 0.058344485126049284, + "z" : 0.041986173402213625, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0254283744607902, + "y" : 0.24169205928048654, + "z" : 0.04057212192335316, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0266319218745057, + "y" : -0.32255398445042305, + "z" : 0.041615879371108555, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.0255996928843913, + "y" : 0.4299430060193219, + "z" : 0.039385070980956435, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.424579962380316, + "y" : 0.43113670704297236, + "z" : 0.09254610128793772, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.4259674753234353, + "y" : 0.24562213822075551, + "z" : 0.09061781008937408, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.4273566781817704, + "y" : 0.0589708940443876, + "z" : 0.08995922266082751, + "qx" : 0.003149536163718456, + "qy" : -1.0842021724855044E-19, + "qz" : 0.0, + "qs" : 0.9999950401986769 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.428751325767549, + "y" : -0.1308297033892502, + "z" : 0.08970254494993041, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.429117948670486, + "y" : -0.319230985173794, + "z" : 0.09054522513823815, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.889698199172627, + "y" : -0.31362771389739263, + "z" : 0.12029186241267142, + "qx" : -2.3889634179018873E-4, + "qy" : 0.09261829908698715, + "qz" : -1.3468733359722775E-6, + "qs" : 0.9957016589325124 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8925994110200004, + "y" : -0.12334371333174532, + "z" : 0.12113892318644881, + "qx" : -2.61922767217173E-4, + "qy" : 0.09538769007356435, + "qz" : 1.6653387936186146E-16, + "qs" : 0.9954401639369861 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.892382190045252, + "y" : 0.06702701050002413, + "z" : 0.1209962797071884, + "qx" : -4.073356737959721E-4, + "qy" : 0.09598398209739224, + "qz" : -1.6912157996040733E-5, + "qs" : 0.995382795196077 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8968050937558998, + "y" : 0.25091822068453606, + "z" : 0.1183742422242321, + "qx" : -7.310595024396378E-4, + "qy" : 0.09724279035586246, + "qz" : -5.628849670202829E-5, + "qs" : 0.995260419240821 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 1.8967171650763754, + "y" : 0.4417683185753126, + "z" : 0.11887800146539662, + "qx" : -7.061977789265678E-4, + "qy" : 0.09361012578193359, + "qz" : -4.8221701554163054E-5, + "qs" : 0.9956086798087164 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.4854011650828687, + "y" : 0.337916486126029, + "z" : 0.030854774019734132, + "qx" : 0.07768412436974872, + "qy" : 0.07593874593882895, + "qz" : -0.7005910590819361, + "qs" : 0.7052451003875139 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.298407879878164, + "y" : 0.3385634336033867, + "z" : 0.030262727840291048, + "qx" : 0.07817291979346568, + "qy" : 0.07754413448983202, + "qz" : -0.7056224031799893, + "qs" : 0.6999806611240655 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.4801712617778033, + "y" : -0.17550711531681293, + "z" : 0.01956208485422517, + "qx" : -0.06847305311043973, + "qy" : 0.06771917439063006, + "qz" : 0.6945107854365579, + "qs" : 0.713007940579824 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2962901687151547, + "y" : -0.17636865286320602, + "z" : 0.022432163496095836, + "qx" : -0.06365197911790865, + "qy" : 0.06844083238574804, + "qz" : 0.6971263806041734, + "qs" : 0.7108298583222602 + } ] +} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels new file mode 100644 index 00000000000..4745350e88f --- /dev/null +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/Uneven Terrain 3 Levels @@ -0,0 +1,256 @@ +{ + "ambientLight" : 0.01, + "objects" : [ { + "type" : "RDXLabFloorObject", + "x" : 0.0, + "y" : 0.0, + "z" : 0.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : 10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : -10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPointLightObject", + "x" : 10.0, + "y" : -10.0, + "z" : 10.0, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXPalletObject", + "x" : 3.0607671409344284, + "y" : -0.06310664378602587, + "z" : 0.07506210377440695, + "qx" : -1.0842021724855047E-19, + "qy" : 0.0028445913904392786, + "qz" : 6.93889390390723E-17, + "qs" : 0.9999959541417263 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.856897603762217, + "y" : -0.06462814957506413, + "z" : 0.04301444223836254, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.7725192909374095, + "y" : -0.3573444838618175, + "z" : 0.04315923678089523, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7033277782232468, + "qs" : 0.7108656950363771 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.9683418781865694, + "y" : -0.35782223569816507, + "z" : 0.04510366318073977, + "qx" : 0.0024008229853511027, + "qy" : -0.002327662684819567, + "qz" : -0.6960790454542465, + "qs" : 0.7179573667808722 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.7692433473042397, + "y" : 0.22736090391018615, + "z" : 0.04463407766635785, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.6991832031499666, + "qs" : 0.7149425490435946 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.958366039907915, + "y" : 0.22510670375295616, + "z" : 0.045219122127525546, + "qx" : 0.6955503250719798, + "qy" : 0.7184704667441876, + "qz" : -0.0029953814049817817, + "qs" : 9.80509505919548E-4 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.5778746753913864, + "y" : -0.35140658275500114, + "z" : 0.04094344602728395, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7108421321498666, + "qs" : 0.7033515928471561 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 1.5814870340806877, + "y" : 0.039696925710773545, + "z" : 0.04093598509427332, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7003398091610843, + "qs" : 0.7138096046595451 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.254567570271429, + "y" : -0.26246316058216784, + "z" : 0.08398364191738705, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2543383177500473, + "y" : -0.45188103127684, + "z" : 0.08437391630526325, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2567335113487488, + "y" : -0.07563787043052153, + "z" : 0.08331043805643118, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.256972731643839, + "y" : 0.1089200658313772, + "z" : 0.0831901145643251, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXLargeCinderBlockRoughed", + "x" : 2.2568512020237517, + "y" : 0.2998872055301931, + "z" : 0.083989292309432, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6524621715840238, + "y" : 0.11525193387083688, + "z" : 0.2115532611096348, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6512512829296737, + "y" : 0.3088006898672824, + "z" : 0.2127774719692071, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6524316022722947, + "y" : -0.07200348851432764, + "z" : 0.2113775293633806, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6534353464148004, + "y" : -0.2624579488098271, + "z" : 0.20899677238844694, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 2.6523451753964458, + "y" : -0.45492220028035296, + "z" : 0.20508528682018912, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.853173395299581, + "y" : -0.07850545679836562, + "z" : 0.046833812609385905, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.0, + "qs" : 1.0 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.7660761367552613, + "y" : -0.3677813682161868, + "z" : 0.04590330560432699, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7068686309521961, + "qs" : 0.7073448512400214 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.9563662963951316, + "y" : -0.37261381461621124, + "z" : 0.04585163099189009, + "qx" : 0.0, + "qy" : 0.0, + "qz" : 0.7079959342146718, + "qs" : 0.7062165086823545 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.761101071744873, + "y" : 0.2114722527067107, + "z" : 0.04291788963533616, + "qx" : 0.0, + "qy" : 0.0, + "qz" : -0.7036791662089575, + "qs" : 0.7105178611713194 + }, { + "type" : "RDXSmallCinderBlockRoughed", + "x" : 3.953645008963975, + "y" : 0.21726026395490247, + "z" : 0.04430687538108835, + "qx" : -6.867242264466872E-4, + "qy" : 7.306747962616092E-4, + "qz" : -0.7066230485210534, + "qs" : 0.7075894726626394 + } ] +} \ No newline at end of file diff --git a/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json b/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json index fdb813bf62b..f89bb16c72f 100644 --- a/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json +++ b/ihmc-high-level-behaviors/src/libgdx/resources/environments/ZZZRoughTerrainOne.json @@ -47,18 +47,18 @@ "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0261326209306865, - "y" : -0.13126422299748575, - "z" : 0.0, + "x" : 1.0261326209306856, + "y" : -0.1312642229974892, + "z" : 0.04168685751514941, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.027816510802349, - "y" : 0.0583444851260503, - "z" : 0.0, + "x" : 1.0278165108023485, + "y" : 0.058344485126049284, + "z" : 0.041986173402213625, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, @@ -66,71 +66,71 @@ }, { "type" : "RDXSmallCinderBlockRoughed", "x" : 1.0254283744607902, - "y" : 0.24169205928048704, - "z" : 1.1102230246251565E-16, + "y" : 0.24169205928048654, + "z" : 0.04057212192335316, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0266319218744941, - "y" : -0.32255398445039835, - "z" : 1.1102230246251565E-16, + "x" : 1.0266319218745057, + "y" : -0.32255398445042305, + "z" : 0.041615879371108555, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXSmallCinderBlockRoughed", - "x" : 1.0255996928843942, - "y" : 0.4299430060193301, - "z" : 0.0, + "x" : 1.0255996928843913, + "y" : 0.4299430060193219, + "z" : 0.039385070980956435, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4245799623803117, - "y" : 0.4311367070429927, - "z" : 0.0, + "x" : 1.424579962380316, + "y" : 0.43113670704297236, + "z" : 0.09254610128793772, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4259674753234355, - "y" : 0.2456221382207675, - "z" : 0.0, + "x" : 1.4259674753234353, + "y" : 0.24562213822075551, + "z" : 0.09061781008937408, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4273566781817708, - "y" : 0.05897089404440137, - "z" : 0.0, - "qx" : 0.0, - "qy" : 0.0, + "x" : 1.4273566781817704, + "y" : 0.0589708940443876, + "z" : 0.08995922266082751, + "qx" : 0.003149536163718456, + "qy" : -1.0842021724855044E-19, "qz" : 0.0, - "qs" : 1.0 + "qs" : 0.9999950401986769 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.428751325767556, - "y" : -0.13082970338925246, - "z" : 0.0, + "x" : 1.428751325767549, + "y" : -0.1308297033892502, + "z" : 0.08970254494993041, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, "qs" : 1.0 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.4291179486704777, - "y" : -0.3192309851737916, - "z" : 0.0, + "x" : 1.429117948670486, + "y" : -0.319230985173794, + "z" : 0.09054522513823815, "qx" : 0.0, "qy" : 0.0, "qz" : 0.0, @@ -138,83 +138,83 @@ }, { "type" : "RDXLargeCinderBlockRoughed", "x" : 1.889698199172627, - "y" : -0.3136277138974171, - "z" : 0.0, - "qx" : -2.3890013852761622E-4, - "qy" : 0.08700325143367432, - "qz" : -1.4571706576721966E-16, - "qs" : 0.9962079989473547 + "y" : -0.31362771389739263, + "z" : 0.12029186241267142, + "qx" : -2.3889634179018873E-4, + "qy" : 0.09261829908698715, + "qz" : -1.3468733359722775E-6, + "qs" : 0.9957016589325124 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.8925994110199995, - "y" : -0.12334371333177716, - "z" : 0.0, - "qx" : -2.619227672171765E-4, - "qy" : 0.09538769007356443, - "qz" : 2.2204403793185408E-16, + "x" : 1.8925994110200004, + "y" : -0.12334371333174532, + "z" : 0.12113892318644881, + "qx" : -2.61922767217173E-4, + "qy" : 0.09538769007356435, + "qz" : 1.6653387936186146E-16, "qs" : 0.9954401639369861 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.8923821900452733, - "y" : 0.06702701050002052, - "z" : 0.0, - "qx" : -4.074078878578608E-4, - "qy" : 0.09148835465765207, - "qz" : -1.5072662212893554E-5, - "qs" : 0.9958060628223065 + "x" : 1.892382190045252, + "y" : 0.06702701050002413, + "z" : 0.1209962797071884, + "qx" : -4.073356737959721E-4, + "qy" : 0.09598398209739224, + "qz" : -1.6912157996040733E-5, + "qs" : 0.995382795196077 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.896805093755894, - "y" : 0.25091822068453806, - "z" : 0.0, - "qx" : -7.314712274236847E-4, - "qy" : 0.08957687143892619, - "qz" : -5.0658014366053786E-5, - "qs" : 0.9959796416025897 + "x" : 1.8968050937558998, + "y" : 0.25091822068453606, + "z" : 0.1183742422242321, + "qx" : -7.310595024396378E-4, + "qy" : 0.09724279035586246, + "qz" : -5.628849670202829E-5, + "qs" : 0.995260419240821 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 1.896717165076377, - "y" : 0.4417683185753008, - "z" : 0.0, - "qx" : -7.061977789265809E-4, - "qy" : 0.09361012578193358, - "qz" : -4.822170155417867E-5, + "x" : 1.8967171650763754, + "y" : 0.4417683185753126, + "z" : 0.11887800146539662, + "qx" : -7.061977789265678E-4, + "qy" : 0.09361012578193359, + "qz" : -4.8221701554163054E-5, "qs" : 0.9956086798087164 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.4854011650828647, - "y" : 0.33791648612603054, - "z" : 0.0, + "x" : 2.4854011650828687, + "y" : 0.337916486126029, + "z" : 0.030854774019734132, "qx" : 0.07768412436974872, "qy" : 0.07593874593882895, "qz" : -0.7005910590819361, "qs" : 0.7052451003875139 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.2984078798781646, - "y" : 0.33856343360339525, - "z" : -0.004837357989063484, - "qx" : 0.08236230486456504, - "qy" : 0.08170002376624297, - "qz" : -0.7051456813991942, - "qs" : 0.6995077732649281 + "x" : 2.298407879878164, + "y" : 0.3385634336033867, + "z" : 0.030262727840291048, + "qx" : 0.07817291979346568, + "qy" : 0.07754413448983202, + "qz" : -0.7056224031799893, + "qs" : 0.6999806611240655 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.480171261777798, - "y" : -0.17550711531681523, - "z" : 0.0, - "qx" : -0.06847305311043997, - "qy" : 0.06771917439063073, - "qz" : 0.6945107854365578, + "x" : 2.4801712617778033, + "y" : -0.17550711531681293, + "z" : 0.01956208485422517, + "qx" : -0.06847305311043973, + "qy" : 0.06771917439063006, + "qz" : 0.6945107854365579, "qs" : 0.713007940579824 }, { "type" : "RDXLargeCinderBlockRoughed", - "x" : 2.296290168715155, - "y" : -0.1763686528631838, - "z" : 0.0, - "qx" : -0.0710345669393736, - "qy" : 0.07596834185010112, - "qz" : 0.6964128483016595, - "qs" : 0.7100648182079236 + "x" : 2.2962901687151547, + "y" : -0.17636865286320602, + "z" : 0.022432163496095836, + "qx" : -0.06365197911790865, + "qy" : 0.06844083238574804, + "qz" : 0.6971263806041734, + "qs" : 0.7108298583222602 } ] } \ No newline at end of file From da4c67b550b1af60bed820d943e612fc3f1654c5 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Tue, 18 Feb 2025 20:09:42 -0600 Subject: [PATCH 11/27] Switched from ROS2Helper to ROS2Node for StoredPropertySets and switched everything upstream that was affected --- .../AtlasOusterLidarOnRobotProcess.java | 2 +- .../colorVision/BlackflyImagePublisher.java | 2 +- .../RapidRegionsAsyncPublisher.java | 2 +- .../property/ROS2StoredPropertySet.java | 17 +++++++------ .../property/ROS2StoredPropertySetGroup.java | 10 ++++---- .../property/StoredPropertySetROS2Input.java | 5 ++-- .../ros2/ROS2DemandGraphNode.java | 3 ++- .../ros2/ROS2HeartbeatMonitor.java | 7 +++--- .../ros2/ROS2DemandGraphNodeTest.java | 10 ++++---- .../perception/RDXContinuousHikingPanel.java | 18 ++++++------- .../perception/RDXRemoteHeightMapPanel.java | 2 +- .../rdx/perception/RDXRemotePerceptionUI.java | 2 +- .../RDXStancePoseSelectionPanel.java | 12 ++++----- .../perception/RDXSteppableRegionsPanel.java | 2 +- .../RDXTerrainPlanningDebugger.java | 8 +++--- .../sceneGraph/RDXSceneGraphDemo.java | 10 ++++---- .../ui/ImGuiRemoteROS2StoredPropertySet.java | 25 ++++++------------- ...ImGuiRemoteROS2StoredPropertySetGroup.java | 9 ++++--- .../ros2/RDXDetectionManagerSettings.java | 3 ++- .../ContinuousHikingProcess.java | 4 +-- .../ControllerFootstepQueueMonitor.java | 13 +++++----- .../perception/HumanoidPerceptionModule.java | 2 +- .../LocalizationAndMappingTask.java | 2 +- .../StandAloneRealsenseProcess.java | 6 ++--- ...StructuralPerceptionProcessWithDriver.java | 4 +-- .../rdx/perception/RDXBallTrackingDemo.java | 4 +-- .../RDXIterativeClosestPointWorkerDemo.java | 4 +-- .../rdx/perception/RDXOpenCVTrackerDemo.java | 4 +-- .../RDXSteppableRegionCalculatorDemo.java | 2 +- .../detections/DetectionManager.java | 3 ++- .../heightMap/RemoteHeightMapUpdater.java | 2 +- .../ouster/OusterDriverAndDepthPublisher.java | 6 +++-- .../ouster/OusterHeightMapUpdater.java | 13 ++++++---- .../RapidPlanarRegionsExtractionThread.java | 5 ++-- .../RemoteSteppableRegionsUpdater.java | 20 +++++++++------ .../tools/PerceptionMessageTools.java | 7 ++++-- .../RealsenseColorDepthPublisher.java | 2 +- 37 files changed, 133 insertions(+), 119 deletions(-) diff --git a/atlas/src/main/java/us/ihmc/atlas/sensors/AtlasOusterLidarOnRobotProcess.java b/atlas/src/main/java/us/ihmc/atlas/sensors/AtlasOusterLidarOnRobotProcess.java index 12b95bd43cb..7ac74d37b4d 100644 --- a/atlas/src/main/java/us/ihmc/atlas/sensors/AtlasOusterLidarOnRobotProcess.java +++ b/atlas/src/main/java/us/ihmc/atlas/sensors/AtlasOusterLidarOnRobotProcess.java @@ -30,7 +30,7 @@ public AtlasOusterLidarOnRobotProcess() ros2Node.destroy(); }, getClass().getSimpleName() + "Shutdown")); - new OusterDriverAndDepthPublisher(controllerHelper, this::sensorFrameUpdater, PerceptionAPI.OUSTER_DEPTH_IMAGE, PerceptionAPI.OUSTER_LIDAR_SCAN); + new OusterDriverAndDepthPublisher(ros2Node, controllerHelper.getRobotName(), this::sensorFrameUpdater, PerceptionAPI.OUSTER_DEPTH_IMAGE, PerceptionAPI.OUSTER_LIDAR_SCAN); } private HumanoidReferenceFrames sensorFrameUpdater() diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/colorVision/BlackflyImagePublisher.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/colorVision/BlackflyImagePublisher.java index 9ae63820ad7..cc91c6dd82b 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/colorVision/BlackflyImagePublisher.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/colorVision/BlackflyImagePublisher.java @@ -50,7 +50,7 @@ public BlackflyImagePublisher(BlackflyLensProperties lensProperties, ROS2Topic(new ROS2Helper(ros2Node), + ousterFisheyeColoringIntrinsicsROS2 = new ROS2StoredPropertySet<>(ros2Node, BlackflyComms.OUSTER_FISHEYE_COLORING_INTRINSICS, ousterFisheyeColoringIntrinsics); diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/perception/RapidRegionsAsyncPublisher.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/perception/RapidRegionsAsyncPublisher.java index 3837ec69b26..16a327b8bc9 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/perception/RapidRegionsAsyncPublisher.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/perception/RapidRegionsAsyncPublisher.java @@ -59,7 +59,7 @@ public void update() FramePlanarRegionsList frameRegions = new FramePlanarRegionsList(); extractor.updateRobotConfigurationData(syncedRobot.getLatestRobotConfigurationData()); extractor.update(bytedecoDepthImage, syncedRobot.getReferenceFrames().getSteppingCameraFrame(), frameRegions); - PerceptionMessageTools.publishFramePlanarRegionsList(frameRegions, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Helper); + PerceptionMessageTools.publishFramePlanarRegionsList(frameRegions, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Helper.getROS2Node()); extractor.setProcessing(false); }); } diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySet.java b/ihmc-communication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySet.java index 6154cf51fac..df2e0eb60a0 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySet.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySet.java @@ -1,6 +1,8 @@ package us.ihmc.communication.property; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; +import ihmc_common_msgs.msg.dds.PrimitiveDataVectorMessage; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.tools.property.StoredPropertySetBasics; import us.ihmc.commons.thread.Throttler; @@ -13,21 +15,22 @@ public class ROS2StoredPropertySet { public static final double STATUS_PERIOD = 1.0; - private final ROS2PublishSubscribeAPI ros2PublishSubscribeAPI; + private final ROS2Node ros2Node; private final StoredPropertySetROS2TopicPair topicPair; private final T storedPropertySet; private final StoredPropertySetROS2Input commandInput; private final Throttler parameterOutputThrottler = new Throttler(); + private final ROS2Publisher publisher; - public ROS2StoredPropertySet(ROS2PublishSubscribeAPI ros2PublishSubscribeAPI, + public ROS2StoredPropertySet(ROS2Node ros2Node, StoredPropertySetROS2TopicPair topicPair, T storedPropertySet) { - this.ros2PublishSubscribeAPI = ros2PublishSubscribeAPI; + this.ros2Node = ros2Node; this.topicPair = topicPair; this.storedPropertySet = storedPropertySet; - commandInput = new StoredPropertySetROS2Input(ros2PublishSubscribeAPI, topicPair.getCommandTopic(), storedPropertySet); - ros2PublishSubscribeAPI.createPublisher(topicPair.getStatusTopic()); + commandInput = new StoredPropertySetROS2Input(ros2Node, topicPair.getCommandTopic(), storedPropertySet); + publisher = ros2Node.createPublisher(topicPair.getStatusTopic()); } public void updateAndPublishThrottledStatus() @@ -62,7 +65,7 @@ public void publishThrottledStatus() public void publishStatus() { - ros2PublishSubscribeAPI.publish(topicPair.getStatusTopic(), StoredPropertySetMessageTools.newMessage(storedPropertySet)); + publisher.publish(StoredPropertySetMessageTools.newMessage(storedPropertySet)); } public StoredPropertySetROS2Input getCommandInput() diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySetGroup.java b/ihmc-communication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySetGroup.java index 0860b6cb47d..eadaf3b44c9 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySetGroup.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/property/ROS2StoredPropertySetGroup.java @@ -1,6 +1,6 @@ package us.ihmc.communication.property; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; +import us.ihmc.ros2.ROS2Node; import us.ihmc.tools.property.StoredPropertySetBasics; import java.util.ArrayList; @@ -10,12 +10,12 @@ */ public class ROS2StoredPropertySetGroup { - private final ROS2PublishSubscribeAPI ros2PublishSubscribeAPI; + private final ROS2Node ros2Node; private final ArrayList> ros2StoredPropertySets = new ArrayList<>(); - public ROS2StoredPropertySetGroup(ROS2PublishSubscribeAPI ros2PublishSubscribeAPI) + public ROS2StoredPropertySetGroup(ROS2Node ros2Node) { - this.ros2PublishSubscribeAPI = ros2PublishSubscribeAPI; + this.ros2Node = ros2Node; } /** @@ -26,7 +26,7 @@ public ROS2StoredPropertySetGroup(ROS2PublishSubscribeAPI ros2PublishSubscribeAP */ public void registerStoredPropertySet(StoredPropertySetROS2TopicPair topicPair, StoredPropertySetBasics storedPropertySet) { - ROS2StoredPropertySet ros2StoredPropertySet = new ROS2StoredPropertySet<>(ros2PublishSubscribeAPI, topicPair, storedPropertySet); + ROS2StoredPropertySet ros2StoredPropertySet = new ROS2StoredPropertySet<>(ros2Node, topicPair, storedPropertySet); ros2StoredPropertySets.add(ros2StoredPropertySet); } diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/property/StoredPropertySetROS2Input.java b/ihmc-communication/src/main/java/us/ihmc/communication/property/StoredPropertySetROS2Input.java index 7b1d887969f..46ffa0ce558 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/property/StoredPropertySetROS2Input.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/property/StoredPropertySetROS2Input.java @@ -6,6 +6,7 @@ import us.ihmc.commons.thread.TypedNotification; import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.log.LogTools; +import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Topic; import us.ihmc.tools.Timer; import us.ihmc.tools.property.StoredPropertyKey; @@ -33,12 +34,12 @@ private record PropertyChangeNotification(StoredPropertyKey propertyKey, private final Timer receptionTimer = new Timer(); private long numberOfMessagesReceived = 0; - public StoredPropertySetROS2Input(ROS2PublishSubscribeAPI ros2PublishSubscribeAPI, + public StoredPropertySetROS2Input(ROS2Node ros2Node, ROS2Topic inputTopic, StoredPropertySetBasics storedPropertySetToUpdate) { this.storedPropertySetToUpdate = storedPropertySetToUpdate; - ros2PublishSubscribeAPI.subscribeViaCallback(inputTopic, this::acceptMessage); + ros2Node.createSubscription(inputTopic, (s) -> acceptMessage(s.takeNextData())); } private void acceptMessage(PrimitiveDataVectorMessage message) diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2DemandGraphNode.java b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2DemandGraphNode.java index 5eefb98780f..239d3e8431f 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2DemandGraphNode.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2DemandGraphNode.java @@ -1,5 +1,6 @@ package us.ihmc.communication.ros2; import std_msgs.msg.dds.Empty; +import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Topic; import java.util.ArrayList; @@ -16,7 +17,7 @@ public class ROS2DemandGraphNode private final Set> demandChangedCallbacks = new HashSet<>(); private final AtomicBoolean wasDemanded = new AtomicBoolean(false); - public ROS2DemandGraphNode(ROS2PublishSubscribeAPI ros2, ROS2Topic heartbeatTopic) + public ROS2DemandGraphNode(ROS2Node ros2, ROS2Topic heartbeatTopic) { nodeHeartbeatMonitor = new ROS2HeartbeatMonitor(ros2, heartbeatTopic); nodeHeartbeatMonitor.setAlivenessChangedCallback(this::checkIfDemandChanged); diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java index beda24349c4..eb8ba200593 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java @@ -6,6 +6,7 @@ import us.ihmc.commons.thread.ThreadTools; import us.ihmc.commons.thread.TypedNotification; import us.ihmc.ros2.ROS2Input; +import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Topic; import us.ihmc.tools.Timer; import us.ihmc.commons.thread.Throttler; @@ -27,7 +28,6 @@ public class ROS2HeartbeatMonitor */ public static final double HEARTBEAT_EXPIRATION = 1.25 * ROS2Heartbeat.HEARTBEAT_PERIOD; private final Timer timer = new Timer(); - private final ROS2Input subscription; // To provide callback when aliveness changes private volatile boolean running = true; @@ -36,10 +36,9 @@ public class ROS2HeartbeatMonitor private Consumer callback = null; private final TypedNotification alivenessChangedNotification = new TypedNotification<>(); - public ROS2HeartbeatMonitor(ROS2PublishSubscribeAPI ros2, ROS2Topic heartbeatTopic) + public ROS2HeartbeatMonitor(ROS2Node ros2Node, ROS2Topic heartbeatTopic) { - subscription = ros2.subscribe(heartbeatTopic); - subscription.addCallback(this::receivedHeartbeat); + ros2Node.createSubscription(heartbeatTopic, (s) -> receivedHeartbeat(s.takeNextData())); ThreadTools.startAsDaemon(() -> ExceptionTools.handle(this::monitorThread, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE), "HeartbeatMonitor"); } diff --git a/ihmc-communication/src/test/java/us/ihmc/communication/ros2/ROS2DemandGraphNodeTest.java b/ihmc-communication/src/test/java/us/ihmc/communication/ros2/ROS2DemandGraphNodeTest.java index 90afe4388d4..14a5ad4bdcc 100644 --- a/ihmc-communication/src/test/java/us/ihmc/communication/ros2/ROS2DemandGraphNodeTest.java +++ b/ihmc-communication/src/test/java/us/ihmc/communication/ros2/ROS2DemandGraphNodeTest.java @@ -24,7 +24,7 @@ public void testIsDemanded() ROS2Helper ros2Helper = new ROS2Helper(ros2Node); ROS2Topic testTopic = ROS2Tools.IHMC_ROOT.withSuffix("demand_graph_test_is_demanded").withType(Empty.class); - ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Helper, testTopic); + ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Node, testTopic); ROS2Heartbeat testHeartbeat = new ROS2Heartbeat(ros2Node, testTopic); assertFalse(testNode.isDemanded()); @@ -51,9 +51,9 @@ public void testDependantDemand() ROS2Topic dependantTopic = testTopic.withPrefix("dependant"); ROS2Heartbeat testHeartbeat = new ROS2Heartbeat(ros2Node, testTopic); - ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Helper, testTopic); + ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Node, testTopic); ROS2Heartbeat dependantHeartbeat = new ROS2Heartbeat(ros2Node, dependantTopic); - ROS2DemandGraphNode dependantNode = new ROS2DemandGraphNode(ros2Helper, dependantTopic); + ROS2DemandGraphNode dependantNode = new ROS2DemandGraphNode(ros2Node, dependantTopic); testNode.addDependents(dependantNode); // No heartbeat is on -> nothing should be demanded @@ -95,9 +95,9 @@ public void testDemandChangedCallback() ROS2Topic dependantTopic = testTopic.withPrefix("dependant"); ROS2Heartbeat testHeartbeat = new ROS2Heartbeat(ros2Node, testTopic); - ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Helper, testTopic); + ROS2DemandGraphNode testNode = new ROS2DemandGraphNode(ros2Node, testTopic); ROS2Heartbeat dependantHeartbeat = new ROS2Heartbeat(ros2Node, dependantTopic); - ROS2DemandGraphNode dependantNode = new ROS2DemandGraphNode(ros2Helper, dependantTopic); + ROS2DemandGraphNode dependantNode = new ROS2DemandGraphNode(ros2Node, dependantTopic); testNode.addDependents(dependantNode); TypedNotification callbackNotification = new TypedNotification<>(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index 004309cf406..65302f03742 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -111,11 +111,11 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper this.robotModel = robotModel; this.syncedRobotModel = syncedRobotModel; - ros2Helper.subscribeViaCallback(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS, this::onStartAndGoalPosesReceived); - ros2Helper.subscribeViaCallback(ContinuousHikingAPI.PLANNED_FOOTSTEPS, this::onPlannedFootstepsReceived); - ros2Helper.subscribeViaCallback(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); + ros2Node.createSubscription(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS, (s) -> onStartAndGoalPosesReceived(s.takeNextData())); + ros2Node.createSubscription(ContinuousHikingAPI.PLANNED_FOOTSTEPS, (s) -> onPlannedFootstepsReceived(s.takeNextData())); + ros2Node.createSubscription(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, (s) -> onMonteCarloPlanReceived(s.takeNextData())); - commandPublisher = ros2Helper.getROS2Node().createPublisher(ContinuousHikingAPI.CONTINUOUS_HIKING_COMMAND); + commandPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CONTINUOUS_HIKING_COMMAND); SegmentDependentList> groundContactPoints = robotModel.getContactPointParameters().getControllerFootGroundContactPoints(); SideDependentList defaultContactPoints = new SideDependentList<>(); @@ -128,7 +128,7 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper } StancePoseCalculator stancePoseCalculator = new StancePoseCalculator(defaultContactPoints); - stancePoseSelectionPanel = new RDXStancePoseSelectionPanel(baseUI, ros2Helper, stancePoseCalculator); + stancePoseSelectionPanel = new RDXStancePoseSelectionPanel(baseUI, ros2Node, stancePoseCalculator); addChild(stancePoseSelectionPanel); MonteCarloFootstepPlannerParameters monteCarloPlannerParameters = new MonteCarloFootstepPlannerParameters(); @@ -136,14 +136,14 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper SwingPlannerParametersBasics swingPlannerParameters = robotModel.getSwingPlannerParameters(); this.swingTrajectoryParameters = robotModel.getWalkingControllerParameters().getSwingTrajectoryParameters(); - terrainPlanningDebugger = new RDXTerrainPlanningDebugger(ros2Helper, + terrainPlanningDebugger = new RDXTerrainPlanningDebugger(ros2Node, monteCarloPlannerParameters, robotModel.getContactPointParameters().getControllerFootGroundContactPoints()); ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, robotModel.getSimpleRobotName()), message -> terrainPlanningDebugger.reset()); - hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper); + hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Node); ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); createParametersPanel(continuousHikingParameters, continuousHikingParametersPanel, @@ -173,7 +173,7 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper PerceptionComms.HEIGHT_MAP_PARAMETERS); continuousHikingLogger = new ContinuousHikingLogger(); - controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Helper, + controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName(), syncedRobotModel.getReferenceFrames(), continuousHikingLogger); @@ -202,7 +202,7 @@ public void startContinuousPlannerSchedulingTask(boolean publishAndSubscribe) this.publishAndSubscribe = publishAndSubscribe; runSubscriberOnly = true; ROS2Helper ros2Helper = new ROS2Helper(ros2Node); - clientStoredPropertySets = new ROS2StoredPropertySetGroup(ros2Helper); + clientStoredPropertySets = new ROS2StoredPropertySetGroup(ros2Node); // Add Continuous Hiking Parameters to be between the UI and this process ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemoteHeightMapPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemoteHeightMapPanel.java index d65d85374cb..53fd06c1b39 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemoteHeightMapPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemoteHeightMapPanel.java @@ -38,7 +38,7 @@ public class RDXRemoteHeightMapPanel public RDXRemoteHeightMapPanel(ROS2Helper ros2Helper) { this.ros2Helper = ros2Helper; - remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper); + remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper.getROS2Node()); remotePropertySets.registerRemotePropertySet(heightMapParameters, HeightMapAPI.PARAMETERS); remotePropertySets.registerRemotePropertySet(heightMapFilterParameters, HeightMapAPI.FILTER_PARAMETERS); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemotePerceptionUI.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemotePerceptionUI.java index dc1093b613b..757908d5b16 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemotePerceptionUI.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXRemotePerceptionUI.java @@ -56,7 +56,7 @@ public RDXRemotePerceptionUI(ROS2Helper ros2Helper, RDXPanel panel) public RDXRemotePerceptionUI(ROS2Helper ros2Helper) { - remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper); + remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper.getROS2Node()); remotePropertySets.registerRemotePropertySet(perceptionConfigurationParameters, PerceptionComms.PERCEPTION_CONFIGURATION_PARAMETERS); remotePropertySets.registerRemotePropertySet(continuousHikingParameters, ContinuousHikingAPI.CONTINUOUS_HIKING_PARAMETERS); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java index 7ddd28c719f..8cc23d736e2 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java @@ -12,7 +12,6 @@ import imgui.type.ImBoolean; import us.ihmc.behaviors.activeMapping.StancePoseCalculator; import us.ihmc.communication.packets.MessageTools; -import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.geometry.Pose3D; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; @@ -35,6 +34,8 @@ import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SegmentDependentList; import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import java.util.ArrayList; @@ -44,6 +45,7 @@ public class RDXStancePoseSelectionPanel extends RDXPanel implements RenderableP { private final RDXBaseUI baseUI; private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); + private final ROS2Publisher publisher; private ModelInstance pickPointSphere; @@ -61,9 +63,7 @@ public class RDXStancePoseSelectionPanel extends RDXPanel implements RenderableP private final ImBoolean calculateStancePose = new ImBoolean(false); private final RDXStoredPropertySetTuner stancePoseCalculatorParametersTuner = new RDXStoredPropertySetTuner("Stance Pose Parameters"); - private final ROS2Helper ros2Helper; - - public RDXStancePoseSelectionPanel(RDXBaseUI baseUI, ROS2Helper ros2Helper, StancePoseCalculator stancePoseCalculator) + public RDXStancePoseSelectionPanel(RDXBaseUI baseUI, ROS2Node ros2Node, StancePoseCalculator stancePoseCalculator) { super("Stance Pose Selection"); this.baseUI = baseUI; @@ -72,7 +72,7 @@ public RDXStancePoseSelectionPanel(RDXBaseUI baseUI, ROS2Helper ros2Helper, Stan stancePoseCalculatorParametersTuner.create(stancePoseCalculator.getStancePoseParameters()); - this.ros2Helper = ros2Helper; + publisher = ros2Node.createPublisher(ContinuousHikingAPI.PLACED_GOAL_FOOTSTEPS); SegmentDependentList> contactPoints = new SideDependentList<>(); contactPoints.set(RobotSide.LEFT, PlannerTools.createFootContactPoints(0.2, 0.1, 0.08)); @@ -263,7 +263,7 @@ private void setGoalFootsteps() PoseListMessage poseListMessage = new PoseListMessage(); MessageTools.packPoseListMessage(poses, poseListMessage); - ros2Helper.publish(ContinuousHikingAPI.PLACED_GOAL_FOOTSTEPS, poseListMessage); + publisher.publish(poseListMessage); } public boolean isSelectionActive() diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXSteppableRegionsPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXSteppableRegionsPanel.java index 756ab1f6a34..9e2595307b9 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXSteppableRegionsPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXSteppableRegionsPanel.java @@ -48,7 +48,7 @@ public class RDXSteppableRegionsPanel public RDXSteppableRegionsPanel(ROS2Helper ros2Helper, SteppableRegionCalculatorParametersReadOnly defaultParameters) { - remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper); + remotePropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Helper.getROS2Node()); parameters = new SteppableRegionCalculatorParameters(defaultParameters); remotePropertySets.registerRemotePropertySet(parameters, SteppableRegionsAPI.PARAMETERS); } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java index 018c797a752..76734bfd316 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java @@ -33,6 +33,7 @@ import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SegmentDependentList; import us.ihmc.robotics.robotSide.SideDependentList; +import us.ihmc.ros2.ROS2Node; import java.util.ArrayList; import java.util.EnumMap; @@ -66,14 +67,15 @@ public class RDXTerrainPlanningDebugger implements RenderableProvider private int leftIndex = 0; private int rightIndex = 0; - public RDXTerrainPlanningDebugger(ROS2Helper ros2Helper, + public RDXTerrainPlanningDebugger(ROS2Node ros2Node, MonteCarloFootstepPlannerParameters monteCarloFootstepPlannerParameters, SegmentDependentList> contactPoints) { this.monteCarloFootstepPlannerParameters = monteCarloFootstepPlannerParameters; - ros2Helper.subscribeViaCallback(ContinuousHikingAPI.MONTE_CARLO_TREE_NODES, this::onMonteCarloTreeNodesReceived); - ros2Helper.subscribeViaCallback(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); + + ros2Node.createSubscription(ContinuousHikingAPI.MONTE_CARLO_TREE_NODES, (s) -> onMonteCarloTreeNodesReceived(s.takeNextData())); + ros2Node.createSubscription(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, (s) -> onMonteCarloPlanReceived(s.takeNextData())); goalFootstepGraphics = new SideDependentList<>(new RDXFootstepGraphic(contactPoints, RobotSide.LEFT), new RDXFootstepGraphic(contactPoints, RobotSide.RIGHT)); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java index f974d0ce73e..e7b1d6fed18 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java @@ -95,7 +95,7 @@ public void create() ros2Node = new ROS2NodeBuilder().build("perception_scene_graph_demo"); ros2Helper = new ROS2Helper(ros2Node); - detectionManager = new DetectionManager(ros2Helper); + detectionManager = new DetectionManager(ros2Node); // Add perception visualizers perceptionVisualizerPanel = new RDXPerceptionVisualizersPanel(); @@ -125,7 +125,7 @@ public void create() zed2ColoredPointCloudVisualizer.setActive(true); perceptionVisualizerPanel.addVisualizer(zed2ColoredPointCloudVisualizer); - perceptionVisualizerPanel.addVisualizer(new RDXDetectionManagerSettings("Detection Manager Settings", ros2Helper)); + perceptionVisualizerPanel.addVisualizer(new RDXDetectionManagerSettings("Detection Manager Settings", ros2Node)); RDXROS2YOLOv8Visualizer yoloSettingsVisualizer = new RDXROS2YOLOv8Visualizer("YOLOv8", ros2Node, PerceptionAPI.YOLO_ANNOTATED_IMAGE); yoloSettingsVisualizer.setActive(true); @@ -161,7 +161,7 @@ public void create() // Add rapid region parameters panel ImGuiRemoteROS2StoredPropertySet rapidRegionsParameterPanel - = new ImGuiRemoteROS2StoredPropertySet(ros2Helper, + = new ImGuiRemoteROS2StoredPropertySet(ros2Node, new RapidRegionsExtractorParameters(), PerceptionComms.PERSPECTIVE_RAPID_REGION_PARAMETERS); baseUI.getImGuiPanelManager().addPanel(rapidRegionsParameterPanel.createPanel()); @@ -188,7 +188,7 @@ public void create() planarRegionsExtractor = new RapidPlanarRegionsExtractor(planarRegionsOpenCLManager, imageHeight, imageWidth, fx, fy, cx, cy); planarRegionsExtractor.getDebugger().setEnabled(false); - planarRegionsExtractorParameterSync = new ROS2StoredPropertySet<>(ros2Helper, + planarRegionsExtractorParameterSync = new ROS2StoredPropertySet<>(ros2Node, PerceptionComms.PERSPECTIVE_RAPID_REGION_PARAMETERS, planarRegionsExtractor.getParameters()); } @@ -209,7 +209,7 @@ public void create() newPlanarRegions.set(planarRegionsInWorldFrame); - PerceptionMessageTools.publishFramePlanarRegionsList(framePlanarRegionsList, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Helper); + PerceptionMessageTools.publishFramePlanarRegionsList(framePlanarRegionsList, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Helper.getROS2Node()); zedDepthImage.release(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/ImGuiRemoteROS2StoredPropertySet.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/ImGuiRemoteROS2StoredPropertySet.java index 654cacd723a..eb1835d0438 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/ImGuiRemoteROS2StoredPropertySet.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/ImGuiRemoteROS2StoredPropertySet.java @@ -1,6 +1,7 @@ package us.ihmc.rdx.ui; import com.badlogic.gdx.graphics.Color; +import ihmc_common_msgs.msg.dds.PrimitiveDataVectorMessage; import imgui.ImGui; import us.ihmc.communication.property.StoredPropertySetMessageTools; import us.ihmc.communication.property.StoredPropertySetROS2Input; @@ -9,39 +10,29 @@ import us.ihmc.rdx.imgui.ImGuiTools; import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; import us.ihmc.rdx.imgui.RDXPanel; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.tools.property.StoredPropertySetBasics; public class ImGuiRemoteROS2StoredPropertySet { - private final ROS2PublishSubscribeAPI ros2PublishSubscribeAPI; private final StoredPropertySetBasics storedPropertySet; - private final StoredPropertySetROS2TopicPair topicPair; private final StoredPropertySetROS2Input storedPropertySetROS2Input; private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); private final RDXStoredPropertySetTuner imGuiStoredPropertySetTuner; private boolean storedPropertySetChangedByImGuiUser = false; private static final Color DARK_RED = new Color(0x781d1dff); private static final Color YELLOW = new Color(0xa6b51bff); + private final ROS2Publisher publisher; - public ImGuiRemoteROS2StoredPropertySet(ROS2PublishSubscribeAPI ros2PublishSubscribeAPI, - StoredPropertySetBasics storedPropertySet, - String moduleTopicName) - { - this(ros2PublishSubscribeAPI, - storedPropertySet, - new StoredPropertySetROS2TopicPair(moduleTopicName, storedPropertySet)); - } - - public ImGuiRemoteROS2StoredPropertySet(ROS2PublishSubscribeAPI ros2PublishSubscribeAPI, + public ImGuiRemoteROS2StoredPropertySet(ROS2Node ros2Node, StoredPropertySetBasics storedPropertySet, StoredPropertySetROS2TopicPair topicPair) { - this.ros2PublishSubscribeAPI = ros2PublishSubscribeAPI; this.storedPropertySet = storedPropertySet; - this.topicPair = topicPair; - ros2PublishSubscribeAPI.createPublisher(topicPair.getCommandTopic()); + publisher = ros2Node.createPublisher(topicPair.getCommandTopic()); - storedPropertySetROS2Input = new StoredPropertySetROS2Input(ros2PublishSubscribeAPI, topicPair.getStatusTopic(), storedPropertySet); + storedPropertySetROS2Input = new StoredPropertySetROS2Input(ros2Node, topicPair.getStatusTopic(), storedPropertySet); imGuiStoredPropertySetTuner = new RDXStoredPropertySetTuner(storedPropertySet.getTitle()); imGuiStoredPropertySetTuner.create(storedPropertySet, false, () -> storedPropertySetChangedByImGuiUser = true); } @@ -95,7 +86,7 @@ private void publishIfNecessary() if (storedPropertySetChangedByImGuiUser) { storedPropertySetChangedByImGuiUser = false; - ros2PublishSubscribeAPI.publish(topicPair.getCommandTopic(), StoredPropertySetMessageTools.newMessage(storedPropertySet)); + publisher.publish(StoredPropertySetMessageTools.newMessage(storedPropertySet)); } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/ImGuiRemoteROS2StoredPropertySetGroup.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/ImGuiRemoteROS2StoredPropertySetGroup.java index 73510d0964c..d7c3445efcb 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/ImGuiRemoteROS2StoredPropertySetGroup.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/ImGuiRemoteROS2StoredPropertySetGroup.java @@ -4,6 +4,7 @@ import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; +import us.ihmc.ros2.ROS2Node; import us.ihmc.tools.property.StoredPropertySetBasics; import java.util.ArrayList; @@ -11,17 +12,17 @@ public class ImGuiRemoteROS2StoredPropertySetGroup { private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); - private final ROS2PublishSubscribeAPI ros2PublishSubscribeAPI; + private final ROS2Node ros2Node; private final ArrayList remotePropertySets = new ArrayList<>(); - public ImGuiRemoteROS2StoredPropertySetGroup(ROS2PublishSubscribeAPI ros2PublishSubscribeAPI) + public ImGuiRemoteROS2StoredPropertySetGroup(ROS2Node ros2Node) { - this.ros2PublishSubscribeAPI = ros2PublishSubscribeAPI; + this.ros2Node = ros2Node; } public void registerRemotePropertySet(StoredPropertySetBasics storedPropertySet, StoredPropertySetROS2TopicPair topicPair) { - remotePropertySets.add(new ImGuiRemoteROS2StoredPropertySet(ros2PublishSubscribeAPI, storedPropertySet, topicPair)); + remotePropertySets.add(new ImGuiRemoteROS2StoredPropertySet(ros2Node, storedPropertySet, topicPair)); } public void renderImGuiWidgets() diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXDetectionManagerSettings.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXDetectionManagerSettings.java index be500ad48dc..0e36ef143d7 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXDetectionManagerSettings.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXDetectionManagerSettings.java @@ -5,6 +5,7 @@ import us.ihmc.perception.detections.DetectionManagerSettings; import us.ihmc.rdx.ui.ImGuiRemoteROS2StoredPropertySet; import us.ihmc.rdx.ui.graphics.RDXVisualizer; +import us.ihmc.ros2.ROS2Node; /* * FIXME: It doesn't make sense to have a visualizer for settings. @@ -16,7 +17,7 @@ public class RDXDetectionManagerSettings extends RDXVisualizer private final DetectionManagerSettings settings = new DetectionManagerSettings(); private final ImGuiRemoteROS2StoredPropertySet remoteStoredPropertySet; - public RDXDetectionManagerSettings(String title, ROS2PublishSubscribeAPI ros2) + public RDXDetectionManagerSettings(String title, ROS2Node ros2) { super(title); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java index a3f80885538..fb94f36f441 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java @@ -36,7 +36,7 @@ public ContinuousHikingProcess(DRCRobotModel robotModel) ROS2Helper ros2Helper = new ROS2Helper(ros2Node); ROS2SyncedRobotModel syncedRobot = new ROS2SyncedRobotModel(robotModel, ros2Node); syncedRobot.initializeToDefaultRobotInitialSetup(0.0, 0.0, 0.0, 0.0); - ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Helper); + ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Node); RepeatingTaskThread robotUpdateThread = new RepeatingTaskThread("SyncedRobotUpdate", syncedRobot::update).setFrequencyLimit(30.0); robotUpdateThread.startRepeating(); @@ -61,7 +61,7 @@ public ContinuousHikingProcess(DRCRobotModel robotModel) ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.HEIGHT_MAP_PARAMETERS, heightMapParameters); ContinuousHikingLogger continuousHikingLogger = new ContinuousHikingLogger(); - ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Helper, + ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName(), syncedRobot.getReferenceFrames(), continuousHikingLogger); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java index 1e49b0170ae..8fb1a092957 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java @@ -6,11 +6,11 @@ import controller_msgs.msg.dds.QueuedFootstepStatusMessage; import controller_msgs.msg.dds.WalkingStatusMessage; import us.ihmc.communication.HumanoidControllerAPI; -import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; import us.ihmc.robotics.robotSide.RobotSide; +import us.ihmc.ros2.ROS2Node; import java.util.List; import java.util.concurrent.atomic.AtomicBoolean; @@ -30,17 +30,18 @@ public class ControllerFootstepQueueMonitor private boolean footstepStarted; private final AtomicBoolean isWalking = new AtomicBoolean(false); - public ControllerFootstepQueueMonitor(ROS2Helper ros2Helper, + public ControllerFootstepQueueMonitor(ROS2Node ros2Node, String simpleRobotName, HumanoidReferenceFrames referenceFrames, ContinuousHikingLogger continuousHikingLogger) { this.referenceFrames = referenceFrames; this.continuousHikingLogger = continuousHikingLogger; - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(FootstepQueueStatusMessage.class, simpleRobotName), this::footstepQueueStatusReceived); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, simpleRobotName), this::footstepStatusReceived); - ros2Helper.subscribeViaCallback(getTopic(PlanOffsetStatus.class, simpleRobotName), this::acceptPlanOffsetStatus); - ros2Helper.subscribeViaCallback(getTopic(WalkingStatusMessage.class, simpleRobotName), this::acceptWalkingStatusMessage); + + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(FootstepQueueStatusMessage.class, simpleRobotName), (s) -> footstepQueueStatusReceived(s.takeNextData())); + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, simpleRobotName), (s) -> footstepStatusReceived(s.takeNextData())); + ros2Node.createSubscription(getTopic(PlanOffsetStatus.class, simpleRobotName), (s) -> acceptPlanOffsetStatus(s.takeNextData())); + ros2Node.createSubscription(getTopic(WalkingStatusMessage.class, simpleRobotName), (s) -> acceptWalkingStatusMessage(s.takeNextData())); } private void footstepQueueStatusReceived(FootstepQueueStatusMessage footstepQueueStatusMessage) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java index fdf1b68d4f0..5733fd3acff 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java @@ -252,7 +252,7 @@ private void updatePlanarRegions(ROS2Helper ros2Helper, ReferenceFrame cameraFra extractFramePlanarRegionsList(rapidPlanarRegionsExtractor, realsenseDepthImage, sensorFrameRegions, cameraFrame); filterFramePlanarRegionsList(); perceptionStatistics.updateTimeToComputeRapidRegions((System.nanoTime() - begin) * 1e-6f); - PerceptionMessageTools.publishFramePlanarRegionsList(sensorFrameRegions, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Helper); + PerceptionMessageTools.publishFramePlanarRegionsList(sensorFrameRegions, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Helper.getROS2Node()); perceptionStatistics.updateTimeToComputeRapidRegions((System.nanoTime() - begin) * 1e-6f); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/LocalizationAndMappingTask.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/LocalizationAndMappingTask.java index 1fb7ff1ad6f..065ccdd31e3 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/LocalizationAndMappingTask.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/LocalizationAndMappingTask.java @@ -109,7 +109,7 @@ public LocalizationAndMappingTask(String simpleRobotName, this.ros2Node = ros2Node; this.ros2Helper = new ROS2Helper(ros2Node); - ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Helper); + ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Node); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERSPECTIVE_PLANAR_REGION_MAPPING_PARAMETERS, planarRegionMap.getParameters()); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERCEPTION_CONFIGURATION_PARAMETERS, configurationParameters); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java index 7d295e4f0a2..a6ae7000b49 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java @@ -55,10 +55,10 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node, this.syncedRobot = syncedRobot; this.controllerFootstepQueueMonitor = controllerFootstepQueueMonitor; - realsensePublishDemandNode = new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_REALSENSE_PUBLICATION); - heightMapDemandNode = new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_HEIGHT_MAP); + realsensePublishDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_REALSENSE_PUBLICATION); + heightMapDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_HEIGHT_MAP); - realsenseDemandNode = new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_REALSENSE); + realsenseDemandNode = new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_REALSENSE); realsenseDemandNode.addDependents(realsensePublishDemandNode, heightMapDemandNode); d455Sensor = new RealSenseImageSensor(RealSenseConfiguration.D455_COLOR_720P_DEPTH_720P_30HZ); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StructuralPerceptionProcessWithDriver.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StructuralPerceptionProcessWithDriver.java index 4ade0666aa6..3696a4fccba 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StructuralPerceptionProcessWithDriver.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StructuralPerceptionProcessWithDriver.java @@ -130,7 +130,7 @@ private void onFrameReceived() rapidRegionsExtractor = new RapidPlanarRegionsExtractor(openCLManager, openCLProgram, depthHeight, depthWidth); rapidRegionsExtractor.setPatchSizeChanged(false); - ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Helper); + ros2PropertySetGroup = new ROS2StoredPropertySetGroup(realtimeROS2Node); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.SPHERICAL_RAPID_REGION_PARAMETERS, rapidRegionsExtractor.getParameters()); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.SPHERICAL_POLYGONIZER_PARAMETERS, rapidRegionsExtractor.getRapidPlanarRegionsCustomizer().getPolygonizerParameters()); @@ -176,7 +176,7 @@ private void extractCompressAndPublish() LogTools.info("Extracted {} planar regions", planarRegionsList.getNumberOfPlanarRegions()); - PerceptionMessageTools.publishFramePlanarRegionsList(framePlanarRegionsList, frameRegionsTopic, ros2Helper); + PerceptionMessageTools.publishFramePlanarRegionsList(framePlanarRegionsList, frameRegionsTopic, ros2Helper.getROS2Node()); } private void extractFramePlanarRegionsList(BytedecoImage depthImage, ReferenceFrame cameraFrame, FramePlanarRegionsList framePlanarRegionsList) diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXBallTrackingDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXBallTrackingDemo.java index da90e266f0a..4a974fed3b0 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXBallTrackingDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXBallTrackingDemo.java @@ -62,8 +62,8 @@ public RDXBallTrackingDemo() imageRetriever = new ZEDColorDepthImageRetriever(0, ReferenceFrame::getWorldFrame, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); + new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, + new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); imageRetriever.start(); imagePublisher = new ZEDColorDepthImagePublisher(PerceptionAPI.ZED2_COLOR_IMAGES, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_CUT_OUT_DEPTH); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointWorkerDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointWorkerDemo.java index 018dbcd0268..01bfe3db7fd 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointWorkerDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointWorkerDemo.java @@ -101,8 +101,8 @@ public RDXIterativeClosestPointWorkerDemo() { zedImageRetriever = new ZEDColorDepthImageRetriever(0, ReferenceFrame::getWorldFrame, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); + new ROS2DemandGraphNode(node, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, + new ROS2DemandGraphNode(node, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); zedImageRetriever.start(); zedImagePublisher = new ZEDColorDepthImagePublisher(PerceptionAPI.ZED2_COLOR_IMAGES, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_CUT_OUT_DEPTH); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXOpenCVTrackerDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXOpenCVTrackerDemo.java index 539015a6707..3d2074f482b 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXOpenCVTrackerDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXOpenCVTrackerDemo.java @@ -65,8 +65,8 @@ public RDXOpenCVTrackerDemo() imageRetriever = new ZEDColorDepthImageRetriever(0, ReferenceFrame::getWorldFrame, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, - new ROS2DemandGraphNode(ros2Helper, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); + new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded, + new ROS2DemandGraphNode(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION)::isDemanded); imageRetriever.start(); imagePublisher = new ZEDColorDepthImagePublisher(PerceptionAPI.ZED2_COLOR_IMAGES, PerceptionAPI.ZED2_DEPTH, PerceptionAPI.ZED2_CUT_OUT_DEPTH); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXSteppableRegionCalculatorDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXSteppableRegionCalculatorDemo.java index 67e3c498ba6..29d5d14d677 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXSteppableRegionCalculatorDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXSteppableRegionCalculatorDemo.java @@ -49,7 +49,7 @@ public RDXSteppableRegionCalculatorDemo() heightMapUI = new RDXRemoteHeightMapPanel(ros2Helper); SteppableRegionCalculatorParametersReadOnly defaultSteppableParameters = new SteppableRegionCalculatorParameters(); - steppableRegionsUpdater = new RemoteSteppableRegionsUpdater(ros2Helper, defaultSteppableParameters); + steppableRegionsUpdater = new RemoteSteppableRegionsUpdater(ros2Node, defaultSteppableParameters); steppableRegionsUpdater.start(); steppableRegionsUI = new RDXSteppableRegionsPanel(ros2Helper, defaultSteppableParameters); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/detections/DetectionManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/detections/DetectionManager.java index 2c69c1e3bde..08e6865dc65 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/detections/DetectionManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/detections/DetectionManager.java @@ -4,6 +4,7 @@ import us.ihmc.communication.property.ROS2StoredPropertySet; import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.robotics.time.TimeTools; +import us.ihmc.ros2.ROS2Node; import java.time.Duration; import java.time.Instant; @@ -26,7 +27,7 @@ public class DetectionManager private final DetectionManagerSettings settings = new DetectionManagerSettings(); private final ROS2StoredPropertySet settingsSync; - public DetectionManager(ROS2PublishSubscribeAPI ros2) + public DetectionManager(ROS2Node ros2) { settingsSync = ros2 == null ? null : new ROS2StoredPropertySet<>(ros2, PerceptionAPI.DETECTION_MANAGER_SETTINGS, settings); } diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/RemoteHeightMapUpdater.java b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/RemoteHeightMapUpdater.java index d05addef9bd..ccb6841ba10 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/RemoteHeightMapUpdater.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/RemoteHeightMapUpdater.java @@ -110,7 +110,7 @@ public void onNewDataMessage(Subscriber subscriber) }); */ - ros2PropertySetGroup = new ROS2StoredPropertySetGroup(new ROS2Helper(ros2Node)); + ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Node); ros2PropertySetGroup.registerStoredPropertySet(HeightMapAPI.PARAMETERS, heightMapUpdater.getHeightMapParameters()); ros2PropertySetGroup.registerStoredPropertySet(HeightMapAPI.FILTER_PARAMETERS, heightMapUpdater.getHeightMapFilterParameters()); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterDriverAndDepthPublisher.java b/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterDriverAndDepthPublisher.java index 083d08fc8aa..905cf77b449 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterDriverAndDepthPublisher.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterDriverAndDepthPublisher.java @@ -12,6 +12,7 @@ import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.steppableRegions.SteppableRegionCalculatorParameters; import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; +import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Topic; import us.ihmc.tools.thread.MissingThreadTools; import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService; @@ -38,7 +39,8 @@ public class OusterDriverAndDepthPublisher private volatile HumanoidReferenceFrames humanoidReferenceFrames; private final MutableReferenceFrame ousterSensorFrame = new MutableReferenceFrame(ReferenceFrame.getWorldFrame()); - public OusterDriverAndDepthPublisher(ROS2ControllerPublishSubscribeAPI ros2, + public OusterDriverAndDepthPublisher(ROS2Node ros2, + String robotName, Supplier humanoidReferenceFramesSupplier, ROS2Topic imageMessageTopic, ROS2Topic lidarScanTopic) @@ -53,7 +55,7 @@ public OusterDriverAndDepthPublisher(ROS2ControllerPublishSubscribeAPI ros2, ouster.start(); depthPublisher = new OusterDepthPublisher(imageMessageTopic, lidarScanTopic, publishLidarScanMonitor::isAlive); - heightMapUpdater = new OusterHeightMapUpdater(ros2); + heightMapUpdater = new OusterHeightMapUpdater(ros2, robotName); steppableRegionsUpdater = new RemoteSteppableRegionsUpdater(ros2, new SteppableRegionCalculatorParameters(), publishSteppableRegionsMonitor::isAlive); heightMapUpdater.attachHeightMapConsumer(steppableRegionsUpdater::submitLatestHeightMapMessage); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterHeightMapUpdater.java b/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterHeightMapUpdater.java index 0d16d8781c2..ae7b5bfff85 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterHeightMapUpdater.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterHeightMapUpdater.java @@ -4,6 +4,7 @@ import controller_msgs.msg.dds.WalkingStatusMessage; import perception_msgs.msg.dds.HeightMapMessage; import perception_msgs.msg.dds.HeightMapStateRequestMessage; +import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.property.ROS2StoredPropertySetGroup; import us.ihmc.communication.ros2.ROS2ControllerPublishSubscribeAPI; @@ -16,6 +17,7 @@ import us.ihmc.perception.heightMap.HeightMapAPI; import us.ihmc.perception.heightMap.HeightMapInputData; import us.ihmc.perception.heightMap.HeightMapUpdater; +import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2NodeBuilder; import us.ihmc.ros2.ROS2Publisher; import us.ihmc.ros2.RealtimeROS2Node; @@ -43,18 +45,19 @@ public class OusterHeightMapUpdater private final PausablePeriodicThread updateThread = new PausablePeriodicThread("OusterHeightMapUpdater", updateDTSeconds, this::update); - public OusterHeightMapUpdater(ROS2ControllerPublishSubscribeAPI ros2) + public OusterHeightMapUpdater(ROS2Node ros2Node, String simpleRobotName) { realtimeROS2Node = new ROS2NodeBuilder().buildRealtime("ouster_height_map_publisher"); heightMapPublisher = realtimeROS2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_OUTPUT); - ros2.subscribeViaCallback(PerceptionAPI.HEIGHT_MAP_STATE_REQUEST, this::consumeStateRequestMessage); - ros2.subscribeToControllerViaCallback(HighLevelStateChangeStatusMessage.class, this::consumeStateChangedMessage); - ros2.subscribeToControllerViaCallback(WalkingStatusMessage.class, this::consumeWalkingStatusMessage); + + ros2Node.createSubscription(PerceptionAPI.HEIGHT_MAP_STATE_REQUEST, (s) -> consumeStateRequestMessage(s.takeNextData())); + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, simpleRobotName), (s) -> consumeStateChangedMessage(s.takeNextData())); + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(WalkingStatusMessage.class, simpleRobotName), (s) -> consumeWalkingStatusMessage(s.takeNextData())); heightMapUpdater = new HeightMapUpdater(); heightMapUpdater.attachHeightMapConsumer(heightMapPublisher::publish); - ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2); + ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Node); ros2PropertySetGroup.registerStoredPropertySet(HeightMapAPI.PARAMETERS, heightMapUpdater.getHeightMapParameters()); ros2PropertySetGroup.registerStoredPropertySet(HeightMapAPI.FILTER_PARAMETERS, heightMapUpdater.getHeightMapFilterParameters()); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/rapidRegions/RapidPlanarRegionsExtractionThread.java b/ihmc-perception/src/main/java/us/ihmc/perception/rapidRegions/RapidPlanarRegionsExtractionThread.java index 3bf4230ff42..842ce7936ef 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/rapidRegions/RapidPlanarRegionsExtractionThread.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/rapidRegions/RapidPlanarRegionsExtractionThread.java @@ -14,6 +14,7 @@ import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.robotics.geometry.FramePlanarRegionsList; import us.ihmc.robotics.referenceFrames.MutableReferenceFrame; +import us.ihmc.ros2.ROS2Node; import us.ihmc.sensors.ImageSensor; import java.util.Collections; @@ -24,7 +25,7 @@ public class RapidPlanarRegionsExtractionThread extends RepeatingTaskThread { private static final double UPDATE_FREQUENCY = 10.0; - private final ROS2PublishSubscribeAPI ros2; + private final ROS2Node ros2; private final OpenCLManager openCLManager; @@ -37,7 +38,7 @@ public class RapidPlanarRegionsExtractionThread extends RepeatingTaskThread private final FramePlanarRegionsList framePlanarRegions = new FramePlanarRegionsList(); private final Set> newPlanarRegionNotifications = Collections.newSetFromMap(new ConcurrentHashMap<>()); - public RapidPlanarRegionsExtractionThread(ROS2PublishSubscribeAPI ros2, OpenCLManager openCLManager, ImageSensor imageSensor, int depthImageKey) + public RapidPlanarRegionsExtractionThread(ROS2Node ros2, OpenCLManager openCLManager, ImageSensor imageSensor, int depthImageKey) { super(imageSensor.getSensorName() + RapidPlanarRegionsExtractionThread.class.getSimpleName()); setFrequencyLimit(UPDATE_FREQUENCY); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/steppableRegions/RemoteSteppableRegionsUpdater.java b/ihmc-perception/src/main/java/us/ihmc/perception/steppableRegions/RemoteSteppableRegionsUpdater.java index 91cfeb1726b..a3f9dec91f1 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/steppableRegions/RemoteSteppableRegionsUpdater.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/steppableRegions/RemoteSteppableRegionsUpdater.java @@ -1,8 +1,12 @@ package us.ihmc.perception.steppableRegions; import perception_msgs.msg.dds.HeightMapMessage; +import perception_msgs.msg.dds.SteppableRegionDebugImagesMessage; +import perception_msgs.msg.dds.SteppableRegionsListCollectionMessage; import us.ihmc.communication.property.ROS2StoredPropertySetGroup; import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.tools.thread.ExecutorServiceTools; import java.util.concurrent.ScheduledExecutorService; @@ -23,25 +27,25 @@ public class RemoteSteppableRegionsUpdater private final ScheduledExecutorService executorService = ExecutorServiceTools.newScheduledThreadPool(1, getClass(), ExecutorServiceTools.ExceptionHandling.CATCH_AND_REPORT); private ScheduledFuture scheduled; - public RemoteSteppableRegionsUpdater(ROS2PublishSubscribeAPI rosHelper, SteppableRegionCalculatorParametersReadOnly defaultParameters) + public RemoteSteppableRegionsUpdater(ROS2Node ros2Node, SteppableRegionCalculatorParametersReadOnly defaultParameters) { - this(rosHelper, defaultParameters, () -> true); + this(ros2Node, defaultParameters, () -> true); } - public RemoteSteppableRegionsUpdater(ROS2PublishSubscribeAPI rosHelper, + public RemoteSteppableRegionsUpdater(ROS2Node ros2Node, SteppableRegionCalculatorParametersReadOnly defaultParameters, Supplier computeSteppableRegions) { this.computeSteppableRegions = computeSteppableRegions; - ros2PropertySetGroup = new ROS2StoredPropertySetGroup(rosHelper); + ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Node); ros2PropertySetGroup.registerStoredPropertySet(SteppableRegionsAPI.PARAMETERS, steppableRegionCalculatorParameters); - rosHelper.createPublisher(SteppableRegionsAPI.STEPPABLE_REGIONS_OUTPUT); - rosHelper.createPublisher(SteppableRegionsAPI.STEPPABLE_REGIONS_DEBUG_OUTPUT); + ROS2Publisher steppableRegionsOutputPublisher = ros2Node.createPublisher(SteppableRegionsAPI.STEPPABLE_REGIONS_OUTPUT); + ROS2Publisher steppableRegionsDebugOutputPublisher = ros2Node.createPublisher(SteppableRegionsAPI.STEPPABLE_REGIONS_DEBUG_OUTPUT); steppableRegionsUpdater = new SteppableRegionsUpdater(defaultParameters); - steppableRegionsUpdater.addSteppableRegionListCollectionOutputConsumer(message -> rosHelper.publish(SteppableRegionsAPI.STEPPABLE_REGIONS_OUTPUT, message)); - steppableRegionsUpdater.addSteppableRegionDebugConsumer(message -> rosHelper.publish(SteppableRegionsAPI.STEPPABLE_REGIONS_DEBUG_OUTPUT, message)); + steppableRegionsUpdater.addSteppableRegionListCollectionOutputConsumer(steppableRegionsOutputPublisher::publish); + steppableRegionsUpdater.addSteppableRegionDebugConsumer(steppableRegionsDebugOutputPublisher::publish); } public void submitLatestHeightMapMessage(HeightMapMessage heightMapMessage) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java index 336fc3a2951..580603a9354 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java @@ -26,6 +26,8 @@ import us.ihmc.perception.imageMessage.CompressionType; import us.ihmc.perception.imageMessage.PixelFormat; import us.ihmc.perception.opencv.OpenCVTools; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensors.realsense.RealSenseDevice; import us.ihmc.robotics.geometry.FramePlanarRegionsList; import us.ihmc.robotics.geometry.PlanarRegionsList; @@ -105,9 +107,10 @@ public static void publishJPGCompressedColorImage(BytePointer compressedColorPoi public static void publishFramePlanarRegionsList(FramePlanarRegionsList framePlanarRegionsList, ROS2Topic topic, - ROS2PublishSubscribeAPI ros2) + ROS2Node ros2Node) { - ros2.publish(topic, PlanarRegionMessageConverter.convertToFramePlanarRegionsListMessage(framePlanarRegionsList)); + ROS2Publisher publisher = ros2Node.createPublisher(topic); + publisher.publish(PlanarRegionMessageConverter.convertToFramePlanarRegionsListMessage(framePlanarRegionsList)); } public static void publishPlanarRegionsList(PlanarRegionsList planarRegionsList, ROS2Topic topic, ROS2Helper ros2Helper) diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java b/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java index 6896bd81984..a7939a670f4 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java @@ -106,7 +106,7 @@ public RealsenseColorDepthPublisher(RealSenseConfiguration realsenseConfiguratio ros2Helper = new ROS2Helper(ros2Node); LogTools.info("Setting up ROS2StoredPropertySetGroup"); - ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Helper); + ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Node); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERCEPTION_CONFIGURATION_PARAMETERS, parameters); Runtime.getRuntime().addShutdownHook(new Thread(this::destroy, "Shutdown")); From f2d2dd813c86440a1c706c125c2abdb2c1cdc3b2 Mon Sep 17 00:00:00 2001 From: Tomasz Bialek <103042423+TomaszTB@users.noreply.github.com> Date: Wed, 19 Feb 2025 13:00:12 -0600 Subject: [PATCH 12/27] Using createSubscription2 where applicable (#679) --- .../property/StoredPropertySetROS2Input.java | 2 +- .../us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java | 2 +- .../us/ihmc/rdx/perception/RDXContinuousHikingPanel.java | 6 +++--- .../ihmc/rdx/perception/RDXTerrainPlanningDebugger.java | 4 ++-- .../ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java | 2 +- .../activeMapping/ControllerFootstepQueueMonitor.java | 8 ++++---- .../us/ihmc/perception/ouster/OusterHeightMapUpdater.java | 6 +++--- 7 files changed, 15 insertions(+), 15 deletions(-) diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/property/StoredPropertySetROS2Input.java b/ihmc-communication/src/main/java/us/ihmc/communication/property/StoredPropertySetROS2Input.java index 46ffa0ce558..a6be82be54e 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/property/StoredPropertySetROS2Input.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/property/StoredPropertySetROS2Input.java @@ -39,7 +39,7 @@ public StoredPropertySetROS2Input(ROS2Node ros2Node, StoredPropertySetBasics storedPropertySetToUpdate) { this.storedPropertySetToUpdate = storedPropertySetToUpdate; - ros2Node.createSubscription(inputTopic, (s) -> acceptMessage(s.takeNextData())); + ros2Node.createSubscription2(inputTopic, this::acceptMessage); } private void acceptMessage(PrimitiveDataVectorMessage message) diff --git a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java index eb8ba200593..9d2750242b4 100644 --- a/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java +++ b/ihmc-communication/src/main/java/us/ihmc/communication/ros2/ROS2HeartbeatMonitor.java @@ -38,7 +38,7 @@ public class ROS2HeartbeatMonitor public ROS2HeartbeatMonitor(ROS2Node ros2Node, ROS2Topic heartbeatTopic) { - ros2Node.createSubscription(heartbeatTopic, (s) -> receivedHeartbeat(s.takeNextData())); + ros2Node.createSubscription2(heartbeatTopic, this::receivedHeartbeat); ThreadTools.startAsDaemon(() -> ExceptionTools.handle(this::monitorThread, DefaultExceptionHandler.MESSAGE_AND_STACKTRACE), "HeartbeatMonitor"); } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index 65302f03742..b13fffca9df 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -111,9 +111,9 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper this.robotModel = robotModel; this.syncedRobotModel = syncedRobotModel; - ros2Node.createSubscription(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS, (s) -> onStartAndGoalPosesReceived(s.takeNextData())); - ros2Node.createSubscription(ContinuousHikingAPI.PLANNED_FOOTSTEPS, (s) -> onPlannedFootstepsReceived(s.takeNextData())); - ros2Node.createSubscription(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, (s) -> onMonteCarloPlanReceived(s.takeNextData())); + ros2Node.createSubscription2(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS, this::onStartAndGoalPosesReceived); + ros2Node.createSubscription2(ContinuousHikingAPI.PLANNED_FOOTSTEPS, this::onPlannedFootstepsReceived); + ros2Node.createSubscription2(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); commandPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CONTINUOUS_HIKING_COMMAND); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java index 76734bfd316..0dbf3280681 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXTerrainPlanningDebugger.java @@ -74,8 +74,8 @@ public RDXTerrainPlanningDebugger(ROS2Node ros2Node, this.monteCarloFootstepPlannerParameters = monteCarloFootstepPlannerParameters; - ros2Node.createSubscription(ContinuousHikingAPI.MONTE_CARLO_TREE_NODES, (s) -> onMonteCarloTreeNodesReceived(s.takeNextData())); - ros2Node.createSubscription(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, (s) -> onMonteCarloPlanReceived(s.takeNextData())); + ros2Node.createSubscription2(ContinuousHikingAPI.MONTE_CARLO_TREE_NODES, this::onMonteCarloTreeNodesReceived); + ros2Node.createSubscription2(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); goalFootstepGraphics = new SideDependentList<>(new RDXFootstepGraphic(contactPoints, RobotSide.LEFT), new RDXFootstepGraphic(contactPoints, RobotSide.RIGHT)); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java index e7b1d6fed18..b102f10ff33 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java @@ -209,7 +209,7 @@ public void create() newPlanarRegions.set(planarRegionsInWorldFrame); - PerceptionMessageTools.publishFramePlanarRegionsList(framePlanarRegionsList, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Helper.getROS2Node()); + PerceptionMessageTools.publishFramePlanarRegionsList(framePlanarRegionsList, PerceptionAPI.PERSPECTIVE_RAPID_REGIONS, ros2Node); zedDepthImage.release(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java index 8fb1a092957..51c827c5be7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java @@ -38,10 +38,10 @@ public ControllerFootstepQueueMonitor(ROS2Node ros2Node, this.referenceFrames = referenceFrames; this.continuousHikingLogger = continuousHikingLogger; - ros2Node.createSubscription(HumanoidControllerAPI.getTopic(FootstepQueueStatusMessage.class, simpleRobotName), (s) -> footstepQueueStatusReceived(s.takeNextData())); - ros2Node.createSubscription(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, simpleRobotName), (s) -> footstepStatusReceived(s.takeNextData())); - ros2Node.createSubscription(getTopic(PlanOffsetStatus.class, simpleRobotName), (s) -> acceptPlanOffsetStatus(s.takeNextData())); - ros2Node.createSubscription(getTopic(WalkingStatusMessage.class, simpleRobotName), (s) -> acceptWalkingStatusMessage(s.takeNextData())); + ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(FootstepQueueStatusMessage.class, simpleRobotName), this::footstepQueueStatusReceived); + ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, simpleRobotName), this::footstepStatusReceived); + ros2Node.createSubscription2(getTopic(PlanOffsetStatus.class, simpleRobotName), this::acceptPlanOffsetStatus); + ros2Node.createSubscription2(getTopic(WalkingStatusMessage.class, simpleRobotName), this::acceptWalkingStatusMessage); } private void footstepQueueStatusReceived(FootstepQueueStatusMessage footstepQueueStatusMessage) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterHeightMapUpdater.java b/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterHeightMapUpdater.java index ae7b5bfff85..6565aed0de2 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterHeightMapUpdater.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/ouster/OusterHeightMapUpdater.java @@ -50,9 +50,9 @@ public OusterHeightMapUpdater(ROS2Node ros2Node, String simpleRobotName) realtimeROS2Node = new ROS2NodeBuilder().buildRealtime("ouster_height_map_publisher"); heightMapPublisher = realtimeROS2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_OUTPUT); - ros2Node.createSubscription(PerceptionAPI.HEIGHT_MAP_STATE_REQUEST, (s) -> consumeStateRequestMessage(s.takeNextData())); - ros2Node.createSubscription(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, simpleRobotName), (s) -> consumeStateChangedMessage(s.takeNextData())); - ros2Node.createSubscription(HumanoidControllerAPI.getTopic(WalkingStatusMessage.class, simpleRobotName), (s) -> consumeWalkingStatusMessage(s.takeNextData())); + ros2Node.createSubscription2(PerceptionAPI.HEIGHT_MAP_STATE_REQUEST, this::consumeStateRequestMessage); + ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, simpleRobotName), this::consumeStateChangedMessage); + ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(WalkingStatusMessage.class, simpleRobotName), this::consumeWalkingStatusMessage); heightMapUpdater = new HeightMapUpdater(); heightMapUpdater.attachHeightMapConsumer(heightMapPublisher::publish); From c51203a1d93253cf1c82f59d2dc0b1d247b40b25 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Thu, 20 Feb 2025 11:20:58 -0600 Subject: [PATCH 13/27] Switch height map to use ros2Node to remove ros2Helper --- .../perception/RDXContinuousHikingPanel.java | 22 +++++++++----- .../RDXHighLevelDepthSensorSimulator.java | 9 +++++- .../rdx/ui/graphics/RDXHeightMapRenderer.java | 16 ++-------- .../ros2/RDXROS2HeightMapVisualizer.java | 5 ++-- .../perception/HumanoidPerceptionModule.java | 23 +++++++++++---- .../perception/RapidHeightMapManager.java | 29 ++++++++++++------- .../RapidHeightMapUpdateThread.java | 10 +++---- .../StandAloneRealsenseProcess.java | 2 +- .../TerrainPerceptionProcessWithDriver.java | 11 +++++-- .../tools/PerceptionMessageTools.java | 5 ++-- .../RealsenseColorDepthPublisher.java | 8 ++++- 11 files changed, 87 insertions(+), 53 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index b13fffca9df..1e1a478fcd0 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -14,6 +14,7 @@ import ihmc_common_msgs.msg.dds.PoseListMessage; import imgui.ImGui; import imgui.type.ImBoolean; +import std_msgs.msg.dds.Empty; import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; @@ -72,7 +73,6 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private static final int numberOfKnotPoints = 12; private static final int maxIterationsOptimization = 100; private final ROS2Node ros2Node; - private final ROS2Helper ros2Helper; private final DRCRobotModel robotModel; private final ROS2SyncedRobotModel syncedRobotModel; private final ROS2Publisher commandPublisher; @@ -91,6 +91,9 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private final ImBoolean useMonteCarloFootstepPlanner = new ImBoolean(false); private final ControllerFootstepQueueMonitor controllerFootstepQueueMonitor; private final ContinuousHikingLogger continuousHikingLogger; + private final ROS2Publisher planOffsetStatusPublisher; + private final ROS2Publisher footstepStatusMessagePublisher; + private final ROS2Publisher clearGoalFootstepsPublisher; private SideDependentList startStancePose = new SideDependentList<>(new FramePose3D(), new FramePose3D()); private FootstepPlan latestFootstepPlan; private List>> swingTrajectories; @@ -101,16 +104,19 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private boolean publishAndSubscribe; private double simulatedDriftInMeters = -0.1; - public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper ros2Helper, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel) + public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel) { super("Continuous Hiking"); - this.ros2Helper = ros2Helper; setRenderMethod(this::renderImGuiWidgets); this.ros2Node = ros2Node; this.robotModel = robotModel; this.syncedRobotModel = syncedRobotModel; + footstepStatusMessagePublisher = ros2Node.createPublisher(getTopic(FootstepStatusMessage.class, robotModel.getSimpleRobotName())); + planOffsetStatusPublisher = ros2Node.createPublisher(getTopic(PlanOffsetStatus.class, robotModel.getSimpleRobotName())); + clearGoalFootstepsPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CLEAR_GOAL_FOOTSTEPS); + ros2Node.createSubscription2(ContinuousHikingAPI.START_AND_GOAL_FOOTSTEPS, this::onStartAndGoalPosesReceived); ros2Node.createSubscription2(ContinuousHikingAPI.PLANNED_FOOTSTEPS, this::onPlannedFootstepsReceived); ros2Node.createSubscription2(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); @@ -140,8 +146,8 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, ROS2Helper monteCarloPlannerParameters, robotModel.getContactPointParameters().getControllerFootGroundContactPoints()); - ros2Helper.subscribeViaCallback(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, robotModel.getSimpleRobotName()), - message -> terrainPlanningDebugger.reset()); + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(WalkingControllerFailureStatusMessage.class, robotModel.getSimpleRobotName()), + (s) -> terrainPlanningDebugger.reset()); hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Node); ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); @@ -305,14 +311,14 @@ public void renderImGuiWidgets() // Simulate that the controller started a step, this part triggers the drift offset kernel FootstepStatusMessage footstepStatusMessage = new FootstepStatusMessage(); footstepStatusMessage.setFootstepStatus(FootstepStatusMessage.FOOTSTEP_STATUS_STARTED); - ros2Helper.publish(getTopic(FootstepStatusMessage.class, "Nadia"), footstepStatusMessage); + footstepStatusMessagePublisher.publish(footstepStatusMessage); // Simulate that the controller has drifted by some z value PlanOffsetStatus planOffsetStatus = new PlanOffsetStatus(); Vector3D planOffset = new Vector3D(0, 0, simulatedDriftInMeters); planOffsetStatus.getOffsetVector().set(planOffset); LogTools.info("Plan Offset Status: " + planOffsetStatus.getOffsetVector()); - ros2Helper.publish(getTopic(PlanOffsetStatus.class, "Nadia"), planOffsetStatus); + planOffsetStatusPublisher.publish(planOffsetStatus); // The amount of drift that we want to simulation and adjust for if we do this over and over if (simulatedDriftInMeters > -1.0) @@ -372,7 +378,7 @@ public void getRenderables(Array renderables, Pool pool) public void clearPlannedFootsteps() { - ros2Helper.publish(ContinuousHikingAPI.CLEAR_GOAL_FOOTSTEPS); + clearGoalFootstepsPublisher.publish(new Empty()); } /** diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java index 7faeaaaddcc..8e1c6bbea10 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java @@ -161,6 +161,7 @@ public class RDXHighLevelDepthSensorSimulator extends RDXPanel private Thread publishImagesThread; private volatile boolean publishImagesRunning; + private ROS2Publisher depthImagePublisher; public RDXHighLevelDepthSensorSimulator(String sensorName, ReferenceFrame sensorFrame, @@ -572,7 +573,13 @@ public void publishROS2DepthImageMessage() sensorPose.setToZero(sensorFrame); sensorPose.changeFrame(ReferenceFrame.getWorldFrame()); OpenCVTools.compressImagePNG(depthImageMat, compressedDepthPointer); - PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, ros2DepthTopic, depthImageMessage, ros2Helper, sensorPose, now, depthSequenceNumber++, + + if (depthImagePublisher == null) + { + depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic); + } + + PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, ros2DepthTopic, depthImageMessage, depthImagePublisher, sensorPose, now, depthSequenceNumber++, depthSensorSimulator.getImageHeight(), depthSensorSimulator.getImageWidth(), 0.001f); }); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java index 9ce9441ef59..4a04febbb06 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java @@ -20,7 +20,6 @@ import us.ihmc.rdx.shader.RDXShader; import us.ihmc.rdx.shader.RDXUniform; -import java.nio.FloatBuffer; /** * Renders a height map as a point cloud. The height map is stored as a 16-bit grayscale image. @@ -29,13 +28,6 @@ * 0 is the metric -3.2768f height, and 65536 is the 3.2768f height. * The height is scaled up by 10,000 for storage as 16-bit value (short) */ - -/** - * This has been updated to use {@link us.ihmc.rdx.ui.graphics.RDXHeightMapGraphicNew}, please use that going forward, this implementation has bugs with - * interacting with collisions - * from the mouse - */ -@Deprecated public class RDXHeightMapRenderer implements RenderableProvider { private Renderable renderable; @@ -134,7 +126,8 @@ public void update(RigidBodyTransform zUpFrameToWorld, intermediateVertexBuffer[vertexIndex + 6] = color.a; // Size - intermediateVertexBuffer[vertexIndex + 7] = 0.02f; + // For visual improvements make the cells slightly bigger than they really are + intermediateVertexBuffer[vertexIndex + 7] = 0.025f; } } @@ -142,11 +135,6 @@ public void update(RigidBodyTransform zUpFrameToWorld, renderable.meshPart.mesh.setVertices(intermediateVertexBuffer, 0, totalCells * FLOATS_PER_CELL); } - public FloatBuffer getVertexBuffer() - { - return renderable.meshPart.mesh.getVerticesBuffer(); - } - @Override public void getRenderables(Array renderables, Pool pool) { diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java index 182aa0e4db4..b416d173f81 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java @@ -45,9 +45,9 @@ public class RDXROS2HeightMapVisualizer extends RDXROS2MultiTopicVisualizer private final RDXGlobalHeightMapGraphic globalHeightMapGraphic = new RDXGlobalHeightMapGraphic(); private final ResettableExceptionHandlingExecutorService executorService; - private final ImBoolean enableHeightMapVisualizer = new ImBoolean(true); + private final ImBoolean enableHeightMapVisualizer = new ImBoolean(false); private final ImBoolean enableGlobalHeightMapVisualizer = new ImBoolean(false); - private final ImBoolean enableHeightMapRenderer = new ImBoolean(false); + private final ImBoolean enableHeightMapRenderer = new ImBoolean(true); private final ImBoolean displayGlobalHeightMapImage = new ImBoolean(false); private final RigidBodyTransform zUpToWorldTransform = new RigidBodyTransform(); @@ -251,7 +251,6 @@ public void update() { if (heightMapImage.ptr(0) != null) { - //PerceptionDebugTools.printMat("Height Map Image", heightMapImage, 10); heightMapRenderer.update(zUpToWorldTransform, heightMapImage.ptr(0), (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java index 5733fd3acff..7aac2ac1b89 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java @@ -34,6 +34,7 @@ import us.ihmc.robotics.geometry.PlanarRegionsList; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.ros2.ROS2Topic; import us.ihmc.ros2.RealtimeROS2Node; import us.ihmc.sensorProcessing.globalHeightMap.GlobalHeightMap; @@ -87,6 +88,8 @@ public class HumanoidPerceptionModule private boolean mappingEnabled = false; private boolean occupancyGridEnabled = false; public boolean heightMapDataBeingProcessed = false; + private ROS2Publisher heightMapPublisher; + private ROS2Publisher heightMapImagePublisher; public HumanoidPerceptionModule(OpenCLManager openCLManager) { @@ -168,7 +171,7 @@ public void updateTerrain(ROS2Helper ros2Helper, if (ros2Helper != null) { - publishHeightMapImage(ros2Helper, + publishHeightMapImage(ros2Helper.getROS2Node(), croppedHeightMapImage, compressedCroppedHeightMapPointer, PerceptionAPI.HEIGHT_MAP_CROPPED, @@ -181,18 +184,23 @@ public void updateTerrain(ROS2Helper ros2Helper, } } - public void publishHeightMapImage(ROS2Helper ros2Helper, + public void publishHeightMapImage(ROS2Node ros2Node, Mat image, BytePointer pointer, ROS2Topic topic, ImageMessage message, Instant acquisitionTime) { + if (heightMapImagePublisher == null) + { + heightMapImagePublisher = ros2Node.createPublisher(topic); + } + OpenCVTools.compressImagePNG(image, pointer); PerceptionMessageTools.publishCompressedDepthImage(pointer, topic, message, - ros2Helper, + heightMapImagePublisher, cameraPose, acquisitionTime, rapidHeightMapExtractor.getSequenceNumber(), @@ -222,8 +230,13 @@ private static void packGlobalMapTileMessage(GlobalMapTileMessage messageToPack, } - public void publishExternalHeightMapImage(ROS2Helper ros2Helper) + public void publishExternalHeightMapImage(ROS2Node ros2Node) { + if (heightMapPublisher == null) + { + heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); + } + executorService.clearTaskQueue(); executorService.submit(() -> { @@ -234,7 +247,7 @@ public void publishExternalHeightMapImage(ROS2Helper ros2Helper) PerceptionMessageTools.publishCompressedDepthImage(compressedInternalHeightMapPointer, PerceptionAPI.HEIGHT_MAP_CROPPED, croppedHeightMapImageMessage, - ros2Helper, + heightMapPublisher, new FramePose3D(ReferenceFrame.getWorldFrame(), rapidHeightMapExtractor.getSensorOrigin(), new Quaternion()), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java index 901139e15ba..e8886dff9e7 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java @@ -12,7 +12,6 @@ import us.ihmc.commons.thread.Notification; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.transform.RigidBodyTransform; @@ -25,6 +24,8 @@ import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.ros2.ROS2Node; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import java.time.Instant; @@ -37,7 +38,7 @@ public class RapidHeightMapManager private final RapidHeightMapExtractorInterface rapidHeightMapExtractor; private final ImageMessage croppedHeightMapImageMessage = new ImageMessage(); private final FramePose3D cameraPose = new FramePose3D(); - private final ROS2PublishSubscribeAPI ros2; + private final ROS2Node ros2Node; private final boolean runWithCUDA; private final Mat hostDepthImage = new Mat(); private final Notification resetHeightMapRequested = new Notification(); @@ -48,8 +49,9 @@ public class RapidHeightMapManager private GpuMat deviceDepthImage; private BytedecoImage heightMapBytedecoImage; + private ROS2Publisher heightMapPublisher; - public RapidHeightMapManager(ROS2PublishSubscribeAPI ros2, + public RapidHeightMapManager(ROS2Node ros2Node, DRCRobotModel robotModel, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, @@ -57,7 +59,7 @@ public RapidHeightMapManager(ROS2PublishSubscribeAPI ros2, CameraIntrinsics depthImageIntrinsics, boolean runWithCUDA) throws Exception { - this.ros2 = ros2; + this.ros2Node = ros2Node; this.runWithCUDA = runWithCUDA; if (runWithCUDA) @@ -81,13 +83,15 @@ public RapidHeightMapManager(ROS2PublishSubscribeAPI ros2, } // We use a notification in order to only call resetting the height map in one place - ros2.subscribeViaVolatileCallback(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); - if (robotModel != null) // Will be null on test bench + ros2Node.createSubscription2(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); + if (robotModel != null) { - ros2.subscribeViaVolatileCallback(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotModel.getSimpleRobotName()), message -> - { // Automatically reset the height map when the robot goes into the walking state - if (message.getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING) + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotModel.getSimpleRobotName()), message -> + { + if (message.takeNextData().getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING) + { resetHeightMapRequested.set(); + } }); } } @@ -166,11 +170,16 @@ public void update(Mat latestDepthImage, Instant imageAcquisitionTime, Reference heightScaleFactor = (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor(); } + if (heightMapPublisher == null) + { + heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); + } + OpenCVTools.compressImagePNG(croppedHeightMapImage, compressedCroppedHeightMapPointer); PerceptionMessageTools.publishCompressedDepthImage(compressedCroppedHeightMapPointer, PerceptionAPI.HEIGHT_MAP_CROPPED, croppedHeightMapImageMessage, - ros2, + heightMapPublisher, cameraPose, imageAcquisitionTime, rapidHeightMapExtractor.getSequenceNumber(), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java index cf1bbea7c50..f71172860cc 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java @@ -3,16 +3,16 @@ import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; import us.ihmc.commons.thread.RepeatingTaskThread; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.log.LogTools; import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.ros2.ROS2Node; import us.ihmc.sensorProcessing.heightMap.HeightMapData; import us.ihmc.sensors.ImageSensor; public class RapidHeightMapUpdateThread extends RepeatingTaskThread { - private final ROS2PublishSubscribeAPI ros2; + private final ROS2Node ros2Node; private final ROS2SyncedRobotModel syncedRobotModel; private final ReferenceFrame leftFootFrame; private final ReferenceFrame rightFootFrame; @@ -27,7 +27,7 @@ public class RapidHeightMapUpdateThread extends RepeatingTaskThread private final boolean runWithCUDA; private final int depthImageKey; - public RapidHeightMapUpdateThread(ROS2PublishSubscribeAPI ros2, + public RapidHeightMapUpdateThread(ROS2Node ros2Node, ROS2SyncedRobotModel syncedRobotModel, ReferenceFrame leftFootFrame, ReferenceFrame rightFootFrame, @@ -38,7 +38,7 @@ public RapidHeightMapUpdateThread(ROS2PublishSubscribeAPI ros2, { super(imageSensor.getSensorName() + RapidHeightMapUpdateThread.class.getSimpleName()); - this.ros2 = ros2; + this.ros2Node = ros2Node; this.syncedRobotModel = syncedRobotModel; this.leftFootFrame = leftFootFrame; this.rightFootFrame = rightFootFrame; @@ -62,7 +62,7 @@ protected void runTask() // Initialize if (heightMapManager == null) { - heightMapManager = new RapidHeightMapManager(ros2, + heightMapManager = new RapidHeightMapManager(ros2Node, syncedRobotModel.getRobotModel(), leftFootFrame, rightFootFrame, diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java index a6ae7000b49..5eb2e6f9e47 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java @@ -78,7 +78,7 @@ public StandAloneRealsenseProcess(ROS2Node ros2Node, private void initializeHeightMap(ControllerFootstepQueueMonitor controllerFootstepQueueMonitor) { boolean runWithCUDA = true; - heightMapUpdateThread = new RapidHeightMapUpdateThread(ros2Helper, + heightMapUpdateThread = new RapidHeightMapUpdateThread(ros2Helper.getROS2Node(), syncedRobot, syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.LEFT), syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.RIGHT), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java index 011a1bdfe2f..eef823b6d76 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java @@ -20,6 +20,7 @@ import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.parameters.PerceptionConfigurationParameters; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensors.realsense.RealSenseConfiguration; import us.ihmc.sensors.realsense.RealSenseDevice; import us.ihmc.sensors.realsense.RealSenseDeviceManager; @@ -101,6 +102,7 @@ public class TerrainPerceptionProcessWithDriver private final int colorHeight; private long depthSequenceNumber = 0; private long colorSequenceNumber = 0; + private ROS2Publisher depthPublisher; public TerrainPerceptionProcessWithDriver(String robotName, CollisionBoxProvider collisionBoxProvider, @@ -245,6 +247,12 @@ private void update() if (parameters.getPublishDepth()) { + + if (depthPublisher == null) + { + depthPublisher = realtimeROS2Node.createPublisher(depthTopic); + } + executorService.submit(() -> { OpenCVTools.compressImagePNG(depth16UC1Image, compressedDepthPointer); @@ -252,8 +260,7 @@ private void update() CameraModel.PINHOLE.packMessageFormat(depthImageMessage); PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, depthTopic, - depthImageMessage, - ros2Helper, + depthImageMessage, depthPublisher, cameraPose, acquisitionTime, depthSequenceNumber++, diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java index 580603a9354..e829439d605 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java @@ -15,7 +15,6 @@ import us.ihmc.communication.packets.PlanarRegionMessageConverter; import us.ihmc.communication.producers.VideoSource; import us.ihmc.communication.ros2.ROS2Helper; -import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; import us.ihmc.euclid.tuple3D.Point3D; import us.ihmc.euclid.tuple4D.Quaternion; @@ -78,7 +77,7 @@ public static void toBoofCV(ImageMessage imageMessage, Object cameraPinholeToPac public static void publishCompressedDepthImage(BytePointer compressedDepthPointer, ROS2Topic topic, ImageMessage depthImageMessage, - ROS2PublishSubscribeAPI helper, + ROS2Publisher publisher, Pose3DReadOnly cameraPose, Instant acquisitionTime, long sequenceNumber, @@ -87,7 +86,7 @@ public static void publishCompressedDepthImage(BytePointer compressedDepthPointe float depthToMetersRatio) { packCompressedDepthImage(compressedDepthPointer, depthImageMessage, cameraPose, acquisitionTime, sequenceNumber, height, width, depthToMetersRatio); - helper.publish(topic, depthImageMessage); + publisher.publish(depthImageMessage); } public static void publishJPGCompressedColorImage(BytePointer compressedColorPointer, diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java b/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java index a7939a670f4..19a71e410c3 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java @@ -26,6 +26,7 @@ import us.ihmc.perception.logging.PerceptionLoggerConstants; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.parameters.PerceptionConfigurationParameters; +import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensors.realsense.RealSenseConfiguration; import us.ihmc.sensors.realsense.RealSenseDevice; import us.ihmc.sensors.realsense.RealSenseDeviceManager; @@ -82,6 +83,7 @@ public class RealsenseColorDepthPublisher private boolean loggerInitialized = false; private volatile boolean running = true; private final Notification destroyedNotification = new Notification(); + private ROS2Publisher depthPublisher; public RealsenseColorDepthPublisher(RealSenseConfiguration realsenseConfiguration, ROS2Topic depthTopic, @@ -174,10 +176,14 @@ private void update() PerceptionMessageTools.setDepthIntrinsicsFromRealsense(realsense, depthImageMessage); CameraModel.PINHOLE.packMessageFormat(depthImageMessage); + if (depthPublisher == null) + { + depthPublisher = ros2Node.createPublisher(depthTopic); + } PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, depthTopic, depthImageMessage, - ros2Helper, + depthPublisher, depthPose, acquisitionTime, depthSequenceNumber++, From 4f5fdd784cfb5d38bbabed51efbfc7d28358e7de Mon Sep 17 00:00:00 2001 From: nkitchel Date: Fri, 21 Feb 2025 19:07:31 -0600 Subject: [PATCH 14/27] Implemented square up and walk to goal as a message within CH --- .../communication/ContinuousHikingAPI.java | 3 + .../perception/RDXContinuousHikingPanel.java | 8 + .../RDXStancePoseSelectionPanel.java | 20 ++ .../ContinuousHikingProcess.java | 1 + .../JustWaitState.java | 245 +++++++++++++++++- .../ContinuousPlannerSchedulingTask.java | 21 +- .../ControllerFootstepQueueMonitor.java | 38 +++ 7 files changed, 331 insertions(+), 5 deletions(-) diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java index 5ae047b78ad..76ada624e60 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java @@ -4,6 +4,7 @@ import behavior_msgs.msg.dds.ContinuousWalkingStatusMessage; import controller_msgs.msg.dds.FootstepDataListMessage; import ihmc_common_msgs.msg.dds.PoseListMessage; +import std_msgs.msg.dds.Empty; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; import us.ihmc.ros2.ROS2Topic; @@ -18,6 +19,8 @@ public class ContinuousHikingAPI public static final ROS2Topic CONTINUOUS_HIKING_COMMAND = IHMC_ROOT.withModule(moduleName).withType(ContinuousHikingCommandMessage.class).withSuffix("command"); public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(std_msgs.msg.dds.Empty.class).withSuffix("clear_goal_footsteps"); public static final ROS2Topic PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("placed_goal_footsteps"); + public static final ROS2Topic ROTATE_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("rotate_goal_footsteps"); + public static final ROS2Topic SQUARE_UP_STEP = IHMC_ROOT.withModule(moduleName).withType(std_msgs.msg.dds.Empty.class).withSuffix("rotate_goal_footsteps"); // Statuses supported from the Continuous Hiking Process public static final ROS2Topic CONTINUOUS_WALKING_STATUS = IHMC_ROOT.withModule(moduleName).withType(ContinuousWalkingStatusMessage.class).withSuffix("status"); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index 1e1a478fcd0..40fb74db91d 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -76,6 +76,7 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private final DRCRobotModel robotModel; private final ROS2SyncedRobotModel syncedRobotModel; private final ROS2Publisher commandPublisher; + private final ROS2Publisher squareUpPublisher; private final ContinuousHikingCommandMessage commandMessage = new ContinuousHikingCommandMessage(); private final RDXStancePoseSelectionPanel stancePoseSelectionPanel; private final PositionOptimizedTrajectoryGenerator positionTrajectoryGenerator = new PositionOptimizedTrajectoryGenerator(numberOfKnotPoints, @@ -122,6 +123,7 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotMod ros2Node.createSubscription2(ContinuousHikingAPI.MONTE_CARLO_FOOTSTEP_PLAN, this::onMonteCarloPlanReceived); commandPublisher = ros2Node.createPublisher(ContinuousHikingAPI.CONTINUOUS_HIKING_COMMAND); + squareUpPublisher = ros2Node.createPublisher(ContinuousHikingAPI.SQUARE_UP_STEP); SegmentDependentList> groundContactPoints = robotModel.getContactPointParameters().getControllerFootGroundContactPoints(); SideDependentList defaultContactPoints = new SideDependentList<>(); @@ -228,6 +230,7 @@ public void startContinuousPlannerSchedulingTask(boolean publishAndSubscribe) continuousPlannerSchedulingTask = new ContinuousPlannerSchedulingTask(robotModel, ros2Node, + syncedRobotModel, syncedRobotModel.getReferenceFrames(), controllerFootstepQueueMonitor, continuousHikingLogger, @@ -294,6 +297,11 @@ public void renderImGuiWidgets() ImGui.separator(); ImGui.text("Options for Continuous Hiking Message"); ImGui.indent(); + if (ImGui.button("Square Up")) + { + squareUpPublisher.publish(new Empty()); + } + ImGui.checkbox("Square Up To Goal", squareUpToGoal); if (ImGui.button("Clear Planned footsteps")) { diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java index 8cc23d736e2..36ec5d12a59 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java @@ -26,6 +26,7 @@ import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; import us.ihmc.rdx.imgui.RDXPanel; import us.ihmc.rdx.input.ImGui3DViewInput; +import us.ihmc.rdx.input.ImGuiMouseDragData; import us.ihmc.rdx.tools.LibGDXTools; import us.ihmc.rdx.tools.RDXModelBuilder; import us.ihmc.rdx.ui.RDXBaseUI; @@ -46,6 +47,7 @@ public class RDXStancePoseSelectionPanel extends RDXPanel implements RenderableP private final RDXBaseUI baseUI; private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass()); private final ROS2Publisher publisher; + private final ROS2Publisher turningPublisher; private ModelInstance pickPointSphere; @@ -73,6 +75,7 @@ public RDXStancePoseSelectionPanel(RDXBaseUI baseUI, ROS2Node ros2Node, StancePo stancePoseCalculatorParametersTuner.create(stancePoseCalculator.getStancePoseParameters()); publisher = ros2Node.createPublisher(ContinuousHikingAPI.PLACED_GOAL_FOOTSTEPS); + turningPublisher = ros2Node.createPublisher(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS); SegmentDependentList> contactPoints = new SideDependentList<>(); contactPoints.set(RobotSide.LEFT, PlannerTools.createFootContactPoints(0.2, 0.1, 0.08)); @@ -210,6 +213,11 @@ else if (dScroll < 0.0) latestPickPoint.getOrientation().setYawPitchRoll(latestFootstepYaw + deltaYaw, 0.0, 0.0); } } + if (input.isWindowHovered() && input.mouseReleasedWithoutDrag(ImGuiMouseButton.Middle) && calculateStancePose.get() && selectionActive) + { + setRotateGoalFootsteps(); + selectionActive = false; + } if (input.isWindowHovered() & input.mouseReleasedWithoutDrag(ImGuiMouseButton.Left) && calculateStancePose.get() && selectionActive) { @@ -254,6 +262,18 @@ public void destroy() rightSpheres.clear(); } + private void setRotateGoalFootsteps() + { + List poses = new ArrayList<>(); + poses.add(new Pose3D(stancePoses.get(RobotSide.LEFT))); + poses.add(new Pose3D(stancePoses.get(RobotSide.RIGHT))); + + PoseListMessage poseListMessage = new PoseListMessage(); + MessageTools.packPoseListMessage(poses, poseListMessage); + + turningPublisher.publish(poseListMessage); + } + private void setGoalFootsteps() { List poses = new ArrayList<>(); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java index fb94f36f441..65f6cef4ebe 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java @@ -70,6 +70,7 @@ public ContinuousHikingProcess(DRCRobotModel robotModel) continuousPlannerSchedulingTask = new ContinuousPlannerSchedulingTask(robotModel, ros2Node, + syncedRobot, syncedRobot.getReferenceFrames(), controllerFootstepQueueMonitor, continuousHikingLogger, diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java index d1351e433e6..31431681d01 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java @@ -1,21 +1,114 @@ package us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine; +import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; +import controller_msgs.msg.dds.FootstepDataListMessage; +import controller_msgs.msg.dds.QueuedFootstepStatusMessage; +import ihmc_common_msgs.msg.dds.PoseListMessage; +import ihmc_common_msgs.msg.dds.QueueableMessage; +import org.jetbrains.annotations.NotNull; +import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; +import us.ihmc.avatar.networkProcessor.footstepPlanningModule.FootstepPlanningModuleLauncher; import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.commons.thread.ThreadTools; +import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.communication.packets.MessageTools; +import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.euclid.geometry.Pose3D; +import us.ihmc.euclid.geometry.interfaces.Pose3DReadOnly; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.ReferenceFrame; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; +import us.ihmc.footstepPlanning.FootstepPlan; +import us.ihmc.footstepPlanning.FootstepPlannerOutput; +import us.ihmc.footstepPlanning.FootstepPlannerRequest; +import us.ihmc.footstepPlanning.FootstepPlanningModule; +import us.ihmc.footstepPlanning.PlannedFootstep; +import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; +import us.ihmc.footstepPlanning.graphSearch.FootstepPlannerEnvironmentHandler; +import us.ihmc.footstepPlanning.graphSearch.footstepSnapping.FootstepSnapAndWiggler; +import us.ihmc.footstepPlanning.graphSearch.graph.DiscreteFootstep; +import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; +import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; +import us.ihmc.footstepPlanning.swing.SwingPlannerType; +import us.ihmc.footstepPlanning.tools.PlannerTools; +import us.ihmc.log.LogTools; +import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.stateMachine.core.State; +import us.ihmc.ros2.ROS2Topic; +import us.ihmc.sensorProcessing.heightMap.HeightMapData; + +import java.util.List; +import java.util.concurrent.atomic.AtomicReference; +import java.util.function.Supplier; public class JustWaitState implements State { + @NotNull + private final ROS2Helper ros2Helper; + @NotNull + private final ROS2SyncedRobotModel syncedRobot; + private final AtomicReference commandMessage; private final ControllerFootstepQueueMonitor controllerQueueMonitor; + private final FootstepPlannerEnvironmentHandler environmentHandler; private boolean isDone; + private final FootstepPlanningModule footstepPlanner; + private final DefaultFootstepPlannerParametersBasics footstepPlannerParameters; + private final SwingPlannerParametersBasics swingPlannerParameters; + private final Supplier heightMapData; + private final Supplier terrainMapData; + + private final ROS2Topic controllerFootstepDataTopic; + private final FramePose3D midFeetZUpPose = new FramePose3D(); + private final MovingReferenceFrame midFeetZUpFrame; + private final FramePose3D startPose = new FramePose3D(); + private final FootstepSnapAndWiggler footstepSnapAndWiggler; - public JustWaitState(ControllerFootstepQueueMonitor controllerQueueMonitor) + public JustWaitState(DRCRobotModel robotModel, + ROS2Helper ros2Helper, + ROS2SyncedRobotModel syncedRobot, + AtomicReference commandMessage, + ControllerFootstepQueueMonitor controllerQueueMonitor, + DefaultFootstepPlannerParametersBasics footstepPlannerParameters, + SwingPlannerParametersBasics swingPlannerParameters, + Supplier heightMapData, + Supplier terrainMapData) { + this.ros2Helper = ros2Helper; + this.syncedRobot = syncedRobot; + this.commandMessage = commandMessage; this.controllerQueueMonitor = controllerQueueMonitor; + + footstepPlanner = FootstepPlanningModuleLauncher.createModule(robotModel); + this.footstepPlannerParameters = footstepPlannerParameters; + this.swingPlannerParameters = swingPlannerParameters; + this.heightMapData = heightMapData; + this.terrainMapData = terrainMapData; + + environmentHandler = new FootstepPlannerEnvironmentHandler(); + footstepSnapAndWiggler = new FootstepSnapAndWiggler(PlannerTools.createDefaultFootPolygons(), footstepPlannerParameters, environmentHandler); + + midFeetZUpFrame = syncedRobot.getReferenceFrames().getMidFeetZUpFrame(); + + controllerFootstepDataTopic = HumanoidControllerAPI.getTopic(FootstepDataListMessage.class, "Nadia"); + ros2Helper.createPublisher(controllerFootstepDataTopic); + + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS, this::planToGoal); + ros2Helper.subscribeViaCallback(ContinuousHikingAPI.SQUARE_UP_STEP, this::squareUpStep); } @Override public void onEntry() { + ContinuousHikingCommandMessage continuousHikingCommandMessage = commandMessage.get(); + + if (continuousHikingCommandMessage.getSquareUpToGoal()) + { + squareUpStep(); + } + isDone = false; } @@ -39,4 +132,154 @@ public boolean isDone(double timeInState) { return isDone; } + + public void planToGoal(PoseListMessage poseListMessage) + { + ThreadTools.startAThread(() -> + { + List poses = MessageTools.unpackPoseListMessage(poseListMessage); + FramePose3D leftFootPose = new FramePose3D(); + FramePose3D rightFootPose = new FramePose3D(); + + leftFootPose.set(poses.get(0)); + rightFootPose.set(poses.get(1)); + + footstepPlanner.getFootstepPlannerParameters().set(footstepPlannerParameters); + footstepPlanner.getSwingPlannerParameters().set(swingPlannerParameters); + + FootstepPlannerRequest footstepPlannerRequest = new FootstepPlannerRequest(); + footstepPlannerRequest.setGoalFootPoses(leftFootPose, rightFootPose); + footstepPlannerRequest.setSwingPlannerType(SwingPlannerType.NONE); + + footstepPlannerRequest.getStartFootPoses().forEach((side, pose3D) -> + { + FramePose3DReadOnly soleFramePose; + if (controllerQueueMonitor.getNumberOfIncompleteFootsteps() > 0) + { + LogTools.info("Yes this queue is not empty"); + // We pass in the opposite side because the method returns the footstep on the opposite side + soleFramePose = controllerQueueMonitor.getLastFootstepQueuedOnOppositeSide( + side.getOppositeSide()); + } + else + { + soleFramePose = syncedRobot.getFramePoseReadOnly(referenceFrames -> referenceFrames.getSoleFrame( + side)); + } + soleFramePose.get(pose3D); + }); + + footstepPlannerRequest.setRequestedInitialStanceSide(RobotSide.LEFT); + footstepPlannerRequest.setHeightMapData(heightMapData.get()); + + footstepPlannerRequest.setSnapGoalSteps(true); + footstepPlannerRequest.setPlanBodyPath(true); + + FramePose3D goalFramePose = new FramePose3D(); + goalFramePose.interpolate(leftFootPose, rightFootPose, 0.5); + + Pose3DReadOnly goalPose = new Pose3D(goalFramePose.getPosition(), goalFramePose.getOrientation()); + + midFeetZUpPose.setToZero(midFeetZUpFrame); + midFeetZUpPose.changeFrame(ReferenceFrame.getWorldFrame()); + startPose.setToZero(midFeetZUpFrame); + startPose.changeFrame(ReferenceFrame.getWorldFrame()); + startPose.getOrientation().set(goalPose.getOrientation()); + footstepPlannerRequest.getBodyPathWaypoints().add(midFeetZUpPose); + footstepPlannerRequest.getBodyPathWaypoints().add(startPose); + footstepPlannerRequest.getBodyPathWaypoints().add(goalPose); + + FootstepPlannerOutput plannerOutput = footstepPlanner.handleRequest(footstepPlannerRequest); + + FootstepPlan newestFootstepPlan = null; + + if (plannerOutput != null) + { + newestFootstepPlan = plannerOutput.getFootstepPlan(); + } + + FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); + footstepDataListMessage.setDefaultSwingDuration(0.8); + footstepDataListMessage.setDefaultTransferDuration(0.4); + footstepDataListMessage.getQueueingProperties().setExecutionMode(QueueableMessage.EXECUTION_MODE_QUEUE); + + for (int i = 0; i < footstepPlanner.getOutput().getFootstepPlan().getNumberOfSteps(); i++) + { + assert newestFootstepPlan != null; + PlannedFootstep footstep = newestFootstepPlan.getFootstep(i); + footstep.limitFootholdVertices(); + footstepDataListMessage.getFootstepDataList().add().set(footstep.getAsMessage()); + } + + ros2Helper.publish(controllerFootstepDataTopic, footstepDataListMessage); + }, "PlanToGoalThread"); + } + + private final FramePose3D tempFramePose = new FramePose3D(); + + public void squareUpStep() + { + environmentHandler.setHeightMap(heightMapData.get()); + environmentHandler.setTerrainMapData(terrainMapData.get()); + + FramePose3DReadOnly lastStepInQueue; + PlannedFootstep squareUpStep = null; + + if (controllerQueueMonitor.getNumberOfIncompleteFootsteps() > 0) + { + lastStepInQueue = controllerQueueMonitor.getLastFootstepInQueue(); + List controllerFootstepQueue = controllerQueueMonitor.getControllerFootstepQueue(); + + RobotSide robotSide = RobotSide.fromByte(controllerFootstepQueue.get(controllerFootstepQueue.size() - 1).getRobotSide()); + + if (robotSide == RobotSide.LEFT) + { + tempFramePose.set(lastStepInQueue); + tempFramePose.getTranslation().addY(-footstepPlannerParameters.getIdealFootstepWidth()); + tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); + + squareUpStep = new PlannedFootstep(robotSide.getOppositeSide(), tempFramePose); + } + else if (robotSide == RobotSide.RIGHT) + { + tempFramePose.set(lastStepInQueue); + tempFramePose.getTranslation().addY(footstepPlannerParameters.getIdealFootstepWidth()); + tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); + + squareUpStep = new PlannedFootstep(robotSide.getOppositeSide(), tempFramePose); + } + } + else + { + ReferenceFrame leftFootFrame = syncedRobot.getReferenceFrames().getFootFrame(RobotSide.LEFT); + FramePose3D rightFootPose = new FramePose3D(ReferenceFrame.getWorldFrame(), + syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.RIGHT).getTransformToWorldFrame()); + rightFootPose.changeFrame(leftFootFrame); + RobotSide furthestForwardFootstep = rightFootPose.getTranslationX() > 0 ? RobotSide.RIGHT : RobotSide.LEFT; + MovingReferenceFrame furthestForwardSoleFrame = syncedRobot.getReferenceFrames().getSoleFrame(furthestForwardFootstep); + + tempFramePose.setToZero(furthestForwardSoleFrame); + tempFramePose.getTranslation().addY(furthestForwardFootstep.negateIfLeftSide(footstepPlannerParameters.getIdealFootstepWidth())); + tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); + + squareUpStep = new PlannedFootstep(furthestForwardFootstep.getOppositeSide(), tempFramePose); + } + + if (squareUpStep == null) + return; + + DiscreteFootstep footstep = new DiscreteFootstep(squareUpStep.getFootstepPose().getX(), squareUpStep.getFootstepPose().getY()); + footstepSnapAndWiggler.snapFootstep(footstep); + + squareUpStep.getFootstepPose().setZ(0.0); + squareUpStep.getFootstepPose().applyTransform(footstep.getSnapData().getSnapTransform()); + + FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); + footstepDataListMessage.setDefaultSwingDuration(0.8); + footstepDataListMessage.setDefaultTransferDuration(0.4); + footstepDataListMessage.getQueueingProperties().setExecutionMode(QueueableMessage.EXECUTION_MODE_QUEUE); + footstepDataListMessage.getFootstepDataList().add().set(squareUpStep.getAsMessage()); + + ros2Helper.publish(controllerFootstepDataTopic, footstepDataListMessage); + } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java index 6d0d8b27e0e..6ab536a2240 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java @@ -2,6 +2,7 @@ import behavior_msgs.msg.dds.ContinuousHikingCommandMessage; import us.ihmc.avatar.drcRobot.DRCRobotModel; +import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.behaviors.activeMapping.ContinuousHikingStateMachine.*; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.referenceFrame.FramePose3D; @@ -45,6 +46,7 @@ public class ContinuousPlannerSchedulingTask public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, ROS2Node ros2Node, + ROS2SyncedRobotModel syncedRobotModel, HumanoidReferenceFrames referenceFrames, ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, ContinuousHikingLogger continuousHikingLogger, @@ -94,7 +96,15 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, controllerFootstepQueueMonitor, continuousHikingParameters, continuousHikingLogger); - State justWaitState = new JustWaitState(controllerFootstepQueueMonitor); + State justWaitState = new JustWaitState(robotModel, + ros2Helper, + syncedRobotModel, + commandMessage, + controllerFootstepQueueMonitor, + footstepPlannerParameters, + swingPlannerParameters, + this::getHeightMapData, + this::getTerrainMap); // Adding the different states stateMachineFactory.addState(ContinuousHikingState.DO_NOTHING, notStartedState); @@ -137,9 +147,12 @@ public ContinuousPlannerSchedulingTask(DRCRobotModel robotModel, if (from == null) { // This means the state machine has just started up, for the visuals put them under the feet till we start walking - SideDependentList robotFeet = new SideDependentList<>(new FramePose3D(), new FramePose3D()); - robotFeet.get(RobotSide.LEFT).set(referenceFrames.getSoleFrame(RobotSide.LEFT).getTransformToWorldFrame()); - robotFeet.get(RobotSide.RIGHT).set(referenceFrames.getSoleFrame(RobotSide.RIGHT).getTransformToWorldFrame()); + SideDependentList robotFeet = new SideDependentList<>(new FramePose3D(), + new FramePose3D()); + robotFeet.get(RobotSide.LEFT) + .set(referenceFrames.getSoleFrame(RobotSide.LEFT).getTransformToWorldFrame()); + robotFeet.get(RobotSide.RIGHT) + .set(referenceFrames.getSoleFrame(RobotSide.RIGHT).getTransformToWorldFrame()); debugger.publishStartAndGoalForVisualization(robotFeet, robotFeet); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java index 51c827c5be7..8727eab7af8 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java @@ -6,6 +6,8 @@ import controller_msgs.msg.dds.QueuedFootstepStatusMessage; import controller_msgs.msg.dds.WalkingStatusMessage; import us.ihmc.communication.HumanoidControllerAPI; +import us.ihmc.euclid.referenceFrame.FramePose3D; +import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; @@ -100,6 +102,42 @@ public AtomicReference getFootstepStatusMessage() return footstepStatusMessage; } + public int getNumberOfIncompleteFootsteps() + { + return controllerQueueSize; + } + + /** + * This method assumes the list is not empty; you need to check outside this method that the list has at least one in it + */ + public FramePose3DReadOnly getLastFootstepInQueue() + { + FramePose3D previousFootstepPose = new FramePose3D(); + + previousFootstepPose.getPosition().set(controllerQueue.get(controllerQueueSize - 1).getLocation()); + previousFootstepPose.getRotation().setToYawOrientation(controllerQueue.get(controllerQueueSize - 1).getOrientation().getYaw()); + + return previousFootstepPose; + } + + /** + * This method assumes the list is not empty; you need to check outside this method that the list has at least one in it + */ + public FramePose3DReadOnly getLastFootstepQueuedOnOppositeSide(RobotSide candidateFootstepSide) + { + FramePose3D previousFootstepPose = new FramePose3D(); + + int i = controllerQueue.size() - 1; + // Moved the index of the list to the last step on the other side + while (i >= 1 && controllerQueue.get(i).getRobotSide() == candidateFootstepSide.toByte()) + --i; + + previousFootstepPose.getPosition().set(controllerQueue.get(i).getLocation()); + previousFootstepPose.getRotation().setToYawOrientation(controllerQueue.get(i).getOrientation().getYaw()); + + return previousFootstepPose; + } + // TODO Polling this in multiple threads may cause issues as the second time its pulled the value will be null. // TODO If needed this should change to handle that case correctly. public PlanOffsetStatus pollPlanOffsetMessage() From b5b09e323da248a09a79c2dcd6972f89b1a1af12 Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Sat, 22 Feb 2025 14:28:11 -0600 Subject: [PATCH 15/27] Fix translation for when robot isn't facing forward --- .../ContinuousHikingStateMachine/JustWaitState.java | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java index 31431681d01..878a546295d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java @@ -224,6 +224,8 @@ public void squareUpStep() FramePose3DReadOnly lastStepInQueue; PlannedFootstep squareUpStep = null; + ReferenceFrame leftFootFrame = syncedRobot.getReferenceFrames().getFootFrame(RobotSide.LEFT); + ReferenceFrame rightFootFrame = syncedRobot.getReferenceFrames().getFootFrame(RobotSide.RIGHT); if (controllerQueueMonitor.getNumberOfIncompleteFootsteps() > 0) { @@ -235,6 +237,7 @@ public void squareUpStep() if (robotSide == RobotSide.LEFT) { tempFramePose.set(lastStepInQueue); + tempFramePose.changeFrame(leftFootFrame); tempFramePose.getTranslation().addY(-footstepPlannerParameters.getIdealFootstepWidth()); tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); @@ -243,6 +246,7 @@ public void squareUpStep() else if (robotSide == RobotSide.RIGHT) { tempFramePose.set(lastStepInQueue); + tempFramePose.changeFrame(rightFootFrame); tempFramePose.getTranslation().addY(footstepPlannerParameters.getIdealFootstepWidth()); tempFramePose.changeFrame(ReferenceFrame.getWorldFrame()); @@ -251,7 +255,6 @@ else if (robotSide == RobotSide.RIGHT) } else { - ReferenceFrame leftFootFrame = syncedRobot.getReferenceFrames().getFootFrame(RobotSide.LEFT); FramePose3D rightFootPose = new FramePose3D(ReferenceFrame.getWorldFrame(), syncedRobot.getReferenceFrames().getSoleFrame(RobotSide.RIGHT).getTransformToWorldFrame()); rightFootPose.changeFrame(leftFootFrame); From 6221b64291f5ddcbad00e350de28dd73536dea0c Mon Sep 17 00:00:00 2001 From: Nick Kitchel Date: Sat, 22 Feb 2025 16:25:07 -0600 Subject: [PATCH 16/27] Cleanup UI look so there is less scrolling involved --- .../perception/RDXContinuousHikingPanel.java | 23 +++++++++++-------- 1 file changed, 13 insertions(+), 10 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index 40fb74db91d..ae5d01ed974 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -28,7 +28,6 @@ import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.property.ROS2StoredPropertySetGroup; import us.ihmc.communication.property.StoredPropertySetROS2TopicPair; -import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.Axis3D; import us.ihmc.euclid.geometry.ConvexPolygon2D; import us.ihmc.euclid.geometry.Pose3D; @@ -87,6 +86,7 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private final ImGuiRemoteROS2StoredPropertySetGroup hostStoredPropertySets; private final ImGuiSliderDouble stepsBeforeSafetyStop = new ImGuiSliderDouble("Steps Before Safety Stop", "%.2f"); private final ImBoolean squareUpToGoal = new ImBoolean(false); + private final ImBoolean continuousHiking = new ImBoolean(false); private final ImBoolean useAStarFootstepPlanner = new ImBoolean(true); private final ImBoolean useMonteCarloReference = new ImBoolean(false); private final ImBoolean useMonteCarloFootstepPlanner = new ImBoolean(false); @@ -95,6 +95,7 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private final ROS2Publisher planOffsetStatusPublisher; private final ROS2Publisher footstepStatusMessagePublisher; private final ROS2Publisher clearGoalFootstepsPublisher; + private final ContinuousHikingParameters continuousHikingParameters; private SideDependentList startStancePose = new SideDependentList<>(new FramePose3D(), new FramePose3D()); private FootstepPlan latestFootstepPlan; private List>> swingTrajectories; @@ -152,7 +153,7 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotMod (s) -> terrainPlanningDebugger.reset()); hostStoredPropertySets = new ImGuiRemoteROS2StoredPropertySetGroup(ros2Node); - ContinuousHikingParameters continuousHikingParameters = new ContinuousHikingParameters(); + continuousHikingParameters = new ContinuousHikingParameters(); createParametersPanel(continuousHikingParameters, continuousHikingParametersPanel, hostStoredPropertySets, @@ -209,7 +210,6 @@ public void startContinuousPlannerSchedulingTask(boolean publishAndSubscribe) { this.publishAndSubscribe = publishAndSubscribe; runSubscriberOnly = true; - ROS2Helper ros2Helper = new ROS2Helper(ros2Node); clientStoredPropertySets = new ROS2StoredPropertySetGroup(ros2Node); // Add Continuous Hiking Parameters to be between the UI and this process @@ -288,11 +288,15 @@ else if (publishAndSubscribe) // Case 2 public void renderImGuiWidgets() { - ImGui.text("The ContinuousHikingProcess must be running"); - ImGui.text("And the enabled checkbox must be checked"); - ImGui.text("By holding CTRL the robot will walk forward"); - ImGui.separator(); - continuousHikingParametersPanel.renderImGuiWidgets(); + ImGui.checkbox("Enable Continuous Hiking", continuousHiking); + continuousHikingParameters.setStepPublisherEnabled(continuousHiking.get()); + + ImGui.checkbox("Square Up To Goal", squareUpToGoal); + + if(ImGui.collapsingHeader("Continuous Hiking Parameters")) + { + continuousHikingParametersPanel.renderImGuiWidgets(); + } ImGui.separator(); ImGui.text("Options for Continuous Hiking Message"); @@ -301,8 +305,7 @@ public void renderImGuiWidgets() { squareUpPublisher.publish(new Empty()); } - - ImGui.checkbox("Square Up To Goal", squareUpToGoal); + ImGui.sameLine(); if (ImGui.button("Clear Planned footsteps")) { clearPlannedFootsteps(); From bdd6eb4d73c8709d52a966c5afb60bce22809969 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 24 Feb 2025 12:13:08 -0600 Subject: [PATCH 17/27] Moved the RapidHeightMapManager into the perception package to reduce the parameters. --- .../CommunicationsSyncedRobotModel.java | 6 ++ .../MonteCarloFootstepPlanningTest.java | 7 +- .../perception/RDXContinuousHikingPanel.java | 74 +++++++++++++++---- .../perception/RDXHumanoidPerceptionUI.java | 22 +++--- .../rdx/ui/graphics/RDXHeightMapRenderer.java | 3 +- .../ros2/RDXROS2HeightMapVisualizer.java | 31 ++++---- .../ContinuousHikingProcess.java | 10 +-- .../JustWaitState.java | 4 +- .../ReadyToPlanState.java | 2 +- .../WaitingToLandState.java | 2 +- .../ContinuousPlannerSchedulingTask.java | 1 + .../perception/HumanoidPerceptionModule.java | 65 ++++++++-------- .../RapidHeightMapUpdateThread.java | 6 +- .../StandAloneRealsenseProcess.java | 3 +- .../TerrainPerceptionProcessWithDriver.java | 8 +- .../RDXRapidHeightMapExtractionDemo.java | 3 +- .../RDXRapidHeightMapExtractorCUDADemo.java | 12 ++- .../ControllerFootstepQueueMonitor.java | 22 +----- .../RapidHeightMapDriftOffset.java | 4 +- .../gpuHeightMap/RapidHeightMapExtractor.java | 29 +++----- .../RapidHeightMapExtractorCUDA.java | 27 ++++--- .../gpuHeightMap}/RapidHeightMapManager.java | 47 ++++++------ .../SnappingHeightMapExtractor.java | 12 ++- .../perception/heightMap/TerrainMapData.java | 32 ++++---- .../tools/PerceptionMessageTools.java | 10 +-- 25 files changed, 245 insertions(+), 197 deletions(-) rename {ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping => ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication}/ControllerFootstepQueueMonitor.java (83%) rename {ihmc-high-level-behaviors/src/main/java/us/ihmc/perception => ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap}/RapidHeightMapDriftOffset.java (95%) rename {ihmc-high-level-behaviors/src/main/java/us/ihmc/perception => ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap}/RapidHeightMapManager.java (85%) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java index e4cea554c9a..d1adc4be4a2 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java @@ -189,6 +189,12 @@ public DRCRobotModel getRobotModel() return robotModel; } + public FullHumanoidRobotModel getFullHumanoidRobotModel() + { + return fullRobotModel; + } + + public SideDependentList getHandWrenchCalculators() { return handWrenchCalculators; diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java index cd779a7e29f..f2754b72d9d 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java @@ -18,6 +18,7 @@ import us.ihmc.perception.BytedecoImage; import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.robotics.robotSide.RobotSide; @@ -43,8 +44,8 @@ public void testMonteCarloFootstepPlanning() LogTools.info("Initializing"); - RapidHeightMapExtractor.getHeightMapParameters().setInternalGlobalWidthInMeters(4.0); - RapidHeightMapExtractor.getHeightMapParameters().setInternalGlobalCellSizeInMeters(0.02); + RapidHeightMapManager.getHeightMapParameters().setInternalGlobalWidthInMeters(4.0); + RapidHeightMapManager.getHeightMapParameters().setInternalGlobalCellSizeInMeters(0.02); heightMapExtractor.initialize(); heightMapExtractor.reset(); @@ -56,7 +57,7 @@ public void testMonteCarloFootstepPlanning() HeightMapTerrainGeneratorTools.fillWithSteppingStones(heightMap, 0.4f, 0.4f, 0.3f, 0.25f, 3); heightMapExtractor.getInternalGlobalHeightMapImage().writeOpenCLImage(openCLManager); - heightMapExtractor.populateParameterBuffers(RapidHeightMapExtractor.getHeightMapParameters(), cameraIntrinsics, new Point3D()); + heightMapExtractor.populateParameterBuffers(RapidHeightMapManager.getHeightMapParameters(), cameraIntrinsics, new Point3D()); heightMapExtractor.computeContactMap(); heightMapExtractor.readContactMapImage(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index ae5d01ed974..e021c855861 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -20,7 +20,7 @@ import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.behaviors.activeMapping.ContinuousPlannerSchedulingTask; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.behaviors.activeMapping.StancePoseCalculator; import us.ihmc.commonWalkingControlModules.configurations.SwingTrajectoryParameters; import us.ihmc.commonWalkingControlModules.trajectories.PositionOptimizedTrajectoryGenerator; @@ -42,11 +42,12 @@ import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; import us.ihmc.footstepPlanning.tools.SwingPlannerTools; import us.ihmc.log.LogTools; +import us.ihmc.mecano.frames.MovingReferenceFrame; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.comms.PerceptionComms; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.rdx.imgui.ImGuiSliderDouble; +import us.ihmc.rdx.imgui.ImGuiTools; import us.ihmc.rdx.imgui.RDXPanel; import us.ihmc.rdx.input.ImGui3DViewInput; import us.ihmc.rdx.ui.ImGuiRemoteROS2StoredPropertySetGroup; @@ -106,6 +107,8 @@ public class RDXContinuousHikingPanel extends RDXPanel implements RenderableProv private boolean publishAndSubscribe; private double simulatedDriftInMeters = -0.1; + private final ROS2Publisher turningPublisher; + public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotModel robotModel, ROS2SyncedRobotModel syncedRobotModel) { super("Continuous Hiking"); @@ -136,6 +139,8 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotMod defaultContactPoints.put(robotSide, defaultFoothold); } + turningPublisher = ros2Node.createPublisher(ContinuousHikingAPI.ROTATE_GOAL_FOOTSTEPS); + StancePoseCalculator stancePoseCalculator = new StancePoseCalculator(defaultContactPoints); stancePoseSelectionPanel = new RDXStancePoseSelectionPanel(baseUI, ros2Node, stancePoseCalculator); addChild(stancePoseSelectionPanel); @@ -171,21 +176,13 @@ public RDXContinuousHikingPanel(RDXBaseUI baseUI, ROS2Node ros2Node, DRCRobotMod RDXStoredPropertySetTuner swingPlannerParametersPanel = new RDXStoredPropertySetTuner("Swing Planner Parameters (CH)"); createParametersPanel(swingPlannerParameters, swingPlannerParametersPanel, hostStoredPropertySets, ContinuousHikingAPI.SWING_PLANNING_PARAMETERS); RDXStoredPropertySetTuner heightMapParametersPanel = new RDXStoredPropertySetTuner("Height Map Parameters (CH)"); - createParametersPanel(RapidHeightMapExtractor.getHeightMapParameters(), + createParametersPanel(RapidHeightMapManager.getHeightMapParameters(), heightMapParametersPanel, hostStoredPropertySets, PerceptionComms.HEIGHT_MAP_PARAMETERS); - RDXStoredPropertySetTuner heightMapParametersPanelCUDA = new RDXStoredPropertySetTuner("CUDA Height Map Parameters (CH)"); - createParametersPanel(RapidHeightMapExtractorCUDA.getHeightMapParameters(), - heightMapParametersPanelCUDA, - hostStoredPropertySets, - PerceptionComms.HEIGHT_MAP_PARAMETERS); continuousHikingLogger = new ContinuousHikingLogger(); - controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, - robotModel.getSimpleRobotName(), - syncedRobotModel.getReferenceFrames(), - continuousHikingLogger); + controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName()); } /** @@ -293,7 +290,18 @@ public void renderImGuiWidgets() ImGui.checkbox("Square Up To Goal", squareUpToGoal); - if(ImGui.collapsingHeader("Continuous Hiking Parameters")) + if (ImGui.button("Turn Left 90°")) + { + turnRobot(Math.PI / 2); + } + ImGui.sameLine(); + + if (ImGui.button("Turn Right 90°")) + { + turnRobot(-Math.PI / 2.0); + } + + if (ImGui.collapsingHeader("Continuous Hiking Parameters")) { continuousHikingParametersPanel.renderImGuiWidgets(); } @@ -369,7 +377,12 @@ else if (controllerConnected) } // Pressing this key will stop Continuous Hiking + // We allow for options for both stopping on the next step, and stopping when the queue expires, so a few more steps if (ImGui.getIO().getKeyAlt()) + { + publishStopContinuousHikingGracefully(); + } + else if (ImGui.isKeyPressed(ImGuiTools.getEscapeKey())) { publishStopContinuousHiking(); } @@ -392,6 +405,32 @@ public void clearPlannedFootsteps() clearGoalFootstepsPublisher.publish(new Empty()); } + public void turnRobot(double rotationRadians) + { + MovingReferenceFrame midFeetZUpFrame = syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame(); + FramePose3D midFeetZUpPose = new FramePose3D(midFeetZUpFrame, midFeetZUpFrame.getTransformToWorldFrame()); + SideDependentList goalPoses = new SideDependentList<>(new FramePose3D(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()), + new FramePose3D(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame())); + + midFeetZUpPose.appendYawRotation(rotationRadians); + goalPoses.get(RobotSide.RIGHT).set(midFeetZUpPose); + goalPoses.get(RobotSide.RIGHT).changeFrame(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()); + goalPoses.get(RobotSide.RIGHT).appendTranslation(0, -0.15, 0); + + goalPoses.get(RobotSide.LEFT).set(midFeetZUpPose); + goalPoses.get(RobotSide.LEFT).changeFrame(syncedRobotModel.getReferenceFrames().getMidFeetZUpFrame()); + goalPoses.get(RobotSide.LEFT).appendTranslation(0, 0.15, 0); + + List poses = new ArrayList<>(); + poses.add(new Pose3D(goalPoses.get(RobotSide.LEFT))); + poses.add(new Pose3D(goalPoses.get(RobotSide.RIGHT))); + + PoseListMessage poseListMessage = new PoseListMessage(); + MessageTools.packPoseListMessage(poses, poseListMessage); + + turningPublisher.publish(poseListMessage); + } + /** * We have received the start and goal pose from the process, lets unpack this message and visualize the start and goal on the UI. */ @@ -449,6 +488,13 @@ private void publishStopContinuousHiking() commandPublisher.publish(commandMessage); } + private void publishStopContinuousHikingGracefully() + { + commandMessage.setEnableContinuousHiking(false); + commandMessage.setSquareUpToGoal(true); + commandPublisher.publish(commandMessage); + } + /** * Publish the status of the joystick controller. We define different buttons to perform different actions which get sent with the message. */ diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXHumanoidPerceptionUI.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXHumanoidPerceptionUI.java index 333b85896fc..2afb80b3a87 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXHumanoidPerceptionUI.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXHumanoidPerceptionUI.java @@ -9,8 +9,8 @@ import imgui.type.ImFloat; import org.bytedeco.opencv.opencv_core.Mat; import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.gpuHeightMap.HeatMapGenerator; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.HumanoidPerceptionModule; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.rdx.imgui.RDXPanel; @@ -219,16 +219,16 @@ public void initializeHeightMapUI(ROS2Helper ros2Helper) // RDXImagePanel.DO_NOT_FLIP_Y); depthImagePanel = new RDXBytedecoImagePanel("Depth Image", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.DO_NOT_FLIP_Y); localHeightMapPanel = new RDXBytedecoImagePanel("Local Height Map", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.FLIP_Y); croppedHeightMapPanel = new RDXBytedecoImagePanel("Cropped Height Map", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.FLIP_Y); snapNormalZMapPanel = new RDXBytedecoImagePanel("Normal Z", humanoidPerception.getRapidHeightMapExtractor().getSnapNormalZImage().getBytedecoOpenCVMat().cols(), @@ -251,12 +251,12 @@ public void initializeHeightMapUI(ROS2Helper ros2Helper) // humanoidPerception.getRapidHeightMapExtractor().getSensorCroppedHeightMapImage().rows(), // RDXImagePanel.DO_NOT_FLIP_Y); terrainCostImagePanel = new RDXBytedecoImagePanel("Terrain Cost Image", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.FLIP_Y); contactMapImagePanel = new RDXBytedecoImagePanel("Contact Map Image", - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), RDXImagePanel.FLIP_Y); addChild(localHeightMapPanel.getImagePanel()); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java index 4a04febbb06..9cea1bed6eb 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/RDXHeightMapRenderer.java @@ -126,8 +126,7 @@ public void update(RigidBodyTransform zUpFrameToWorld, intermediateVertexBuffer[vertexIndex + 6] = color.a; // Size - // For visual improvements make the cells slightly bigger than they really are - intermediateVertexBuffer[vertexIndex + 7] = 0.025f; + intermediateVertexBuffer[vertexIndex + 7] = 0.02f; } } diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java index b416d173f81..9ff7f37c035 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/ui/graphics/ros2/RDXROS2HeightMapVisualizer.java @@ -16,7 +16,7 @@ import us.ihmc.communication.ros2.ROS2PublishSubscribeAPI; import us.ihmc.euclid.transform.RigidBodyTransform; import us.ihmc.log.LogTools; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.tools.NativeMemoryTools; import us.ihmc.perception.tools.PerceptionMessageTools; @@ -51,8 +51,8 @@ public class RDXROS2HeightMapVisualizer extends RDXROS2MultiTopicVisualizer private final ImBoolean displayGlobalHeightMapImage = new ImBoolean(false); private final RigidBodyTransform zUpToWorldTransform = new RigidBodyTransform(); - private final TerrainMapData terrainMapData = new TerrainMapData(RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(), - RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize()); + private final TerrainMapData terrainMapData = new TerrainMapData(RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(), + RapidHeightMapManager.getHeightMapParameters().getCropWindowSize()); private ROS2PublishSubscribeAPI ros2; private HeightMapMessage latestHeightMapMessage = new HeightMapMessage(); @@ -62,7 +62,7 @@ public class RDXROS2HeightMapVisualizer extends RDXROS2MultiTopicVisualizer private ByteBuffer incomingCompressedImageBuffer; private BytePointer incomingCompressedImageBytePointer; - private int compressedBufferDefaultSize = 100000; + private final int compressedBufferDefaultSize = 1000000; private float pixelScalingFactor = 10000.0f; private boolean heightMapMessageGenerated = false; @@ -102,7 +102,7 @@ public void setupForGlobalHeightMapTileMessage(ROS2PublishSubscribeAPI ros2) public void create() { super.create(); - int cellsPerAxis = RapidHeightMapExtractor.getHeightMapParameters().getCropWindowSize(); + int cellsPerAxis = RapidHeightMapManager.getHeightMapParameters().getCropWindowSize(); heightMapRenderer.create(cellsPerAxis * cellsPerAxis); } @@ -167,10 +167,10 @@ public void acceptImageMessage(ImageMessage imageMessage) if (latestHeightMapData == null) { - latestHeightMapData = new HeightMapData(RapidHeightMapExtractor.getHeightMapParameters() - .getGlobalCellSizeInMeters(), - RapidHeightMapExtractor.getHeightMapParameters() - .getGlobalWidthInMeters(), + latestHeightMapData = new HeightMapData(RapidHeightMapManager.getHeightMapParameters() + .getGlobalCellSizeInMeters(), + RapidHeightMapManager.getHeightMapParameters() + .getGlobalWidthInMeters(), imageMessage.getPosition().getX(), imageMessage.getPosition().getY()); } @@ -186,14 +186,13 @@ public void acceptImageMessage(ImageMessage imageMessage) PerceptionMessageTools.convertToHeightMapData(heightMapImage, latestHeightMapData, imageMessage.getPosition(), - (float) RapidHeightMapExtractor.getHeightMapParameters() - .getGlobalWidthInMeters(), - (float) RapidHeightMapExtractor.getHeightMapParameters() - .getGlobalCellSizeInMeters()); + (float) RapidHeightMapManager.getHeightMapParameters() + .getGlobalWidthInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters() + .getGlobalCellSizeInMeters()); latestHeightMapMessage = HeightMapMessageTools.toMessage(latestHeightMapData); heightMapMessageGenerated = true; } - }); } @@ -253,11 +252,11 @@ public void update() { heightMapRenderer.update(zUpToWorldTransform, heightMapImage.ptr(0), - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(), + (float) RapidHeightMapManager.getHeightMapParameters().getHeightOffset(), zUpToWorldTransform.getTranslation().getX32(), zUpToWorldTransform.getTranslation().getY32(), heightMapImage.rows() / 2, - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalCellSizeInMeters(), pixelScalingFactor); } } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java index 65f6cef4ebe..bba7dcf5277 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingProcess.java @@ -9,9 +9,10 @@ import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.perception.StandAloneRealsenseProcess; import us.ihmc.perception.comms.PerceptionComms; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2NodeBuilder; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; @@ -57,14 +58,11 @@ public ContinuousHikingProcess(DRCRobotModel robotModel) SwingPlannerParametersBasics swingPlannerParameters = robotModel.getSwingPlannerParameters(); ros2PropertySetGroup.registerStoredPropertySet(ContinuousHikingAPI.SWING_PLANNING_PARAMETERS, swingPlannerParameters); - HeightMapParameters heightMapParameters = RapidHeightMapExtractorCUDA.getHeightMapParameters(); + HeightMapParameters heightMapParameters = RapidHeightMapManager.getHeightMapParameters(); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.HEIGHT_MAP_PARAMETERS, heightMapParameters); ContinuousHikingLogger continuousHikingLogger = new ContinuousHikingLogger(); - ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, - robotModel.getSimpleRobotName(), - syncedRobot.getReferenceFrames(), - continuousHikingLogger); + ControllerFootstepQueueMonitor controllerFootstepQueueMonitor = new ControllerFootstepQueueMonitor(ros2Node, robotModel.getSimpleRobotName()); standAloneRealsenseProcess = new StandAloneRealsenseProcess(ros2Node, ros2Helper, syncedRobot, controllerFootstepQueueMonitor); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java index 878a546295d..fed6d7dfa77 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/JustWaitState.java @@ -9,7 +9,7 @@ import us.ihmc.avatar.drcRobot.DRCRobotModel; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; import us.ihmc.avatar.networkProcessor.footstepPlanningModule.FootstepPlanningModuleLauncher; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.commons.thread.ThreadTools; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.packets.MessageTools; @@ -104,7 +104,7 @@ public void onEntry() { ContinuousHikingCommandMessage continuousHikingCommandMessage = commandMessage.get(); - if (continuousHikingCommandMessage.getSquareUpToGoal()) + if (continuousHikingCommandMessage.getSquareUpToGoal() && !controllerQueueMonitor.getControllerFootstepQueue().isEmpty()) { squareUpStep(); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java index a201c67be51..088fa375d02 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/ReadyToPlanState.java @@ -8,7 +8,7 @@ import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.behaviors.activeMapping.ContinuousPlanner; import us.ihmc.behaviors.activeMapping.ContinuousPlannerTools; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.communication.packets.MessageTools; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.euclid.geometry.Pose3D; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java index 0d2ab1a1d74..50dc960d600 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/WaitingToLandState.java @@ -5,7 +5,7 @@ import us.ihmc.behaviors.activeMapping.ContinuousHikingLogger; import us.ihmc.behaviors.activeMapping.ContinuousHikingParameters; import us.ihmc.behaviors.activeMapping.ContinuousPlanner; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.ros2.ROS2Helper; import us.ihmc.log.LogTools; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java index 6ab536a2240..a4568b1a9f1 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousPlannerSchedulingTask.java @@ -10,6 +10,7 @@ import us.ihmc.footstepPlanning.communication.ContinuousHikingAPI; import us.ihmc.footstepPlanning.graphSearch.parameters.DefaultFootstepPlannerParametersBasics; import us.ihmc.footstepPlanning.swing.SwingPlannerParametersBasics; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; import us.ihmc.perception.heightMap.TerrainMapData; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java index 7aac2ac1b89..b496aea2baa 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java @@ -21,6 +21,7 @@ import us.ihmc.perception.depthData.CollisionBoxProvider; import us.ihmc.perception.filters.CollidingScanRegionFilter; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.RemoteHeightMapUpdater; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; @@ -118,11 +119,7 @@ public boolean isHeightMapDataBeingProcessed() return heightMapDataBeingProcessed; } - public void updateTerrain(ROS2Helper ros2Helper, - Mat incomingDepth, - ReferenceFrame cameraFrame, - ReferenceFrame cameraZUpFrame, - boolean metricDepth) + public void updateTerrain(ROS2Helper ros2Helper, Mat incomingDepth, ReferenceFrame cameraFrame, ReferenceFrame cameraZUpFrame, boolean metricDepth) { if (localizationAndMappingTask != null) localizationAndMappingTask.setEnableLiveMode(mappingEnabled); @@ -155,7 +152,7 @@ public void updateTerrain(ROS2Helper ros2Helper, { if (!heightMapDataBeingProcessed) { - if (rapidHeightMapExtractor.getHeightMapParameters().getResetHeightMap()) + if (RapidHeightMapManager.getHeightMapParameters().getResetHeightMap()) { rapidHeightMapExtractor.reset(); } @@ -206,10 +203,13 @@ public void publishHeightMapImage(ROS2Node ros2Node, rapidHeightMapExtractor.getSequenceNumber(), image.rows(), image.cols(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()); + (float) RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()); } - private static void publishGlobalHeightMapTile(ROS2Helper ros2Helper, GlobalHeightMap globalHeightMap, Instant acquisitionTime, ROS2Topic topic) + private static void publishGlobalHeightMapTile(ROS2Helper ros2Helper, + GlobalHeightMap globalHeightMap, + Instant acquisitionTime, + ROS2Topic topic) { // Get tiles (made out of modified cells) from the global height map class and publish them in a for loop Collection modifiedCells = globalHeightMap.getModifiedMapTiles(); @@ -229,7 +229,6 @@ private static void packGlobalMapTileMessage(GlobalMapTileMessage messageToPack, messageToPack.getHeightMap().set(HeightMapMessageTools.toMessage(tile)); } - public void publishExternalHeightMapImage(ROS2Node ros2Node) { if (heightMapPublisher == null) @@ -239,24 +238,25 @@ public void publishExternalHeightMapImage(ROS2Node ros2Node) executorService.clearTaskQueue(); executorService.submit(() -> - { - Instant acquisitionTime = Instant.now(); - Mat heightMapImage = rapidHeightMapExtractor.getInternalGlobalHeightMapImage().getBytedecoOpenCVMat(); - OpenCVTools.compressImagePNG(heightMapImage, compressedInternalHeightMapPointer); - //PerceptionDebugTools.displayDepth("Published Global Height Map", heightMapImage, 1); - PerceptionMessageTools.publishCompressedDepthImage(compressedInternalHeightMapPointer, - PerceptionAPI.HEIGHT_MAP_CROPPED, - croppedHeightMapImageMessage, - heightMapPublisher, - new FramePose3D(ReferenceFrame.getWorldFrame(), - rapidHeightMapExtractor.getSensorOrigin(), - new Quaternion()), - acquisitionTime, - rapidHeightMapExtractor.getSequenceNumber(), - heightMapImage.rows(), - heightMapImage.cols(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()); - }); + { + Instant acquisitionTime = Instant.now(); + Mat heightMapImage = rapidHeightMapExtractor.getInternalGlobalHeightMapImage().getBytedecoOpenCVMat(); + OpenCVTools.compressImagePNG(heightMapImage, compressedInternalHeightMapPointer); + //PerceptionDebugTools.displayDepth("Published Global Height Map", heightMapImage, 1); + PerceptionMessageTools.publishCompressedDepthImage(compressedInternalHeightMapPointer, + PerceptionAPI.HEIGHT_MAP_CROPPED, + croppedHeightMapImageMessage, + heightMapPublisher, + new FramePose3D(ReferenceFrame.getWorldFrame(), + rapidHeightMapExtractor.getSensorOrigin(), + new Quaternion()), + acquisitionTime, + rapidHeightMapExtractor.getSequenceNumber(), + heightMapImage.rows(), + heightMapImage.cols(), + (float) RapidHeightMapManager.getHeightMapParameters() + .getHeightScaleFactor()); + }); } private void updatePlanarRegions(ROS2Helper ros2Helper, ReferenceFrame cameraFrame) @@ -329,7 +329,8 @@ public void initializeHeightMapExtractor(ROS2Helper ros2Helper, HumanoidReferenc referenceFrames.getSoleFrame(RobotSide.RIGHT), realsenseDepthImage, cameraIntrinsics, - 1); + 1, + RapidHeightMapManager.getHeightMapParameters()); if (ros2Helper != null) ros2Helper.subscribeViaVolatileCallback(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); @@ -456,16 +457,16 @@ public HeightMapMessage getGlobalHeightMapMessage() Mat heightMapMat = heightMapImage.getBytedecoOpenCVMat().clone(); if (latestHeightMapData == null) { - latestHeightMapData = new HeightMapData((float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalWidthInMeters(), + latestHeightMapData = new HeightMapData((float) RapidHeightMapManager.getHeightMapParameters().getGlobalCellSizeInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalWidthInMeters(), rapidHeightMapExtractor.getSensorOrigin().getX(), rapidHeightMapExtractor.getSensorOrigin().getY()); } PerceptionMessageTools.convertToHeightMapData(heightMapMat, latestHeightMapData, rapidHeightMapExtractor.getSensorOrigin(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalWidthInMeters(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters()); + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalWidthInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalCellSizeInMeters()); return HeightMapMessageTools.toMessage(latestHeightMapData); } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java index f71172860cc..b2fa79a438d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java @@ -1,10 +1,11 @@ package us.ihmc.perception; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.commons.thread.RepeatingTaskThread; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.log.LogTools; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.ros2.ROS2Node; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -63,7 +64,8 @@ protected void runTask() if (heightMapManager == null) { heightMapManager = new RapidHeightMapManager(ros2Node, - syncedRobotModel.getRobotModel(), + syncedRobotModel.getFullHumanoidRobotModel(), + syncedRobotModel.getRobotModel().getSimpleRobotName(), leftFootFrame, rightFootFrame, controllerFootstepQueueMonitor, diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java index 5eb2e6f9e47..2dfd0a0e876 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/StandAloneRealsenseProcess.java @@ -1,12 +1,13 @@ package us.ihmc.perception; import us.ihmc.avatar.drcRobot.ROS2SyncedRobotModel; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; import us.ihmc.commons.thread.RepeatingTaskThread; import us.ihmc.communication.PerceptionAPI; import us.ihmc.communication.packets.Packet; import us.ihmc.communication.ros2.ROS2DemandGraphNode; import us.ihmc.communication.ros2.ROS2Helper; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.sensors.realsense.RealSenseConfiguration; diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java index eef823b6d76..e3ad0463c1d 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java @@ -17,6 +17,7 @@ import us.ihmc.log.LogTools; import us.ihmc.perception.comms.PerceptionComms; import us.ihmc.perception.depthData.CollisionBoxProvider; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.parameters.PerceptionConfigurationParameters; @@ -95,7 +96,6 @@ public class TerrainPerceptionProcessWithDriver private final double outputPeriod = UnitConversions.hertzToSeconds(30.0f); private volatile boolean running = true; - private final int depthWidth; private final int depthHeight; private final int colorWidth; @@ -173,8 +173,7 @@ public TerrainPerceptionProcessWithDriver(String robotName, humanoidPerception.getRapidRegionsExtractor().setEnabled(true); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERCEPTION_CONFIGURATION_PARAMETERS, parameters); - ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.HEIGHT_MAP_PARAMETERS, - humanoidPerception.getRapidHeightMapExtractor().getHeightMapParameters()); + ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.HEIGHT_MAP_PARAMETERS, RapidHeightMapManager.getHeightMapParameters()); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERSPECTIVE_RAPID_REGION_PARAMETERS, humanoidPerception.getRapidRegionsExtractor().getParameters()); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERSPECTIVE_POLYGONIZER_PARAMETERS, @@ -260,7 +259,8 @@ private void update() CameraModel.PINHOLE.packMessageFormat(depthImageMessage); PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, depthTopic, - depthImageMessage, depthPublisher, + depthImageMessage, + depthPublisher, cameraPose, acquisitionTime, depthSequenceNumber++, diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractionDemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractionDemo.java index c8a2d807a84..c4d5b7e1aee 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractionDemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractionDemo.java @@ -13,6 +13,7 @@ import us.ihmc.log.LogTools; import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.HumanoidPerceptionModule; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.logging.PerceptionDataLoader; import us.ihmc.perception.logging.PerceptionLoggerConstants; import us.ihmc.perception.opencl.OpenCLManager; @@ -100,7 +101,7 @@ public void create() humanoidPerceptionUI = new RDXHumanoidPerceptionUI(humanoidPerception, ros2Helper); humanoidPerceptionUI.initializeHeightMapUI(ros2Helper); - HeightMapParameters heightMapParameters = humanoidPerception.getRapidHeightMapExtractor().getHeightMapParameters(); + HeightMapParameters heightMapParameters = RapidHeightMapManager.getHeightMapParameters(); LogTools.info("Height Map Parameters Save File " + heightMapParameters.findSaveFileDirectory().getFileName().toString()); heightMapParametersTuner.create(heightMapParameters, false); humanoidPerceptionUI.addChild(heightMapParametersTuner); diff --git a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java index 0497fa8fc0a..9532d552dfe 100644 --- a/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java +++ b/ihmc-high-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXRapidHeightMapExtractorCUDADemo.java @@ -14,6 +14,7 @@ import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.rdx.Lwjgl3ApplicationAdapter; import us.ihmc.rdx.sceneManager.RDXSceneLevel; @@ -94,7 +95,12 @@ public void create() heightMapImage = new GpuMat(intrinsics.getWidth(), intrinsics.getHeight(), opencv_core.CV_16UC1); - extractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, rightFootSoleFrame, heightMapImage, intrinsics, 1); + extractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, + rightFootSoleFrame, + heightMapImage, + intrinsics, + 1, + RapidHeightMapManager.getHeightMapParameters()); baseUI.getPrimaryScene().addRenderableProvider(heightMapGraphic, RDXSceneLevel.MODEL); } @@ -144,8 +150,8 @@ public void render() PerceptionMessageTools.convertToHeightMapData(sensorSimulator.getDepthImage().getCpuImageMat(), heightMapData, croppedHeightMapImageMessage.getPosition(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalWidthInMeters(), - (float) RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters()); + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalWidthInMeters(), + (float) RapidHeightMapManager.getHeightMapParameters().getGlobalCellSizeInMeters()); HeightMapMessageTools.toMessage(heightMapData, heightMapMessage); // List multiColorMeshBuilders = RDXHeightMapGraphicNew.generateHeightCells(heightsProvided, diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/ControllerFootstepQueueMonitor.java similarity index 83% rename from ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java rename to ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/ControllerFootstepQueueMonitor.java index 8727eab7af8..208c77454b3 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ControllerFootstepQueueMonitor.java +++ b/ihmc-humanoid-robotics/src/main/java/us/ihmc/humanoidRobotics/communication/ControllerFootstepQueueMonitor.java @@ -1,4 +1,4 @@ -package us.ihmc.behaviors.activeMapping; +package us.ihmc.humanoidRobotics.communication; import controller_msgs.msg.dds.FootstepQueueStatusMessage; import controller_msgs.msg.dds.FootstepStatusMessage; @@ -9,7 +9,6 @@ import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.interfaces.FramePose3DReadOnly; import us.ihmc.humanoidRobotics.communication.packets.walking.WalkingStatus; -import us.ihmc.humanoidRobotics.frames.HumanoidReferenceFrames; import us.ihmc.log.LogTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.ros2.ROS2Node; @@ -27,19 +26,11 @@ public class ControllerFootstepQueueMonitor private final AtomicReference footstepStatusMessage = new AtomicReference<>(new FootstepStatusMessage()); private final AtomicReference planOffsetMessage = new AtomicReference<>(new PlanOffsetStatus()); - private final HumanoidReferenceFrames referenceFrames; - private final ContinuousHikingLogger continuousHikingLogger; private boolean footstepStarted; private final AtomicBoolean isWalking = new AtomicBoolean(false); - public ControllerFootstepQueueMonitor(ROS2Node ros2Node, - String simpleRobotName, - HumanoidReferenceFrames referenceFrames, - ContinuousHikingLogger continuousHikingLogger) + public ControllerFootstepQueueMonitor(ROS2Node ros2Node, String simpleRobotName) { - this.referenceFrames = referenceFrames; - this.continuousHikingLogger = continuousHikingLogger; - ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(FootstepQueueStatusMessage.class, simpleRobotName), this::footstepQueueStatusReceived); ros2Node.createSubscription2(HumanoidControllerAPI.getTopic(FootstepStatusMessage.class, simpleRobotName), this::footstepStatusReceived); ros2Node.createSubscription2(getTopic(PlanOffsetStatus.class, simpleRobotName), this::acceptPlanOffsetStatus); @@ -53,7 +44,6 @@ private void footstepQueueStatusReceived(FootstepQueueStatusMessage footstepQueu { String message = String.format("Latest Controller Queue Footstep Size: " + footstepQueueStatusMessage.getQueuedFootstepList().size()); LogTools.info(message); - continuousHikingLogger.appendString(message); } // For the statistics set the that controller queue size before getting the new one @@ -62,14 +52,6 @@ private void footstepQueueStatusReceived(FootstepQueueStatusMessage footstepQueu private void footstepStatusReceived(FootstepStatusMessage footstepStatusMessage) { - if (footstepStatusMessage.getFootstepStatus() == FootstepStatusMessage.FOOTSTEP_STATUS_COMPLETED) - { - double distance = referenceFrames.getSoleFrame(RobotSide.LEFT) - .getTransformToDesiredFrame(referenceFrames.getSoleFrame(RobotSide.RIGHT)) - .getTranslation() - .norm(); - } - footstepStarted = footstepStatusMessage.getFootstepStatus() == FootstepStatusMessage.FOOTSTEP_STATUS_STARTED; this.footstepStatusMessage.set(footstepStatusMessage); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapDriftOffset.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapDriftOffset.java similarity index 95% rename from ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapDriftOffset.java rename to ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapDriftOffset.java index 04419b02d6b..d1c58799a55 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapDriftOffset.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapDriftOffset.java @@ -1,8 +1,8 @@ -package us.ihmc.perception; +package us.ihmc.perception.gpuHeightMap; import controller_msgs.msg.dds.PlanOffsetStatus; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; import us.ihmc.euclid.tuple3D.Vector3D; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; /** * This class is meant to keep track of the drift in Z that is being calculated in the controller on the robot. diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java index da3bcd87ad2..4e2c95613da 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java @@ -60,7 +60,6 @@ public class RapidHeightMapExtractor implements RapidHeightMapExtractorInterface private final SteppableRegionCalculatorParameters steppableRegionParameters = new SteppableRegionCalculatorParameters(); - private static HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); private final RigidBodyTransform currentSensorToWorldTransform = new RigidBodyTransform(); private final RigidBodyTransform currentGroundToWorldTransform = new RigidBodyTransform(); private final Point3D sensorOrigin = new Point3D(); @@ -119,15 +118,18 @@ public class RapidHeightMapExtractor implements RapidHeightMapExtractorInterface private TerrainMapData terrainMapData; private Mat denoisedHeightMapImage; private Rect cropWindowRectangle; + private HeightMapParameters heightMapParameters; public RapidHeightMapExtractor(OpenCLManager openCLManager, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, BytedecoImage depthImage, CameraIntrinsics depthCameraIntrinsics, - int mode) + int mode, + HeightMapParameters heightMapParameters) { this(openCLManager, depthImage, depthCameraIntrinsics, mode); + this.heightMapParameters = heightMapParameters; footSoleFrames.put(RobotSide.LEFT, leftFootSoleFrame); footSoleFrames.put(RobotSide.RIGHT, rightFootSoleFrame); } @@ -787,14 +789,8 @@ else if (steppableCell.isBorderCell()) public Mat getCroppedImage(Point3DReadOnly origin, int globalCenterIndex, Mat imageToCrop) { - int xIndex = HeightMapTools.coordinateToIndex(origin.getX(), - 0, - RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters(), - globalCenterIndex); - int yIndex = HeightMapTools.coordinateToIndex(origin.getY(), - 0, - RapidHeightMapExtractor.getHeightMapParameters().getGlobalCellSizeInMeters(), - globalCenterIndex); + int xIndex = HeightMapTools.coordinateToIndex(origin.getX(), 0, heightMapParameters.getGlobalCellSizeInMeters(), globalCenterIndex); + int yIndex = HeightMapTools.coordinateToIndex(origin.getY(), 0, heightMapParameters.getGlobalCellSizeInMeters(), globalCenterIndex); cropWindowRectangle = new Rect((yIndex - heightMapParameters.getCropWindowSize() / 2), (xIndex - heightMapParameters.getCropWindowSize() / 2), heightMapParameters.getCropWindowSize(), @@ -804,8 +800,8 @@ public Mat getCroppedImage(Point3DReadOnly origin, int globalCenterIndex, Mat im public HeightMapData getHeightMapData() { - HeightMapData latestHeightMapData = new HeightMapData((float) getHeightMapParameters().getGlobalCellSizeInMeters(), - (float) getHeightMapParameters().getGlobalWidthInMeters(), + HeightMapData latestHeightMapData = new HeightMapData((float) heightMapParameters.getGlobalCellSizeInMeters(), + (float) heightMapParameters.getGlobalWidthInMeters(), getSensorOrigin().getX(), getSensorOrigin().getY()); @@ -813,8 +809,8 @@ public HeightMapData getHeightMapData() PerceptionMessageTools.convertToHeightMapData(heightMapMat, latestHeightMapData, getSensorOrigin(), - (float) getHeightMapParameters().getGlobalWidthInMeters(), - (float) getHeightMapParameters().getGlobalCellSizeInMeters()); + (float) heightMapParameters.getGlobalWidthInMeters(), + (float) heightMapParameters.getGlobalCellSizeInMeters()); return latestHeightMapData; } @@ -864,11 +860,6 @@ public boolean isHeightMapDataAvailable() return heightMapDataAvailable; } - public static HeightMapParameters getHeightMapParameters() - { - return heightMapParameters; - } - public boolean isInitialized() { return initialized; diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java index 3b9ad9b593f..1a31028c845 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java @@ -18,6 +18,7 @@ import us.ihmc.perception.cuda.CUDAStreamManager; import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.heightMap.TerrainMapData; +import us.ihmc.perception.tools.PerceptionDebugTools; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; @@ -33,13 +34,13 @@ public class RapidHeightMapExtractorCUDA implements RapidHeightMapExtractorInter { private static final boolean PRINT_TIMING_FOR_KERNELS = false; static final int BLOCK_SIZE_XY = 32; - static final HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); private final SideDependentList footSoleFrames = new SideDependentList<>(); private final TerrainMapData terrainMapData; private final CameraIntrinsics cameraIntrinsics; private final Point3D sensorOrigin = new Point3D(); private final int mode; // 0 -> Ouster, 1 -> Realsense + private final HeightMapParameters heightMapParameters; private final GpuMat inputDepthImage; private final GpuMat localHeightMapImage; @@ -92,11 +93,13 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, GpuMat depthImage, CameraIntrinsics depthImageIntrinsics, - int mode) + int mode, + HeightMapParameters heightMapParameters) { inputDepthImage = depthImage; this.cameraIntrinsics = depthImageIntrinsics; this.mode = mode; + this.heightMapParameters = heightMapParameters; footSoleFrames.put(RobotSide.LEFT, leftFootSoleFrame); footSoleFrames.put(RobotSide.RIGHT, rightFootSoleFrame); @@ -150,7 +153,7 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, parametersHostPointer = new FloatPointer(37); parametersDevicePointer = new FloatPointer(); - snappedFootstepsExtractor = new SnappingHeightMapExtractor(terrainMapData); + snappedFootstepsExtractor = new SnappingHeightMapExtractor(heightMapParameters, terrainMapData); } catch (Exception e) { @@ -160,11 +163,6 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, reset(); } - public static HeightMapParameters getHeightMapParameters() - { - return heightMapParameters; - } - private void recomputeDerivedParameters() { centerIndex = HeightMapTools.computeCenterIndex(heightMapParameters.getLocalWidthInMeters(), heightMapParameters.getLocalCellSizeInMeters()); @@ -303,6 +301,11 @@ public void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform //Update the terrain map data with the new results terrainMapData.setSensorOrigin(groundToWorldTransform.getTranslationX(), groundToWorldTransform.getTranslationY()); + Mat temp = new Mat(); + sensorCroppedHeightMapImage.download(temp); +// PerceptionDebugTools.printMat("", temp, 10); + + Mat finalCroppedHeightMap = new Mat(); // Assuming the height map is 201x201 sensorCroppedHeightMapImage.download(finalCroppedHeightMap); // Download the image from the GPU to the Mat object error = cudaStreamSynchronize(stream); @@ -460,8 +463,8 @@ public int getSequenceNumber() public HeightMapData getHeightMapData() { - HeightMapData latestHeightMapData = new HeightMapData((float) getHeightMapParameters().getGlobalCellSizeInMeters(), - (float) getHeightMapParameters().getGlobalWidthInMeters(), + HeightMapData latestHeightMapData = new HeightMapData((float) heightMapParameters.getGlobalCellSizeInMeters(), + (float) heightMapParameters.getGlobalWidthInMeters(), getSensorOrigin().getX(), getSensorOrigin().getY()); @@ -469,8 +472,8 @@ public HeightMapData getHeightMapData() PerceptionMessageTools.convertToHeightMapData(heightMapMat, latestHeightMapData, getSensorOrigin(), - (float) getHeightMapParameters().getGlobalWidthInMeters(), - (float) getHeightMapParameters().getGlobalCellSizeInMeters()); + (float) heightMapParameters.getGlobalWidthInMeters(), + (float) heightMapParameters.getGlobalCellSizeInMeters()); return latestHeightMapData; } diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java similarity index 85% rename from ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java rename to ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java index e8886dff9e7..2a2edc3e75f 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java @@ -1,4 +1,4 @@ -package us.ihmc.perception; +package us.ihmc.perception.gpuHeightMap; import controller_msgs.msg.dds.HighLevelStateChangeStatusMessage; import org.bytedeco.javacpp.BytePointer; @@ -7,26 +7,25 @@ import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; import perception_msgs.msg.dds.ImageMessage; -import us.ihmc.avatar.drcRobot.DRCRobotModel; -import us.ihmc.behaviors.activeMapping.ControllerFootstepQueueMonitor; import us.ihmc.commons.thread.Notification; import us.ihmc.communication.HumanoidControllerAPI; import us.ihmc.communication.PerceptionAPI; import us.ihmc.euclid.referenceFrame.FramePose3D; import us.ihmc.euclid.referenceFrame.ReferenceFrame; import us.ihmc.euclid.transform.RigidBodyTransform; +import us.ihmc.humanoidRobotics.communication.ControllerFootstepQueueMonitor; +import us.ihmc.perception.BytedecoImage; import us.ihmc.perception.camera.CameraIntrinsics; import us.ihmc.perception.filters.CUDAFlyingPointsFilter; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorInterface; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.opencl.OpenCLManager; import us.ihmc.perception.opencv.OpenCVTools; import us.ihmc.perception.tools.PerceptionMessageTools; +import us.ihmc.robotModels.FullHumanoidRobotModel; import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensorProcessing.heightMap.HeightMapData; +import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; import java.time.Instant; @@ -35,6 +34,8 @@ */ public class RapidHeightMapManager { + static final HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); + private final RapidHeightMapExtractorInterface rapidHeightMapExtractor; private final ImageMessage croppedHeightMapImageMessage = new ImageMessage(); private final FramePose3D cameraPose = new FramePose3D(); @@ -52,7 +53,8 @@ public class RapidHeightMapManager private ROS2Publisher heightMapPublisher; public RapidHeightMapManager(ROS2Node ros2Node, - DRCRobotModel robotModel, + FullHumanoidRobotModel robotModel, + String robotName, ReferenceFrame leftFootSoleFrame, ReferenceFrame rightFootSoleFrame, ControllerFootstepQueueMonitor controllerFootstepQueueMonitor, @@ -65,7 +67,12 @@ public RapidHeightMapManager(ROS2Node ros2Node, if (runWithCUDA) { deviceDepthImage = new GpuMat(depthImageIntrinsics.getHeight(), depthImageIntrinsics.getWidth(), opencv_core.CV_16UC1); - rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, rightFootSoleFrame, deviceDepthImage, depthImageIntrinsics, 1); + rapidHeightMapExtractor = new RapidHeightMapExtractorCUDA(leftFootSoleFrame, + rightFootSoleFrame, + deviceDepthImage, + depthImageIntrinsics, + 1, + heightMapParameters); rapidHeightMapDriftOffset = new RapidHeightMapDriftOffset(controllerFootstepQueueMonitor); flyingPointsFilter = new CUDAFlyingPointsFilter(); } @@ -79,14 +86,15 @@ public RapidHeightMapManager(ROS2Node ros2Node, rightFootSoleFrame, heightMapBytedecoImage, depthImageIntrinsics, - 1); + 1, + heightMapParameters); } // We use a notification in order to only call resetting the height map in one place ros2Node.createSubscription2(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); if (robotModel != null) { - ros2Node.createSubscription(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotModel.getSimpleRobotName()), message -> + ros2Node.createSubscription(HumanoidControllerAPI.getTopic(HighLevelStateChangeStatusMessage.class, robotName), message -> { if (message.takeNextData().getEndHighLevelControllerName() == HighLevelStateChangeStatusMessage.WALKING) { @@ -150,7 +158,7 @@ public void update(Mat latestDepthImage, Instant imageAcquisitionTime, Reference Mat croppedHeightMapImage = rapidHeightMapExtractor.getTerrainMapData().getHeightMap(); - if (runWithCUDA && RapidHeightMapExtractorCUDA.getHeightMapParameters().getFlyingPointsFilter()) + if (runWithCUDA && getHeightMapParameters().getFlyingPointsFilter()) { GpuMat deviceCroppedHeightMapImage = new GpuMat(); deviceCroppedHeightMapImage.upload(croppedHeightMapImage); @@ -160,16 +168,6 @@ public void update(Mat latestDepthImage, Instant imageAcquisitionTime, Reference deviceCroppedHeightMapImage.close(); } - float heightScaleFactor; - if (runWithCUDA) - { - heightScaleFactor = (float) RapidHeightMapExtractorCUDA.getHeightMapParameters().getHeightScaleFactor(); - } - else - { - heightScaleFactor = (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor(); - } - if (heightMapPublisher == null) { heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); @@ -185,7 +183,7 @@ public void update(Mat latestDepthImage, Instant imageAcquisitionTime, Reference rapidHeightMapExtractor.getSequenceNumber(), croppedHeightMapImage.rows(), croppedHeightMapImage.cols(), - heightScaleFactor); + (float) getHeightMapParameters().getHeightScaleFactor()); } public HeightMapData getLatestHeightMapData() @@ -198,6 +196,11 @@ public TerrainMapData getTerrainMapData() return rapidHeightMapExtractor.getTerrainMapData(); } + public static HeightMapParameters getHeightMapParameters() + { + return heightMapParameters; + } + public void destroy() { rapidHeightMapExtractor.destroy(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java index 199da1f2f59..29a5c6e1baa 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java @@ -15,13 +15,14 @@ import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.steppableRegions.SteppableRegionCalculatorParameters; +import us.ihmc.perception.tools.PerceptionDebugTools; +import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; import java.net.URL; import static org.bytedeco.cuda.global.cudart.cudaFree; import static org.bytedeco.cuda.global.cudart.cudaStreamSynchronize; import static us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA.BLOCK_SIZE_XY; -import static us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractorCUDA.heightMapParameters; public class SnappingHeightMapExtractor { @@ -29,6 +30,7 @@ public class SnappingHeightMapExtractor private final SteppableRegionCalculatorParameters steppableRegionParameters = new SteppableRegionCalculatorParameters(); + private final HeightMapParameters heightMapParameters; private final TerrainMapData terrainMapData; private final CUstream_st stream; @@ -48,8 +50,9 @@ public class SnappingHeightMapExtractor private final GpuMat snapNormalZImage; private final GpuMat snappedAreaFractionImage; - public SnappingHeightMapExtractor(TerrainMapData terrainMapData) + public SnappingHeightMapExtractor(HeightMapParameters heightMapParameters, TerrainMapData terrainMapData) { + this.heightMapParameters = heightMapParameters; this.terrainMapData = terrainMapData; try @@ -120,6 +123,11 @@ public void update(GpuMat globalHeightMapImage, Point3D sensorOrigin, int global error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); + Mat temp = new Mat(); + steppabilityImage.download(temp); +// PerceptionDebugTools.printMat("", temp, 10); + + // Download all the data from the GPU and set the terrain data object { Mat cpuSteppabilityMap = new Mat(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java index a8f1e6f2217..aa490119757 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/heightMap/TerrainMapData.java @@ -8,6 +8,7 @@ import us.ihmc.euclid.tuple3D.UnitVector3D; import us.ihmc.euclid.tuple3D.interfaces.UnitVector3DReadOnly; import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.steppableRegions.SnapResult; import us.ihmc.robotics.geometry.LeastSquaresZPlaneFitter; import us.ihmc.sensorProcessing.heightMap.HeightMapData; @@ -25,7 +26,7 @@ public class TerrainMapData private final LeastSquaresZPlaneFitter planeFitter = new LeastSquaresZPlaneFitter(); - private int localGridSize = 201; + private final int localGridSize; private int cellsPerMeter = 50; private Mat heightMap; @@ -35,12 +36,12 @@ public class TerrainMapData private Mat steppableRegionAssignmentMat; private Mat steppableRegionRingMat; private Mat steppabilityImage; + private Mat steppabilityConnectionsImage; private Mat snapHeightImage; private Mat snappedAreaFractionImage; private Mat snapNormalXImage; private Mat snapNormalYImage; private Mat snapNormalZImage; - private Mat steppabilityConnectionsImage; public TerrainMapData(Mat heightMap, Mat contactMap) { @@ -62,20 +63,15 @@ public TerrainMapData(Mat heightMap, setContactMap(contactMap); setTerrainCostMap(terrainCostMap); setSteppabilityImage(steppability); + setSnappedAreaFractionImage(snappedAreaFractionImage); setSnapNormalXImage(snapNormalXImage); setSnapNormalYImage(snapNormalYImage); setSnapNormalZImage(snapNormalZImage); - setSnappedAreaFractionImage(snappedAreaFractionImage); this.localGridSize = heightMap.rows(); // TODO need to add cells per meter } - public TerrainMapData(TerrainMapData other) - { - set(other); - } - public TerrainMapData(int height, int width) { heightMap = new Mat(height, width, opencv_core.CV_16UC1); @@ -84,27 +80,31 @@ public TerrainMapData(int height, int width) localGridSize = height; } - public void set(TerrainMapData other) + public TerrainMapData(TerrainMapData other) { this.localGridSize = other.localGridSize; this.cellsPerMeter = other.cellsPerMeter; heightMapCenter.set(other.heightMapCenter); setHeightMap(other.heightMap); - setSnapHeightImage(other.snapHeightImage); setContactMap(other.contactMap); setTerrainCostMap(other.terrainCostMap); + + setSteppableRegionAssignmentMat(other.steppableRegionAssignmentMat); + setSteppableRegionRingMat(other.steppableRegionRingMat); setSteppabilityImage(other.steppabilityImage); + setSteppabilityConnectionsImage(other.steppabilityConnectionsImage); + setSnapHeightImage(other.snapHeightImage); + setSnappedAreaFractionImage(other.snappedAreaFractionImage); setSnapNormalXImage(other.snapNormalXImage); setSnapNormalYImage(other.snapNormalYImage); setSnapNormalZImage(other.snapNormalZImage); - setSnappedAreaFractionImage(other.snappedAreaFractionImage); } private int getLocalIndex(double coordinate, double center) { // TODO probably a height map tools method for this. - return (int) ((coordinate - center) * cellsPerMeter + localGridSize / 2); + return (int) ((coordinate - center) * cellsPerMeter + (double) localGridSize / 2); } public float getSnappedAreaFractionInWorld(double x, double y) @@ -173,8 +173,8 @@ private boolean isOutOfBounds(int rIndex, int cIndex) private static float convertScaledAndOffsetValue(float value) { - return (float) (value / RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()) - - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(); + return (float) (value / RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()) - (float) RapidHeightMapManager.getHeightMapParameters() + .getHeightOffset(); } private float getHeightLocal(int rIndex, int cIndex) @@ -246,8 +246,8 @@ private int getSteppabilityLocal(int rIndex, int cIndex) public void setHeightLocal(float height, int rIndex, int cIndex) { - float offsetHeight = height + (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(); - int finalHeight = (int) (offsetHeight * RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()); + float offsetHeight = height + (float) RapidHeightMapManager.getHeightMapParameters().getHeightOffset(); + int finalHeight = (int) (offsetHeight * RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()); heightMap.ptr(rIndex, cIndex).putShort((short) finalHeight); } diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java index e829439d605..0030c775f9a 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java @@ -20,7 +20,7 @@ import us.ihmc.euclid.tuple4D.Quaternion; import us.ihmc.idl.IDLSequence; import us.ihmc.perception.RawImage; -import us.ihmc.perception.gpuHeightMap.RapidHeightMapExtractor; +import us.ihmc.perception.gpuHeightMap.RapidHeightMapManager; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.imageMessage.CompressionType; import us.ihmc.perception.imageMessage.PixelFormat; @@ -276,8 +276,8 @@ public static void convertToHeightMapData(Mat heightMapPointer, HeightMapData he for (int yIndex = 0; yIndex < cellsPerAxis; yIndex++) { int height = ((int) heightMapPointer.ptr(xIndex, yIndex).getShort() & 0xFFFF); - float cellHeight = (float) ((float) (height) / RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()) - - (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset(); + float cellHeight = (float) ((float) (height) / RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()) + - (float) RapidHeightMapManager.getHeightMapParameters().getHeightOffset(); int key = HeightMapTools.indicesToKey(xIndex, yIndex, centerIndex); heightMapDataToPack.setHeightAt(key, cellHeight); @@ -301,8 +301,8 @@ public static Mat convertHeightMapDataToMat(HeightMapData heightMapData, float w double cellHeight = heightMapData.getHeightAt(key); // Reverse the height calculation to get the raw height value - int height = (int) ((cellHeight + (float) RapidHeightMapExtractor.getHeightMapParameters().getHeightOffset()) - * RapidHeightMapExtractor.getHeightMapParameters().getHeightScaleFactor()); + int height = (int) ((cellHeight + (float) RapidHeightMapManager.getHeightMapParameters().getHeightOffset()) + * RapidHeightMapManager.getHeightMapParameters().getHeightScaleFactor()); // Store the height value in the Mat object heightMapMat.ptr(xIndex, yIndex).putShort((short) height); From b98358d26995800e811747788093e6bf9c808ff5 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 24 Feb 2025 13:16:16 -0600 Subject: [PATCH 18/27] Add reset method for visuals --- .../ContinuousHikingStateMachine/DoNothingState.java | 1 + .../activeMapping/TerrainPlanningDebugger.java | 10 ++++++++++ 2 files changed, 11 insertions(+) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java index f865afd87d1..ccf5a0165e2 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/ContinuousHikingStateMachine/DoNothingState.java @@ -56,6 +56,7 @@ public void doAction(double timeInState) message.setClearRemainingFootstepQueue(true); continuousPlanner.setLatestFootstepPlan(null); pauseWalkingPublisher.publish(message); + debugger.resetVisualizationForUIPublisher(); } robotFeet.get(RobotSide.LEFT).set(referenceFrames.getSoleFrame(RobotSide.LEFT).getTransformToWorldFrame()); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java index 13055597ef1..eff0611a7c5 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/behaviors/activeMapping/TerrainPlanningDebugger.java @@ -241,6 +241,16 @@ public void printScoreStats(MonteCarloFootstepNode root, MonteCarloFootstepPlann } } + public void resetVisualizationForUIPublisher() + { + PoseListMessage poseListMessage = new PoseListMessage(); + FootstepDataListMessage footstepDataListMessage = new FootstepDataListMessage(); + + startAndGoalPublisherForUI.publish(poseListMessage); + monteCarloNodesPublisherForUI.publish(poseListMessage); + plannedFootstesPublisherForUI.publish(footstepDataListMessage); + } + public void publishStartAndGoalForVisualization(SideDependentList startPoses, SideDependentList goalPoses) { List poses = new ArrayList<>(); From 3109bc80f05546b87e75d7b022ebbd38cd02f584 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 24 Feb 2025 15:12:04 -0600 Subject: [PATCH 19/27] Cleanup files and remove testing lines --- .../CommunicationsSyncedRobotModel.java | 1 - .../MonteCarloFootstepPlanningTest.java | 7 +- .../RDXHighLevelDepthSensorSimulator.java | 2 +- .../perception/HumanoidPerceptionModule.java | 2 - .../TerrainPerceptionProcessWithDriver.java | 11 +-- .../FilteredRapidHeightMapExtractor.java | 56 +++++++---- .../gpuHeightMap/RapidHeightMapExtractor.java | 10 +- .../RapidHeightMapExtractorCUDA.java | 10 +- .../gpuHeightMap/RapidHeightMapManager.java | 10 +- .../SnappingHeightMapExtractor.java | 6 -- .../tools/PerceptionMessageTools.java | 1 - .../RealsenseColorDepthPublisher.java | 11 +-- .../FilteredRapidHeightMapExtractor.cu | 21 ++-- .../cuda/examples/CUDAExampleKernel3D.java | 3 - .../FilteredRapidHeightMapExtractorTest.java | 97 +++++++++---------- 15 files changed, 117 insertions(+), 131 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java index d1adc4be4a2..8139c9dc2ce 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java @@ -194,7 +194,6 @@ public FullHumanoidRobotModel getFullHumanoidRobotModel() return fullRobotModel; } - public SideDependentList getHandWrenchCalculators() { return handWrenchCalculators; diff --git a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java index f2754b72d9d..0fbd5b0d918 100644 --- a/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java +++ b/ihmc-footstep-planning/src/test/java/us/ihmc/footstepPlanning/monteCarloPlanning/MonteCarloFootstepPlanningTest.java @@ -40,7 +40,12 @@ public void testMonteCarloFootstepPlanning() CameraIntrinsics depthImageIntrinsics = new CameraIntrinsics(); BytedecoImage heightMapBytedecoImage = new BytedecoImage(depthImageIntrinsics.getWidth(), depthImageIntrinsics.getHeight(), opencv_core.CV_16UC1); heightMapBytedecoImage.createOpenCLImage(openCLManager, OpenCL.CL_MEM_READ_WRITE); - RapidHeightMapExtractor heightMapExtractor = new RapidHeightMapExtractor(openCLManager, heightMapBytedecoImage, cameraIntrinsics, 1); + + RapidHeightMapExtractor heightMapExtractor = new RapidHeightMapExtractor(openCLManager, + heightMapBytedecoImage, + cameraIntrinsics, + 1, + RapidHeightMapManager.getHeightMapParameters()); LogTools.info("Initializing"); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java index 8e1c6bbea10..289a76603f0 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java @@ -579,7 +579,7 @@ public void publishROS2DepthImageMessage() depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic); } - PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, ros2DepthTopic, depthImageMessage, depthImagePublisher, sensorPose, now, depthSequenceNumber++, + PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, depthImageMessage, depthImagePublisher, sensorPose, now, depthSequenceNumber++, depthSensorSimulator.getImageHeight(), depthSensorSimulator.getImageWidth(), 0.001f); }); diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java index b496aea2baa..6e153921d43 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/HumanoidPerceptionModule.java @@ -195,7 +195,6 @@ public void publishHeightMapImage(ROS2Node ros2Node, OpenCVTools.compressImagePNG(image, pointer); PerceptionMessageTools.publishCompressedDepthImage(pointer, - topic, message, heightMapImagePublisher, cameraPose, @@ -244,7 +243,6 @@ public void publishExternalHeightMapImage(ROS2Node ros2Node) OpenCVTools.compressImagePNG(heightMapImage, compressedInternalHeightMapPointer); //PerceptionDebugTools.displayDepth("Published Global Height Map", heightMapImage, 1); PerceptionMessageTools.publishCompressedDepthImage(compressedInternalHeightMapPointer, - PerceptionAPI.HEIGHT_MAP_CROPPED, croppedHeightMapImageMessage, heightMapPublisher, new FramePose3D(ReferenceFrame.getWorldFrame(), diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java index e3ad0463c1d..8905b64aebb 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/TerrainPerceptionProcessWithDriver.java @@ -102,7 +102,7 @@ public class TerrainPerceptionProcessWithDriver private final int colorHeight; private long depthSequenceNumber = 0; private long colorSequenceNumber = 0; - private ROS2Publisher depthPublisher; + private final ROS2Publisher depthPublisher; public TerrainPerceptionProcessWithDriver(String robotName, CollisionBoxProvider collisionBoxProvider, @@ -131,6 +131,8 @@ public TerrainPerceptionProcessWithDriver(String robotName, realtimeROS2Node.spin(); realsenseDeviceManager = new RealSenseDeviceManager(); + depthPublisher = realtimeROS2Node.createPublisher(depthTopic); + LogTools.info("Creating Bytedeco Realsense"); realsense = realsenseDeviceManager.createBytedecoRealsenseDevice(realsenseConfiguration); if (realsense.getDevice() == null) @@ -246,19 +248,12 @@ private void update() if (parameters.getPublishDepth()) { - - if (depthPublisher == null) - { - depthPublisher = realtimeROS2Node.createPublisher(depthTopic); - } - executorService.submit(() -> { OpenCVTools.compressImagePNG(depth16UC1Image, compressedDepthPointer); PerceptionMessageTools.setDepthIntrinsicsFromRealsense(realsense, depthImageMessage); CameraModel.PINHOLE.packMessageFormat(depthImageMessage); PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, - depthTopic, depthImageMessage, depthPublisher, cameraPose, diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index e55e1d2f72b..bcd9aaf6f38 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -4,15 +4,13 @@ import org.bytedeco.cuda.cudart.cudaExtent; import org.bytedeco.cuda.cudart.cudaPitchedPtr; import org.bytedeco.cuda.cudart.dim3; +import org.bytedeco.javacpp.FloatPointer; import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; -import org.bytedeco.opencv.opencv_core.Scalar; -import org.jetbrains.annotations.NotNull; import us.ihmc.perception.cuda.CUDAKernel; import us.ihmc.perception.cuda.CUDAProgram; import us.ihmc.perception.cuda.CUDATools; -import us.ihmc.perception.tools.PerceptionDebugTools; import java.net.URL; @@ -24,7 +22,7 @@ public class FilteredRapidHeightMapExtractor private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; - int layers = 6; + int layers; private final CUstream_st stream; private final int rows; @@ -32,13 +30,18 @@ public class FilteredRapidHeightMapExtractor private final CUDAKernel kernel; private final CUDAProgram program; private int loopTracker = 0; - private int defaultValue; + private final FloatPointer parametersHostPointer; + private final FloatPointer parametersDevicePointer; - public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) + public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols, int layers) { this.stream = stream; this.rows = rows; this.cols = cols; + this.layers = layers; + + parametersHostPointer = new FloatPointer(1); + parametersDevicePointer = new FloatPointer(); // Load header and main file URL kernelPath = getClass().getResource("FilteredRapidHeightMapExtractor.cu"); @@ -60,21 +63,26 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols) currentIndex = 0; } - public GpuMat update(GpuMat latestGlobalHeightMap, int defaultValue) + public GpuMat update(GpuMat latestGlobalHeightMap) { - this.defaultValue = defaultValue; + int error; - return computerHeightMapHistoryAverage(latestGlobalHeightMap); - } + // Populate parameter buffers with the necessary values + float[] parametersArray = populateParametersArray(layers); + parametersHostPointer.put(parametersArray); - @NotNull - private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) - { // Only want to compute the average if we have the past values to use if (loopTracker < layers) { loopTracker++; + CUDATools.mallocAsync(parametersDevicePointer, parametersArray.length, stream); + + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + CUDATools.memcpyAsync(parametersDevicePointer, parametersHostPointer, parametersArray.length, stream); + cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), pointerTo3DArray.pitch(), latestGlobalHeightMap.data(), @@ -87,18 +95,22 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) return latestGlobalHeightMap; } - int error; + CUDATools.mallocAsync(parametersDevicePointer, parametersArray.length, stream); + + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + + CUDATools.memcpyAsync(parametersDevicePointer, parametersHostPointer, parametersArray.length, stream); GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); kernel.withPointer(latestGlobalHeightMap.data()).withLong(latestGlobalHeightMap.step()); + kernel.withPointer(parametersDevicePointer); kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); - kernel.withInt(layers); - kernel.withInt(defaultValue); //Execute the CUDA kernels with the provided stream //Each kernel performs a specific task related to the height map update, registration, and cropping @@ -112,12 +124,10 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); - // Print the result to make sure we get what we expect - // In this example we go through 2,4,6,8 so we expect the average to be 5 Mat cpuResult = new Mat(); result.download(cpuResult); -// PerceptionDebugTools.printMat("Result", cpuResult, 1); + // Upload the latest height map to the GPU for the next iteration cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), pointerTo3DArray.pitch(), latestGlobalHeightMap.data(), @@ -131,6 +141,11 @@ private GpuMat computerHeightMapHistoryAverage(GpuMat latestGlobalHeightMap) return result; } + public float[] populateParametersArray(int layers) + { + return new float[] {(float) layers}; + } + public void reset() { loopTracker = 0; @@ -140,5 +155,8 @@ public void destroy() { program.close(); kernel.close(); + parametersHostPointer.close(); + parametersDevicePointer.close(); + cudaFree(parametersDevicePointer); } } diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java index 4e2c95613da..60a23f082a5 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractor.java @@ -128,14 +128,18 @@ public RapidHeightMapExtractor(OpenCLManager openCLManager, int mode, HeightMapParameters heightMapParameters) { - this(openCLManager, depthImage, depthCameraIntrinsics, mode); - this.heightMapParameters = heightMapParameters; + this(openCLManager, depthImage, depthCameraIntrinsics, mode, heightMapParameters); footSoleFrames.put(RobotSide.LEFT, leftFootSoleFrame); footSoleFrames.put(RobotSide.RIGHT, rightFootSoleFrame); } - public RapidHeightMapExtractor(OpenCLManager openCLManager, BytedecoImage depthImage, CameraIntrinsics depthCameraIntrinsics, int mode) + public RapidHeightMapExtractor(OpenCLManager openCLManager, + BytedecoImage depthImage, + CameraIntrinsics depthCameraIntrinsics, + int mode, + HeightMapParameters heightMapParameters) { + this.heightMapParameters = heightMapParameters; this.openCLManager = openCLManager; this.inputDepthImage = depthImage; this.depthCameraIntrinsics = depthCameraIntrinsics; diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java index a71706419fb..864aaeaa150 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapExtractorCUDA.java @@ -18,7 +18,6 @@ import us.ihmc.perception.cuda.CUDAStreamManager; import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.heightMap.TerrainMapData; -import us.ihmc.perception.tools.PerceptionDebugTools; import us.ihmc.perception.tools.PerceptionMessageTools; import us.ihmc.robotics.robotSide.RobotSide; import us.ihmc.robotics.robotSide.SideDependentList; @@ -116,7 +115,7 @@ public RapidHeightMapExtractorCUDA(ReferenceFrame leftFootSoleFrame, recomputeDerivedParameters(); // Need to initialize this after the parameters have been computed to get the right size - filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, globalCellsPerAxis, globalCellsPerAxis); + filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, globalCellsPerAxis, globalCellsPerAxis, 6); try { @@ -289,7 +288,7 @@ public void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform CUDATools.checkCUDAError(error); if (heightMapParameters.getEnableAlphaFilter()) - globalHeightMapImage = filteredRapidHeightMapExtractor.update(globalHeightMapImage, 0); + globalHeightMapImage = filteredRapidHeightMapExtractor.update(globalHeightMapImage); // Run the cropping kernel croppingKernel.withPointer(globalHeightMapImage.data()).withLong(globalHeightMapImage.step()); @@ -308,11 +307,6 @@ public void update(RigidBodyTransform sensorToWorldTransform, RigidBodyTransform //Update the terrain map data with the new results terrainMapData.setSensorOrigin(groundToWorldTransform.getTranslationX(), groundToWorldTransform.getTranslationY()); - Mat temp = new Mat(); - sensorCroppedHeightMapImage.download(temp); -// PerceptionDebugTools.printMat("", temp, 10); - - Mat finalCroppedHeightMap = new Mat(); // Assuming the height map is 201x201 sensorCroppedHeightMapImage.download(finalCroppedHeightMap); // Download the image from the GPU to the Mat object error = cudaStreamSynchronize(stream); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java index 2a2edc3e75f..54de36170d7 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java @@ -50,7 +50,7 @@ public class RapidHeightMapManager private GpuMat deviceDepthImage; private BytedecoImage heightMapBytedecoImage; - private ROS2Publisher heightMapPublisher; + private final ROS2Publisher heightMapPublisher; public RapidHeightMapManager(ROS2Node ros2Node, FullHumanoidRobotModel robotModel, @@ -90,6 +90,8 @@ public RapidHeightMapManager(ROS2Node ros2Node, heightMapParameters); } + heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); + // We use a notification in order to only call resetting the height map in one place ros2Node.createSubscription2(PerceptionAPI.RESET_HEIGHT_MAP, message -> resetHeightMapRequested.set()); if (robotModel != null) @@ -168,14 +170,8 @@ public void update(Mat latestDepthImage, Instant imageAcquisitionTime, Reference deviceCroppedHeightMapImage.close(); } - if (heightMapPublisher == null) - { - heightMapPublisher = ros2Node.createPublisher(PerceptionAPI.HEIGHT_MAP_CROPPED); - } - OpenCVTools.compressImagePNG(croppedHeightMapImage, compressedCroppedHeightMapPointer); PerceptionMessageTools.publishCompressedDepthImage(compressedCroppedHeightMapPointer, - PerceptionAPI.HEIGHT_MAP_CROPPED, croppedHeightMapImageMessage, heightMapPublisher, cameraPose, diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java index 29a5c6e1baa..952dbfbabd0 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/SnappingHeightMapExtractor.java @@ -15,7 +15,6 @@ import us.ihmc.perception.cuda.CUDATools; import us.ihmc.perception.heightMap.TerrainMapData; import us.ihmc.perception.steppableRegions.SteppableRegionCalculatorParameters; -import us.ihmc.perception.tools.PerceptionDebugTools; import us.ihmc.sensorProcessing.heightMap.HeightMapParameters; import java.net.URL; @@ -123,11 +122,6 @@ public void update(GpuMat globalHeightMapImage, Point3D sensorOrigin, int global error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); - Mat temp = new Mat(); - steppabilityImage.download(temp); -// PerceptionDebugTools.printMat("", temp, 10); - - // Download all the data from the GPU and set the terrain data object { Mat cpuSteppabilityMap = new Mat(); diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java index 0030c775f9a..ee6acb4c54a 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java @@ -75,7 +75,6 @@ public static void toBoofCV(ImageMessage imageMessage, Object cameraPinholeToPac } public static void publishCompressedDepthImage(BytePointer compressedDepthPointer, - ROS2Topic topic, ImageMessage depthImageMessage, ROS2Publisher publisher, Pose3DReadOnly cameraPose, diff --git a/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java b/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java index 19a71e410c3..fbae05ff085 100644 --- a/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java +++ b/ihmc-perception/src/main/java/us/ihmc/sensors/deprecated/RealsenseColorDepthPublisher.java @@ -58,7 +58,6 @@ public class RealsenseColorDepthPublisher private final ROS2StoredPropertySetGroup ros2PropertySetGroup; private final Supplier sensorFrameUpdater; private final ROS2Topic colorTopic; - private final ROS2Topic depthTopic; private final ROS2Node ros2Node; private final ROS2Helper ros2Helper; private final PerceptionConfigurationParameters parameters = new PerceptionConfigurationParameters(); @@ -83,7 +82,7 @@ public class RealsenseColorDepthPublisher private boolean loggerInitialized = false; private volatile boolean running = true; private final Notification destroyedNotification = new Notification(); - private ROS2Publisher depthPublisher; + private final ROS2Publisher depthPublisher; public RealsenseColorDepthPublisher(RealSenseConfiguration realsenseConfiguration, ROS2Topic depthTopic, @@ -91,7 +90,6 @@ public RealsenseColorDepthPublisher(RealSenseConfiguration realsenseConfiguratio Supplier sensorFrameUpdater) { this.colorTopic = colorTopic; - this.depthTopic = depthTopic; this.sensorFrameUpdater = sensorFrameUpdater; realsenseDeviceManager = new RealSenseDeviceManager(); @@ -107,6 +105,8 @@ public RealsenseColorDepthPublisher(RealSenseConfiguration realsenseConfiguratio ros2Node = new ROS2NodeBuilder().build("realsense_color_and_depth_publisher"); ros2Helper = new ROS2Helper(ros2Node); + depthPublisher = ros2Node.createPublisher(depthTopic); + LogTools.info("Setting up ROS2StoredPropertySetGroup"); ros2PropertySetGroup = new ROS2StoredPropertySetGroup(ros2Node); ros2PropertySetGroup.registerStoredPropertySet(PerceptionComms.PERCEPTION_CONFIGURATION_PARAMETERS, parameters); @@ -176,12 +176,7 @@ private void update() PerceptionMessageTools.setDepthIntrinsicsFromRealsense(realsense, depthImageMessage); CameraModel.PINHOLE.packMessageFormat(depthImageMessage); - if (depthPublisher == null) - { - depthPublisher = ros2Node.createPublisher(depthTopic); - } PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, - depthTopic, depthImageMessage, depthPublisher, depthPose, diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index b1719835419..43be54370c3 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -1,11 +1,11 @@ -#define LAYERS 6 // Set a fixed size - extern "C" +#define LAYERS 0 + __global__ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * resultPointer, size_t pitchResult, unsigned short * newestHeightMap, size_t pitchNewestHeightMap, - size_t layerSize, int rows, int cols, int defaultValue) + float *params, size_t layerSize, int rows, int cols) { int indexX = blockIdx.x * blockDim.x + threadIdx.x; int indexY = blockIdx.y * blockDim.y + threadIdx.y; @@ -14,10 +14,10 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, return; int sum = 0; - float variance[LAYERS] = {0}; + float* variance = (float*)malloc(params[LAYERS] * sizeof(float)); // Compute the average height of the history in order to get the variance at each layer - for (int layer = 0; layer < LAYERS; layer++) + for (int layer = 0; layer < params[LAYERS]; layer++) { unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; @@ -26,10 +26,10 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, sum += (int) *matrixPointerRow; } - unsigned short avg = sum / LAYERS; + unsigned short avg = sum / params[LAYERS]; // Compute the variance for each layer - for (int layer = 0; layer < LAYERS; layer++) + for (int layer = 0; layer < params[LAYERS]; layer++) { unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; @@ -42,7 +42,7 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, double varianceSum = 0; // Compute the height and variance sum - for (int layer = 0; layer < LAYERS; layer++) + for (int layer = 0; layer < params[LAYERS]; layer++) { unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; @@ -64,10 +64,6 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, float diff = (float) heightValueInt - avg; float newVariance = abs(diff); - if (*heightValue == defaultValue) - return; - - float alpha = 0.2; // printf("Equation parameters (heightValue %hu) (alpha %f) (avg %hu)\n", *heightValue, alpha, avg); float heightEstimate = (float) *heightValue * alpha + (avg * (1.0 - alpha)); // (newHeight * newVariance) / newVariance; @@ -75,6 +71,7 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; *outputPointer = (unsigned short) heightEstimate; + free(variance); // printf("GPU Height Estimate: %f\n", heightEstimate); } diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java index 0c22ac6dbf3..5d13536e259 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -4,7 +4,6 @@ import org.bytedeco.cuda.cudart.cudaExtent; import org.bytedeco.cuda.cudart.cudaPitchedPtr; import org.bytedeco.cuda.cudart.dim3; -import org.bytedeco.javacpp.ShortPointer; import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; import org.bytedeco.opencv.opencv_core.Mat; @@ -16,8 +15,6 @@ import us.ihmc.perception.tools.PerceptionDebugTools; import java.net.URL; -import java.util.ArrayList; -import java.util.List; import static org.bytedeco.cuda.global.cudart.*; diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java index 859faa73d8d..c93e3e3b1f6 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractorTest.java @@ -9,15 +9,23 @@ import us.ihmc.perception.cuda.CUDAStreamManager; import us.ihmc.perception.tools.PerceptionDebugTools; +import static org.junit.jupiter.api.Assertions.*; + +/** + * Test that we can compute the average and keep a history. + * Also ensures that things are printed correctly. + */ public class FilteredRapidHeightMapExtractorTest { @Test public void testGettingAverage() { - int rows = 1501; - int cols = 1501; + int rows = 10; + int cols = 10; + int layers = 2; + CUstream_st stream = CUDAStreamManager.getStream(); - FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols, layers); for (int i = 0; i < 8; i++) { @@ -25,10 +33,10 @@ public void testGettingAverage() GpuMat latestDepthMat = new GpuMat(); latestDepthMat.upload(cpuData); - GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); Mat temp = new Mat(); currentAverage.download(temp); -// PerceptionDebugTools.printMat("current", temp, 1); + PerceptionDebugTools.printMat("Current", temp, 1); } filteredRapidHeightMapExtractor.destroy(); @@ -39,28 +47,40 @@ public void testChangeAfterSteadyState() { int rows = 2; int cols = 2; + int layers = 2; + CUstream_st stream = CUDAStreamManager.getStream(); - FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols, layers); // This example fills the history with the same value, all 8's - // Lets see how this behaves when we later then introduce a change (noise) + // Lets see how this behaves when we later than introduce a change (noise) + Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(800)); for (int i = 0; i < 8; i++) { - Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(800)); + cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(800)); GpuMat latestDepthMat = new GpuMat(); latestDepthMat.upload(cpuData); - GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + filteredRapidHeightMapExtractor.update(latestDepthMat); } - Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(1000)); + Mat cpuDataAdjusted = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(1000)); GpuMat latestDepthMat = new GpuMat(); - latestDepthMat.upload(cpuData); + latestDepthMat.upload(cpuDataAdjusted); - GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); Mat temp = new Mat(); currentAverage.download(temp); - PerceptionDebugTools.printMat("current", temp, 1); + PerceptionDebugTools.printMat("Current", temp, 1); + + for (int i = 0; i < temp.rows(); i++) + { + for (int j = 0; j < temp.cols(); j++) + { + // This data should be more than the latest data because it's weighted towards the previous average + assertTrue(temp.col(i).row(j).data().getShort() > cpuData.col(j).data().getShort()); + } + } filteredRapidHeightMapExtractor.destroy(); } @@ -70,18 +90,20 @@ public void testChangedAfterSteadyStateRealDepthValuesUnsignedShort() { int rows = 2; int cols = 2; + int layers = 2; + CUstream_st stream = CUDAStreamManager.getStream(); - FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols, layers); // This example fills the history with the same value, all 8's - // Lets see how this behaves when we later then introduce a change (noise) + // Lets see how this behaves when we later than introduce a change (noise) for (int i = 0; i < 8; i++) { Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(32768)); GpuMat latestDepthMat = new GpuMat(); latestDepthMat.upload(cpuData); - filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + filteredRapidHeightMapExtractor.update(latestDepthMat); } // 400 is about 20 centimeters? Ish depending on the parameters, this is hard coded could change it to be based on the parameters @@ -89,47 +111,20 @@ public void testChangedAfterSteadyStateRealDepthValuesUnsignedShort() GpuMat latestDepthMat = new GpuMat(); latestDepthMat.upload(cpuData); - GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat); Mat temp = new Mat(); currentAverage.download(temp); - PerceptionDebugTools.printMat("current", temp, 1); - - filteredRapidHeightMapExtractor.destroy(); - } - - - // TODO remove this guy heheeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeeee - @Test - public void testDefaultValueSkipping() - { - int rows = 3; - int cols = 3; - CUstream_st stream = CUDAStreamManager.getStream(); - FilteredRapidHeightMapExtractor filteredRapidHeightMapExtractor = new FilteredRapidHeightMapExtractor(stream, rows, cols); + PerceptionDebugTools.printMat("Current", temp, 1); - // Set everything to 1, I'm gonna pass this in as the default value so these get skipped hopefully - for (int i = 0; i < 4; i++) + for (int i = 0; i < temp.rows(); i++) { - Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(1)); - GpuMat latestDepthMat = new GpuMat(); - latestDepthMat.upload(cpuData); - - filteredRapidHeightMapExtractor.update(latestDepthMat, 0); + for (int j = 0; j < temp.cols(); j++) + { + // This data should be less than the latest data because it's weighted towards the previous average + assertTrue(temp.col(i).row(j).data().getShort() < cpuData.col(j).data().getShort()); + } } - //TODO maybe we can just skip this? Does it matter - // I guess the newest height map will have the same values, so we can skip it - - // 400 is about 20 centimeters? Ish depending on the parameters, this is hard coded could change it to be based on the parameters - Mat cpuData = new Mat(rows, cols, opencv_core.CV_16UC1, new Scalar(33100)); - GpuMat latestDepthMat = new GpuMat(); - latestDepthMat.upload(cpuData); - - GpuMat currentAverage = filteredRapidHeightMapExtractor.update(latestDepthMat, 0); - Mat temp = new Mat(); - currentAverage.download(temp); - PerceptionDebugTools.printMat("current", temp, 1); - filteredRapidHeightMapExtractor.destroy(); } } From a78af41a9ee4348ec2c15bd17a150d6abd4074b3 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 24 Feb 2025 15:19:45 -0600 Subject: [PATCH 20/27] Moved publisher to constructor --- .../sensors/RDXHighLevelDepthSensorSimulator.java | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java index 289a76603f0..36de7bb4023 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java @@ -245,6 +245,9 @@ public void setupForROS2PointCloud(ROS2Node ros2Node, ROS2Topic ros2PointClou { this.ros2Node = ros2Node; this.ros2PointCloudTopic = ros2PointCloudTopic; + + depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic); + ros2PointsToPublish = new RecyclingArrayList<>(imageWidth * imageHeight, Point3D::new); pointCloudMessageType = ros2PointCloudTopic.getType(); if (pointCloudMessageType.equals(StereoVisionPointCloudMessage.class)) @@ -574,11 +577,6 @@ public void publishROS2DepthImageMessage() sensorPose.changeFrame(ReferenceFrame.getWorldFrame()); OpenCVTools.compressImagePNG(depthImageMat, compressedDepthPointer); - if (depthImagePublisher == null) - { - depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic); - } - PerceptionMessageTools.publishCompressedDepthImage(compressedDepthPointer, depthImageMessage, depthImagePublisher, sensorPose, now, depthSequenceNumber++, depthSensorSimulator.getImageHeight(), depthSensorSimulator.getImageWidth(), 0.001f); From 5431014c656ff78b205bf560c51aceb069fdd185 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 24 Feb 2025 15:54:06 -0600 Subject: [PATCH 21/27] Change planar region to use ros2helper still, merge conflict --- .../java/us/ihmc/perception/tools/PerceptionMessageTools.java | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java index 68e852ca3b1..381dc796f2f 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/tools/PerceptionMessageTools.java @@ -25,7 +25,6 @@ import us.ihmc.perception.imageMessage.CompressionType; import us.ihmc.perception.imageMessage.PixelFormat; import us.ihmc.perception.opencv.OpenCVTools; -import us.ihmc.ros2.ROS2Node; import us.ihmc.ros2.ROS2Publisher; import us.ihmc.sensors.realsense.RealSenseDevice; import us.ihmc.robotics.geometry.FramePlanarRegionsList; @@ -105,7 +104,7 @@ public static void publishJPGCompressedColorImage(BytePointer compressedColorPoi public static void publishFramePlanarRegionsList(FramePlanarRegionsList framePlanarRegionsList, ROS2Topic topic, - ROS2PublishSubscribeAPI ros2) + ROS2Helper ros2) { ros2.publish(topic, PlanarRegionMessageConverter.convertToFramePlanarRegionsListMessage(framePlanarRegionsList)); } From 9bec35b9484a01c2acf7b8770fb4c03bed35e7fc Mon Sep 17 00:00:00 2001 From: nkitchel Date: Mon, 24 Feb 2025 15:12:04 -0600 Subject: [PATCH 22/27] Cleanup code mainly for height map filter --- .../CommunicationsSyncedRobotModel.java | 5 --- .../RDXHighLevelDepthSensorSimulator.java | 6 ++-- .../RapidHeightMapUpdateThread.java | 2 +- .../FilteredRapidHeightMapExtractor.cu | 35 ------------------- 4 files changed, 4 insertions(+), 44 deletions(-) diff --git a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java index 8139c9dc2ce..e4cea554c9a 100644 --- a/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java +++ b/ihmc-avatar-interfaces/src/main/java/us/ihmc/avatar/drcRobot/CommunicationsSyncedRobotModel.java @@ -189,11 +189,6 @@ public DRCRobotModel getRobotModel() return robotModel; } - public FullHumanoidRobotModel getFullHumanoidRobotModel() - { - return fullRobotModel; - } - public SideDependentList getHandWrenchCalculators() { return handWrenchCalculators; diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java index 36de7bb4023..60cf189d907 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java @@ -234,6 +234,9 @@ public void setupForROS2ImageMessages(ROS2Node ros2Node, ROS2Topic { this.ros2Node = ros2Node; this.ros2Helper = new ROS2Helper(ros2Node); + + depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic); + this.ros2DepthTopic = ros2DepthTopic; this.ros2ColorTopic = ros2ColorTopic; @@ -245,9 +248,6 @@ public void setupForROS2PointCloud(ROS2Node ros2Node, ROS2Topic ros2PointClou { this.ros2Node = ros2Node; this.ros2PointCloudTopic = ros2PointCloudTopic; - - depthImagePublisher = ros2Node.createPublisher(ros2DepthTopic); - ros2PointsToPublish = new RecyclingArrayList<>(imageWidth * imageHeight, Point3D::new); pointCloudMessageType = ros2PointCloudTopic.getType(); if (pointCloudMessageType.equals(StereoVisionPointCloudMessage.class)) diff --git a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java index b2fa79a438d..baaf42479a3 100644 --- a/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java +++ b/ihmc-high-level-behaviors/src/main/java/us/ihmc/perception/RapidHeightMapUpdateThread.java @@ -64,7 +64,7 @@ protected void runTask() if (heightMapManager == null) { heightMapManager = new RapidHeightMapManager(ros2Node, - syncedRobotModel.getFullHumanoidRobotModel(), + syncedRobotModel.getFullRobotModel(), syncedRobotModel.getRobotModel().getSimpleRobotName(), leftFootFrame, rightFootFrame, diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 43be54370c3..8974b9b4ef0 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -14,7 +14,6 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, return; int sum = 0; - float* variance = (float*)malloc(params[LAYERS] * sizeof(float)); // Compute the average height of the history in order to get the variance at each layer for (int layer = 0; layer < params[LAYERS]; layer++) @@ -28,36 +27,6 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short avg = sum / params[LAYERS]; - // Compute the variance for each layer - for (int layer = 0; layer < params[LAYERS]; layer++) - { - unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); - unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - - float diff = (float)(*matrixPointerRow) - avg; - variance[layer] = abs(diff); - } - - double heightSum = 0; - double varianceSum = 0; - - // Compute the height and variance sum - for (int layer = 0; layer < params[LAYERS]; layer++) - { - unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); - unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; - - if (variance[layer] > 0.0f) // Prevent division by zero - { - heightSum += (double)(*matrixPointerRow) / (double)variance[layer]; - varianceSum += 1.0 / (double)variance[layer]; - } - } - -// printf("Height sum: %f\n", heightSum); -// printf("Variance sum: %f\n", varianceSum); - unsigned short newHeight = heightSum / varianceSum; - unsigned short * heightValue = (unsigned short*)((char*) newestHeightMap + indexY * pitchNewestHeightMap) + indexX; int heightValueInt = (int) *heightValue; @@ -70,8 +39,4 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short *outputPointer = (unsigned short *)((char*) resultPointer + indexY * pitchResult) + indexX; *outputPointer = (unsigned short) heightEstimate; - - free(variance); -// printf("GPU Height Estimate: %f\n", heightEstimate); } - From e22e2e6de6db3ffdfd6db0f9ba06ec22598b43a3 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Wed, 26 Feb 2025 13:30:06 -0600 Subject: [PATCH 23/27] Cleanup filtered kernel --- .../FilteredRapidHeightMapExtractor.java | 45 +++---------------- .../FilteredRapidHeightMapExtractor.cu | 16 +++---- .../cuda/examples/CUDAExampleKernel3D.java | 2 +- 3 files changed, 14 insertions(+), 49 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index bcd9aaf6f38..79b4e23448f 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -4,10 +4,8 @@ import org.bytedeco.cuda.cudart.cudaExtent; import org.bytedeco.cuda.cudart.cudaPitchedPtr; import org.bytedeco.cuda.cudart.dim3; -import org.bytedeco.javacpp.FloatPointer; import org.bytedeco.opencv.global.opencv_core; import org.bytedeco.opencv.opencv_core.GpuMat; -import org.bytedeco.opencv.opencv_core.Mat; import us.ihmc.perception.cuda.CUDAKernel; import us.ihmc.perception.cuda.CUDAProgram; import us.ihmc.perception.cuda.CUDATools; @@ -18,11 +16,12 @@ public class FilteredRapidHeightMapExtractor { - static final int BLOCK_SIZE_XY = 32; + private static final int BLOCK_SIZE_XY = 32; + private static final double ALPHA = 0.2; private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; - int layers; + private final int layers; private final CUstream_st stream; private final int rows; @@ -30,8 +29,6 @@ public class FilteredRapidHeightMapExtractor private final CUDAKernel kernel; private final CUDAProgram program; private int loopTracker = 0; - private final FloatPointer parametersHostPointer; - private final FloatPointer parametersDevicePointer; public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols, int layers) { @@ -40,9 +37,6 @@ public FilteredRapidHeightMapExtractor(CUstream_st stream, int rows, int cols, i this.cols = cols; this.layers = layers; - parametersHostPointer = new FloatPointer(1); - parametersDevicePointer = new FloatPointer(); - // Load header and main file URL kernelPath = getClass().getResource("FilteredRapidHeightMapExtractor.cu"); try @@ -67,22 +61,11 @@ public GpuMat update(GpuMat latestGlobalHeightMap) { int error; - // Populate parameter buffers with the necessary values - float[] parametersArray = populateParametersArray(layers); - parametersHostPointer.put(parametersArray); - // Only want to compute the average if we have the past values to use if (loopTracker < layers) { loopTracker++; - CUDATools.mallocAsync(parametersDevicePointer, parametersArray.length, stream); - - error = cudaStreamSynchronize(stream); - CUDATools.checkCUDAError(error); - - CUDATools.memcpyAsync(parametersDevicePointer, parametersHostPointer, parametersArray.length, stream); - cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), pointerTo3DArray.pitch(), latestGlobalHeightMap.data(), @@ -95,22 +78,16 @@ public GpuMat update(GpuMat latestGlobalHeightMap) return latestGlobalHeightMap; } - CUDATools.mallocAsync(parametersDevicePointer, parametersArray.length, stream); - - error = cudaStreamSynchronize(stream); - CUDATools.checkCUDAError(error); - - CUDATools.memcpyAsync(parametersDevicePointer, parametersHostPointer, parametersArray.length, stream); - GpuMat result = new GpuMat(rows, cols, opencv_core.CV_16UC1); kernel.withPointer(pointerTo3DArray.ptr()).withLong(pointerTo3DArray.pitch()); kernel.withPointer(result.data()).withLong(result.step()); kernel.withPointer(latestGlobalHeightMap.data()).withLong(latestGlobalHeightMap.step()); - kernel.withPointer(parametersDevicePointer); + kernel.withInt(layers); kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); + kernel.withDouble(ALPHA); //Execute the CUDA kernels with the provided stream //Each kernel performs a specific task related to the height map update, registration, and cropping @@ -124,9 +101,6 @@ public GpuMat update(GpuMat latestGlobalHeightMap) error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); - Mat cpuResult = new Mat(); - result.download(cpuResult); - // Upload the latest height map to the GPU for the next iteration cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), pointerTo3DArray.pitch(), @@ -141,11 +115,6 @@ public GpuMat update(GpuMat latestGlobalHeightMap) return result; } - public float[] populateParametersArray(int layers) - { - return new float[] {(float) layers}; - } - public void reset() { loopTracker = 0; @@ -155,8 +124,6 @@ public void destroy() { program.close(); kernel.close(); - parametersHostPointer.close(); - parametersDevicePointer.close(); - cudaFree(parametersDevicePointer); + cudaFree(pointerTo3DArray); } } diff --git a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu index 8974b9b4ef0..9da95db011f 100644 --- a/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu +++ b/ihmc-perception/src/main/resources/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.cu @@ -1,11 +1,10 @@ extern "C" -#define LAYERS 0 __global__ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, unsigned short * resultPointer, size_t pitchResult, unsigned short * newestHeightMap, size_t pitchNewestHeightMap, - float *params, size_t layerSize, int rows, int cols) + int layers, size_t layerSize, int rows, int cols, float alpha) { int indexX = blockIdx.x * blockDim.x + threadIdx.x; int indexY = blockIdx.y * blockDim.y + threadIdx.y; @@ -13,19 +12,19 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, if (indexX >= cols || indexY >= rows) return; - int sum = 0; + unsigned int sum = 0; // Compute the average height of the history in order to get the variance at each layer - for (int layer = 0; layer < params[LAYERS]; layer++) + for (int layer = 0; layer < layers; layer++) { unsigned short * currentLayer = (unsigned short*)((char*) matrixPointer + layer * layerSize); - unsigned short * matrixPointerRow = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; + unsigned short * matrixCell = (unsigned short*)((char*) currentLayer + indexY * pitchA) + indexX; -// printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixPointerRow); - sum += (int) *matrixPointerRow; +// printf("Layer: %d, Value: %d, %d, and %d\n", layer, indexY, indexX, (int) *matrixCell); + sum += (int) *matrixCell; } - unsigned short avg = sum / params[LAYERS]; + unsigned short avg = sum / layers; unsigned short * heightValue = (unsigned short*)((char*) newestHeightMap + indexY * pitchNewestHeightMap) + indexX; @@ -33,7 +32,6 @@ void filterRapidHeightMap(unsigned short * matrixPointer, size_t pitchA, float diff = (float) heightValueInt - avg; float newVariance = abs(diff); - float alpha = 0.2; // printf("Equation parameters (heightValue %hu) (alpha %f) (avg %hu)\n", *heightValue, alpha, avg); float heightEstimate = (float) *heightValue * alpha + (avg * (1.0 - alpha)); // (newHeight * newVariance) / newVariance; diff --git a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java index 5d13536e259..0d8c29bcc8d 100644 --- a/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java +++ b/ihmc-perception/src/test/java/us/ihmc/perception/cuda/examples/CUDAExampleKernel3D.java @@ -27,7 +27,7 @@ public class CUDAExampleKernel3D */ public CUDAExampleKernel3D() throws Exception { - // Create a 2x2 with 5 layers + // Create a 2x2 with some amount of layers int rows = 2; int cols = 2; int layers = 4; From 7fb78e16d5480cf8f5424f563fed68cb9cc8164c Mon Sep 17 00:00:00 2001 From: nkitchel Date: Wed, 26 Feb 2025 15:35:42 -0600 Subject: [PATCH 24/27] Check error after mem copy --- .../gpuHeightMap/FilteredRapidHeightMapExtractor.java | 10 ++++++++-- 1 file changed, 8 insertions(+), 2 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 79b4e23448f..212d5f3bee5 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -17,7 +17,7 @@ public class FilteredRapidHeightMapExtractor { private static final int BLOCK_SIZE_XY = 32; - private static final double ALPHA = 0.2; + private static final float ALPHA = 0.2F; private final cudaPitchedPtr pointerTo3DArray; private int currentIndex; @@ -74,6 +74,9 @@ public GpuMat update(GpuMat latestGlobalHeightMap) pointerTo3DArray.ysize(), cudaMemcpyDefault); + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + currentIndex = (currentIndex + 1) % layers; return latestGlobalHeightMap; } @@ -87,7 +90,7 @@ public GpuMat update(GpuMat latestGlobalHeightMap) kernel.withLong(pointerTo3DArray.pitch() * rows); kernel.withInt(rows); kernel.withInt(cols); - kernel.withDouble(ALPHA); + kernel.withFloat(ALPHA); //Execute the CUDA kernels with the provided stream //Each kernel performs a specific task related to the height map update, registration, and cropping @@ -110,6 +113,9 @@ public GpuMat update(GpuMat latestGlobalHeightMap) pointerTo3DArray.ysize(), cudaMemcpyDefault); + error = cudaStreamSynchronize(stream); + CUDATools.checkCUDAError(error); + currentIndex = (currentIndex + 1) % layers; return result; From 4025d101b0353101da5a60839b58c84806cea961 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Wed, 26 Feb 2025 15:43:09 -0600 Subject: [PATCH 25/27] Change error checking --- .../FilteredRapidHeightMapExtractor.java | 32 +++++++++---------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index 212d5f3bee5..eecebea6389 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -66,15 +66,14 @@ public GpuMat update(GpuMat latestGlobalHeightMap) { loopTracker++; - cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), - pointerTo3DArray.pitch(), - latestGlobalHeightMap.data(), - latestGlobalHeightMap.step(), - pointerTo3DArray.xsize(), - pointerTo3DArray.ysize(), - cudaMemcpyDefault); - - error = cudaStreamSynchronize(stream); + error = cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + latestGlobalHeightMap.data(), + latestGlobalHeightMap.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); + CUDATools.checkCUDAError(error); currentIndex = (currentIndex + 1) % layers; @@ -105,15 +104,14 @@ public GpuMat update(GpuMat latestGlobalHeightMap) CUDATools.checkCUDAError(error); // Upload the latest height map to the GPU for the next iteration - cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), - pointerTo3DArray.pitch(), - latestGlobalHeightMap.data(), - latestGlobalHeightMap.step(), - pointerTo3DArray.xsize(), - pointerTo3DArray.ysize(), - cudaMemcpyDefault); + error = cudaMemcpy2D(pointerTo3DArray.ptr().position(currentIndex * pointerTo3DArray.pitch() * pointerTo3DArray.ysize()), + pointerTo3DArray.pitch(), + latestGlobalHeightMap.data(), + latestGlobalHeightMap.step(), + pointerTo3DArray.xsize(), + pointerTo3DArray.ysize(), + cudaMemcpyDefault); - error = cudaStreamSynchronize(stream); CUDATools.checkCUDAError(error); currentIndex = (currentIndex + 1) % layers; From 7873a70ee9805240834663746e86ac861b065428 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Wed, 26 Feb 2025 15:46:13 -0600 Subject: [PATCH 26/27] Change error checking --- .../gpuHeightMap/FilteredRapidHeightMapExtractor.java | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java index eecebea6389..1c3fe4b32e3 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/FilteredRapidHeightMapExtractor.java @@ -128,6 +128,7 @@ public void destroy() { program.close(); kernel.close(); - cudaFree(pointerTo3DArray); + int error = cudaFree(pointerTo3DArray); + CUDATools.checkCUDAError(error); } } From 9adb15db7cedaeb6e13f6a633d5d5bcde3481355 Mon Sep 17 00:00:00 2001 From: nkitchel Date: Wed, 26 Feb 2025 17:26:16 -0600 Subject: [PATCH 27/27] Cleanup fields --- .../footstepPlanning/communication/ContinuousHikingAPI.java | 4 ++-- .../java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java | 2 +- .../us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java | 1 - .../ihmc/perception/gpuHeightMap/RapidHeightMapManager.java | 4 +--- 4 files changed, 4 insertions(+), 7 deletions(-) diff --git a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java index 76ada624e60..ec32f05f2bc 100644 --- a/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java +++ b/ihmc-footstep-planning/src/main/java/us/ihmc/footstepPlanning/communication/ContinuousHikingAPI.java @@ -17,10 +17,10 @@ public class ContinuousHikingAPI // Commands supported for the Continuous Hiking Process public static final ROS2Topic CONTINUOUS_HIKING_COMMAND = IHMC_ROOT.withModule(moduleName).withType(ContinuousHikingCommandMessage.class).withSuffix("command"); - public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(std_msgs.msg.dds.Empty.class).withSuffix("clear_goal_footsteps"); + public static final ROS2Topic CLEAR_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("clear_goal_footsteps"); public static final ROS2Topic PLACED_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("placed_goal_footsteps"); public static final ROS2Topic ROTATE_GOAL_FOOTSTEPS = IHMC_ROOT.withModule(moduleName).withType(PoseListMessage.class).withSuffix("rotate_goal_footsteps"); - public static final ROS2Topic SQUARE_UP_STEP = IHMC_ROOT.withModule(moduleName).withType(std_msgs.msg.dds.Empty.class).withSuffix("rotate_goal_footsteps"); + public static final ROS2Topic SQUARE_UP_STEP = IHMC_ROOT.withModule(moduleName).withType(Empty.class).withSuffix("rotate_goal_footsteps"); // Statuses supported from the Continuous Hiking Process public static final ROS2Topic CONTINUOUS_WALKING_STATUS = IHMC_ROOT.withModule(moduleName).withType(ContinuousWalkingStatusMessage.class).withSuffix("status"); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java index e021c855861..b338a4b2fc5 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXContinuousHikingPanel.java @@ -292,7 +292,7 @@ public void renderImGuiWidgets() if (ImGui.button("Turn Left 90°")) { - turnRobot(Math.PI / 2); + turnRobot(Math.PI / 2.0); } ImGui.sameLine(); diff --git a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java index 36ec5d12a59..a5498af9508 100644 --- a/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java +++ b/ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXStancePoseSelectionPanel.java @@ -26,7 +26,6 @@ import us.ihmc.rdx.imgui.ImGuiUniqueLabelMap; import us.ihmc.rdx.imgui.RDXPanel; import us.ihmc.rdx.input.ImGui3DViewInput; -import us.ihmc.rdx.input.ImGuiMouseDragData; import us.ihmc.rdx.tools.LibGDXTools; import us.ihmc.rdx.tools.RDXModelBuilder; import us.ihmc.rdx.ui.RDXBaseUI; diff --git a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java index 54de36170d7..a0f1b560938 100644 --- a/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java +++ b/ihmc-perception/src/main/java/us/ihmc/perception/gpuHeightMap/RapidHeightMapManager.java @@ -34,12 +34,11 @@ */ public class RapidHeightMapManager { - static final HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); + private static final HeightMapParameters heightMapParameters = new HeightMapParameters("GPU"); private final RapidHeightMapExtractorInterface rapidHeightMapExtractor; private final ImageMessage croppedHeightMapImageMessage = new ImageMessage(); private final FramePose3D cameraPose = new FramePose3D(); - private final ROS2Node ros2Node; private final boolean runWithCUDA; private final Mat hostDepthImage = new Mat(); private final Notification resetHeightMapRequested = new Notification(); @@ -61,7 +60,6 @@ public RapidHeightMapManager(ROS2Node ros2Node, CameraIntrinsics depthImageIntrinsics, boolean runWithCUDA) throws Exception { - this.ros2Node = ros2Node; this.runWithCUDA = runWithCUDA; if (runWithCUDA)