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..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); @@ -19,11 +21,13 @@ 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); private: + void WaitForClockMessage(); + std::shared_ptr m_pandaArm; std::shared_ptr m_hand; 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 380513a..4fafc69 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" -ArmController::ArmController() { +#include + +void ArmController::Initialize() { 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() { @@ -93,13 +102,29 @@ 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) { 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..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)}); @@ -17,6 +18,7 @@ int main(int argc, char *argv[]) { startingPose = armController->CaptureJointValues(); armController->SetJointValues(startingPose); + armController->Close(); StateController state; state.Begin(*armController); 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();