Skip to content

Commit 10073a3

Browse files
author
Jonathan Cacace
committed
removed not bt nodes from the plugin list
1 parent 19e957d commit 10073a3

3 files changed

Lines changed: 398 additions & 23 deletions

File tree

‎behaviortree_ros2/CMakeLists.txt‎

Lines changed: 1 addition & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -55,7 +55,7 @@ include_directories(${behaviortree_cpp_INCLUDE_DIRS} PRIVATE ${CMAKE_CURRENT_SOU
5555
ament_target_dependencies(bt_ros_plugins_to_xml behaviortree_cpp rclcpp)
5656

5757
# Build btros2 plugin library
58-
add_library(${PROJECT_NAME}_nodemodels SHARED src/plugin.cpp)
58+
add_library(${PROJECT_NAME}_nodemodels SHARED src/plugin.cpp) # src/navigation.cpp
5959
target_include_directories(${PROJECT_NAME}_nodemodels PRIVATE
6060
${CMAKE_CURRENT_SOURCE_DIR}/include
6161
)

‎behaviortree_ros2/src/navigation.cpp‎

Lines changed: 215 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,215 @@
1+
#include "behaviortree_ros2/behaviortree_ros2.hpp"
2+
3+
//#include "behaviortree_ros2/actions/ros_log.hpp"
4+
//#include "behaviortree_ros2/actions/GetTransformAnglesNode.hpp"
5+
//#include "behaviortree_ros2/actions/GetTransformDistanceNode.hpp"
6+
//#include "behaviortree_ros2/actions/GetTransformHorizontalDistance.hpp"
7+
//#include "behaviortree_ros2/actions/LookupTransformNode.hpp"
8+
9+
#include <std_msgs/msg/empty.hpp>
10+
#include <std_msgs/msg/bool.hpp>
11+
#include <std_msgs/msg/int8.hpp>
12+
#include <std_msgs/msg/u_int8.hpp>
13+
#include <std_msgs/msg/int16.hpp>
14+
#include <std_msgs/msg/u_int16.hpp>
15+
#include <std_msgs/msg/int32.hpp>
16+
#include <std_msgs/msg/u_int32.hpp>
17+
#include <std_msgs/msg/int64.hpp>
18+
#include <std_msgs/msg/u_int64.hpp>
19+
#include <std_msgs/msg/float32.hpp>
20+
#include <std_msgs/msg/float64.hpp>
21+
#include <std_msgs/msg/string.hpp>
22+
#include <geometry_msgs/msg/pose_stamped.hpp>
23+
24+
#include <std_srvs/srv/empty.hpp>
25+
#include <std_srvs/srv/set_bool.hpp>
26+
#include <std_srvs/srv/trigger.hpp>
27+
28+
//TEST
29+
#include <std_msgs/msg/int8_multi_array.hpp>
30+
#include "btcpp_ros2_interfaces/action/sleep.hpp"
31+
#include "btcpp_ros2_interfaces/action/execute_tree.hpp"
32+
#include "btcpp_ros2_interfaces/msg/custom_msg.hpp"
33+
34+
35+
36+
// Custom ROS2-related nodes creation (todo: put into a separate library)
37+
class CheckPoseReached : public BT::StatefulActionNode
38+
{
39+
public:
40+
CheckPoseReached(const std::string& name, const BT::NodeConfig& config)
41+
: BT::StatefulActionNode(name, config) {}
42+
43+
static BT::PortsList providedPorts()
44+
{
45+
return {
46+
// Target pose
47+
BT::InputPort<double>("d_x"),
48+
BT::InputPort<double>("d_y"),
49+
BT::InputPort<double>("d_z"),
50+
BT::InputPort<double>("d_qx"),
51+
BT::InputPort<double>("d_qy"),
52+
BT::InputPort<double>("d_qz"),
53+
BT::InputPort<double>("d_qw"),
54+
// Current pose
55+
BT::InputPort<double>("x"),
56+
BT::InputPort<double>("y"),
57+
BT::InputPort<double>("z"),
58+
BT::InputPort<double>("qx"),
59+
BT::InputPort<double>("qy"),
60+
BT::InputPort<double>("qz"),
61+
BT::InputPort<double>("qw"),
62+
// Thresholds
63+
BT::InputPort<double>("pos_tolerance", 0.05, "Position tolerance in meters"),
64+
BT::InputPort<double>("ang_tolerance", 0.1, "Orientation tolerance in radians")
65+
};
66+
}
67+
68+
BT::NodeStatus onStart() override
69+
{
70+
// Read all inputs
71+
if (!getPoseInputs(target_, "d_x", "d_y", "d_z", "d_qx", "d_qy", "d_qz", "d_qw")) {
72+
std::cerr << "[CheckPoseReached] Missing target pose input!" << std::endl;
73+
return BT::NodeStatus::FAILURE;
74+
}
75+
76+
if (!getPoseInputs(current_, "x", "y", "z",
77+
"qx", "qy", "qz", "qw")) {
78+
std::cerr << "[CheckPoseReached] Missing current pose input!" << std::endl;
79+
return BT::NodeStatus::FAILURE;
80+
}
81+
82+
getInput("pos_tolerance", pos_tol_);
83+
getInput("ang_tolerance", ang_tol_);
84+
85+
std::cout << "[CheckPoseReached] Checking if pose is reached..." << std::endl;
86+
return BT::NodeStatus::RUNNING;
87+
}
88+
89+
90+
91+
92+
BT::NodeStatus onRunning() override
93+
{
94+
// Compute position difference
95+
double dx = target_.x - current_.x;
96+
double dy = target_.y - current_.y;
97+
double dz = target_.z - current_.z;
98+
double dist = std::sqrt(dx*dx + dy*dy + dz*dz);
99+
100+
// Compute orientation difference (using quaternion dot product)
101+
double dot = target_.qx * current_.qx + target_.qy * current_.qy +
102+
target_.qz * current_.qz + target_.qw * current_.qw;
103+
double angle_diff = 2 * std::acos(std::abs(dot));
104+
105+
if (dist < pos_tol_ && angle_diff < ang_tol_)
106+
{
107+
std::cout << "[CheckPoseReached] Target reached! dist=" << dist
108+
<< ", angle=" << angle_diff << std::endl;
109+
return BT::NodeStatus::SUCCESS;
110+
}
111+
else
112+
{
113+
std::cout << "[CheckPoseReached] Not yet reached. dist=" << dist
114+
<< ", angle=" << angle_diff << std::endl;
115+
return BT::NodeStatus::FAILURE;
116+
}
117+
}
118+
119+
void onHalted() override
120+
{
121+
std::cout << "[CheckPoseReached] Halted." << std::endl;
122+
}
123+
124+
private:
125+
struct Pose {
126+
double x, y, z;
127+
double qx, qy, qz, qw;
128+
} target_, current_;
129+
130+
double pos_tol_ = 0.05;
131+
double ang_tol_ = 0.1;
132+
133+
bool getPoseInputs(Pose& pose,
134+
const std::string& x_key, const std::string& y_key, const std::string& z_key,
135+
const std::string& qx_key, const std::string& qy_key,
136+
const std::string& qz_key, const std::string& qw_key)
137+
{
138+
return getInput(x_key, pose.x) &&
139+
getInput(y_key, pose.y) &&
140+
getInput(z_key, pose.z) &&
141+
getInput(qx_key, pose.qx) &&
142+
getInput(qy_key, pose.qy) &&
143+
getInput(qz_key, pose.qz) &&
144+
getInput(qw_key, pose.qw);
145+
}
146+
};
147+
148+
149+
150+
151+
152+
class SetPoseGoal : public BT::SyncActionNode
153+
{
154+
public:
155+
SetPoseGoal(const std::string& name, const BT::NodeConfig& config)
156+
: BT::SyncActionNode(name, config) {}
157+
158+
static BT::PortsList providedPorts()
159+
{
160+
return {
161+
// Input pose parameters
162+
BT::InputPort<double>("x"),
163+
BT::InputPort<double>("y"),
164+
BT::InputPort<double>("z"),
165+
BT::InputPort<double>("qx"),
166+
BT::InputPort<double>("qy"),
167+
BT::InputPort<double>("qz"),
168+
BT::InputPort<double>("qw"),
169+
170+
// Output ports to blackboard
171+
BT::OutputPort<double>("d_x"),
172+
BT::OutputPort<double>("d_y"),
173+
BT::OutputPort<double>("d_z"),
174+
BT::OutputPort<double>("d_qx"),
175+
BT::OutputPort<double>("d_qy"),
176+
BT::OutputPort<double>("d_qz"),
177+
BT::OutputPort<double>("d_qw")
178+
};
179+
}
180+
181+
BT::NodeStatus tick() override
182+
{
183+
double x, y, z, qx, qy, qz, qw;
184+
185+
if (!getInput("x", x) || !getInput("y", y) || !getInput("z", z) ||
186+
!getInput("qx", qx) || !getInput("qy", qy) ||
187+
!getInput("qz", qz) || !getInput("qw", qw))
188+
{
189+
std::cerr << "[SetPoseGoal] Missing one or more input values!" << std::endl;
190+
return BT::NodeStatus::FAILURE;
191+
}
192+
193+
// Set outputs on the blackboard
194+
setOutput("d_x", x);
195+
setOutput("d_y", y);
196+
setOutput("d_z", z);
197+
setOutput("d_qx", qx);
198+
setOutput("d_qy", qy);
199+
setOutput("d_qz", qz);
200+
setOutput("d_qw", qw);
201+
202+
return BT::NodeStatus::SUCCESS;
203+
}
204+
};
205+
206+
using namespace BT;
207+
208+
/*
209+
BT_REGISTER_ROS_NODES(factory, params)
210+
{
211+
212+
factory.registerNodeType<SetPoseGoal>("SetPoseGoal");
213+
factory.registerNodeType<CheckPoseReached>("CheckPoseReached");
214+
215+
};*/

0 commit comments

Comments
 (0)