Fuck yes. move_group_interface is basically done

src/action_server/include/action_server/action_server.hpp

@@ -1,20 +0,0 @@
-#include "rclcpp/rclcpp.hpp"
-
-#include "geometry_msgs/msg/pose.hpp"
-
-#include <string>
-
-class ActionServer : public rclcpp::Node {
-    public:
-        ActionServer();
-
-    private:
-        void set_pose_cb(geometry_msgs::msg::Pose);
-
-    private:
-        rclcpp::Node::SharedPtr _move_group_node;
-
-        rclcpp::Subscription<geometry_msgs::msg::Pose> _goal;
-
-        const std::string PLANNING_GROUP = "ar3_arm";
-};

src/action_server/src/action_server.cpp

@@ -1,40 +0,0 @@
-// https://github.com/ros-planning/moveit2_tutorials/blob/foxy/doc/move_group_interface/src/move_group_interface_tutorial.cpp
-// maybe use this: https://github.com/ros-planning/moveit2/issues/775
-
-#include <action_server/action_server.hpp>
-#include <moveit/move_group_interface/move_group_interface.h>
-
-ActionServer::ActionServer() : rclcpp::Node("action_server") {
-    // Get the move_group we'll apply transformations to
-    moveit::planning_interface::MoveGroupInterface move_group(ActionServer::_move_group_node, PLANNING_GROUP);
-
-    // Define the planning scene
-    moveit::planning_interface::PlanningSceneInterface planning_scene_interface;
-
-    // Define a pointer to the planning scene because tutorial says it's faster
-    const moveit::core::JointModelGroup* joint_model_group = move_group.getCurrentState()->getJointModelGroup(ActionServer::PLANNING_GROUP);
-
-    // Set up the callback for settings the pose from camera/joystick/etc
-    ActionServer::_goal = _move_group_node->create_subscription<geometry_msgs::msg::Pose>("/set_pose", 10, std::bind(&ActionServer::set_pose_cb, this, std::placeholders::_1));
-}
-
-void set_pose_cb(geometry_msgs::msg::Pose::SharedPtr pose) {
-    move_group.setPoseTarget(pose);
-
-    // plan the trajectory for the move
-    moveit::planning_interface::MoveGroupInterface::Plan my_plan;
-    bool success = (move_group.plan(my_plan) == moveit::planning_interface::MoveItErrorCode::SUCCESS);
-
-    move_group.move();
-}
-
-int main(int argc, char **argv) {
-    rclcpp::init(argc, argv);
-
-    _move_group_node = std::make_shared<ActionServer>();
-    rclcpp::spin(move_group_node);
-
-    rclcpp::shutdown();
-    return 0;
-}
-

src/move_group_interface/CMakeLists.txt

@@ -1,5 +1,5 @@
 cmake_minimum_required(VERSION 3.5)
-project(action_server)
+project(move_group_interface)
 
 # Default to C99
 if(NOT CMAKE_C_STANDARD)
@@ -19,20 +19,22 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(rosidl_default_generators REQUIRED)
 
-# uncomment the following section in order to fill in
+# Create service stuff
-# further dependencies manually.
+rosidl_generate_interfaces(${PROJECT_NAME}
-# find_package(<dependency> REQUIRED)
+    "srv/MovePose.srv"
+  DEPENDENCIES geometry_msgs
+)
 
-add_executable(action_server src/action_server.cpp)
+# move_group_node
-target_include_directories(action_server PUBLIC
+add_executable(move_group_interface_node src/move_group_interface_node.cpp)
-  $<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>
+ament_target_dependencies(move_group_interface_node rclcpp geometry_msgs)
-  $<INSTALL_INTERFACE:include>)
+rosidl_target_interfaces(move_group_interface_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
-ament_target_dependencies(action_server rclcpp geometry_msgs)
+add_executable(client_node src/client_node.cpp)
-
+ament_target_dependencies(client_node rclcpp)
-install(TARGETS action_server
+rosidl_target_interfaces(client_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
-  DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -45,4 +47,8 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
+# Mark executables for installation
+install(TARGETS move_group_interface_node client_node
+    DESTINATION lib/${PROJECT_NAME})
+
 ament_package()

src/move_group_interface/package.xml

@@ -1,14 +1,17 @@
 <?xml version="1.0"?>
 <?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
-  <name>action_server</name>
+  <name>move_group_interface</name>
   <version>1.0.0</version>
-  <description>Server for handling absolute movements</description>
+  <description>MoveGroupInterface but for ros2</description>
-  <maintainer email="tmuller2017@my.fit.edu">Thomas Muller</maintainer>
+  <maintainer email="farrell2017@my.fit.edu">john</maintainer>
-  <maintainer email="farrell2017@my.fit.edu">John Farrell</maintainer>
   <license>TODO: License declaration</license>
+  <member_of_group>rosidl_interface_packages</member_of_group>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
+
+  <build_depend>rosidl_default_generators</build_depend>
+  <exec_depend>rosidl_default_runtime</exec_depend>
   <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
 

src/move_group_interface/src/client_node.cpp

@@ -0,0 +1,59 @@
+#include <rclcpp/rclcpp.hpp>
+#include <move_group_interface/srv/move_pose.hpp>
+
+class ScanNPlan : public rclcpp::Node {
+public:
+    ScanNPlan() : Node("scan_n_plan") {
+        move_client_ = this->create_client<move_group_interface::srv::MovePose>("update_pose");
+    }
+
+    void start() {
+        RCLCPP_INFO(get_logger(), "Attempting to move to pose");
+
+        // Wait for service to be available
+        if (!move_client_->wait_for_service(std::chrono::seconds(5))) {
+            RCLCPP_ERROR(get_logger(), "Unable to find move_pose service. Start move_pose first.");
+            return;
+        }
+
+        // Create a request for the MovePose service call
+        auto request = std::make_shared<move_group_interface::srv::MovePose::Request>();
+
+        auto future = move_client_->async_send_request(request);
+
+        if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS) {
+            RCLCPP_ERROR(this->get_logger(), "Failed to receive MovePose service response");
+            return;
+        }
+
+        auto response = future.get();
+        if (!response->success) {
+            RCLCPP_ERROR(this->get_logger(), "MovePose service failed");
+            return;
+        }
+
+        RCLCPP_INFO(this->get_logger(), "Moved to pose: x: %f, y: %f, z: %f - x: %f, y: %f, z: %f, w: %f",
+            response->pose.position.x,
+            response->pose.position.y,
+            response->pose.position.z,
+
+            response->pose.orientation.x,
+            response->pose.orientation.y,
+            response->pose.orientation.z,
+            response->pose.orientation.w
+        );
+    }
+private:
+    rclcpp::Client<move_group_interface::srv::MovePose>::SharedPtr move_client_;
+
+};
+
+int main(int argc, char **argv) {
+    rclcpp::init(argc, argv);
+
+    auto app = std::make_shared<ScanNPlan>();
+    app->start();
+
+    rclcpp::shutdown();
+    return 0;
+}

src/move_group_interface/src/move_group_interface_node.cpp

@@ -0,0 +1,54 @@
+#include <rclcpp/rclcpp.hpp>
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+#include <move_group_interface/srv/move_pose.hpp>
+
+class Localizer : public rclcpp::Node {
+    public:
+        Localizer() : Node("move_group_interface_node"), last_msg_{nullptr} {
+            pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+                "move_pose",
+                rclcpp::QoS(1),
+                std::bind(&Localizer::moveCallback, this, std::placeholders::_1)
+            );
+
+            server_ = this->create_service<move_group_interface::srv::MovePose>(
+                "update_pose",
+                std::bind(&Localizer::movePose, this, std::placeholders::_1, std::placeholders::_2)
+            );
+        }
+
+        void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+            last_msg_ = msg;
+        }
+
+        void movePose(move_group_interface::srv::MovePose::Request::SharedPtr req,
+                      move_group_interface::srv::MovePose::Response::SharedPtr res) {
+            // Read last message
+            geometry_msgs::msg::PoseStamped::SharedPtr p = last_msg_;
+
+            if (!p) {
+                RCLCPP_ERROR(this->get_logger(), "no data");
+                res->success = false;
+                return;
+            }
+
+            res->header = p->header;
+            res->pose = p->pose;
+            res->success = true;
+        }
+
+        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+        geometry_msgs::msg::PoseStamped::SharedPtr last_msg_;
+        rclcpp::Service<move_group_interface::srv::MovePose>::SharedPtr server_;
+};
+
+int main(int argc, char* argv[]) {
+    rclcpp::init(argc, argv);
+
+    auto node = std::make_shared<Localizer>();
+
+    RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
+
+    rclcpp::spin(node);
+}

src/move_group_interface/srv/MovePose.srv

@@ -0,0 +1,7 @@
+#request
+string base_frame
+---
+#response
+std_msgs/Header header
+geometry_msgs/Pose pose
+bool success