Skip to content

Commit 2bc1b8e

Browse files
MikeWrockMikeWrock
committed
Squashed 'src/example_behaviors/' changes from d1451e3..b360dc0
b360dc0 Merge commit '7e8c6066177e2fd16ea46cfb16d8c207e17fd929' a71ffeb fixed build and test for example behaviors (#39) 72ad640 Sam naming (#38) 46a17ce Sync 6.5 to main (#37) d41f38d Add example SAM 2 behavior and objective (#23) f6ac548 Sync V6.5 to main (#29) e932e45 Merge branch 'main' into pr-update-to-make-ports-explicit 6a14c44 Fix depend of moveit_studio_behavior 1416547 version bump 5f6056a formatting a06f504 pre-commit formatting 5bfeaa5 Moving registration behaviors 93dd16f Added mtc to jtc converter git-subtree-dir: src/example_behaviors git-subtree-split: b360dc0970f6dbe4b633cb33edb9ef75042f00d7
1 parent d0dad84 commit 2bc1b8e

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

41 files changed

+1749
-136
lines changed

‎CMakeLists.txt‎

Lines changed: 25 additions & 12 deletions
Original file line numberDiff line numberDiff line change
@@ -5,30 +5,43 @@ find_package(moveit_studio_common REQUIRED)
55
find_package(example_interfaces REQUIRED)
66
moveit_studio_package()
77

8-
set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior_interface pluginlib example_interfaces)
8+
set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_studio_behavior moveit_studio_behavior_interface pluginlib
9+
moveit_studio_vision
10+
moveit_studio_vision_msgs
11+
PCL
12+
pcl_conversions
13+
pcl_ros
14+
example_interfaces)
915
foreach(package IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS})
1016
find_package(${package} REQUIRED)
1117
endforeach()
18+
find_package(moveit_pro_ml REQUIRED)
1219

1320
add_library(
1421
example_behaviors
1522
SHARED
16-
src/add_two_ints_service_client.cpp
17-
src/delayed_message.cpp
18-
src/get_string_from_topic.cpp
19-
src/fibonacci_action_client.cpp
20-
src/hello_world.cpp
21-
src/publish_color_rgba.cpp
22-
src/setup_mtc_pick_from_pose.cpp
23-
src/setup_mtc_place_from_pose.cpp
24-
src/setup_mtc_wave_hand.cpp
23+
src/example_add_two_ints_service_client.cpp
24+
src/example_convert_mtc_solution_to_joint_trajectory.cpp
25+
src/example_delayed_message.cpp
26+
src/example_get_string_from_topic.cpp
27+
src/example_fibonacci_action_client.cpp
28+
src/example_hello_world.cpp
29+
src/example_publish_color_rgba.cpp
30+
src/example_setup_mtc_pick_from_pose.cpp
31+
src/example_setup_mtc_place_from_pose.cpp
32+
src/example_setup_mtc_wave_hand.cpp
33+
src/example_ndt_registration.cpp
34+
src/example_ransac_registration.cpp
35+
src/example_sam2_segmentation.cpp
2536
src/register_behaviors.cpp)
2637
target_include_directories(
2738
example_behaviors
2839
PUBLIC $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
29-
$<INSTALL_INTERFACE:include>)
40+
$<INSTALL_INTERFACE:include>
41+
PRIVATE ${PCL_INCLUDE_DIRS})
3042
ament_target_dependencies(example_behaviors
3143
${THIS_PACKAGE_INCLUDE_DEPENDS})
44+
target_link_libraries(example_behaviors onnx_sam2)
3245

3346
# Install Libraries
3447
install(
@@ -40,7 +53,7 @@ install(
4053
INCLUDES
4154
DESTINATION include)
4255

43-
install(DIRECTORY config DESTINATION share/${PROJECT_NAME})
56+
install(DIRECTORY config models DESTINATION share/${PROJECT_NAME})
4457

4558
if(BUILD_TESTING)
4659
moveit_pro_behavior_test(example_behaviors)

‎COLCON_IGNORE‎

Whitespace-only changes.

‎behavior_plugin.yaml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,4 +1,4 @@
11
objectives:
22
behavior_loader_plugins:
33
example_behaviors:
4-
- "example_behaviors::ExampleBehaviorsBehaviorsLoader"
4+
- "example_behaviors::ExampleBehaviorsLoader"

‎example_behaviors_plugin_description.xml‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0" encoding="utf-8" ?>
22
<library path="example_behaviors">
33
<class
4-
type="example_behaviors::ExampleBehaviorsBehaviorsLoader"
4+
type="example_behaviors::ExampleBehaviorsLoader"
55
base_class_type="moveit_studio::behaviors::SharedResourcesNodeLoaderBase"
66
/>
77
</library>

‎include/example_behaviors/convert_mtc_solution_to_joint_trajectory.hpp‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
#pragma once
2+
3+
#include <Eigen/Core>
4+
#include <behaviortree_cpp/action_node.h>
5+
#include <behaviortree_cpp/bt_factory.h>
6+
#include <moveit/robot_model_loader/robot_model_loader.h>
7+
#include <moveit_msgs/msg/robot_trajectory.hpp>
8+
#include <moveit_studio_behavior/utils/trajectory_utils.hpp>
9+
#include <moveit_studio_behavior_interface/behavior_context.hpp>
10+
#include <moveit_studio_behavior_interface/check_for_error.hpp>
11+
#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
12+
#include <moveit_task_constructor_msgs/msg/solution.hpp>
13+
#include <spdlog/spdlog.h>
14+
#include <trajectory_msgs/msg/joint_trajectory.hpp>
15+
#include <yaml-cpp/yaml.h>
16+
17+
namespace example_behaviors
18+
{
19+
/**
20+
* @brief Converts a MoveIt Task Constructor Solution into a JointTrajectory.
21+
*
22+
* @details
23+
* | Data Port Name | Port Type | Object Type |
24+
* | ----------------- |---------------|---------------------------------------------------------|
25+
* | solution | Input | moveit_task_constructor_msgs::msg::Solution |
26+
* | joint_group | Input | std::string |
27+
* | velocity_scaling_factor | Input | double |
28+
* | acceleration_scaling_factor | Input | double |
29+
* | sampling_rate | Input | int |
30+
* | joint_trajectory | Output | trajectory_msgs::msg::JointTrajectory |
31+
*/
32+
class ConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
33+
{
34+
public:
35+
ConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config,
36+
const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
37+
38+
static BT::PortsList providedPorts();
39+
40+
static BT::KeyValueVector metadata();
41+
42+
BT::NodeStatus tick() override;
43+
44+
private:
45+
std::unique_ptr<robot_model_loader::RobotModelLoader> robot_model_loader_;
46+
47+
const std::vector<Eigen::VectorXd>& extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution);
48+
};
49+
} // namespace example_behaviors

include/example_behaviors/add_two_ints_service_client.hpp renamed to include/example_behaviors/example_add_two_ints_service_client.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,10 @@ using AddTwoInts = example_interfaces::srv::AddTwoInts;
99

1010
namespace example_behaviors
1111
{
12-
class AddTwoIntsServiceClient final : public ServiceClientBehaviorBase<AddTwoInts>
12+
class ExampleAddTwoIntsServiceClient final : public ServiceClientBehaviorBase<AddTwoInts>
1313
{
1414
public:
15-
AddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config,
15+
ExampleAddTwoIntsServiceClient(const std::string& name, const BT::NodeConfiguration& config,
1616
const std::shared_ptr<BehaviorContext>& shared_resources);
1717

1818
/** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */

‎include/example_behaviors/example_convert_mtc_solution_to_joint_trajectory.hpp‎

Lines changed: 49 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,49 @@
1+
#pragma once
2+
3+
#include <Eigen/Core>
4+
#include <behaviortree_cpp/action_node.h>
5+
#include <behaviortree_cpp/bt_factory.h>
6+
#include <moveit/robot_model_loader/robot_model_loader.h>
7+
#include <moveit_msgs/msg/robot_trajectory.hpp>
8+
#include <moveit_studio_behavior/utils/trajectory_utils.hpp>
9+
#include <moveit_studio_behavior_interface/behavior_context.hpp>
10+
#include <moveit_studio_behavior_interface/check_for_error.hpp>
11+
#include <moveit_studio_behavior_interface/shared_resources_node.hpp>
12+
#include <moveit_task_constructor_msgs/msg/solution.hpp>
13+
#include <spdlog/spdlog.h>
14+
#include <trajectory_msgs/msg/joint_trajectory.hpp>
15+
#include <yaml-cpp/yaml.h>
16+
17+
namespace example_behaviors
18+
{
19+
/**
20+
* @brief Converts a MoveIt Task Constructor Solution into a JointTrajectory.
21+
*
22+
* @details
23+
* | Data Port Name | Port Type | Object Type |
24+
* | ----------------- |---------------|---------------------------------------------------------|
25+
* | solution | Input | moveit_task_constructor_msgs::msg::Solution |
26+
* | joint_group | Input | std::string |
27+
* | velocity_scaling_factor | Input | double |
28+
* | acceleration_scaling_factor | Input | double |
29+
* | sampling_rate | Input | int |
30+
* | joint_trajectory | Output | trajectory_msgs::msg::JointTrajectory |
31+
*/
32+
class ExampleConvertMtcSolutionToJointTrajectory final : public moveit_studio::behaviors::SharedResourcesNode<BT::SyncActionNode>
33+
{
34+
public:
35+
ExampleConvertMtcSolutionToJointTrajectory(const std::string& name, const BT::NodeConfiguration& config,
36+
const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
37+
38+
static BT::PortsList providedPorts();
39+
40+
static BT::KeyValueVector metadata();
41+
42+
BT::NodeStatus tick() override;
43+
44+
private:
45+
std::unique_ptr<robot_model_loader::RobotModelLoader> robot_model_loader_;
46+
47+
const std::vector<Eigen::VectorXd>& extractJointPositions(const moveit_task_constructor_msgs::msg::Solution& solution);
48+
};
49+
} // namespace example_behaviors

include/example_behaviors/delayed_message.hpp renamed to include/example_behaviors/example_delayed_message.hpp

Lines changed: 3 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,9 +12,9 @@
1212
namespace example_behaviors
1313
{
1414
/**
15-
* @brief DelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on the input port
15+
* @brief ExampleDelayedMessage will use FailureLoggerROS to log a "Hello World" message after the duration specified on the input port
1616
*/
17-
class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode<BT::StatefulActionNode>
17+
class ExampleDelayedMessage : public moveit_studio::behaviors::SharedResourcesNode<BT::StatefulActionNode>
1818
{
1919
private:
2020
std::chrono::time_point<std::chrono::steady_clock> start_time_;
@@ -33,7 +33,7 @@ class DelayedMessage : public moveit_studio::behaviors::SharedResourcesNode<BT::
3333
* @details An important limitation is that the members of the base Behavior class are not instantiated until after
3434
* the initialize() function is called, so these classes should not be used within the constructor.
3535
*/
36-
DelayedMessage(const std::string& name, const BT::NodeConfiguration& config,
36+
ExampleDelayedMessage(const std::string& name, const BT::NodeConfiguration& config,
3737
const std::shared_ptr<moveit_studio::behaviors::BehaviorContext>& shared_resources);
3838

3939
/**

include/example_behaviors/fibonacci_action_client.hpp renamed to include/example_behaviors/example_fibonacci_action_client.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -9,10 +9,10 @@ using Fibonacci = example_interfaces::action::Fibonacci;
99

1010
namespace example_behaviors
1111
{
12-
class FibonacciActionClient final : public ActionClientBehaviorBase<Fibonacci>
12+
class ExampleFibonacciActionClient final : public ActionClientBehaviorBase<Fibonacci>
1313
{
1414
public:
15-
FibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config,
15+
ExampleFibonacciActionClient(const std::string& name, const BT::NodeConfiguration& config,
1616
const std::shared_ptr<BehaviorContext>& shared_resources);
1717

1818
/** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */

include/example_behaviors/get_string_from_topic.hpp renamed to include/example_behaviors/example_get_string_from_topic.hpp

Lines changed: 2 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -8,10 +8,10 @@ using moveit_studio::behaviors::GetMessageFromTopicBehaviorBase;
88

99
namespace example_behaviors
1010
{
11-
class GetStringFromTopic final : public GetMessageFromTopicBehaviorBase<std_msgs::msg::String>
11+
class ExampleGetStringFromTopic final : public GetMessageFromTopicBehaviorBase<std_msgs::msg::String>
1212
{
1313
public:
14-
GetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config,
14+
ExampleGetStringFromTopic(const std::string& name, const BT::NodeConfiguration& config,
1515
const std::shared_ptr<BehaviorContext>& shared_resources);
1616

1717
/** @brief Implementation of the required providedPorts() function for the hello_world Behavior. */

0 commit comments

Comments
 (0)