Reviewed structure to account for three cubes.
This commit is contained in:
parent
aa2a1d6d41
commit
cd6f6dd377
|
|
@ -1,6 +1,6 @@
|
||||||
#include <chrono>
|
#include <chrono>
|
||||||
#include <cmath>
|
|
||||||
#include <memory>
|
#include <memory>
|
||||||
|
#include <vector>
|
||||||
|
|
||||||
#include "rclcpp/rclcpp.hpp"
|
#include "rclcpp/rclcpp.hpp"
|
||||||
#include "advrobotics_lab3_interfaces/msg/joints.hpp"
|
#include "advrobotics_lab3_interfaces/msg/joints.hpp"
|
||||||
|
|
@ -14,188 +14,106 @@ class BuildTower : public rclcpp::Node
|
||||||
public:
|
public:
|
||||||
BuildTower() : Node("build_tower")
|
BuildTower() : Node("build_tower")
|
||||||
{
|
{
|
||||||
joint_cmd_pub_ = create_publisher<
|
// Publishers
|
||||||
advrobotics_lab3_interfaces::msg::Joints>("joint_cmd", 10);
|
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<
|
// IK client
|
||||||
advrobotics_lab3_interfaces::msg::Gripper>("gripper_cmd", 10);
|
ik_client_ = this->create_client<advrobotics_lab3_interfaces::srv::Invkin>("invkin");
|
||||||
|
|
||||||
joint_pos_sub_ = create_subscription<
|
RCLCPP_INFO(this->get_logger(), "Waiting for IK service...");
|
||||||
advrobotics_lab3_interfaces::msg::Joints>(
|
while (!ik_client_->wait_for_service(1s)) {
|
||||||
"joint_position", 10,
|
RCLCPP_WARN(this->get_logger(), "IK service not available, waiting...");
|
||||||
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...");
|
|
||||||
}
|
}
|
||||||
|
|
||||||
// Start execution once node is fully constructed
|
// Start the tower building sequence
|
||||||
start_timer_ = create_wall_timer(
|
build_tower();
|
||||||
500ms, std::bind(&BuildTower::startOnce, this));
|
|
||||||
}
|
}
|
||||||
|
|
||||||
private:
|
private:
|
||||||
/* ================= CALLBACKS ================= */
|
void build_tower()
|
||||||
|
|
||||||
void jointPosCallback(
|
|
||||||
const advrobotics_lab3_interfaces::msg::Joints::SharedPtr msg)
|
|
||||||
{
|
{
|
||||||
current_q1_ = msg->q1;
|
// Define cube positions and tower location (x,y,z)
|
||||||
current_q2_ = msg->q2;
|
std::vector<std::vector<double>> cubes = {
|
||||||
current_q3_ = msg->q3;
|
{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);
|
||||||
|
}
|
||||||
|
|
||||||
|
RCLCPP_INFO(this->get_logger(), "Tower building completed!");
|
||||||
}
|
}
|
||||||
|
|
||||||
void startOnce()
|
void move_to(double x, double y, double z)
|
||||||
{
|
{
|
||||||
start_timer_->cancel();
|
auto request = std::make_shared<advrobotics_lab3_interfaces::srv::Invkin::Request>();
|
||||||
executeTower();
|
|
||||||
}
|
|
||||||
|
|
||||||
/* ================= MOTION PRIMITIVES ================= */
|
|
||||||
|
|
||||||
bool moveToCartesian(double x, double y, double z)
|
|
||||||
{
|
|
||||||
auto request =
|
|
||||||
std::make_shared<advrobotics_lab3_interfaces::srv::Invkin::Request>();
|
|
||||||
request->x = x;
|
request->x = x;
|
||||||
request->y = y;
|
request->y = y;
|
||||||
request->z = z;
|
request->z = z;
|
||||||
|
|
||||||
auto future = ik_client_->async_send_request(request);
|
auto future = ik_client_->async_send_request(request);
|
||||||
|
if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) == rclcpp::FutureReturnCode::SUCCESS)
|
||||||
constexpr auto ik_timeout = 2s;
|
|
||||||
auto start_time = now();
|
|
||||||
|
|
||||||
while (rclcpp::ok())
|
|
||||||
{
|
{
|
||||||
if (future.wait_for(10ms) == std::future_status::ready)
|
auto response = future.get();
|
||||||
break;
|
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)
|
// Wait for the robot to move (rough approximation)
|
||||||
{
|
rclcpp::sleep_for(1s);
|
||||||
RCLCPP_ERROR(get_logger(),
|
|
||||||
"IK timeout for target (%.2f, %.2f, %.2f)", x, y, z);
|
|
||||||
return false;
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
|
else
|
||||||
auto response = future.get();
|
|
||||||
|
|
||||||
if (response->sol == 0)
|
|
||||||
{
|
{
|
||||||
RCLCPP_ERROR(get_logger(),
|
RCLCPP_ERROR(this->get_logger(), "Failed to call IK service for (%f,%f,%f)", x, y, z);
|
||||||
"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(
|
rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Joints>::SharedPtr joint_pub_;
|
||||||
const advrobotics_lab3_interfaces::msg::Joints &target)
|
rclcpp::Publisher<advrobotics_lab3_interfaces::msg::Gripper>::SharedPtr gripper_pub_;
|
||||||
{
|
rclcpp::Client<advrobotics_lab3_interfaces::srv::Invkin>::SharedPtr ik_client_;
|
||||||
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<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};
|
|
||||||
};
|
};
|
||||||
|
|
||||||
int main(int argc, char **argv)
|
int main(int argc, char **argv)
|
||||||
|
|
|
||||||
Loading…
Reference in New Issue