diff --git a/ros2_ws/src/robotic_manipulation/.clang-format b/ros2_ws/src/robotic_manipulation/.clang-format new file mode 100644 index 0000000..619b4ab --- /dev/null +++ b/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 diff --git a/ros2_ws/src/robotic_manipulation/CMakeLists.txt b/ros2_ws/src/robotic_manipulation/CMakeLists.txt index 6130b8e..0561e2e 100644 --- a/ros2_ws/src/robotic_manipulation/CMakeLists.txt +++ b/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 $ $) -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" 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 7566c85..2bf709d 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,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 -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& waypoints); + bool MoveThroughWaypoints(std::vector const & waypoints); void Open(); void Close(); @@ -21,11 +36,21 @@ class ArmController { bool GetGripper(); std::vector CaptureJointValues(); - bool SetJointValues(std::vector const &jointValues); + bool SetJointValues(std::vector 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 m_pandaArm; diff --git a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/rai_manipulation_interface_node.h b/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/rai_manipulation_interface_node.h new file mode 100644 index 0000000..614e5dd --- /dev/null +++ b/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 + +#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::SharedPtr m_moveToService; + rclcpp::Service::SharedPtr m_resetService; + + rclcpp::executors::SingleThreadedExecutor m_executor; + + std::vector m_startingPose; +}; \ No newline at end of file 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 deleted file mode 100644 index 10dfd28..0000000 --- a/ros2_ws/src/robotic_manipulation/include/robotic_manipulation/state_controller.h +++ /dev/null @@ -1,17 +0,0 @@ -#pragma once - -#include "robotic_manipulation/arm_controller.h" - -class StateController { -public: - StateController(); - ~StateController(); - - void Begin(ArmController &arm); - -private: - 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/package.xml b/ros2_ws/src/robotic_manipulation/package.xml index ece9822..791f954 100644 --- a/ros2_ws/src/robotic_manipulation/package.xml +++ b/ros2_ws/src/robotic_manipulation/package.xml @@ -3,9 +3,9 @@ robotic_manipulation 0.0.0 - TODO: Package description + A package providing services for robotic manipulation using RAI. kdabrowski - TODO: License declaration + Apache 2.0 ament_cmake diff --git a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp b/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp index 4fafc69..4d68921 100644 --- a/ros2_ws/src/robotic_manipulation/src/arm_controller.cpp +++ b/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 #include -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( - m_node, "panda_arm"); - m_hand = std::make_shared( - m_node, "hand"); + m_pandaArm = + std::make_shared(m_node, "panda_arm"); + m_hand = std::make_shared(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& waypoints) { +bool ArmController::MoveThroughWaypoints(std::vector 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 ArmController::GetEffectorPose() { +std::vector 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 ArmController::CaptureJointValues() { +std::vector ArmController::CaptureJointValues() +{ return m_pandaArm->getCurrentJointValues(); } -bool 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"); @@ -111,19 +138,19 @@ bool ArmController::SetJointValues(std::vector 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( - "/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(); diff --git a/ros2_ws/src/robotic_manipulation/src/rai_manipulation_interface_node.cpp b/ros2_ws/src/robotic_manipulation/src/rai_manipulation_interface_node.cpp new file mode 100644 index 0000000..60ded72 --- /dev/null +++ b/ros2_ws/src/robotic_manipulation/src/rai_manipulation_interface_node.cpp @@ -0,0 +1,126 @@ +/* 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 + +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( + "/manipulator_move_to", + [&]( + std::shared_ptr const request, + std::shared_ptr 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 constexpr MinimumZ = 0.3; + double constexpr MaximumZ = 0.4; + double constexpr 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( + "/reset_manipulator", + [&]( + [[maybe_unused]] std::shared_ptr const request, + std::shared_ptr 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(); +} \ 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 6583e8e..89d8447 100644 --- a/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp +++ b/ros2_ws/src/robotic_manipulation/src/robotic_manipulation.cpp @@ -1,18 +1,38 @@ +/* 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 +#include #include "robotic_manipulation/arm_controller.h" -#include "robotic_manipulation/state_controller.h" +#include "robotic_manipulation/rai_manipulation_interface_node.h" -#include - -int main(int argc, char *argv[]) { +int main(int argc, char * argv[]) +{ // Initialize ROS and create the Node rclcpp::init(argc, argv); auto armController = std::make_shared(); armController->Initialize(); - armController->MoveThroughWaypoints({armController->CalculatePose(0.3, 0.0, 0.35)}); + double constexpr EndEffectorBaseX = 0.3; + double constexpr EndEffectorBaseY = 0.0; + double constexpr EndEffectorBaseZ = 0.35; + + armController->MoveThroughWaypoints( + {armController->CalculatePose(EndEffectorBaseX, EndEffectorBaseY, EndEffectorBaseZ)}); std::vector startingPose; startingPose = armController->CaptureJointValues(); @@ -20,8 +40,11 @@ int main(int argc, char *argv[]) { armController->SetJointValues(startingPose); armController->Close(); - StateController state; - state.Begin(*armController); + { + RaiManipulationInterfaceNode raiInterface; + raiInterface.Initialize(*armController); + raiInterface.Spin(); + } armController->SetJointValues(startingPose); diff --git a/ros2_ws/src/robotic_manipulation/src/state_controller.cpp b/ros2_ws/src/robotic_manipulation/src/state_controller.cpp deleted file mode 100644 index f89ae4e..0000000 --- a/ros2_ws/src/robotic_manipulation/src/state_controller.cpp +++ /dev/null @@ -1,112 +0,0 @@ -#include "robotic_manipulation/state_controller.h" - -#include -#include -#include "rai_interfaces/srv/manipulator_move_to.hpp" - -StateController::StateController() - : m_node(rclcpp::Node::make_shared("state_controller")) -{ - m_node->set_parameter(rclcpp::Parameter("use_sim_time", true)); -} - -StateController::~StateController() { - m_executor.cancel(); - if (m_spinner.joinable()) { - m_spinner.join(); - } -} - -void StateController::Begin(ArmController &arm) { - auto logger = m_node->get_logger(); - - auto current_pose = arm.GetEffectorPose(); - auto [current_x, current_y, current_z, current_rx, current_ry, current_rz] = - std::make_tuple(current_pose[0], current_pose[1], current_pose[2], - current_pose[3], current_pose[4], current_pose[5]); - - 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); - - auto service = m_node->create_service( - "/manipulator_move_to", - [&](const std::shared_ptr request, - std::shared_ptr 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 current_pose = arm.GetEffectorPose(); - auto [current_x, current_y, current_z, current_rx, current_ry, current_rz] = - std::make_tuple(current_pose[0], current_pose[1], current_pose[2], - current_pose[3], current_pose[4], current_pose[5]); - - 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, "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 current_pose = arm.GetEffectorPose(); - auto above_current = current_pose; - above_current[2] = min(0.4, max(above_current[2] + 0.1, 0.3)); - auto above_target = request->target_pose.pose; - - above_target.position.z = min(0.4, max(above_target.position.z + 0.1, 0.3)); - if (!arm.MoveThroughWaypoints( - {arm.CalculatePose(above_current[0], above_current[1], - above_current[2]), - above_target, 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"); - - 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(); -} \ No newline at end of file