Skip to content

Commit

Permalink
Perception Autonomy Cleanup (#450)
Browse files Browse the repository at this point in the history 
* Added basic Perception Autonomy Process with ZED

* WIP - added ImageSensor + YOLO

* Added RealsenseImageSensor

* Reset ROS2DemandGraphNode to develop

* Created ImageSensorPublishThread

* Minor ImageSensor cleanup

* Use PausableLoopingThread, add YOLO

* Cleanup

* Correct YOLO input to BGR

* Add scene graph update thread, fix YOLO null pointer issue

* WIP - Adding planar regions

* WIP - adding ZEDSVOPlayback

* Fixes

* Improve planar regions extraction

* Working planar regions

* Added ArUco detection thread

* Added ArUco to SceneGraph update

* Added Height Map

* Added ICP

* Added Behavior Tree Update Thread

* Cleanup

* Remove old perception autonomy process

* Allow specifying ZED input type

* Polishing
  • Loading branch information
@TomaszTB @rjgriffin42
TomaszTB authored and rjgriffin42 committed Jan 8, 2025
1 parent 18815c7 commit 1d5f994
Show file tree
Hide file tree
Showing 30 changed files with 1,638 additions and 842 deletions.
8 changes: 4 additions & 4 deletions ihmc-communication/src/main/java/us/ihmc/communication/PerceptionAPI.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -108,7 +108,8 @@ public final class PerceptionAPI
public static final ROS2Topic<BigVideoPacket> L515_DEBUG_EXTRACTION = BEST_EFFORT.withModule(L515_NAME)
.withType(BigVideoPacket.class)
.withSuffix("debug_extraction");
public static final ROS2Topic<Empty> REQUEST_REALSENSE_POINT_CLOUD = PERCEPTION_MODULE.withSuffix("request_realsense_point_cloud").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_REALSENSE = PERCEPTION_MODULE.withSuffix("request_realsense").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_REALSENSE_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_realsense_publication").withType(Empty.class);
public static final SideDependentList<ROS2Topic<BigVideoPacket>> BLACKFLY_VIDEO = new SideDependentList<>(BEST_EFFORT.withModule(BLACKFLY_NAME + "left")
.withType(BigVideoPacket.class)
.withSuffix("video"),
Expand Down Expand Up @@ -139,9 +140,8 @@ public final class PerceptionAPI
public static final ROS2Topic<Int64> ZED_SVO_SET_POSITION = PERCEPTION_MODULE.withSuffix("zed_svo_set_position").withType(Int64.class);
public static final ROS2Topic<Empty> ZED_SVO_PAUSE = PERCEPTION_MODULE.withSuffix("zed_svo_pause").withType(Empty.class);
public static final ROS2Topic<Empty> ZED_SVO_PLAY = PERCEPTION_MODULE.withSuffix("zed_svo_play").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_ZED_COLOR = PERCEPTION_MODULE.withSuffix("request_zed_color").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_ZED_DEPTH = PERCEPTION_MODULE.withSuffix("request_zed_depth").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_ZED_POINT_CLOUD = PERCEPTION_MODULE.withSuffix("request_zed_point_cloud").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_ZED = PERCEPTION_MODULE.withSuffix("request_zed").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_ZED_PUBLICATION = PERCEPTION_MODULE.withSuffix("request_zed_publication").withType(Empty.class);
public static final ROS2Topic<Empty> REQUEST_CENTERPOSE = PERCEPTION_MODULE.withSuffix("request_centerpose").withType(Empty.class);
public static final ROS2Topic<DetectedObjectPacket> CENTERPOSE_DETECTED_OBJECT = IHMC_ROOT.withModule("centerpose").withType(DetectedObjectPacket.class);
public static final ROS2Topic<Empty> REQUEST_YOLO_ZED = PERCEPTION_MODULE.withSuffix("request_yolo_zed").withType(Empty.class);
Expand Down
8 changes: 4 additions & 4 deletions ihmc-high-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/RDXZEDSVORecordingDemo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -136,7 +136,7 @@ public void renderImGuiWidgets()
}
}
};
zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_POINT_CLOUD);
zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
zed2ColoredPointCloudVisualizer.setActive(true);
perceptionVisualizerPanel.addVisualizer(zed2ColoredPointCloudVisualizer);
}
Expand All @@ -146,7 +146,7 @@ public void renderImGuiWidgets()
RDXROS2ImageMessageVisualizer zedLeftColorImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Color Left",
ros2Node,
PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT));
zedLeftColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR);
zedLeftColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
perceptionVisualizerPanel.addVisualizer(zedLeftColorImageVisualizer);
}

Expand All @@ -155,7 +155,7 @@ public void renderImGuiWidgets()
RDXROS2ImageMessageVisualizer zedRightColorImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Color Right",
ros2Node,
PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.RIGHT));
zedRightColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR);
zedRightColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
perceptionVisualizerPanel.addVisualizer(zedRightColorImageVisualizer);
}

Expand All @@ -164,7 +164,7 @@ public void renderImGuiWidgets()
RDXROS2ImageMessageVisualizer zed2DepthImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Depth Image",
ros2Node,
PerceptionAPI.ZED2_DEPTH);
zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_DEPTH);
zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
perceptionVisualizerPanel.addVisualizer(zed2DepthImageVisualizer);
}
}
Expand Down
6 changes: 3 additions & 3 deletions ...-level-behaviors/src/libgdx/java/us/ihmc/rdx/perception/sceneGraph/RDXSceneGraphDemo.java
View file
Original file line number Diff line number Diff line change
Expand Up @@ -104,22 +104,22 @@ public void create()
= new RDXROS2ImageMessageVisualizer("ZED 2 Color %s".formatted(side.getPascalCaseName()),
ros2Node,
PerceptionAPI.ZED2_COLOR_IMAGES.get(side));
zedColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_COLOR);
zedColorImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
perceptionVisualizerPanel.addVisualizer(zedColorImageVisualizer);
}

RDXROS2ImageMessageVisualizer zed2DepthImageVisualizer = new RDXROS2ImageMessageVisualizer("ZED 2 Depth Image",
ros2Node,
PerceptionAPI.ZED2_DEPTH);
zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_DEPTH);
zed2DepthImageVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
perceptionVisualizerPanel.addVisualizer(zed2DepthImageVisualizer);

RDXROS2ColoredPointCloudVisualizer zed2ColoredPointCloudVisualizer
= new RDXROS2ColoredPointCloudVisualizer("ZED 2 Colored Point Cloud",
ros2Node,
PerceptionAPI.ZED2_DEPTH,
PerceptionAPI.ZED2_COLOR_IMAGES.get(RobotSide.LEFT));
zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_POINT_CLOUD);
zed2ColoredPointCloudVisualizer.createRequestHeartbeat(ros2Node, PerceptionAPI.REQUEST_ZED_PUBLICATION);
zed2ColoredPointCloudVisualizer.setActive(true);
perceptionVisualizerPanel.addVisualizer(zed2ColoredPointCloudVisualizer);

Expand Down
Loading

0 comments on commit 1d5f994

Please sign in to comment.