diff --git a/src/advrobotics_lab3_app/src/build_tower.cpp b/src/advrobotics_lab3_app/src/build_tower.cpp index 3ac5c8b..a2de2a5 100644 --- a/src/advrobotics_lab3_app/src/build_tower.cpp +++ b/src/advrobotics_lab3_app/src/build_tower.cpp @@ -1,6 +1,6 @@ #include -#include #include +#include #include "rclcpp/rclcpp.hpp" #include "advrobotics_lab3_interfaces/msg/joints.hpp" @@ -14,188 +14,106 @@ class BuildTower : public rclcpp::Node public: BuildTower() : Node("build_tower") { - joint_cmd_pub_ = create_publisher< - advrobotics_lab3_interfaces::msg::Joints>("joint_cmd", 10); + // Publishers + joint_pub_ = this->create_publisher("joint_cmd", 10); + gripper_pub_ = this->create_publisher("gripper_cmd", 10); - gripper_cmd_pub_ = create_publisher< - advrobotics_lab3_interfaces::msg::Gripper>("gripper_cmd", 10); + // IK client + ik_client_ = this->create_client("invkin"); - joint_pos_sub_ = create_subscription< - advrobotics_lab3_interfaces::msg::Joints>( - "joint_position", 10, - std::bind(&BuildTower::jointPosCallback, this, std::placeholders::_1)); - - 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..."); + RCLCPP_INFO(this->get_logger(), "Waiting for IK service..."); + while (!ik_client_->wait_for_service(1s)) { + RCLCPP_WARN(this->get_logger(), "IK service not available, waiting..."); } - // Start execution once node is fully constructed - start_timer_ = create_wall_timer( - 500ms, std::bind(&BuildTower::startOnce, this)); + // Start the tower building sequence + build_tower(); } private: - /* ================= CALLBACKS ================= */ - - void jointPosCallback( - const advrobotics_lab3_interfaces::msg::Joints::SharedPtr msg) + void build_tower() { - current_q1_ = msg->q1; - current_q2_ = msg->q2; - current_q3_ = msg->q3; + // Define cube positions and tower location (x,y,z) + std::vector> cubes = { + {6.0, 9.0, 0.0}, + {6.5, 9.0, 0.0}, + {7.0, 9.0, 0.0} + }; + std::vector tower = {8.0, 12.0, 0.0}; + double safe_height = 5.0; // height above blocks to avoid collisions + + // Open gripper at start + advrobotics_lab3_interfaces::msg::Gripper gripper_msg; + gripper_msg.gripper = 0.0; // open + gripper_pub_->publish(gripper_msg); + rclcpp::sleep_for(500ms); + + // Loop through all cubes + for (size_t i = 0; i < cubes.size(); i++) + { + RCLCPP_INFO(this->get_logger(), "Picking cube %zu", i+1); + + // 1. Move above cube (safe height) + move_to(cubes[i][0], cubes[i][1], safe_height); + + // 2. Move down to cube + move_to(cubes[i][0], cubes[i][1], cubes[i][2]); + + // 3. Close gripper + gripper_msg.gripper = 150.0; // closed + gripper_pub_->publish(gripper_msg); + rclcpp::sleep_for(500ms); + + // 4. Lift cube to safe height + move_to(cubes[i][0], cubes[i][1], safe_height); + + // 5. Move above tower (safe height) + move_to(tower[0], tower[1], safe_height + i*2.0); // stack cubes + + // 6. Lower to tower target + move_to(tower[0], tower[1], tower[2] + i*2.0); + + // 7. Open gripper to release + gripper_msg.gripper = 0.0; // open + gripper_pub_->publish(gripper_msg); + rclcpp::sleep_for(500ms); + + // 8. Lift gripper after releasing + move_to(tower[0], tower[1], safe_height + (i+1)*2.0); + } + + RCLCPP_INFO(this->get_logger(), "Tower building completed!"); } - void startOnce() + void move_to(double x, double y, double z) { - start_timer_->cancel(); - executeTower(); - } - - /* ================= MOTION PRIMITIVES ================= */ - - bool moveToCartesian(double x, double y, double z) - { - auto request = - std::make_shared(); + auto request = std::make_shared(); request->x = x; request->y = y; request->z = z; auto future = ik_client_->async_send_request(request); - - constexpr auto ik_timeout = 2s; - auto start_time = now(); - - while (rclcpp::ok()) + if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS) { - if (future.wait_for(10ms) == std::future_status::ready) - break; + auto response = future.get(); + advrobotics_lab3_interfaces::msg::Joints joint_msg; + joint_msg.q1 = response->q1 * 180.0 / M_PI; // radians to degrees + joint_msg.q2 = response->q2 * 180.0 / M_PI; + joint_msg.q3 = response->q3 * 180.0 / M_PI; + joint_pub_->publish(joint_msg); - if ((now() - start_time) > ik_timeout) - { - RCLCPP_ERROR(get_logger(), - "IK timeout for target (%.2f, %.2f, %.2f)", x, y, z); - return false; - } + // Wait for the robot to move (rough approximation) + rclcpp::sleep_for(1s); } - - auto response = future.get(); - - if (response->sol == 0) + else { - RCLCPP_ERROR(get_logger(), - "No IK solution for (%.2f, %.2f, %.2f)", x, y, z); - return false; + RCLCPP_ERROR(this->get_logger(), "Failed to call IK service for (%f,%f,%f)", x, y, z); } - - 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); - return waitUntilReached(cmd); } - 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) - { - 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) - { - advrobotics_lab3_interfaces::msg::Gripper g; - g.gripper = angle; - gripper_cmd_pub_->publish(g); - rclcpp::sleep_for(1s); - } - - /* ================= MAIN TASK ================= */ - - void executeTower() - { - constexpr double pick_x = 0.10; - constexpr double pick_y = 0.10; - constexpr double pick_z = 0.02; - - constexpr double place_x = 0.15; - constexpr double place_y = 0.00; - 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_closed = -10.0; - - setGripper(gripper_open); - - for (int i = 0; i < 3; ++i) - { - double place_z = base_z + i * block_height; - - if (!moveToCartesian(pick_x, pick_y, approach_z)) return; - if (!moveToCartesian(pick_x, pick_y, pick_z)) return; - setGripper(gripper_closed); - - if (!moveToCartesian(pick_x, pick_y, approach_z)) return; - if (!moveToCartesian(place_x, place_y, approach_z)) return; - if (!moveToCartesian(place_x, place_y, place_z)) return; - - setGripper(gripper_open); - - if (!moveToCartesian(place_x, place_y, approach_z)) return; - } - - RCLCPP_INFO(get_logger(), "Tower successfully built."); - } - -private: - /* ================= ROS ENTITIES ================= */ - - rclcpp::Publisher::SharedPtr - joint_cmd_pub_; - - rclcpp::Publisher::SharedPtr - gripper_cmd_pub_; - - rclcpp::Subscription::SharedPtr - joint_pos_sub_; - - rclcpp::Client::SharedPtr - ik_client_; - - rclcpp::TimerBase::SharedPtr start_timer_; - - double current_q1_{0.0}; - double current_q2_{0.0}; - double current_q3_{0.0}; + rclcpp::Publisher::SharedPtr joint_pub_; + rclcpp::Publisher::SharedPtr gripper_pub_; + rclcpp::Client::SharedPtr ik_client_; }; int main(int argc, char **argv)