charles.stelandre
/
AdvancedRobotics_Lab3
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

Added control for timeouts.

This commit is contained in:
Charles STELANDRE 2025-12-17 10:46:17 +01:00
parent e3f9bd8ed1
commit ebcca88f5d
1 changed files with 62 additions and 28 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

82
src/advrobotics_lab3_app/src/build_tower.cpp
View File

@ -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", 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);
gripper_cmd_pub_ = create_publisher<
advrobotics_lab3_interfaces::msg::Gripper>("gripper_cmd", 10);
joint_pos_sub_ = create_subscription<advrobotics_lab3_interfaces::msg::Joints>(
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");
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(
get_node_base_interface(), future);
constexpr auto ik_timeout = 2s;
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()
{
@ -124,27 +160,25 @@ private:
{
double place_z = base_z + i * block_height;
// Pick
moveToCartesian(pick_x, pick_y, approach_z);
moveToCartesian(pick_x, pick_y, pick_z);
if (!moveToCartesian(pick_x, pick_y, approach_z)) return;
if (!moveToCartesian(pick_x, pick_y, pick_z)) return;
setGripper(gripper_closed);
// Lift & move
moveToCartesian(pick_x, pick_y, approach_z);
moveToCartesian(place_x, place_y, approach_z);
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;
// Place
moveToCartesian(place_x, place_y, place_z);
setGripper(gripper_open);
// Retreat
moveToCartesian(place_x, place_y, approach_z);
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_;