Refactor the robotic manipulation package

ros2_ws/src/robotic_manipulation/.clang-format

@@ -0,0 +1,18 @@
+Language: Cpp
+BasedOnStyle: Google
+
+AccessModifierOffset: -2
+AlignAfterOpenBracket: AlwaysBreak
+BraceWrapping:
+  AfterClass: true
+  AfterFunction: true
+  AfterNamespace: true
+  AfterStruct: true
+  AfterEnum: true
+BreakBeforeBraces: Custom
+ColumnLimit: 100
+ConstructorInitializerIndentWidth: 0
+ContinuationIndentWidth: 2
+DerivePointerAlignment: false
+PointerAlignment: Middle
+ReflowComments: false

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,18 +1,33 @@
+/* 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>
 
-class ArmController {
+class ArmController
+{
 public:
   ArmController() = default;
   ~ArmController();
 
   void Initialize();
 
-  geometry_msgs::msg::Pose CalculatePose(double x, double y, double z,
-                                         double r = 0.0);
+  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();
@@ -21,11 +36,21 @@ class ArmController {
   bool GetGripper();
 
   std::vector<double> CaptureJointValues();
-  bool SetJointValues(std::vector<double> const &jointValues);
+  bool SetJointValues(std::vector<double> const & jointValues);
 
-  void SetReferenceFrame(std::string const &frame);
+  void SetReferenceFrame(std::string const & frame);
 
 private:
+  // The joint values for the gripper to be open and closed.
+  static double constexpr OpenGripperJointValue = 0.038;
+  static double constexpr ClosedGripperJointValue = 0.002;
+
+  // The base orientation of the end effector resulting in the gripper pointing
+  // straight down.
+  static double constexpr EndEffectorBaseRoll = 0.0;
+  static double constexpr EndEffectorBasePitch = std::numbers::pi;
+  static double constexpr EndEffectorBaseYaw = 45.0 * std::numbers::pi / 180.0;
+
   void WaitForClockMessage();
 
   std::shared_ptr<moveit::planning_interface::MoveGroupInterface> m_pandaArm;

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 <std_srvs/srv/trigger.hpp>
+
+#include "rai_interfaces/srv/manipulator_move_to.hpp"
+#include "robotic_manipulation/arm_controller.h"
+
+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,8 +1,25 @@
+/* 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 <numbers>
 #include <rosgraph_msgs/msg/clock.hpp>
 
-void ArmController::Initialize() {
+void ArmController::Initialize()
+{
   m_node = rclcpp::Node::make_shared("arm_controller");
   m_node->set_parameter(rclcpp::Parameter("use_sim_time", true));
 
@@ -11,31 +28,31 @@ void ArmController::Initialize() {
   m_executor.add_node(m_node);
   m_spinner = std::thread([this]() { m_executor.spin(); });
 
-  m_pandaArm = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
-      m_node, "panda_arm");
-  m_hand = std::make_shared<moveit::planning_interface::MoveGroupInterface>(
-      m_node, "hand");
+  m_pandaArm =
+    std::make_shared<moveit::planning_interface::MoveGroupInterface>(m_node, "panda_arm");
+  m_hand = std::make_shared<moveit::planning_interface::MoveGroupInterface>(m_node, "hand");
 
   m_pandaArm->setMaxVelocityScalingFactor(1.0);
   m_pandaArm->setMaxAccelerationScalingFactor(1.0);
 }
 
-ArmController::~ArmController() {
+ArmController::~ArmController()
+{
   m_executor.cancel();
   if (m_spinner.joinable()) {
     m_spinner.join();
   }
 }
 
-geometry_msgs::msg::Pose ArmController::CalculatePose(double x, double y,
-                                                      double z, double r) {
+geometry_msgs::msg::Pose ArmController::CalculatePose(double x, double y, double z, double r)
+{
   geometry_msgs::msg::Pose pose;
   pose.position.x = x;
   pose.position.y = 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,65 +61,75 @@ 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 constexpr 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");
+    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...");
+    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() {
+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() {
+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");
   }
 }
 
-std::vector<double> ArmController::GetEffectorPose() {
+std::vector<double> ArmController::GetEffectorPose()
+{
   auto pose = m_pandaArm->getCurrentPose().pose;
-  auto rotation = tf2::Quaternion(pose.orientation.x, pose.orientation.y,
-                                  pose.orientation.z, pose.orientation.w);
+  auto rotation =
+    tf2::Quaternion(pose.orientation.x, pose.orientation.y, 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(); }
 
-std::vector<double> ArmController::CaptureJointValues() {
+std::vector<double> ArmController::CaptureJointValues()
+{
   return m_pandaArm->getCurrentJointValues();
 }
 
-bool 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");
@@ -111,19 +138,19 @@ bool ArmController::SetJointValues(std::vector<double> const &jointValues) {
   return true;
 }
 
-void ArmController::SetReferenceFrame(std::string const &frame) {
+void ArmController::SetReferenceFrame(std::string const & frame)
+{
   m_pandaArm->setPoseReferenceFrame(frame);
 }
 
-void ArmController::WaitForClockMessage() {
-  bool clock_received = false;
+void ArmController::WaitForClockMessage()
+{
+  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();