Improve robotic_manipulation_node stability

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<double> CaptureJointValues();
-  void SetJointValues(std::vector<double> const &jointValues);
+  bool SetJointValues(std::vector<double> const &jointValues);
 
   void SetReferenceFrame(std::string const &frame);
 
 private:
+  void WaitForClockMessage();
+
   std::shared_ptr<moveit::planning_interface::MoveGroupInterface> m_pandaArm;
   std::shared_ptr<moveit::planning_interface::MoveGroupInterface> m_hand;
 

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<double> m_startingPose;
 };

ros2_ws/src/robotic_manipulation/src/arm_controller.cpp

@@ -1,8 +1,13 @@
 #include "robotic_manipulation/arm_controller.h"
 
-ArmController::ArmController() {
+#include <rosgraph_msgs/msg/clock.hpp>
+
+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<geometry_msgs::msg::Pose>& 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<double> ArmController::CaptureJointValues() {
   return m_pandaArm->getCurrentJointValues();
 }
 
-void ArmController::SetJointValues(std::vector<double> const &jointValues) {
+bool ArmController::SetJointValues(std::vector<double> 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<rosgraph_msgs::msg::Clock>(
+      "/clock", qos, [&] (rosgraph_msgs::msg::Clock::SharedPtr) {
+        clock_received = true;
+      });
+  while (!clock_received) {
+    rclcpp::spin_some(m_node);
+  }
+  subscription.reset();
 }

ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp

@@ -10,13 +10,15 @@ int main(int argc, char *argv[]) {
   rclcpp::init(argc, argv);
 
   auto armController = std::make_shared<ArmController>();
+  armController->Initialize();
 
   armController->MoveThroughWaypoints({armController->CalculatePose(0.3, 0.0, 0.35)});
 
   std::vector<double> startingPose;
   startingPose = armController->CaptureJointValues();
 
   armController->SetJointValues(startingPose);
+  armController->Close();
 
   StateController state;
   state.Begin(*armController);

ros2_ws/src/robotic_manipulation/src/state_controller.cpp

@@ -1,6 +1,7 @@
 #include "robotic_manipulation/state_controller.h"
 
 #include <std_msgs/msg/float32_multi_array.hpp>
+#include <std_srvs/srv/trigger.hpp>
 #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<std_srvs::srv::Trigger>(
+    "/reset_manipulator",
+    [&]([[maybe_unused]] const std::shared_ptr<std_srvs::srv::Trigger::Request> request,
+        std::shared_ptr<std_srvs::srv::Trigger::Response> 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();