Skip to content

Commit 3ff1db7

Browse files
MikeWrockMikeWrock
committed
Merge commit '223d75d81b631439cd1200139786061f3f66a8ef' as 'src/picknik_registration'
2 parents 393838e + 223d75d commit 3ff1db7

File tree

12 files changed

+668
-0
lines changed

12 files changed

+668
-0
lines changed

‎src/picknik_registration/CMakeLists.txt‎

Lines changed: 61 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,61 @@
1+
cmake_minimum_required(VERSION 3.22)
2+
project(picknik_registration CXX)
3+
4+
find_package(moveit_studio_common REQUIRED)
5+
moveit_studio_package()
6+
7+
find_package(moveit_studio_behavior_interface REQUIRED)
8+
find_package(pluginlib REQUIRED)
9+
10+
set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib
11+
moveit_studio_vision
12+
moveit_studio_vision_msgs
13+
PCL
14+
pcl_conversions
15+
pcl_ros
16+
moveit_ros_planning_interface
17+
geometric_shapes
18+
shape_msgs)
19+
foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
20+
find_package(${package} REQUIRED)
21+
endforeach()
22+
23+
add_library(
24+
picknik_registration
25+
SHARED
26+
src/ndt_registration.cpp
27+
src/ransac_registration.cpp
28+
src/register_behaviors.cpp)
29+
target_include_directories(
30+
picknik_registration
31+
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
32+
$<INSTALL_INTERFACE:include>
33+
PRIVATE ${PCL_INCLUDE_DIRS})
34+
ament_target_dependencies(picknik_registration
35+
${THIS_PACKAGE_INCLUDE_DEPENDS})
36+
37+
# Install Libraries
38+
install(
39+
TARGETS picknik_registration
40+
EXPORT picknik_registrationTargets
41+
ARCHIVE DESTINATION lib
42+
LIBRARY DESTINATION lib
43+
RUNTIME DESTINATION bin
44+
INCLUDES
45+
DESTINATION include)
46+
47+
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
48+
49+
if(BUILD_TESTING)
50+
moveit_pro_behavior_test(picknik_registration)
51+
endif()
52+
53+
# Export the behavior plugins defined in this package so they are available to
54+
# plugin loaders that load the behavior base class library from the
55+
# moveit_studio_behavior package.
56+
pluginlib_export_plugin_description_file(
57+
moveit_studio_behavior_interface picknik_registration_plugin_description.xml)
58+
59+
ament_export_targets(picknik_registrationTargets HAS_LIBRARY_TARGET)
60+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
61+
ament_package()

‎src/picknik_registration/behavior_plugin.yaml‎

Lines changed: 4 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
objectives:
2+
behavior_loader_plugins:
3+
picknik_registration:
4+
- "picknik_registration::PicknikRegistrationBehaviorsLoader"

‎src/picknik_registration/config/tree_nodes_model.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
<root>
2+
<TreeNodesModel>
3+
<!-- Include additional SubTree metadata in this file. -->
4+
</TreeNodesModel>
5+
</root>

‎src/picknik_registration/include/picknik_registration/ndt_registration.hpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
#pragma once
2+
3+
#include <behaviortree_cpp/action_node.h>
4+
5+
// This header includes the SharedResourcesNode type
6+
#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
7+
#include <moveit_studio_behavior_interface/async_behavior_base.hpp>
8+
9+
10+
namespace picknik_registration
11+
{
12+
/**
13+
* @brief TODO(...)
14+
*/
15+
class NDTRegistration : public moveit_studio::behaviors::AsyncBehaviorBase
16+
{
17+
public:
18+
/**
19+
* @brief Constructor for the picknik_registration behavior.
20+
* @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree.
21+
* @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
22+
* @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree.
23+
* @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor.
24+
*/
25+
NDTRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
26+
27+
/**
28+
* @brief Implementation of the required providedPorts() function for the picknik_registration Behavior.
29+
* @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList.
30+
* This function returns a list of ports with their names and port info, which is used internally by the behavior tree.
31+
* @return picknik_registration does not use expose any ports, so this function returns an empty list.
32+
*/
33+
static BT::PortsList providedPorts();
34+
35+
/**
36+
* @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
37+
* subcategory, in the MoveIt Studio Developer Tool.
38+
* @return A BT::KeyValueVector containing the Behavior metadata.
39+
*/
40+
static BT::KeyValueVector metadata();
41+
42+
protected:
43+
tl::expected<bool, std::string> doWork() override;
44+
45+
private:
46+
/** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
47+
std::shared_future<tl::expected<bool, std::string>>& getFuture() override
48+
{
49+
return future_;
50+
}
51+
52+
/** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
53+
std::shared_future<tl::expected<bool, std::string>> future_;
54+
};
55+
} // namespace picknik_registration

‎src/picknik_registration/include/picknik_registration/ransac_registration.hpp‎

Lines changed: 55 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,55 @@
1+
#pragma once
2+
3+
#include <behaviortree_cpp/action_node.h>
4+
5+
// This header includes the SharedResourcesNode type
6+
#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
7+
#include <moveit_studio_behavior_interface/async_behavior_base.hpp>
8+
9+
10+
namespace picknik_registration
11+
{
12+
/**
13+
* @brief TODO(...)
14+
*/
15+
class RANSACRegistration : public moveit_studio::behaviors::AsyncBehaviorBase
16+
{
17+
public:
18+
/**
19+
* @brief Constructor for the picknik_registration behavior.
20+
* @param name The name of a particular instance of this Behavior. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree.
21+
* @param shared_resources A shared_ptr to a BehaviorContext that is shared among all SharedResourcesNode Behaviors in the behavior tree. This BehaviorContext is owned by the Studio Agent's ObjectiveServerNode.
22+
* @param config This contains runtime configuration info for this Behavior, such as the mapping between the Behavior's data ports on the behavior tree's blackboard. This will be set by the behavior tree factory when this Behavior is created within a new behavior tree.
23+
* @details An important limitation is that the members of the base Behavior class are not instantiated until after the initialize() function is called, so these classes should not be used within the constructor.
24+
*/
25+
RANSACRegistration(const std::string& name, const BT::NodeConfiguration& config, const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
26+
27+
/**
28+
* @brief Implementation of the required providedPorts() function for the picknik_registration Behavior.
29+
* @details The BehaviorTree.CPP library requires that Behaviors must implement a static function named providedPorts() which defines their input and output ports. If the Behavior does not use any ports, this function must return an empty BT::PortsList.
30+
* This function returns a list of ports with their names and port info, which is used internally by the behavior tree.
31+
* @return picknik_registration does not use expose any ports, so this function returns an empty list.
32+
*/
33+
static BT::PortsList providedPorts();
34+
35+
/**
36+
* @brief Implementation of the metadata() function for displaying metadata, such as Behavior description and
37+
* subcategory, in the MoveIt Studio Developer Tool.
38+
* @return A BT::KeyValueVector containing the Behavior metadata.
39+
*/
40+
static BT::KeyValueVector metadata();
41+
42+
protected:
43+
tl::expected<bool, std::string> doWork() override;
44+
45+
private:
46+
/** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
47+
std::shared_future<tl::expected<bool, std::string>>& getFuture() override
48+
{
49+
return future_;
50+
}
51+
52+
/** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
53+
std::shared_future<tl::expected<bool, std::string>> future_;
54+
};
55+
} // namespace picknik_registration

‎src/picknik_registration/package.xml‎

Lines changed: 29 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,29 @@
1+
<?xml version="1.0"?>
2+
<package format="3">
3+
<name>picknik_registration</name>
4+
<version>0.0.0</version>
5+
<description>Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algoritm</description>
6+
7+
<maintainer email="[email protected]">MoveIt Pro Maintainer</maintainer>
8+
9+
<license>BSD-3-Clause</license>
10+
11+
<buildtool_depend>ament_cmake</buildtool_depend>
12+
13+
<build_depend>moveit_studio_common</build_depend>
14+
15+
<depend>moveit_studio_behavior_interface</depend>
16+
<depend>perception_pcl</depend>
17+
<depend>moveit_ros_planning_interface</depend>
18+
<depend>moveit_studio_vision_msgs</depend>
19+
<depend>moveit_studio_vision</depend>
20+
21+
<test_depend>ament_lint_auto</test_depend>
22+
<test_depend>ament_cmake_gtest</test_depend>
23+
<test_depend>ament_clang_format</test_depend>
24+
<test_depend>ament_clang_tidy</test_depend>
25+
26+
<export>
27+
<build_type>ament_cmake</build_type>
28+
</export>
29+
</package>

‎src/picknik_registration/picknik_registration_plugin_description.xml‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,5 @@
1+
<library path="picknik_registration">
2+
<class type="picknik_registration::PicknikRegistrationBehaviorsLoader"
3+
base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase">
4+
</class>
5+
</library>

‎src/picknik_registration/src/ndt_registration.cpp‎

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
#include <picknik_registration/ndt_registration.hpp>
2+
#include <pcl/registration/ndt.h>
3+
#include <tl_expected/expected.hpp>
4+
#include <moveit_studio_behavior_interface/async_behavior_base.hpp>
5+
#include <moveit_studio_behavior_interface/check_for_error.hpp>
6+
// #include <moveit_studio_vision/geometry_types.hpp>
7+
#include <geometry_msgs/msg/pose_stamped.hpp>
8+
#include <sensor_msgs/msg/point_cloud2.hpp>
9+
#include <std_msgs/msg/header.hpp>
10+
#include <tf2_eigen/tf2_eigen.hpp>
11+
#include <tl_expected/expected.hpp>
12+
#include <moveit_studio_vision_msgs/msg/mask3_d.hpp>
13+
#include <pcl_conversions/pcl_conversions.h>
14+
#include <moveit_studio_vision/common/get_all_indices.hpp>
15+
#include <moveit_studio_vision/common/select_point_indices.hpp>
16+
#include <moveit_studio_vision/pointcloud/point_cloud_tools.hpp>
17+
#include <pcl/filters/approximate_voxel_grid.h>
18+
#include <pcl/point_types.h>
19+
#include <pcl/point_cloud.h>
20+
#include <pcl/features/normal_3d.h>
21+
#include <pcl/features/fpfh.h>
22+
#include <pcl/keypoints/uniform_sampling.h>
23+
#include <pcl/registration/sample_consensus_prerejective.h>
24+
#include <pcl/search/kdtree.h>
25+
26+
namespace picknik_registration
27+
{
28+
namespace
29+
{
30+
31+
std::shared_ptr<pcl::PointCloud<pcl::PointXYZRGB>>
32+
getFilteredPointCloudFromMessage(const sensor_msgs::msg::PointCloud2& cloud_msg)
33+
{
34+
const auto cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
35+
pcl::fromROSMsg(cloud_msg, *cloud);
36+
37+
const pcl::PointIndices valid_indices = moveit_studio::selectPointIndices(*cloud, moveit_studio::point_cloud_tools::getAllIndices(cloud),
38+
moveit_studio::NeitherNanNorNearZeroPointValidator<pcl::PointXYZRGB>);
39+
auto filtered_cloud = std::make_shared<pcl::PointCloud<pcl::PointXYZRGB>>();
40+
pcl::copyPointCloud(*cloud, valid_indices, *filtered_cloud);
41+
42+
return filtered_cloud;
43+
}
44+
45+
} // namespace
46+
47+
NDTRegistration::NDTRegistration(
48+
const std::string& name, const BT::NodeConfiguration& config,
49+
const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
50+
: moveit_studio::behaviors::AsyncBehaviorBase(name, config, shared_resources)
51+
{
52+
}
53+
54+
55+
BT::PortsList NDTRegistration::providedPorts()
56+
{
57+
return { BT::InputPort<sensor_msgs::msg::PointCloud2>("base_point_cloud", "{point_cloud}",
58+
"Point cloud to align with the target point cloud."),
59+
BT::InputPort<sensor_msgs::msg::PointCloud2>("target_point_cloud", "{target_point_cloud}",
60+
"Point cloud to which align the base point cloud."),
61+
BT::InputPort<int>("max_iterations", 30,
62+
"Maximum number of attempts to find the transform. Setting a higher number of iterations "
63+
"will allow the solver to converge even if the initial estimate of the transform was far "
64+
"from the actual transform, but it may take longer to complete."),
65+
BT::InputPort<double>("transformation_epsilon", 0.001,
66+
"Minimum transformation difference for termination condition <double>"),
67+
BT::InputPort<double>("step_size", 0.1,
68+
"Maximum step size for More-Thuente line search <double>"),
69+
BT::InputPort<double>("resolution", 1.0,
70+
"Resolution of NDT grid structure (VoxelGridCovariance) <double>"),
71+
BT::InputPort<double>("inlier_threshold", 1.0,
72+
"Resolution of NDT grid structure (VoxelGridCovariance) <double>"),
73+
BT::OutputPort<geometry_msgs::msg::PoseStamped>("target_pose_in_base_frame", "{target_pose}",
74+
"The pose of the target point cloud relative to the frame "
75+
"of the base point cloud.") };
76+
}
77+
BT::KeyValueVector NDTRegistration::metadata()
78+
{
79+
return { {"description", "Finds the pose of a target point cloud relative to the base frame of a base point cloud using the Normal Distributions Transform (NDT) algorithm"} };
80+
}
81+
82+
83+
tl::expected<bool, std::string> NDTRegistration::doWork()
84+
{
85+
const auto base_point_cloud_msg = getInput<sensor_msgs::msg::PointCloud2>("base_point_cloud");
86+
const auto target_point_cloud_msg = getInput<sensor_msgs::msg::PointCloud2>("target_point_cloud");
87+
const auto max_iterations = getInput<int>("max_iterations");
88+
const auto transformation_epsilon = getInput<double>("transformation_epsilon");
89+
const auto step_size = getInput<double>("step_size");
90+
const auto resolution = getInput<double>("resolution");
91+
92+
if (const auto error =
93+
moveit_studio::behaviors::maybe_error(base_point_cloud_msg, target_point_cloud_msg, max_iterations, transformation_epsilon, step_size, resolution);
94+
error)
95+
{
96+
return tl::make_unexpected("Failed to get required value from input data port: " + error.value());
97+
}
98+
99+
const auto base_cloud = getFilteredPointCloudFromMessage(base_point_cloud_msg.value());
100+
if (base_cloud->empty())
101+
{
102+
return tl::make_unexpected("Base point cloud has no valid points");
103+
}
104+
const auto target_cloud = getFilteredPointCloudFromMessage(target_point_cloud_msg.value());
105+
if (target_cloud->empty())
106+
{
107+
return tl::make_unexpected("Target point cloud has no valid points");
108+
}
109+
// Initializing Normal Distributions Transform (NDT).
110+
pcl::NormalDistributionsTransform<pcl::PointXYZRGB, pcl::PointXYZRGB> ndt;
111+
112+
// Setting scale dependent NDT parameters
113+
// Setting minimum transformation difference for termination condition.
114+
ndt.setTransformationEpsilon (transformation_epsilon.value());
115+
// Setting maximum step size for More-Thuente line search.
116+
ndt.setStepSize (step_size.value());
117+
//Setting Resolution of NDT grid structure (VoxelGridCovariance).
118+
ndt.setResolution (resolution.value());
119+
120+
// Setting max number of registration iterations.
121+
ndt.setMaximumIterations (max_iterations.value());
122+
123+
// Setting point cloud to be aligned.
124+
ndt.setInputSource (base_cloud);
125+
// Setting point cloud to be aligned to.
126+
ndt.setInputTarget (target_cloud);
127+
128+
129+
// Calculating required rigid transform to align the input cloud to the target cloud.
130+
pcl::PointCloud<pcl::PointXYZRGB>::Ptr output_cloud (new pcl::PointCloud<pcl::PointXYZRGB>);
131+
ndt.align (*output_cloud);
132+
133+
if (!ndt.hasConverged())
134+
{
135+
return tl::make_unexpected("NDT could not converge to a transform that aligns both point clouds");
136+
}
137+
138+
geometry_msgs::msg::PoseStamped out;
139+
out.pose = tf2::toMsg(Eigen::Isometry3d{ ndt.getFinalTransformation().cast<double>() });
140+
out.header.frame_id = base_point_cloud_msg.value().header.frame_id;
141+
out.header.stamp = base_point_cloud_msg.value().header.stamp;
142+
setOutput("target_pose_in_base_frame", out);
143+
144+
return true;
145+
}
146+
147+
148+
} // namespace picknik_registration

0 commit comments

Comments
 (0)