From 81492fb1f92775c50e9cca8d2ffe43aaf4bcc382 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Wed, 5 Mar 2025 14:13:12 +0100 Subject: [PATCH 1/4] Add waiting for /clock message MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../robotic_manipulation/arm_controller.h | 2 + .../src/arm_controller.cpp | 49 ++++++++++++++----- .../src/robotic_manipulation.cpp | 1 + 3 files changed, 39 insertions(+), 13 deletions(-) diff --git a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h index 2ea9b88..f943574 100644 --- a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h +++ b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h @@ -23,6 +23,8 @@ class ArmController { void SetReferenceFrame(std::string const &frame); + void WaitForClockMessage(); + private: std::shared_ptr m_pandaArm; std::shared_ptr m_hand; diff --git a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp index 380513a..3c3dd70 100644 --- a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp +++ b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp @@ -1,8 +1,13 @@ #include "robotic_manipulation/arm_controller.h" +#include + ArmController::ArmController() { m_node = rclcpp::Node::make_shared("arm_controller"); m_node->set_parameter(rclcpp::Parameter("use_sim_time", true)); + + WaitForClockMessage(); + m_executor.add_node(m_node); m_spinner = std::thread([this]() { m_executor.spin(); }); @@ -41,21 +46,25 @@ geometry_msgs::msg::Pose ArmController::CalculatePose(double x, double y, bool ArmController::MoveThroughWaypoints(const std::vector& waypoints) { auto logger = m_node->get_logger(); - moveit_msgs::msg::RobotTrajectory trajectory; - if (m_pandaArm->computeCartesianPath(waypoints, 0.01, 0.0, trajectory) == - -1) { - RCLCPP_ERROR(logger, - "MoveThroughWaypoints: Failed to compute Cartesian path"); - return false; - } - while (m_pandaArm->execute(trajectory) != - moveit::core::MoveItErrorCode::SUCCESS) { - RCLCPP_ERROR(logger, "MoveThroughWaypoints: Failed to execute trajectory"); - return false; + const int NumTries = 10; + for (int i = 0; i < NumTries; i++) { + moveit_msgs::msg::RobotTrajectory trajectory; + if (m_pandaArm->computeCartesianPath(waypoints, 0.01, 0.0, trajectory) == + -1) { + RCLCPP_ERROR(logger, + "MoveThroughWaypoints: Failed to compute Cartesian path"); + continue; + } + + if (m_pandaArm->execute(trajectory) == moveit::core::MoveItErrorCode::SUCCESS) { + return true; + } + RCLCPP_ERROR(logger, "MoveThroughWaypoints: Failed to execute trajectory, trying again..."); } - - return true; + + RCLCPP_ERROR(logger, "MoveThroughWaypoints: Failed to execute trajectory after %d tries", NumTries); + return false; } void ArmController::Open() { @@ -102,4 +111,18 @@ void ArmController::SetJointValues(std::vector const &jointValues) { void ArmController::SetReferenceFrame(std::string const &frame) { m_pandaArm->setPoseReferenceFrame(frame); +} + +void ArmController::WaitForClockMessage() { + bool clock_received = false; + auto qos = rclcpp::QoS(rclcpp::KeepLast(1)); + qos.best_effort(); + auto subscription = m_node->create_subscription( + "/clock", qos, [&] (rosgraph_msgs::msg::Clock::SharedPtr) { + clock_received = true; + }); + while (!clock_received) { + rclcpp::spin_some(m_node); + } + subscription.reset(); } \ No newline at end of file diff --git a/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp b/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp index 61e62b6..086f5ce 100644 --- a/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp +++ b/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp @@ -17,6 +17,7 @@ int main(int argc, char *argv[]) { startingPose = armController->CaptureJointValues(); armController->SetJointValues(startingPose); + armController->Close(); StateController state; state.Begin(*armController); From 806576d43838033e9c66567df9d06b69f5f63d5b Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Wed, 19 Mar 2025 14:55:32 +0100 Subject: [PATCH 2/4] Add /reset_manipulator service MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../include/robotic_manipulation/arm_controller.h | 4 ++-- .../include/robotic_manipulation/state_controller.h | 1 + .../src/robotic_manipulation/src/arm_controller.cpp | 6 ++++-- .../robotic_manipulation/src/state_controller.cpp | 12 ++++++++++++ 4 files changed, 19 insertions(+), 4 deletions(-) diff --git a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h index f943574..441421f 100644 --- a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h +++ b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h @@ -1,6 +1,6 @@ #pragma once -#include +#include class ArmController { public: @@ -19,7 +19,7 @@ class ArmController { bool GetGripper(); std::vector CaptureJointValues(); - void SetJointValues(std::vector const &jointValues); + bool SetJointValues(std::vector const &jointValues); void SetReferenceFrame(std::string const &frame); diff --git a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/state_controller.h b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/state_controller.h index b1e1077..10dfd28 100644 --- a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/state_controller.h +++ b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/state_controller.h @@ -13,4 +13,5 @@ class StateController { rclcpp::Node::SharedPtr m_node; rclcpp::executors::SingleThreadedExecutor m_executor; std::thread m_spinner; + std::vector m_startingPose; }; \ No newline at end of file diff --git a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp index 3c3dd70..5e011af 100644 --- a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp +++ b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp @@ -50,7 +50,7 @@ bool ArmController::MoveThroughWaypoints(const std::vectorcomputeCartesianPath(waypoints, 0.01, 0.0, trajectory) == + if (m_pandaArm->computeCartesianPath(waypoints, 0.01, trajectory) == -1) { RCLCPP_ERROR(logger, "MoveThroughWaypoints: Failed to compute Cartesian path"); @@ -102,11 +102,13 @@ std::vector ArmController::CaptureJointValues() { return m_pandaArm->getCurrentJointValues(); } -void ArmController::SetJointValues(std::vector const &jointValues) { +bool ArmController::SetJointValues(std::vector const &jointValues) { m_pandaArm->setJointValueTarget(jointValues); if (m_pandaArm->move() != moveit::core::MoveItErrorCode::SUCCESS) { RCLCPP_ERROR(m_node->get_logger(), "Failed to set joint values"); + return false; } + return true; } void ArmController::SetReferenceFrame(std::string const &frame) { diff --git a/ros2_ws/src/robotic_manipulation/src/state_controller.cpp b/ros2_ws/src/robotic_manipulation/src/state_controller.cpp index 13afd25..f89ae4e 100644 --- a/ros2_ws/src/robotic_manipulation/src/state_controller.cpp +++ b/ros2_ws/src/robotic_manipulation/src/state_controller.cpp @@ -1,6 +1,7 @@ #include "robotic_manipulation/state_controller.h" #include +#include #include "rai_interfaces/srv/manipulator_move_to.hpp" StateController::StateController() @@ -26,6 +27,8 @@ void StateController::Begin(ArmController &arm) { auto current_gripper_state = arm.GetGripper(); + m_startingPose = arm.CaptureJointValues(); + RCLCPP_INFO(logger, "Current pose: %f %f %f %f %f %f", current_x, current_y, current_z, current_rx, current_ry, current_rz); RCLCPP_INFO(logger, "Current gripper state: %d", current_gripper_state); @@ -94,6 +97,15 @@ void StateController::Begin(ArmController &arm) { RCLCPP_INFO(logger, "Service /manipulator_move_to is ready"); + auto reset_service = m_node->create_service( + "/reset_manipulator", + [&]([[maybe_unused]] const std::shared_ptr request, + std::shared_ptr response) { + RCLCPP_INFO(logger, "Received reset request"); + response->success = arm.SetJointValues(m_startingPose); + } + ); + // Add node to executor and spin in the main thread m_executor.add_node(m_node); m_executor.spin(); From cc5dd94a2cad322d84425f6709f2d17eede4efd3 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Wed, 19 Mar 2025 16:42:34 +0100 Subject: [PATCH 3/4] Revert header updates for backward compatibility with humble MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../include/robotic_manipulation/arm_controller.h | 2 +- ros2_ws/src/robotic_manipulation/src/arm_controller.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h index 441421f..99403fa 100644 --- a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h +++ b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h @@ -1,6 +1,6 @@ #pragma once -#include +#include class ArmController { public: diff --git a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp index 5e011af..85d0c4d 100644 --- a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp +++ b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp @@ -50,7 +50,7 @@ bool ArmController::MoveThroughWaypoints(const std::vectorcomputeCartesianPath(waypoints, 0.01, trajectory) == + if (m_pandaArm->computeCartesianPath(waypoints, 0.01, 0.0, trajectory) == -1) { RCLCPP_ERROR(logger, "MoveThroughWaypoints: Failed to compute Cartesian path"); From e08670f610d11473a10f7f3337937b195be59ec4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Kacper=20D=C4=85browski?= Date: Mon, 24 Mar 2025 15:31:29 +0100 Subject: [PATCH 4/4] Move the initialization code from the constructor to a separate method MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Kacper Dąbrowski --- .../include/robotic_manipulation/arm_controller.h | 6 ++++-- ros2_ws/src/robotic_manipulation/src/arm_controller.cpp | 2 +- .../src/robotic_manipulation/src/robotic_manipulation.cpp | 1 + 3 files changed, 6 insertions(+), 3 deletions(-) diff --git a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h index 99403fa..7566c85 100644 --- a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h +++ b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h @@ -4,9 +4,11 @@ class ArmController { public: - ArmController(); + ArmController() = default; ~ArmController(); + void Initialize(); + geometry_msgs::msg::Pose CalculatePose(double x, double y, double z, double r = 0.0); @@ -23,9 +25,9 @@ class ArmController { void SetReferenceFrame(std::string const &frame); +private: void WaitForClockMessage(); -private: std::shared_ptr m_pandaArm; std::shared_ptr m_hand; diff --git a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp index 85d0c4d..4fafc69 100644 --- a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp +++ b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp @@ -2,7 +2,7 @@ #include -ArmController::ArmController() { +void ArmController::Initialize() { m_node = rclcpp::Node::make_shared("arm_controller"); m_node->set_parameter(rclcpp::Parameter("use_sim_time", true)); diff --git a/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp b/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp index 086f5ce..6583e8e 100644 --- a/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp +++ b/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp @@ -10,6 +10,7 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); auto armController = std::make_shared(); + armController->Initialize(); armController->MoveThroughWaypoints({armController->CalculatePose(0.3, 0.0, 0.35)});