Skip to content

Commit b7410b4

Browse files
sjahrsjahr
committed
Add GetPoseStampedFromTopic behavior
1 parent 59ce4be commit b7410b4

9 files changed

+238
-0
lines changed

‎CMakeLists.txt

+46
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,46 @@
1+
cmake_minimum_required(VERSION 3.22)
2+
project(experimental_behaviors CXX)
3+
4+
find_package(moveit_studio_common REQUIRED)
5+
moveit_studio_package()
6+
7+
set(THIS_PACKAGE_INCLUDE_DEPENDS geometry_msgs moveit_studio_behavior_interface pluginlib moveit_studio_common behaviortree_cpp tl_expected)
8+
foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
9+
find_package(${package} REQUIRED)
10+
endforeach()
11+
12+
add_library(
13+
experimental_behaviors
14+
SHARED
15+
src/get_pose_stamped_from_topic.cpp
16+
src/register_behaviors.cpp)
17+
target_include_directories(
18+
experimental_behaviors
19+
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
20+
$<INSTALL_INTERFACE:include>)
21+
ament_target_dependencies(experimental_behaviors
22+
${THIS_PACKAGE_INCLUDE_DEPENDS})
23+
24+
# Install Libraries
25+
install(
26+
TARGETS experimental_behaviors
27+
EXPORT experimental_behaviorsTargets
28+
ARCHIVE DESTINATION lib
29+
LIBRARY DESTINATION lib
30+
RUNTIME DESTINATION bin
31+
INCLUDES
32+
DESTINATION include)
33+
34+
if(BUILD_TESTING)
35+
moveit_pro_behavior_test(experimental_behaviors)
36+
endif()
37+
38+
# Export the behavior plugins defined in this package so they are available to
39+
# plugin loaders that load the behavior base class library from the
40+
# moveit_studio_behavior package.
41+
pluginlib_export_plugin_description_file(
42+
moveit_studio_behavior_interface experimental_behaviors_plugin_description.xml)
43+
44+
ament_export_targets(experimental_behaviorsTargets HAS_LIBRARY_TARGET)
45+
ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS})
46+
ament_package()

‎config/tree_nodes_model.xml

+6
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<root>
3+
<TreeNodesModel>
4+
<!-- Include additional SubTree metadata in this file. -->
5+
</TreeNodesModel>
6+
</root>

‎experimental_behaviors_plugin_description.xml

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<library path="experimental_behaviors">
3+
<class
4+
type="experimental_behaviors::ExperimentalBehaviorsLoader"
5+
base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"
6+
/>
7+
</library>

‎include/experimental_behaviors/get_pose_stamped_from_topic.hpp

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
#pragma once
2+
3+
#include <geometry_msgs/msg/pose_stamped.hpp>
4+
#include <moveit_studio_behavior_interface/get_message_from_topic.hpp>
5+
6+
namespace experimental_behaviors
7+
{
8+
/**
9+
* @brief Subscribe to PoseStamped message and write received message to the blackboard.
10+
*/
11+
class GetPoseStampedFromTopic final
12+
: public moveit_studio::behaviors::GetMessageFromTopicBehaviorBase<geometry_msgs::msg::PoseStamped>
13+
{
14+
public:
15+
GetPoseStampedFromTopic(const std::string& name, const BT::NodeConfiguration& config,
16+
const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
17+
18+
static BT::PortsList providedPorts();
19+
tl::expected<std::chrono::duration<double>, std::string> getWaitForMessageTimeout() override;
20+
21+
private:
22+
/** @brief Classes derived from AsyncBehaviorBase must implement getFuture() so that it returns a shared_future class member */
23+
std::shared_future<tl::expected<bool, std::string>>& getFuture() override
24+
{
25+
return future_;
26+
}
27+
28+
/** @brief Classes derived from AsyncBehaviorBase must have this shared_future as a class member */
29+
std::shared_future<tl::expected<bool, std::string>> future_;
30+
};
31+
} // namespace experimental_behaviors

‎package.xml

+31
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,31 @@
1+
<?xml version="1.0" encoding="utf-8" ?>
2+
<package format="3">
3+
<name>experimental_behaviors</name>
4+
<version>8.0.0</version>
5+
<description>Example behaviors 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>geometry_msgs</depend>
17+
<depend>moveit_studio_behavior_interface</depend>
18+
<depend>pluginlib</depend>
19+
<depend>moveit_studio_common</depend>
20+
<depend>behaviortree_cpp</depend>
21+
<depend>tl_expected</depend>
22+
23+
<test_depend>ament_lint_auto</test_depend>
24+
<test_depend>ament_cmake_gtest</test_depend>
25+
<test_depend>ament_clang_format</test_depend>
26+
<test_depend>ament_clang_tidy</test_depend>
27+
28+
<export>
29+
<build_type>ament_cmake</build_type>
30+
</export>
31+
</package>

‎src/get_pose_stamped_from_topic.cpp

+51
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,51 @@
1+
#include "experimental_behaviors/get_pose_stamped_from_topic.hpp"
2+
3+
#include <moveit_studio_behavior_interface/get_required_ports.hpp>
4+
#include <moveit_studio_behavior_interface/impl/get_message_from_topic_impl.hpp>
5+
6+
namespace experimental_behaviors
7+
{
8+
9+
namespace
10+
{
11+
constexpr auto kPortIDTimeoutDuration = "timeout_sec";
12+
} // namespace
13+
14+
GetPoseStampedFromTopic::GetPoseStampedFromTopic(
15+
const std::string& name, const BT::NodeConfiguration& config,
16+
const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources)
17+
: moveit_studio::behaviors::GetMessageFromTopicBehaviorBase<geometry_msgs::msg::PoseStamped>(name, config,
18+
shared_resources)
19+
{
20+
}
21+
22+
BT::PortsList GetPoseStampedFromTopic::providedPorts()
23+
{
24+
return BT::PortsList({
25+
BT::InputPort<std::string>(kPortIDTopicName, "", "PoseStamped topic name."),
26+
BT::InputPort<double>(kPortIDTimeoutDuration, -1.0,
27+
"Maximum duration in seconds to wait for pose message to be published before "
28+
"failing. Set to -1.0 to wait indefinitely."),
29+
BT::OutputPort<geometry_msgs::msg::PoseStamped>(kPortIDMessageOut, "PoseStamped message."),
30+
});
31+
}
32+
33+
tl::expected<std::chrono::duration<double>, std::string> GetPoseStampedFromTopic::getWaitForMessageTimeout()
34+
{
35+
const auto port = moveit_studio::behaviors::getRequiredInputs(getInput<double>(kPortIDTimeoutDuration));
36+
37+
// Check that input data ports were set.
38+
if (!port.has_value())
39+
{
40+
return tl::make_unexpected("Failed to get required value from input data port: " + port.error());
41+
}
42+
const auto& [timeout] = port.value();
43+
if (timeout == 0.0)
44+
{
45+
return tl::make_unexpected("Timeout value is 0.0s.");
46+
}
47+
return std::chrono::duration<double>{ timeout };
48+
}
49+
} // namespace experimental_behaviors
50+
51+
template class moveit_studio::behaviors::GetMessageFromTopicBehaviorBase<geometry_msgs::msg::PoseStamped>;

‎src/register_behaviors.cpp

+25
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,25 @@
1+
#include <behaviortree_cpp/bt_factory.h>
2+
#include <moveit_studio_behavior_interface/behavior_context.hpp>
3+
#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
4+
5+
#include "experimental_behaviors/get_pose_stamped_from_topic.hpp"
6+
7+
#include <pluginlib/class_list_macros.hpp>
8+
9+
namespace experimental_behaviors
10+
{
11+
class ExperimentalBehaviorsLoader : public moveit_studio::behaviors::SharedResourcesNodeLoaderBase
12+
{
13+
public:
14+
void registerBehaviors(
15+
BT::BehaviorTreeFactory& factory,
16+
[[maybe_unused]] const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources) override
17+
{
18+
moveit_studio::behaviors::registerBehavior<GetPoseStampedFromTopic>(factory, "GetPoseStampedFromTopic",
19+
shared_resources);
20+
}
21+
};
22+
} // namespace experimental_behaviors
23+
24+
PLUGINLIB_EXPORT_CLASS(experimental_behaviors::ExperimentalBehaviorsLoader,
25+
moveit_studio::behaviors::SharedResourcesNodeLoaderBase);

‎test/CMakeLists.txt

+4
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,4 @@
1+
find_package(ament_cmake_gtest REQUIRED)
2+
3+
ament_add_gtest(test_behavior_plugins test_behavior_plugins.cpp)
4+
ament_target_dependencies(test_behavior_plugins ${THIS_PACKAGE_INCLUDE_DEPENDS})

‎test/test_behavior_plugins.cpp

+37
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,37 @@
1+
#include <gtest/gtest.h>
2+
3+
#include <behaviortree_cpp/bt_factory.h>
4+
#include <moveit_studio_behavior_interface/shared_resources_node_loader.hpp>
5+
#include <pluginlib/class_loader.hpp>
6+
#include <rclcpp/node.hpp>
7+
8+
/**
9+
* @brief This test makes sure that the Behaviors provided in this package can be successfully registered and
10+
* instantiated by the behavior tree factory.
11+
*/
12+
TEST(BehaviorTests, test_load_behavior_plugins)
13+
{
14+
pluginlib::ClassLoader<moveit_studio::behaviors::SharedResourcesNodeLoaderBase> class_loader(
15+
"moveit_studio_behavior_interface", "moveit_studio::behaviors::SharedResourcesNodeLoaderBase");
16+
17+
auto node = std::make_shared<rclcpp::Node>("BehaviorTests");
18+
auto shared_resources = std::make_shared<moveit_studio::behaviors::BehaviorContext>(node);
19+
20+
BT::BehaviorTreeFactory factory;
21+
{
22+
auto plugin_instance = class_loader.createUniqueInstance("experimental_behaviors::ExperimentalBehaviorsLoader");
23+
ASSERT_NO_THROW(plugin_instance->registerBehaviors(factory, shared_resources));
24+
}
25+
26+
// Test that ClassLoader is able to find and instantiate each behavior using the package's plugin description info.
27+
EXPECT_NO_THROW((void)factory.instantiateTreeNode("test_get_pose_stamped_from_topic", "GetPoseStampedFromTopic",
28+
BT::NodeConfiguration()));
29+
}
30+
31+
int main(int argc, char** argv)
32+
{
33+
rclcpp::init(argc, argv);
34+
35+
testing::InitGoogleTest(&argc, argv);
36+
return RUN_ALL_TESTS();
37+
}

0 commit comments

Comments
 (0)