res/req and vizualization not working reeeee

src/ar3_config/launch/dev.launch.py

@@ -42,6 +42,16 @@ def generate_launch_description():
             arguments=['0', '0', '0', '0', '0', '0', 'world', 'base_link']
         ),
 
+        Node(
+            package='move_group_interface',
+            executable='move_group_interface_ndde',
+            output='screen',
+        ),
+        Node(
+            package='move_group_interface',
+            executable='client_node',
+            output='screen',
+        ),
         Node(
             package='joystickmove',
             executable='move',

src/move_group_interface/src/client_node.cpp

@@ -1,13 +1,19 @@
 #include <rclcpp/rclcpp.hpp>
 #include <move_group_interface/srv/move_pose.hpp>
+#include "geometry_msgs/msg/pose_stamped.hpp"
 
 class ScanNPlan : public rclcpp::Node {
 public:
     ScanNPlan() : Node("scan_n_plan") {
         move_client_ = this->create_client<move_group_interface::srv::MovePose>("update_pose");
+        pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+            "move_pose",
+            rclcpp::QoS(1),
+            std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1)
+        );
     }
 
-    void start() {
+    void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
         RCLCPP_INFO(get_logger(), "Attempting to move to pose");
 
         // Wait for service to be available
@@ -18,7 +24,7 @@ public:
 
         // Create a request for the MovePose service call
         auto request = std::make_shared<move_group_interface::srv::MovePose::Request>();
-
+        request->pose = msg;
         auto future = move_client_->async_send_request(request);
 
         if (rclcpp::spin_until_future_complete(this->get_node_base_interface(), future) != rclcpp::executor::FutureReturnCode::SUCCESS) {
@@ -32,6 +38,9 @@ public:
             return;
         }
 
+        // Successfully moved
+
+        /*
         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,
@@ -42,17 +51,20 @@ public:
             response->pose.orientation.z,
             response->pose.orientation.w
         );
+        */
     }
 private:
     rclcpp::Client<move_group_interface::srv::MovePose>::SharedPtr move_client_;
-
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
 };
 
 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

@@ -1,16 +1,31 @@
 #include <rclcpp/rclcpp.hpp>
-#include "geometry_msgs/msg/pose_stamped.hpp"
+#include <string>
+
+// MoveitCpp
+#include <moveit/moveit_cpp/moveit_cpp.h>
+#include <moveit/moveit_cpp/planning_component.h>
+
+// https://github.com/ros-planning/moveit_visual_tools/tree/foxy
+// #include <moveit_visual_tools/moveit_visual_tools.h>
 
 #include <move_group_interface/srv/move_pose.hpp>
 
+#include "geometry_msgs/msg/pose_stamped.hpp"
+
+// namespace rvt = rviz_visual_tools;
+
+const std::string PLANNING_GROUP = "base_link";
+
 class Localizer : public rclcpp::Node {
     public:
-        Localizer() : Node("move_group_interface_node"), last_msg_{nullptr} {
+        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",
@@ -18,14 +33,16 @@ class Localizer : public rclcpp::Node {
             );
         }
 
+        /*
         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_;
+            geometry_msgs::msg::PoseStamped::SharedPtr p = req->pose; // last_msg_;
 
             if (!p) {
                 RCLCPP_ERROR(this->get_logger(), "no data");
@@ -33,13 +50,64 @@ class Localizer : public rclcpp::Node {
                 return;
             }
 
-            res->header = p->header;
+            // https://moveit.picknik.ai/galactic/doc/examples/moveit_cpp/moveitcpp_tutorial.html
+
+            // set up with current position
+            auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(this);
+            moveit_cpp_ptr->getPlanningSceneMonitor()->providePlanningSceneService();
+
+            auto planning_components = std::make_shared<moveit_cpp::PlanningComponent>(PLANNING_GROUP, moveit_cpp_ptr);
+            auto robot_model_ptr = moveit_cpp_ptr->getRobotModel();
+            auto robot_start_state = planning_components->getStartState();
+            // auto joint_model_group_ptr = robot_model_ptr->getJointModelGroup(PLANNING_GROUP);
+
+            // set up visualizations
+            /*
+            moveit_visual_tools::MoveItVisualTools visual_tools(node, "base_link", "move_group_interface", moveit_cpp_ptr->getPlanningSceneMonitor());
+            visual_tools.deleteAllMarkers();
+            visual_tools.loadRemoteControl();
+
+            Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
+            text_pose.translation().z() = 1.75;
+
+            visual_tools.trigger();
+            */
+
+            // just in case
+            planning_components->setStartStateToCurrentState();
+
+            // set the goal to the request pose
+            planning_components->setGoal(p, "ar3_arm");
+
+            // actually plan
+            auto plan_solution = planning_components->plan();
+
+            // plan is valid
+            if (plan_solution) {
+                // display the robot in rviz
+                /*
+                visual_tools.publishAxisLabeled(robot_start_state->getGlobalLinkTransform("base_link"), "start_pose");
+                visual_tools.publishText(text_pose, "Start Pose", rvt::WHITE, rvt::XLARGE);
+
+                visual_tools.publishAxisLabeled(target_pose1.pose, "target_pose");
+                visual_tools.publishText(text_pose, "Goal Pose", rvt::WHITE, rvt::XLARGE);
+
+                visual_tools.publishTrajectoryLine(plan_solution.trajectory, joint_model_group_ptr);
+                visual_tools.trigger();
+                */
+
+                // Execute plan
+                planning_components->execute();
+            }
+
+            // set response variables to updated robot states
+            res->header = p->header; // TODO: change these to current state (post move)
             res->pose = p->pose;
             res->success = true;
         }
 
-        rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+        // rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
-        geometry_msgs::msg::PoseStamped::SharedPtr last_msg_;
+        // geometry_msgs::msg::PoseStamped::SharedPtr last_msg_;
         rclcpp::Service<move_group_interface::srv::MovePose>::SharedPtr server_;
 };
 

src/move_group_interface/srv/MovePose.srv

@@ -1,5 +1,6 @@
 #request
 string base_frame
+geometry_msgs/PoseStamped pose
 ---
 #response
 std_msgs/Header header