diff --git a/toid_bt/behavior_trees/behavior1.xml b/toid_bt/behavior_trees/behavior1.xml
index 11880ca..990968a 100644
--- a/toid_bt/behavior_trees/behavior1.xml
+++ b/toid_bt/behavior_trees/behavior1.xml
@@ -6,7 +6,7 @@
-
diff --git a/toid_bt/behavior_trees/calibration_routines.xml b/toid_bt/behavior_trees/calibration_routines.xml
index d7c4d79..b2085e1 100644
--- a/toid_bt/behavior_trees/calibration_routines.xml
+++ b/toid_bt/behavior_trees/calibration_routines.xml
@@ -47,7 +47,7 @@
-
+
-
-
- Action server name
+
+
+ Action server name
-
+
- Service name
+ Service name
-
+
-
-
- Action server name
+
+
+ Action server name
-
-
- Action server name
+
+
+ Action server name
-
-
- Action server name
+
+
+ Action server name
-
-
- Action server name
+
+
+ Action server name
- Service name
+ Service name
- Service name
+ Service name
- Service name
+ Service name
+
+ Y position in meters
+ Heading in degrees
+ Reference frame
+ X position in meters
+ Topic name
+
@@ -65,20 +72,20 @@
-
+
- Service name
+ Service name
-
- Action server name
+
+ Action server name
- Action server name
+ Action server name
- Service name
+ Service name
diff --git a/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp b/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp
new file mode 100644
index 0000000..24618c3
--- /dev/null
+++ b/toid_bt/include/toid_bt/plugins/set_initial_pose_action.hpp
@@ -0,0 +1,60 @@
+#pragma once
+
+#include "angles/angles.h"
+#include "behaviortree_ros2/bt_topic_pub_node.hpp"
+#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp"
+#include "tf2/LinearMath/Quaternion.h"
+#include "tf2_geometry_msgs/tf2_geometry_msgs.hpp"
+
+namespace toid
+{
+
+class SetInitialPoseNode
+: public BT::RosTopicPubNode
+{
+public:
+ SetInitialPoseNode(
+ const std::string & name, const BT::NodeConfig & conf, const BT::RosNodeParams & params)
+ : BT::RosTopicPubNode(name, conf, params)
+ {
+ }
+
+ static BT::PortsList providedPorts()
+ {
+ return providedBasicPorts({
+ BT::InputPort("x", 0.0, "X position in meters"),
+ BT::InputPort("y", 0.0, "Y position in meters"),
+ BT::InputPort("theta", 0.0, "Heading in degrees"),
+ BT::InputPort("frame_id", "map", "Reference frame"),
+ });
+ }
+
+ bool setMessage(geometry_msgs::msg::PoseWithCovarianceStamped & msg) override
+ {
+ double x = getInput("x").value_or(0.0);
+ double y = getInput("y").value_or(0.0);
+ double theta_deg = getInput("theta").value_or(0.0);
+ std::string frame_id = getInput("frame_id").value_or("map");
+
+ msg.header.stamp = node_->get_clock()->now();
+ msg.header.frame_id = frame_id;
+
+ msg.pose.pose.position.x = x;
+ msg.pose.pose.position.y = y;
+ msg.pose.pose.position.z = 0.0;
+
+ tf2::Quaternion q;
+ q.setRPY(0.0, 0.0, angles::from_degrees(theta_deg));
+ msg.pose.pose.orientation = tf2::toMsg(q);
+
+ // Default covariance: small diagonal for x, y, yaw
+ msg.pose.covariance = {};
+ msg.pose.covariance[0] = 0.25; // x
+ msg.pose.covariance[7] = 0.25; // y
+ msg.pose.covariance[35] = 0.068; // yaw
+
+ return true;
+ }
+};
+
+} // namespace toid
diff --git a/toid_bt/src/bt_executor.cpp b/toid_bt/src/bt_executor.cpp
index 93e7874..ff50348 100644
--- a/toid_bt/src/bt_executor.cpp
+++ b/toid_bt/src/bt_executor.cpp
@@ -14,6 +14,7 @@
#include "toid_bt/plugins/send_text_action.hpp"
#include "toid_bt/plugins/set_parameter_action.hpp"
#include "toid_bt/plugins/stuck_detector_decorator.hpp"
+#include "toid_bt/plugins/set_initial_pose_action.hpp"
#include "toid_bt/plugins/translate_x_action.hpp"
namespace toid
@@ -84,6 +85,9 @@ void TreeExecutor::registerNodesIntoFactory(BT::BehaviorTreeFactory & factory)
factory.registerNodeType(
"SetParameter", BT::RosNodeParams(nh, "/compressed_image_subscriber/set_parameters"));
+ factory.registerNodeType(
+ "SetInitialPose", BT::RosNodeParams(nh, "/initialpose"));
+
factory.registerNodeType("ZeroOdom", BT::RosNodeParams(nh, "/zero"));
factory.registerNodeType("EndCalib", BT::RosNodeParams(nh, "/end_calib"));