The node doesn't instantly die anymore

src/ar3_config/launch/dev.launch.py

@@ -44,14 +44,10 @@ def generate_launch_description():
 
         Node(
             package='move_group_interface',
-            executable='move_group_interface_ndde',
+            executable='move_group_interface_node',
-            output='screen',
-        ),
-        Node(
-            package='move_group_interface',
-            executable='client_node',
             output='screen',
         ),
+
         Node(
             package='joystickmove',
             executable='move',

src/move_group_interface/CMakeLists.txt

@@ -19,22 +19,23 @@ endif()
 find_package(ament_cmake REQUIRED)
 find_package(rclcpp REQUIRED)
 find_package(geometry_msgs REQUIRED)
+find_package(moveit_ros_planning_interface REQUIRED)
 find_package(rosidl_default_generators REQUIRED)
 
 # Create service stuff
-rosidl_generate_interfaces(${PROJECT_NAME}
+#rosidl_generate_interfaces(${PROJECT_NAME}
-    "srv/MovePose.srv"
+#    "srv/MovePose.srv"
-  DEPENDENCIES geometry_msgs
+#  DEPENDENCIES geometry_msgs
-)
+#)
 
 # move_group_node
 add_executable(move_group_interface_node src/move_group_interface_node.cpp)
-ament_target_dependencies(move_group_interface_node rclcpp geometry_msgs)
+ament_target_dependencies(move_group_interface_node rclcpp geometry_msgs moveit_ros_planning_interface)
-rosidl_target_interfaces(move_group_interface_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
+#rosidl_target_interfaces(move_group_interface_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
-add_executable(client_node src/client_node.cpp)
+#add_executable(client_node src/client_node.cpp)
-ament_target_dependencies(client_node rclcpp)
+#ament_target_dependencies(client_node rclcpp)
-rosidl_target_interfaces(client_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
+#rosidl_target_interfaces(client_node ${PROJECT_NAME} "rosidl_typesupport_cpp")
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -48,7 +49,7 @@ if(BUILD_TESTING)
 endif()
 
 # Mark executables for installation
-install(TARGETS move_group_interface_node client_node
+install(TARGETS move_group_interface_node
     DESTINATION lib/${PROJECT_NAME})
 
 ament_package()

src/move_group_interface/package.xml

@@ -14,6 +14,7 @@
   <exec_depend>rosidl_default_runtime</exec_depend>
   <depend>rclcpp</depend>
   <depend>geometry_msgs</depend>
+  <depend>moveit_ros_planning_interface</depend>
 
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>

src/move_group_interface/src/client_node.cpp

@@ -1,71 +0,0 @@
-#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 moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
-        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>();
-        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) {
-            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;
-        }
-
-        // 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,
-            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_;
-    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,122 +1,93 @@
+// https://github.com/ros-planning/moveit2_tutorials/blob/main/doc/examples/moveit_cpp/src/moveit_cpp_tutorial.cpp
+
 #include <rclcpp/rclcpp.hpp>
 #include <string>
+#include <thread>
 
 // 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 = "ar3_arm";
 
-const std::string PLANNING_GROUP = "base_link";
+class ScanNPlan : public rclcpp::Node {
-
-class Localizer : public rclcpp::Node {
 public:
-        Localizer() : Node("move_group_interface_node") {//, last_msg_{nullptr} {
+    ScanNPlan() : Node("scan_n_plan") {
+        // create the node and start it
+        nh_ = std::make_shared<rclcpp::Node>("scan_n_plan");
+
+        // spin the node
+        executor_ = std::make_shared<rclcpp::executors::SingleThreadedExecutor>();
+        executor_->add_node(nh_);
+
         /*
-            pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
+        RCLCPP_INFO(nh_->get_logger(), "scan and plan in constructor");
+        auto spin = [this]() {
+          while(rclcpp::ok()) {
+            executor_->spin_once();
+            RCLCPP_INFO(nh_->get_logger(), "scan and plan still running");
+          }
+        };
+        thread_executor_spin_ = std::thread(spin);
+        */
+
+        // create subscriber
+        pose_sub_ = nh_->create_subscription<geometry_msgs::msg::PoseStamped>(
             "move_pose",
             rclcpp::QoS(1),
-                std::bind(&Localizer::moveCallback, this, std::placeholders::_1)
+            std::bind(&ScanNPlan::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)
         );
     }
 
-        /*
+private:
-        void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr msg) {
+    void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
-            last_msg_ = msg;
+        auto moveit_cpp_ptr = std::make_shared<moveit_cpp::MoveItCpp>(nh_);
-        }
-        */
 
-        void movePose(move_group_interface::srv::MovePose::Request::SharedPtr req,
+        //auto psm = std::make_shared<planning_scene_monitor::PlanningSceneMonitor>("robot_description");
-                      move_group_interface::srv::MovePose::Response::SharedPtr res) {
+        //psm->startSceneMonitor();
-            // Read last message
-            geometry_msgs::msg::PoseStamped::SharedPtr p = req->pose; // last_msg_;
-
-            if (!p) {
-                RCLCPP_ERROR(this->get_logger(), "no data");
-                res->success = false;
-                return;
-            }
-
-            // 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();
 
+
+        // planning_scene_monitor::LockedPlanningSceneRW scene{moveit_cpp_ptr->getPlanningSceneMonitor()};
+
         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");
+        planning_components->setGoal(*p, "base_link");
 
         // 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;
     }
+private:
+    rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
 
-        // rclcpp::Subscription<geometry_msgs::msg::PoseStamped>::SharedPtr pose_sub_;
+    //please
-        // geometry_msgs::msg::PoseStamped::SharedPtr last_msg_;
+    rclcpp::Node::SharedPtr nh_;
-        rclcpp::Service<move_group_interface::srv::MovePose>::SharedPtr server_;
+    rclcpp::executors::SingleThreadedExecutor::SharedPtr executor_;
+    std::thread thread_executor_spin_;
 };
 
 int main(int argc, char* argv[]) {
     rclcpp::init(argc, argv);
 
-    auto node = std::make_shared<Localizer>();
+    auto node = std::make_shared<ScanNPlan>();
-
     RCLCPP_INFO(node->get_logger(), "MoveGroup node started.");
 
     rclcpp::spin(node);
+
+    RCLCPP_INFO(node->get_logger(), "Terminating MoveGroup");
+    rclcpp::shutdown();
+    return 0;
 }
+