Skip to content

Commit a3d0b64

Browse files
davetcolemanclaude
davetcoleman
and
claude
committed
Add lab_sim_behaviors package with ComputeTrayPlacePositionsUsingAprilTags
Move the tray place positions behavior from moveit_pro core into a lab_sim-specific behavior plugin package, since it is specific to the lab sim configuration. Register via pluginlib in lab_sim config.yaml. Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
1 parent 6fbaf04 commit a3d0b64

8 files changed

Lines changed: 460 additions & 0 deletions

File tree

‎src/lab_sim/config/config.yaml‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -48,6 +48,8 @@ objectives:
4848
- "moveit_pro::behaviors::VisionBehaviorsLoader"
4949
- "moveit_pro::behaviors::ConverterBehaviorsLoader"
5050
- "moveit_pro::behaviors::MujocoBehaviorsLoader"
51+
lab_sim:
52+
- "lab_sim_behaviors::LabSimBehaviorsLoader"
5153
# Specify source folder for objectives
5254
# [Required]
5355
objective_library_paths:

‎src/lab_sim/package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -28,6 +28,7 @@
2828
<exec_depend>robotiq_description</exec_depend>
2929
<exec_depend>ur_description</exec_depend>
3030
<exec_depend>velocity_force_controller</exec_depend>
31+
<exec_depend>lab_sim_behaviors</exec_depend>
3132

3233
<test_depend>ament_lint_auto</test_depend>
3334

‎src/lab_sim_behaviors/CMakeLists.txt‎

Lines changed: 46 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
cmake_minimum_required(VERSION 3.22)
2+
project(lab_sim_behaviors CXX)
3+
4+
find_package(moveit_studio_common REQUIRED)
5+
moveit_studio_package()
6+
7+
set(THIS_PACKAGE_INCLUDE_DEPENDS
8+
moveit_pro_behavior
9+
moveit_pro_behavior_interface
10+
moveit_studio_vision
11+
moveit_studio_vision_msgs
12+
pluginlib)
13+
foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
14+
find_package(${package} REQUIRED)
15+
endforeach()
16+
17+
add_library(
18+
lab_sim_behaviors
19+
SHARED
20+
src/compute_tray_place_positions_using_apriltags.cpp
21+
src/register_behaviors.cpp)
22+
target_include_directories(
23+
lab_sim_behaviors
24+
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
25+
$<INSTALL_INTERFACE:include>)
26+
ament_target_dependencies(lab_sim_behaviors ${THIS_PACKAGE_INCLUDE_DEPENDS})
27+
28+
install(
29+
TARGETS lab_sim_behaviors
30+
EXPORT lab_sim_behaviorsTargets
31+
ARCHIVE DESTINATION lib
32+
LIBRARY DESTINATION lib
33+
RUNTIME DESTINATION bin
34+
INCLUDES
35+
DESTINATION include)
36+
37+
if(BUILD_TESTING)
38+
moveit_pro_behavior_test(lab_sim_behaviors)
39+
endif()
40+
41+
pluginlib_export_plugin_description_file(
42+
moveit_pro_behavior_interface lab_sim_behaviors_plugin_description.xml)
43+
44+
ament_export_targets(lab_sim_behaviorsTargets HAS_LIBRARY_TARGET)
45+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
46+
ament_package()

‎src/lab_sim_behaviors/include/lab_sim_behaviors/compute_tray_place_positions_using_apriltags.hpp‎

Lines changed: 62 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,62 @@
1+
// Copyright 2026 PickNik Inc.
2+
// All rights reserved.
3+
//
4+
// Unauthorized copying of this code base via any medium is strictly prohibited.
5+
// Proprietary and confidential.
6+
7+
#pragma once
8+
9+
#include <behaviortree_cpp/action_node.h>
10+
#include <moveit_pro_behavior/behaviors/visualization/ros_publisher_handle.hpp>
11+
#include <moveit_pro_behavior_interface/behavior_context.hpp>
12+
#include <moveit_pro_behavior_interface/shared_resources_node.hpp>
13+
14+
namespace lab_sim_behaviors
15+
{
16+
/**
17+
* @brief Computes a grid of place positions within a tray detected via its AprilTag.
18+
*
19+
* @details Given an array of AprilTag detections and camera intrinsics, this Behavior finds the
20+
* tray's AprilTag by ID, uses its 3D pose plus a known offset to locate the tray center, then
21+
* generates a grid of place positions (multiple columns per row). Rows fill from the far end
22+
* of the tray toward the tag (top-to-bottom). Positions are transformed from the camera frame
23+
* to the world frame with a fixed gripper-down orientation.
24+
*
25+
* If an input image is provided, the Behavior annotates it with green circles sized to match
26+
* the bottle diameter at each computed place position and publishes the result.
27+
*
28+
* | Data Port Name | Port Type | Object Type |
29+
* | ----------------------- | --------- | --------------------------------------------------- |
30+
* | detections | input | moveit_studio_vision_msgs::msg::ObjectDetectionArray |
31+
* | camera_info | input | sensor_msgs::msg::CameraInfo |
32+
* | input_image | input | sensor_msgs::msg::Image (optional) |
33+
* | tray_apriltag_id | input | int |
34+
* | num_rows | input | int |
35+
* | columns_per_row | input | int |
36+
* | row_spacing | input | double |
37+
* | column_spacing | input | double |
38+
* | tag_to_tray_center | input | double |
39+
* | bottle_diameter | input | double |
40+
* | visualization_topic | input | std::string |
41+
* | place_positions | output | std::vector<geometry_msgs::msg::PoseStamped> |
42+
*/
43+
class ComputeTrayPlacePositionsUsingAprilTags final
44+
: public moveit_pro::behaviors::SharedResourcesNode<BT::SyncActionNode>
45+
{
46+
public:
47+
ComputeTrayPlacePositionsUsingAprilTags(
48+
const std::string& name, const BT::NodeConfiguration& config,
49+
const std::shared_ptr<moveit_pro::behaviors::BehaviorContext>& shared_resources,
50+
std::unique_ptr<moveit_pro::behaviors::ROSPublisherHandle> ros_publisher_interface =
51+
std::make_unique<moveit_pro::behaviors::ROSPublisherHandle>());
52+
53+
static BT::PortsList providedPorts();
54+
55+
static BT::KeyValueVector metadata();
56+
57+
BT::NodeStatus tick() override;
58+
59+
private:
60+
std::unique_ptr<moveit_pro::behaviors::ROSPublisherHandle> ros_publisher_interface_;
61+
};
62+
} // namespace lab_sim_behaviors

‎src/lab_sim_behaviors/lab_sim_behaviors_plugin_description.xml‎

Lines changed: 7 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<library path="lab_sim_behaviors">
3+
<class
4+
type="lab_sim_behaviors::LabSimBehaviorsLoader"
5+
base_class_type="moveit_pro::behaviors::SharedResourcesNodeLoaderBase"
6+
/>
7+
</library>

‎src/lab_sim_behaviors/package.xml‎

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<package format="3">
3+
<name>lab_sim_behaviors</name>
4+
<version>6.5.0</version>
5+
<description>Lab sim specific behavior plugins for MoveIt Pro</description>
6+
7+
<maintainer email="support@picknik.ai">MoveIt Pro Maintainer</maintainer>
8+
<author email="support@picknik.ai">MoveIt Pro Maintainer</author>
9+
10+
<license>BSD-3-Clause</license>
11+
12+
<buildtool_depend>ament_cmake</buildtool_depend>
13+
14+
<build_depend>moveit_studio_common</build_depend>
15+
16+
<depend>moveit_pro_behavior</depend>
17+
<depend>moveit_pro_behavior_interface</depend>
18+
<depend>moveit_studio_vision</depend>
19+
<depend>moveit_studio_vision_msgs</depend>
20+
<depend>pluginlib</depend>
21+
22+
<test_depend>ament_lint_auto</test_depend>
23+
<test_depend>ament_cmake_gtest</test_depend>
24+
<test_depend>ament_clang_format</test_depend>
25+
<test_depend>ament_clang_tidy</test_depend>
26+
27+
<export>
28+
<build_type>ament_cmake</build_type>
29+
</export>
30+
</package>

0 commit comments

Comments
 (0)