Corrected wrong definition.

src/advrobotics_lab3_app/src/build_tower.cpp

@@ -1,6 +1,6 @@
 #include <chrono>
-#include <memory>
 #include <cmath>
+#include <memory>
 
 #include "rclcpp/rclcpp.hpp"
 #include "advrobotics_lab3_interfaces/msg/joints.hpp"
@@ -14,8 +14,11 @@ class BuildTower : public rclcpp::Node
 public:
     BuildTower() : Node("build_tower")
     {
-        joint_cmd_pub_ = create_publisher<advrobotics_lab3_interfaces::msg::Joints>("joint_cmd", 10);
-        gripper_cmd_pub_ = create_publisher<advrobotics_lab3_interfaces::msg::Gripper>("gripper_cmd", 10);
+        joint_cmd_pub_ = create_publisher<advrobotics_lab3_interfaces::msg::Joints>(
+            "joint_cmd", 10);
+
+        gripper_cmd_pub_ = create_publisher<advrobotics_lab3_interfaces::msg::Gripper>(
+            "gripper_cmd", 10);
 
         joint_pos_sub_ = create_subscription<advrobotics_lab3_interfaces::msg::Joints>(
             "joint_position", 10,
@@ -24,32 +27,46 @@ public:
         ik_client_ = create_client<advrobotics_lab3_interfaces::srv::Invkin>("invkin");
 
         while (!ik_client_->wait_for_service(1s))
+        {
             RCLCPP_INFO(get_logger(), "Waiting for IK service...");
+        }
 
-        executeTower();
+        // Run once AFTER construction is complete
+        start_timer_ = create_wall_timer(
+            500ms, std::bind(&BuildTower::startOnce, this));
     }
 
 private:
-    /* ---------------- CALLBACKS ---------------- */
+    /* ================= CALLBACKS ================= */
 
-    void jointPosCallback(const advrobotics_lab3_interfaces::msg::Joints::SharedPtr msg)
+    void jointPosCallback(
+        const advrobotics_lab3_interfaces::msg::Joints::SharedPtr msg)
     {
         current_q1_ = msg->q1;
         current_q2_ = msg->q2;
         current_q3_ = msg->q3;
     }
 
-    /* ---------------- MOTION PRIMITIVES ---------------- */
+    void startOnce()
+    {
+        start_timer_->cancel();
+        executeTower();
+    }
+
+    /* ================= MOTION PRIMITIVES ================= */
 
     void moveToCartesian(double x, double y, double z)
     {
-        auto request = std::make_shared<advrobotics_lab3_interfaces::srv::Invkin::Request>();
+        auto request =
+            std::make_shared<advrobotics_lab3_interfaces::srv::Invkin::Request>();
         request->x = x;
         request->y = y;
         request->z = z;
 
         auto future = ik_client_->async_send_request(request);
-        rclcpp::spin_until_future_complete(shared_from_this(), future);
+
+        rclcpp::spin_until_future_complete(
+            get_node_base_interface(), future);
 
         auto response = future.get();
 
@@ -62,7 +79,8 @@ private:
         waitUntilReached(cmd);
     }
 
-    void waitUntilReached(const advrobotics_lab3_interfaces::msg::Joints &target)
+    void waitUntilReached(
+        const advrobotics_lab3_interfaces::msg::Joints &target)
     {
         while (rclcpp::ok())
         {
@@ -83,7 +101,7 @@ private:
         rclcpp::sleep_for(1s);
     }
 
-    /* ---------------- MAIN LOGIC ---------------- */
+    /* ================= MAIN TOWER LOGIC ================= */
 
     void executeTower()
     {
@@ -92,7 +110,7 @@ private:
         constexpr double pick_z = 0.02;
 
         constexpr double place_x = 0.15;
-        constexpr double place_y = 0.0;
+        constexpr double place_y = 0.00;
         constexpr double base_z = 0.02;
         constexpr double block_height = 0.03;
 
@@ -106,28 +124,44 @@ private:
         {
             double place_z = base_z + i * block_height;
 
+            // Pick
             moveToCartesian(pick_x, pick_y, approach_z);
             moveToCartesian(pick_x, pick_y, pick_z);
             setGripper(gripper_closed);
 
+            // Lift & move
             moveToCartesian(pick_x, pick_y, approach_z);
             moveToCartesian(place_x, place_y, approach_z);
-            moveToCartesian(place_x, place_y, place_z);
 
+            // Place
+            moveToCartesian(place_x, place_y, place_z);
             setGripper(gripper_open);
+
+            // Retreat
             moveToCartesian(place_x, place_y, approach_z);
         }
 
-        RCLCPP_INFO(get_logger(), "Tower completed.");
+        RCLCPP_INFO(get_logger(), "Tower successfully built.");
     }
 
 private:
-    rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Joints>::SharedPtr joint_cmd_pub_;
-    rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Gripper>::SharedPtr gripper_cmd_pub_;
-    rclcpp::Subscription<advrobotics_lab3_interfaces::msg::Joints>::SharedPtr joint_pos_sub_;
-    rclcpp::Client<advrobotics_lab3_interfaces::srv::Invkin>::SharedPtr ik_client_;
+    rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Joints>::SharedPtr
+        joint_cmd_pub_;
 
-    double current_q1_{0.0}, current_q2_{0.0}, current_q3_{0.0};
+    rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Gripper>::SharedPtr
+        gripper_cmd_pub_;
+
+    rclcpp::Subscription<advrobotics_lab3_interfaces::msg::Joints>::SharedPtr
+        joint_pos_sub_;
+
+    rclcpp::Client<advrobotics_lab3_interfaces::srv::Invkin>::SharedPtr
+        ik_client_;
+
+    rclcpp::TimerBase::SharedPtr start_timer_;
+
+    double current_q1_{0.0};
+    double current_q2_{0.0};
+    double current_q3_{0.0};
 };
 
 int main(int argc, char **argv)