Skip to content

Commit

Permalink
Remove ROS 1 features from the RDX sensor simulator.
Browse files Browse the repository at this point in the history
  • Loading branch information
@calvertdw
calvertdw committed Aug 21, 2024
1 parent 8467800 commit ec1bccd
Show file tree
Hide file tree
Showing 8 changed files with 2 additions and 219 deletions.
2 changes: 0 additions & 2 deletions ...gh-level-behaviors/src/libgdx/java/us/ihmc/rdx/logging/RDXFFMPEGL515DepthLoggingDemo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -91,11 +91,9 @@ public void create()
l515.setSensorEnabled(true);
l515.setPublishPointCloudROS2(false);
l515.setRenderPointCloudDirectly(false);
l515.setPublishDepthImageROS1(false);
l515.setDebugCoordinateFrame(false);
l515.setRenderColorVideoDirectly(true);
l515.setRenderDepthVideoDirectly(true);
l515.setPublishColorImageROS1(false);
l515.setPublishColorImageROS2(false);
CameraPinholeBrown cameraIntrinsics = l515.getDepthCameraIntrinsics();
baseUI.getPrimaryScene().addRenderableProvider(l515::getRenderables);
Expand Down
203 changes: 1 addition & 202 deletions ...iors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXHighLevelDepthSensorSimulator.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -2,12 +2,10 @@

import boofcv.struct.calib.CameraPinholeBrown;
import com.badlogic.gdx.graphics.Color;
import com.badlogic.gdx.graphics.PerspectiveCamera;
import com.badlogic.gdx.graphics.g3d.ModelInstance;
import com.badlogic.gdx.graphics.g3d.Renderable;
import com.badlogic.gdx.math.Matrix4;
import com.badlogic.gdx.utils.Array;
import com.badlogic.gdx.utils.BufferUtils;
import com.badlogic.gdx.utils.Pool;
import controller_msgs.msg.dds.StereoVisionPointCloudMessage;
import imgui.ImGui;
Expand All @@ -26,13 +24,10 @@
import org.bytedeco.opencv.global.opencv_imgcodecs;
import org.bytedeco.opencv.global.opencv_imgproc;
import org.bytedeco.opencv.opencv_core.Mat;
import org.jboss.netty.buffer.ChannelBuffer;
import org.ros.message.Time;
import perception_msgs.msg.dds.BigVideoPacket;
import perception_msgs.msg.dds.FusedSensorHeadPointCloudMessage;
import perception_msgs.msg.dds.ImageMessage;
import perception_msgs.msg.dds.LidarScanMessage;
import sensor_msgs.Image;
import us.ihmc.commons.Conversions;
import us.ihmc.commons.lists.RecyclingArrayList;
import us.ihmc.commons.thread.ThreadTools;
Expand Down Expand Up @@ -70,11 +65,6 @@
import us.ihmc.tools.thread.MissingThreadTools;
import us.ihmc.tools.thread.ResettableExceptionHandlingExecutorService;
import us.ihmc.tools.thread.Throttler;
import us.ihmc.utilities.ros.RosNodeInterface;
import us.ihmc.utilities.ros.publisher.RosCameraInfoPublisher;
import us.ihmc.utilities.ros.publisher.RosImagePublisher;
import us.ihmc.utilities.ros.publisher.RosPointCloudPublisher;
import us.ihmc.utilities.ros.types.PointType;

import java.nio.ByteBuffer;
import java.nio.ByteOrder;
Expand Down Expand Up @@ -114,22 +104,10 @@ public class RDXHighLevelDepthSensorSimulator extends RDXPanel
int depthSequenceNumber = 0;
int colorSequenceNumber = 0;

private RosNodeInterface ros1Node;
private String ros1DepthImageTopic;
private String ros1ColorImageTopic;
private ROS2Topic<?> ros2PointCloudTopic;
private ROS2Topic<ImageMessage> ros2DepthTopic;
private ROS2Topic<ImageMessage> ros2ColorTopic;
private String ros1PointCloudTopic;
private RosImagePublisher ros1DepthPublisher;
private RosPointCloudPublisher ros1PointCloudPublisher;
private RosCameraInfoPublisher ros1DepthCameraInfoPublisher;
private ChannelBuffer ros1DepthChannelBuffer;
private RosImagePublisher ros1ColorPublisher;
private RosCameraInfoPublisher ros1ColorCameraInfoPublisher;
private ChannelBuffer ros1ColorChannelBuffer;
private Mat rgb8Mat;
private ByteBuffer rgb8Buffer;

private ROS2NodeInterface ros2Node;
private ROS2Helper ros2Helper;
Expand All @@ -141,7 +119,6 @@ public class RDXHighLevelDepthSensorSimulator extends RDXPanel
private BytePointer jpegImageBytePointer;
private Mat yuv420Image;
private IntPointer compressionParameters;
private RecyclingArrayList<Point3D> ros1PointsToPublish;
private RecyclingArrayList<Point3D> ros2PointsToPublish;
private int[] ros2ColorsToPublish;
private final FramePose3D tempSensorFramePose = new FramePose3D();
Expand All @@ -158,13 +135,9 @@ public class RDXHighLevelDepthSensorSimulator extends RDXPanel
private ModelInstance coordinateFrame;
private RigidBodyTransform sensorFrameToWorldTransform;

private boolean tuning = false;
private final ImGuiUniqueLabelMap labels = new ImGuiUniqueLabelMap(getClass());
private final ImBoolean sensorEnabled = new ImBoolean(false);
private final ImBoolean renderPointCloudDirectly = new ImBoolean(false);
private final ImBoolean publishDepthImageROS1 = new ImBoolean(false);
private final ImBoolean publishColorImageROS1 = new ImBoolean(false);
private final ImBoolean publishPointCloudROS1 = new ImBoolean(false);
private final ImBoolean publishColorImageROS2 = new ImBoolean(false);
private final ImBoolean publishPointCloudROS2 = new ImBoolean(false);
private final ImBoolean publishDepthImageMessageROS2 = new ImBoolean(false);
Expand Down Expand Up @@ -258,41 +231,6 @@ public RDXHighLevelDepthSensorSimulator(String sensorName,
publishImagesThread.start();
}

public void setupForROS1Depth(RosNodeInterface ros1Node, String ros1DepthImageTopic, String ros1DepthCameraInfoTopic)
{
this.ros1Node = ros1Node;
this.ros1DepthImageTopic = ros1DepthImageTopic;
LogTools.info("Publishing ROS 1 depth: {} {}", ros1DepthImageTopic, ros1DepthCameraInfoTopic);
ros1DepthPublisher = new RosImagePublisher();
ros1DepthCameraInfoPublisher = new RosCameraInfoPublisher();
ros1Node.attachPublisher(ros1DepthCameraInfoTopic, ros1DepthCameraInfoPublisher);
ros1Node.attachPublisher(ros1DepthImageTopic, ros1DepthPublisher);
ros1DepthChannelBuffer = ros1DepthPublisher.getChannelBufferFactory().getBuffer(2 * imageWidth * imageHeight);
}

public void setupForROS1Color(RosNodeInterface ros1Node, String ros1ColorImageTopic, String ros1ColorCameraInfoTopic)
{
this.ros1Node = ros1Node;
this.ros1ColorImageTopic = ros1ColorImageTopic;
LogTools.info("Publishing ROS 1 color: {} {}", ros1ColorImageTopic, ros1ColorCameraInfoTopic);
ros1ColorPublisher = new RosImagePublisher();
ros1ColorCameraInfoPublisher = new RosCameraInfoPublisher();
ros1Node.attachPublisher(ros1ColorCameraInfoTopic, ros1ColorCameraInfoPublisher);
ros1Node.attachPublisher(ros1ColorImageTopic, ros1ColorPublisher);
ros1ColorChannelBuffer = ros1ColorPublisher.getChannelBufferFactory().getBuffer(3 * imageWidth * imageHeight);
rgb8Buffer = BufferUtils.newByteBuffer(imageWidth * imageHeight * 3);
rgb8Mat = new Mat(imageHeight, imageWidth, opencv_core.CV_8UC3, new BytePointer(rgb8Buffer));
}

public void setupForROS1PointCloud(RosNodeInterface ros1Node, String ros1PointCloudTopic)
{
this.ros1Node = ros1Node;
this.ros1PointCloudTopic = ros1PointCloudTopic;
ros1PointsToPublish = new RecyclingArrayList<>(imageWidth * imageHeight, Point3D::new);
ros1PointCloudPublisher = new RosPointCloudPublisher(PointType.XYZ, false);
ros1Node.attachPublisher(ros1PointCloudTopic, ros1PointCloudPublisher);
}

public void setupForROS2ImageMessages(ROS2NodeInterface ros2Node, ROS2Topic<ImageMessage> ros2DepthTopic, ROS2Topic<ImageMessage> ros2ColorTopic)
{
this.ros2Node = ros2Node;
Expand Down Expand Up @@ -383,8 +321,6 @@ public void render(RDX3DScene scene)

lastUpdateTimeMs = now;
}

tuning = false;
}
}

Expand All @@ -393,16 +329,6 @@ public void publishImages()
double publishPeriod = Conversions.hertzToSeconds(publishRateHz.get());
if (throttler.run(publishPeriod))
{
if (ros1Node != null && ros1Node.isStarted())
{
if (publishDepthImageROS1.get())
publishDepthImageROS1();
if (publishColorImageROS1.get())
publishColorImageROS1();
if (publishPointCloudROS1.get())
publishPointCloudROS1();
}

if (realtimeROS2Node != null)
{
if (publishColorImageROS2.get())
Expand All @@ -429,28 +355,6 @@ public void publishImages()
}
}

private void publishColorImageROS1()
{
if (ros1ColorPublisher.isConnected() && ros1ColorCameraInfoPublisher.isConnected() && !colorExecutor.isExecuting())
{
opencv_imgproc.cvtColor(rgba8Mat, rgb8Mat, opencv_imgproc.COLOR_RGBA2RGB);

ros1ColorChannelBuffer.clear();
rgb8Buffer.rewind();
ros1ColorChannelBuffer.writeBytes(rgb8Buffer);

colorExecutor.execute(() ->
{
Image message = ros1ColorPublisher.createMessage(imageWidth, imageHeight, 3, "rgb8", ros1ColorChannelBuffer);

if (timestampSupplier != null)
message.getHeader().setStamp(new Time(Conversions.nanosecondsToSeconds(timestampSupplier.getAsLong())));

ros1ColorPublisher.publish(message);
});
}
}

private void publishColorImageROS2()
{
if (!colorROS2Executor.isExecuting())
Expand All @@ -474,55 +378,6 @@ private void publishColorImageROS2()
}
}

private void publishDepthImageROS1()
{
if (ros1DepthPublisher.isConnected() && ros1DepthCameraInfoPublisher.isConnected() && !depthExecutor.isExecuting())
{
PerspectiveCamera camera = depthSensorSimulator.getCamera();
ByteBuffer depthFloatBuffer = depthSensorSimulator.getMetersDepthFloatBuffer();
depthFloatBuffer.rewind();
ros1DepthChannelBuffer.clear();
int size = 2 * imageWidth * imageHeight;
for (int y = 0; y < imageHeight; y++)
{
for (int x = 0; x < imageWidth; x++)
{
float eyeDepthMeters = depthFloatBuffer.getFloat();

int row = y + 1;
int backForY = row * imageWidth * 2;
int forwardForX = x * 2;
int index = size - backForY + forwardForX;
if (eyeDepthMeters > camera.near && eyeDepthMeters < depthSensorSimulator.getMaxRange())
{
char depthChar16 = (char) Math.round(eyeDepthMeters * 1000.0f); // 1000 is 1 meter
ros1DepthChannelBuffer.setChar(index, depthChar16);
}
else
{
ros1DepthChannelBuffer.setChar(index, 0);
}
}
}

ros1DepthChannelBuffer.readerIndex(0);
ros1DepthChannelBuffer.writerIndex(size);

depthExecutor.execute(() ->
{
if (tuning)
updateCameraPinholeBrown();
ros1DepthCameraInfoPublisher.publish("camera_depth_optical_frame", depthCameraIntrinsics, new Time());
Image message = ros1DepthPublisher.createMessage(imageWidth, imageHeight, 2, "16UC1", ros1DepthChannelBuffer); // maybe need to copy here if there are errors

if(timestampSupplier != null)
message.getHeader().setStamp(new Time(Conversions.nanosecondsToSeconds(timestampSupplier.getAsLong())));

ros1DepthPublisher.publish(message);
});
}
}

private void updateCameraPinholeBrown()
{
depthCameraIntrinsics.setFx(depthSensorSimulator.getFocalLengthPixels().get());
Expand All @@ -534,7 +389,6 @@ private void updateCameraPinholeBrown()

public void renderImGuiWidgets()
{
tuning = true;
ImGui.text("Resolution: " + imageWidth + " x " + imageHeight);
ImGui.checkbox(labels.get("Sensor Enabled"), sensorEnabled);
ImGui.sameLine();
Expand All @@ -546,22 +400,13 @@ public void renderImGuiWidgets()
ImGui.checkbox(labels.get("Depth video"), getLowLevelSimulator().getDepthPanel().getIsShowing());
ImGui.sameLine();
ImGui.checkbox(labels.get("Color video"), getLowLevelSimulator().getColorPanel().getIsShowing());
boolean publishing = ros1DepthImageTopic != null;
publishing |= ros1ColorImageTopic != null;
publishing |= ros1PointCloudTopic != null;
publishing |= ros2PointCloudTopic != null;
boolean publishing = ros2PointCloudTopic != null;
publishing |= realtimeROS2Node != null;
if (publishing)
{
ImGui.text("Publish:");
ImGui.sameLine();
ImGui.text("(" + publishRateHz.get() + " Hz)");
if (ros1DepthImageTopic != null)
ImGui.checkbox(labels.get("ROS 1 Depth image (" + ros1DepthImageTopic + ")"), publishDepthImageROS1);
if (ros1ColorImageTopic != null)
ImGui.checkbox(labels.get("ROS 1 Color image (" + ros1ColorImageTopic + ")"), publishColorImageROS1);
if (ros1PointCloudTopic != null)
ImGui.checkbox(labels.get("ROS 1 Point Cloud (" + ros1PointCloudTopic + ")"), publishPointCloudROS1);
if (ros2PointCloudTopic != null)
{
ImGui.checkbox(labels.get("ROS 2 Point cloud (" + ros2PointCloudTopic + ")"), publishPointCloudROS2);
Expand Down Expand Up @@ -760,37 +605,6 @@ public void publishROS2ColorImageMessage()

}

private void publishPointCloudROS1()
{
if (!pointCloudExecutor.isExecuting())
{
ros1PointsToPublish.clear();
for (int i = 0; i < depthSensorSimulator.getNumberOfPoints()
&& (FLOATS_PER_POINT * i + 2) < depthSensorSimulator.getPointCloudBuffer().limit(); i++)
{
float x = depthSensorSimulator.getPointCloudBuffer().get(FLOATS_PER_POINT * i);
float y = depthSensorSimulator.getPointCloudBuffer().get(FLOATS_PER_POINT * i + 1);
float z = depthSensorSimulator.getPointCloudBuffer().get(FLOATS_PER_POINT * i + 2);
ros1PointsToPublish.add().set(x, y, z);
}

if (!ros1PointsToPublish.isEmpty())
{
pointCloudExecutor.execute(() ->
{
// long timestamp = timestampSupplier == null ? System.nanoTime() : timestampSupplier.getAsLong();
// tempSensorFramePose.setToZero(sensorFrame);
// tempSensorFramePose.changeFrame(ReferenceFrame.getWorldFrame());

int size = ros1PointsToPublish.size();
Point3D[] points = ros1PointsToPublish.toArray(new Point3D[size]);

ros1PointCloudPublisher.publish(points, new float[0], "os_sensor");
});
}
}
}

public List<Point3D> getPointCloud()
{
pointCloud.clear();
Expand Down Expand Up @@ -912,21 +726,6 @@ public void setRenderColorVideoDirectly(boolean renderColorVideoDirectly)
getLowLevelSimulator().getColorPanel().getIsShowing().set(renderColorVideoDirectly);
}

public void setPublishDepthImageROS1(boolean publish)
{
publishDepthImageROS1.set(publish);
}

public void setPublishColorImageROS1(boolean publish)
{
publishColorImageROS1.set(publish);
}

public void setPublishPointCloudROS1(boolean publish)
{
publishPointCloudROS1.set(publish);
}

public void setPublishColorImageROS2(boolean publish)
{
publishColorImageROS2.set(publish);
Expand Down
6 changes: 1 addition & 5 deletions ...l-behaviors/src/libgdx/java/us/ihmc/rdx/simulation/sensors/RDXSimulatedSensorFactory.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,8 +7,6 @@
import us.ihmc.pubsub.DomainFactory.PubSubImplementation;
import us.ihmc.robotics.robotSide.RobotSide;
import us.ihmc.ros2.ROS2NodeInterface;
import us.ihmc.utilities.ros.RosNodeInterface;
import us.ihmc.utilities.ros.RosTools;

import java.util.function.LongSupplier;

Expand Down Expand Up @@ -66,7 +64,7 @@ public static RDXHighLevelDepthSensorSimulator createMultisenseLeftEye(ROS2Synce
return highLevelDepthSensorSimulator;
}

public static RDXHighLevelDepthSensorSimulator createChestD435ForObjectDetection(ROS2SyncedRobotModel syncedRobot, RosNodeInterface ros1Node)
public static RDXHighLevelDepthSensorSimulator createChestD435ForObjectDetection(ROS2SyncedRobotModel syncedRobot)
{
double publishRateHz = 5.0;
double verticalFOV = 57.0;
Expand All @@ -92,8 +90,6 @@ public static RDXHighLevelDepthSensorSimulator createChestD435ForObjectDetection
0.001,
false,
publishRateHz);
highLevelDepthSensorSimulator.setupForROS1Depth(ros1Node, RosTools.D435_DEPTH, RosTools.D435_DEPTH_CAMERA_INFO);
highLevelDepthSensorSimulator.setupForROS1Color(ros1Node, RosTools.D435_VIDEO, RosTools.D435_CAMERA_INFO);
return highLevelDepthSensorSimulator;
}

Expand Down
2 changes: 0 additions & 2 deletions ...igh-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXHighLevelDepthSensorDemo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -79,11 +79,9 @@ public void create()
highLevelDepthSensorSimulator.setSensorEnabled(true);
highLevelDepthSensorSimulator.setPublishPointCloudROS2(false);
highLevelDepthSensorSimulator.setRenderPointCloudDirectly(true);
highLevelDepthSensorSimulator.setPublishDepthImageROS1(false);
highLevelDepthSensorSimulator.setDebugCoordinateFrame(false);
highLevelDepthSensorSimulator.setRenderColorVideoDirectly(true);
highLevelDepthSensorSimulator.setRenderDepthVideoDirectly(true);
highLevelDepthSensorSimulator.setPublishColorImageROS1(false);
highLevelDepthSensorSimulator.setPublishColorImageROS2(false);
baseUI.getPrimaryScene().addRenderableProvider(highLevelDepthSensorSimulator::getRenderables);

Expand Down
2 changes: 0 additions & 2 deletions ...gh-level-behaviors/src/test/java/us/ihmc/rdx/perception/RDXIterativeClosestPointDemo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -152,11 +152,9 @@ public void create()
highLevelDepthSensorSimulator.setSensorEnabled(true);
highLevelDepthSensorSimulator.setPublishPointCloudROS2(false);
highLevelDepthSensorSimulator.setRenderPointCloudDirectly(true);
highLevelDepthSensorSimulator.setPublishDepthImageROS1(false);
highLevelDepthSensorSimulator.setDebugCoordinateFrame(true);
highLevelDepthSensorSimulator.setRenderColorVideoDirectly(true);
highLevelDepthSensorSimulator.setRenderDepthVideoDirectly(true);
highLevelDepthSensorSimulator.setPublishColorImageROS1(false);
highLevelDepthSensorSimulator.setPublishColorImageROS2(false);
baseUI.getPrimaryScene().addRenderableProvider(highLevelDepthSensorSimulator::getRenderables);

Expand Down
Loading

0 comments on commit ec1bccd

Please sign in to comment.