Added control for timeouts.

src/advrobotics_lab3_app/src/build_tower.cpp

@@ -14,24 +14,26 @@ class BuildTower : public rclcpp::Node
 public:
     BuildTower() : Node("build_tower")
     {
-        joint_cmd_pub_ = create_publisher<advrobotics_lab3_interfaces::msg::Joints>(
+        joint_cmd_pub_ = create_publisher<
-            "joint_cmd", 10);
+            advrobotics_lab3_interfaces::msg::Joints>("joint_cmd", 10);
 
-        gripper_cmd_pub_ = create_publisher<advrobotics_lab3_interfaces::msg::Gripper>(
+        gripper_cmd_pub_ = create_publisher<
-            "gripper_cmd", 10);
+            advrobotics_lab3_interfaces::msg::Gripper>("gripper_cmd", 10);
 
-        joint_pos_sub_ = create_subscription<advrobotics_lab3_interfaces::msg::Joints>(
+        joint_pos_sub_ = create_subscription<
-            "joint_position", 10,
+            advrobotics_lab3_interfaces::msg::Joints>(
-            std::bind(&BuildTower::jointPosCallback, this, std::placeholders::_1));
+                "joint_position", 10,
+                std::bind(&BuildTower::jointPosCallback, this, std::placeholders::_1));
 
-        ik_client_ = create_client<advrobotics_lab3_interfaces::srv::Invkin>("invkin");
+        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...");
         }
 
-        // Run once AFTER construction is complete
+        // Start execution once node is fully constructed
         start_timer_ = create_wall_timer(
             500ms, std::bind(&BuildTower::startOnce, this));
     }
@@ -55,7 +57,7 @@ private:
 
     /* ================= MOTION PRIMITIVES ================= */
 
-    void moveToCartesian(double x, double y, double z)
+    bool moveToCartesian(double x, double y, double z)
     {
         auto request =
             std::make_shared<advrobotics_lab3_interfaces::srv::Invkin::Request>();
@@ -65,32 +67,66 @@ private:
 
         auto future = ik_client_->async_send_request(request);
 
-        rclcpp::spin_until_future_complete(
+        constexpr auto ik_timeout = 2s;
-            get_node_base_interface(), future);
+        auto start_time = now();
+
+        while (rclcpp::ok())
+        {
+            if (future.wait_for(10ms) == std::future_status::ready)
+                break;
+
+            if ((now() - start_time) > ik_timeout)
+            {
+                RCLCPP_ERROR(get_logger(),
+                    "IK timeout for target (%.2f, %.2f, %.2f)", x, y, z);
+                return false;
+            }
+        }
 
         auto response = future.get();
 
+        if (response->sol == 0)
+        {
+            RCLCPP_ERROR(get_logger(),
+                "No IK solution for (%.2f, %.2f, %.2f)", x, y, z);
+            return false;
+        }
+
         advrobotics_lab3_interfaces::msg::Joints cmd;
         cmd.q1 = response->q1 * 180.0 / M_PI;
         cmd.q2 = response->q2 * 180.0 / M_PI;
         cmd.q3 = response->q3 * 180.0 / M_PI;
 
         joint_cmd_pub_->publish(cmd);
-        waitUntilReached(cmd);
+        return waitUntilReached(cmd);
     }
 
-    void waitUntilReached(
+    bool waitUntilReached(
         const advrobotics_lab3_interfaces::msg::Joints &target)
     {
+        constexpr auto motion_timeout = 5s;
+        auto start_time = now();
+
         while (rclcpp::ok())
         {
             if (std::fabs(current_q1_ - target.q1) < 2.0 &&
                 std::fabs(current_q2_ - target.q2) < 2.0 &&
                 std::fabs(current_q3_ - target.q3) < 2.0)
-                break;
+            {
+                return true;
+            }
+
+            if ((now() - start_time) > motion_timeout)
+            {
+                RCLCPP_ERROR(get_logger(),
+                    "Motion timeout (target not reached)");
+                return false;
+            }
 
             rclcpp::sleep_for(100ms);
         }
+
+        return false;
     }
 
     void setGripper(double angle)
@@ -101,7 +137,7 @@ private:
         rclcpp::sleep_for(1s);
     }
 
-    /* ================= MAIN TOWER LOGIC ================= */
+    /* ================= MAIN TASK ================= */
 
     void executeTower()
     {
@@ -111,11 +147,11 @@ private:
 
         constexpr double place_x = 0.15;
         constexpr double place_y = 0.00;
-        constexpr double base_z = 0.02;
+        constexpr double base_z  = 0.02;
         constexpr double block_height = 0.03;
 
         constexpr double approach_z = 0.10;
-        constexpr double gripper_open = 30.0;
+        constexpr double gripper_open   = 30.0;
         constexpr double gripper_closed = -10.0;
 
         setGripper(gripper_open);
@@ -124,27 +160,25 @@ private:
         {
             double place_z = base_z + i * block_height;
 
-            // Pick
+            if (!moveToCartesian(pick_x, pick_y, approach_z)) return;
-            moveToCartesian(pick_x, pick_y, approach_z);
+            if (!moveToCartesian(pick_x, pick_y, pick_z)) return;
-            moveToCartesian(pick_x, pick_y, pick_z);
             setGripper(gripper_closed);
 
-            // Lift & move
+            if (!moveToCartesian(pick_x, pick_y, approach_z)) return;
-            moveToCartesian(pick_x, pick_y, approach_z);
+            if (!moveToCartesian(place_x, place_y, approach_z)) return;
-            moveToCartesian(place_x, place_y, approach_z);
+            if (!moveToCartesian(place_x, place_y, place_z)) return;
 
-            // Place
-            moveToCartesian(place_x, place_y, place_z);
             setGripper(gripper_open);
 
-            // Retreat
+            if (!moveToCartesian(place_x, place_y, approach_z)) return;
-            moveToCartesian(place_x, place_y, approach_z);
         }
 
         RCLCPP_INFO(get_logger(), "Tower successfully built.");
     }
 
 private:
+    /* ================= ROS ENTITIES ================= */
+
     rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Joints>::SharedPtr
         joint_cmd_pub_;