Reviewed structure to account for three cubes.
This commit is contained in:
parent
aa2a1d6d41
commit
cd6f6dd377
|
|
@ -1,6 +1,6 @@
|
|||
#include <chrono>
|
||||
#include <cmath>
|
||||
#include <memory>
|
||||
#include <vector>
|
||||
|
||||
#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<advrobotics_lab3_interfaces::msg::Joints>("joint_cmd", 10);
|
||||
gripper_pub_ = this->create_publisher<advrobotics_lab3_interfaces::msg::Gripper>("gripper_cmd", 10);
|
||||
|
||||
gripper_cmd_pub_ = create_publisher<
|
||||
advrobotics_lab3_interfaces::msg::Gripper>("gripper_cmd", 10);
|
||||
// IK client
|
||||
ik_client_ = this->create_client<advrobotics_lab3_interfaces::srv::Invkin>("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<std::vector<double>> cubes = {
|
||||
{6.0, 9.0, 0.0},
|
||||
{6.5, 9.0, 0.0},
|
||||
{7.0, 9.0, 0.0}
|
||||
};
|
||||
std::vector<double> 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);
|
||||
}
|
||||
|
||||
void startOnce()
|
||||
{
|
||||
start_timer_->cancel();
|
||||
executeTower();
|
||||
RCLCPP_INFO(this->get_logger(), "Tower building completed!");
|
||||
}
|
||||
|
||||
/* ================= MOTION PRIMITIVES ================= */
|
||||
|
||||
bool moveToCartesian(double x, double y, double z)
|
||||
void move_to(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);
|
||||
|
||||
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;
|
||||
|
||||
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();
|
||||
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 (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);
|
||||
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);
|
||||
// Wait for the robot to move (rough approximation)
|
||||
rclcpp::sleep_for(1s);
|
||||
}
|
||||
|
||||
/* ================= MAIN TASK ================= */
|
||||
|
||||
void executeTower()
|
||||
else
|
||||
{
|
||||
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_ERROR(this->get_logger(), "Failed to call IK service for (%f,%f,%f)", x, y, z);
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(get_logger(), "Tower successfully built.");
|
||||
}
|
||||
|
||||
private:
|
||||
/* ================= ROS ENTITIES ================= */
|
||||
|
||||
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::TimerBase::SharedPtr start_timer_;
|
||||
|
||||
double current_q1_{0.0};
|
||||
double current_q2_{0.0};
|
||||
double current_q3_{0.0};
|
||||
rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Joints>::SharedPtr joint_pub_;
|
||||
rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Gripper>::SharedPtr gripper_pub_;
|
||||
rclcpp::Client<advrobotics_lab3_interfaces::srv::Invkin>::SharedPtr ik_client_;
|
||||
};
|
||||
|
||||
int main(int argc, char **argv)
|
||||
|
|
|
|||
Loading…
Reference in New Issue