Refactor the robotic manipulation package

ros2_ws/src/robotic_manipulation/CMakeLists.txt

@@ -14,11 +14,11 @@ find_package(vision_msgs REQUIRED)
 find_package(rosbag2_cpp REQUIRED)
 find_package(rai_interfaces REQUIRED)
 
-add_executable(robotic_manipulation src/robotic_manipulation.cpp src/arm_controller.cpp src/state_controller.cpp)
+add_executable(robotic_manipulation src/robotic_manipulation.cpp src/arm_controller.cpp src/rai_manipulation_interface_node.cpp)
 target_include_directories(robotic_manipulation PUBLIC
   $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
   $<INSTALL_INTERFACE:include>)
-target_compile_features(robotic_manipulation PUBLIC c_std_99 cxx_std_17)  # Require C99 and C++17
+target_compile_features(robotic_manipulation PUBLIC c_std_99 cxx_std_20)  # Require C99 and C++20
 ament_target_dependencies(
   robotic_manipulation
   "moveit_ros_planning_interface"

ros2_ws/src/robotic_manipulation/include/robotic_manipulation/arm_controller.h

@@ -1,3 +1,18 @@
+/* Copyright (C) 2025 Robotec.AI
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *         http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
 #pragma once
 
 #include <moveit/move_group_interface/move_group_interface.h>
@@ -12,7 +27,8 @@ class ArmController {
   geometry_msgs::msg::Pose CalculatePose(double x, double y, double z,
                                          double r = 0.0);
 
-  bool MoveThroughWaypoints(const std::vector<geometry_msgs::msg::Pose>& waypoints);
+  bool
+  MoveThroughWaypoints(std::vector<geometry_msgs::msg::Pose> const &waypoints);
 
   void Open();
   void Close();

ros2_ws/src/robotic_manipulation/include/robotic_manipulation/rai_manipulation_interface_node.h

@@ -0,0 +1,39 @@
+/* Copyright (C) 2025 Robotec.AI
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *         http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#pragma once
+
+#include "robotic_manipulation/arm_controller.h"
+
+#include "rai_interfaces/srv/manipulator_move_to.hpp"
+#include <std_srvs/srv/trigger.hpp>
+
+class RaiManipulationInterfaceNode {
+public:
+  RaiManipulationInterfaceNode();
+
+  void Initialize(ArmController &arm);
+  void Spin();
+
+private:
+  rclcpp::Node::SharedPtr m_node;
+  rclcpp::Service<rai_interfaces::srv::ManipulatorMoveTo>::SharedPtr
+      m_moveToService;
+  rclcpp::Service<std_srvs::srv::Trigger>::SharedPtr m_resetService;
+
+  rclcpp::executors::SingleThreadedExecutor m_executor;
+
+  std::vector<double> m_startingPose;
+};

ros2_ws/src/robotic_manipulation/package.xml

@@ -3,9 +3,9 @@
 <package format="3">
   <name>robotic_manipulation</name>
   <version>0.0.0</version>
-  <description>TODO: Package description</description>
+  <description>A package providing services for robotic manipulation using RAI.</description>
   <maintainer email="[email protected]">kdabrowski</maintainer>
-  <license>TODO: License declaration</license>
+  <license>Apache 2.0</license>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
 

ros2_ws/src/robotic_manipulation/src/arm_controller.cpp

@@ -1,7 +1,34 @@
+/* Copyright (C) 2025 Robotec.AI
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *         http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
 #include "robotic_manipulation/arm_controller.h"
 
 #include <rosgraph_msgs/msg/clock.hpp>
 
+#include <numbers>
+
+// The joint values for the gripper to be open and closed.
+double const OpenGripperJointValue = 0.038;
+double const ClosedGripperJointValue = 0.002;
+
+// The base orientation of the end effector resulting in the gripper pointing
+// straight down.
+double const EndEffectorBaseRoll = 0.0;
+double const EndEffectorBasePitch = std::numbers::pi;
+double const EndEffectorBaseYaw = tf2Radians(45.0);
+
 void ArmController::Initialize() {
   m_node = rclcpp::Node::make_shared("arm_controller");
   m_node->set_parameter(rclcpp::Parameter("use_sim_time", true));
@@ -35,7 +62,8 @@ geometry_msgs::msg::Pose ArmController::CalculatePose(double x, double y,
   pose.position.z = z;
 
   auto quat = tf2::Quaternion();
-  quat.setEuler(0.0, M_PI, tf2Radians(45.0) + r);
+  quat.setEuler(EndEffectorBaseRoll, EndEffectorBasePitch,
+                EndEffectorBaseYaw + r);
   pose.orientation.x = quat.x();
   pose.orientation.y = quat.y();
   pose.orientation.z = quat.z();
@@ -44,42 +72,46 @@ geometry_msgs::msg::Pose ArmController::CalculatePose(double x, double y,
   return pose;
 }
 
-bool ArmController::MoveThroughWaypoints(const std::vector<geometry_msgs::msg::Pose>& waypoints) {
+bool ArmController::MoveThroughWaypoints(
+    std::vector<geometry_msgs::msg::Pose> const &waypoints) {
   auto logger = m_node->get_logger();
 
-  const int NumTries = 10;
+  int const 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");
+                   "MoveThroughWaypoints: Failed to compute Cartesian path");
       continue;
     }
 
-    if (m_pandaArm->execute(trajectory) == moveit::core::MoveItErrorCode::SUCCESS) {
+    if (m_pandaArm->execute(trajectory) ==
+        moveit::core::MoveItErrorCode::SUCCESS) {
       return true;
     }
-    RCLCPP_ERROR(logger, "MoveThroughWaypoints: Failed to execute trajectory, trying again...");
+    RCLCPP_ERROR(logger, "MoveThroughWaypoints: Failed to execute "
+                         "trajectory, trying again...");
   }
 
-  RCLCPP_ERROR(logger, "MoveThroughWaypoints: Failed to execute trajectory after %d tries", NumTries);
+  RCLCPP_ERROR(
+      logger,
+      "MoveThroughWaypoints: Failed to execute trajectory after %d tries",
+      NumTries);
   return false;
 }
 
 void ArmController::Open() {
   gripper.store(true);
- // m_hand->setNamedTarget("open");
-   m_hand->setJointValueTarget("panda_finger_joint1", 0.038);
+  m_hand->setJointValueTarget("panda_finger_joint1", OpenGripperJointValue);
   while (m_hand->move() != moveit::core::MoveItErrorCode::SUCCESS) {
     RCLCPP_ERROR(m_node->get_logger(), "Failed to open hand");
   }
 }
 
 void ArmController::Close() {
   gripper.store(false);
-  m_hand->setJointValueTarget("panda_finger_joint1", 0.002);
-  // m_hand->setNamedTarget("close");
+  m_hand->setJointValueTarget("panda_finger_joint1", ClosedGripperJointValue);
   while (m_hand->move() != moveit::core::MoveItErrorCode::SUCCESS) {
     RCLCPP_ERROR(m_node->get_logger(), "Failed to close hand");
   }
@@ -91,9 +123,12 @@ std::vector<double> ArmController::GetEffectorPose() {
                                   pose.orientation.z, pose.orientation.w);
   tf2::Matrix3x3 m(rotation);
   m.getRPY(pose.orientation.x, pose.orientation.y, pose.orientation.z, 0);
-  return {pose.position.x,           pose.position.y,
-          pose.position.z,           pose.orientation.x,
-          pose.orientation.y - M_PI, pose.orientation.z - tf2Radians(135.0)};
+  return {pose.position.x,
+          pose.position.y,
+          pose.position.z,
+          pose.orientation.x,
+          pose.orientation.y - EndEffectorBasePitch,
+          pose.orientation.z - (tf2Radians(180.0) - EndEffectorBaseYaw)};
 };
 
 bool ArmController::GetGripper() { return gripper.load(); }
@@ -116,14 +151,13 @@ void ArmController::SetReferenceFrame(std::string const &frame) {
 }
 
 void ArmController::WaitForClockMessage() {
-  bool clock_received = false;
+  bool clockReceived = 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) {
+      "/clock", qos,
+      [&](rosgraph_msgs::msg::Clock::SharedPtr) { clockReceived = true; });
+  while (!clockReceived) {
     rclcpp::spin_some(m_node);
   }
   subscription.reset();

ros2_ws/src/robotic_manipulation/src/rai_manipulation_interface_node.cpp

@@ -0,0 +1,129 @@
+/* Copyright (C) 2025 Robotec.AI
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *         http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "robotic_manipulation/rai_manipulation_interface_node.h"
+
+#include <std_msgs/msg/float32_multi_array.hpp>
+
+RaiManipulationInterfaceNode::RaiManipulationInterfaceNode()
+    : m_node(rclcpp::Node::make_shared("state_controller")) {
+  m_node->set_parameter(rclcpp::Parameter("use_sim_time", true));
+}
+
+void RaiManipulationInterfaceNode::Initialize(ArmController &arm) {
+  auto logger = m_node->get_logger();
+
+  auto currentPose = arm.GetEffectorPose();
+  auto [currentX, currentY, currentZ, currentRX, currentRY, currentRZ] =
+      std::make_tuple(currentPose[0], currentPose[1], currentPose[2],
+                      currentPose[3], currentPose[4], currentPose[5]);
+
+  auto currentGripperState = arm.GetGripper();
+
+  m_startingPose = arm.CaptureJointValues();
+
+  RCLCPP_INFO(logger, "Current pose: %f %f %f %f %f %f", currentX, currentY,
+              currentRZ, currentRX, currentRY, currentRZ);
+  RCLCPP_INFO(logger, "Current gripper state: %d", currentGripperState);
+
+  m_moveToService = m_node->create_service<
+      rai_interfaces::srv::ManipulatorMoveTo>(
+      "/manipulator_move_to",
+      [&](std::shared_ptr<rai_interfaces::srv::ManipulatorMoveTo::Request> const
+              request,
+          std::shared_ptr<rai_interfaces::srv::ManipulatorMoveTo::Response>
+              response) {
+        RCLCPP_INFO(logger, "Received move request");
+
+        response->success = false;
+
+        arm.SetReferenceFrame(request->target_pose.header.frame_id);
+        RCLCPP_INFO(logger, "Set reference frame to: %s",
+                    request->target_pose.header.frame_id.c_str());
+
+        // Print current pose
+        auto currentPose = arm.GetEffectorPose();
+        auto [currentX, currentY, currentZ, currentRX, currentRY, currentRZ] =
+            std::make_tuple(currentPose[0], currentPose[1], currentPose[2],
+                            currentPose[3], currentPose[4], currentPose[5]);
+
+        RCLCPP_INFO(logger, "Current pose: %f %f %f %f %f %f", currentX,
+                    currentY, currentRZ, currentRX, currentRY, currentRZ);
+        RCLCPP_INFO(logger, "Target pose: %f %f %f %f %f %f",
+                    request->target_pose.pose.position.x,
+                    request->target_pose.pose.position.y,
+                    request->target_pose.pose.position.z,
+                    request->target_pose.pose.orientation.x,
+                    request->target_pose.pose.orientation.y,
+                    request->target_pose.pose.orientation.z);
+
+        if (request->initial_gripper_state) {
+          arm.Open();
+        } else {
+          arm.Close();
+        }
+
+        {
+          using std::min;
+          using std::max;
+          auto currentPose = arm.GetEffectorPose();
+          auto aboveCurrent = currentPose;
+          auto calculateZAboveTarget = [&](double targetZ) {
+            double const MinimumZ = 0.3;
+            double const MaximumZ = 0.4;
+            double const ZOffset = 0.1;
+
+            return min(MaximumZ, max(targetZ + ZOffset, MinimumZ));
+          };
+          aboveCurrent[2] = calculateZAboveTarget(aboveCurrent[2]);
+          auto aboveTarget = request->target_pose.pose;
+
+          aboveTarget.position.z =
+              calculateZAboveTarget(aboveTarget.position.z);
+          if (!arm.MoveThroughWaypoints(
+                  {arm.CalculatePose(aboveCurrent[0], aboveCurrent[1],
+                                     aboveCurrent[2]),
+                   aboveTarget, request->target_pose.pose}))
+            return;
+
+          if (request->final_gripper_state) {
+            arm.Open();
+          } else {
+            arm.Close();
+          }
+        }
+
+        if (!arm.MoveThroughWaypoints({request->target_pose.pose}))
+          return;
+
+        response->success = true;
+      });
+
+  RCLCPP_INFO(logger, "Service /manipulator_move_to is ready");
+
+  m_resetService = m_node->create_service<std_srvs::srv::Trigger>(
+      "/reset_manipulator",
+      [&]([[maybe_unused]] std::shared_ptr<
+              std_srvs::srv::Trigger::Request> const request,
+          std::shared_ptr<std_srvs::srv::Trigger::Response> response) {
+        RCLCPP_INFO(logger, "Received reset request");
+        response->success = arm.SetJointValues(m_startingPose);
+      });
+}
+
+void RaiManipulationInterfaceNode::Spin() {
+  m_executor.add_node(m_node);
+  m_executor.spin();
+}