From 9bc25c69e15e7e71d7b1fefb0f5694ec20203193 Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Mon, 27 Mar 2023 11:44:25 +0300
Subject: [PATCH] Move execution to new ROS2 node

---
 src/lite6_controller/CMakeLists.txt           | 11 +++++++
 .../launch/lite6_gazebo.launch.py             | 11 +++++++
 src/lite6_controller/src/lite6_controller.cpp | 30 ++++++++++++++++++-
 3 files changed, 51 insertions(+), 1 deletion(-)

diff --git a/src/lite6_controller/CMakeLists.txt b/src/lite6_controller/CMakeLists.txt
index 5c26af6..5766c48 100644
--- a/src/lite6_controller/CMakeLists.txt
+++ b/src/lite6_controller/CMakeLists.txt
@@ -44,6 +44,16 @@ ament_target_dependencies(lite6_controller
   "robot_controller"
   "moveit_ros_planning_interface"
   "robot_interfaces")
+add_executable(lite6_trajectory_executor src/lite6_trajectory_executor.cpp)
+ament_target_dependencies(lite6_trajectory_executor
+  "rclcpp"
+  "moveit"
+  "rclcpp_action"
+  "Eigen3"
+  "pilz_industrial_motion_planner"
+  "robot_controller"
+  "moveit_ros_planning_interface"
+  "robot_interfaces")
 
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
@@ -52,6 +62,7 @@ endif()
 
 install(TARGETS
   lite6_controller
+  lite6_trajectory_executor
   DESTINATION lib/${PROJECT_NAME})
 
 install(DIRECTORY launch DESTINATION share/${PROJECT_NAME})
diff --git a/src/lite6_controller/launch/lite6_gazebo.launch.py b/src/lite6_controller/launch/lite6_gazebo.launch.py
index 421a0f8..aac6518 100644
--- a/src/lite6_controller/launch/lite6_gazebo.launch.py
+++ b/src/lite6_controller/launch/lite6_gazebo.launch.py
@@ -191,6 +191,17 @@ def launch_setup(context, *args, **kwargs):
                 {"use_sim_time": use_sim_time},
             ],
         ),
+        Node(
+            package="lite6_controller",
+            executable="lite6_trajectory_executor",
+            output="log",
+            arguments=["--ros-args", "--log-level", log_level],
+            parameters=[
+                robot_description,
+                robot_description_semantic,
+                {"use_sim_time": use_sim_time},
+            ],
+        ),
         Node(
             package="virtual_drawing_surface",
             executable="drawing_surface.py",
diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp
index 7d0b9b4..ee6225a 100644
--- a/src/lite6_controller/src/lite6_controller.cpp
+++ b/src/lite6_controller/src/lite6_controller.cpp
@@ -72,6 +72,8 @@ public:
   rclcpp::TimerBase::SharedPtr trajectory_timer_;
   bool busy = false;
 
+  rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr trajectory_pub;
+
   /**
    * TODO Use instead of MoveGroupInterface
    * https://industrial-training-master.readthedocs.io/en/foxy/_source/session4/ros2/0-Motion-Planning-CPP.html
@@ -124,6 +126,9 @@ public:
     //this->move_group.setMaxVelocityScalingFactor(1.0);
     this->move_group.setMaxVelocityScalingFactor(0.8);
 
+
+    trajectory_pub = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("lite6_trajectory", 100);
+
     // Subscribe to target pose
     //target_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>("/target_pose", rclcpp::QoS(1), std::bind(&MoveItFollowTarget::target_pose_callback, this, std::placeholders::_1));
 
@@ -441,7 +446,8 @@ public:
       RCLCPP_INFO(this->get_logger(), "Got %ld trajectories", ts.size());
       RCLCPP_INFO(this->get_logger(), "Adding result to motion queue");
 
-      trajectory_queue.push(ts[0]);
+      //trajectory_queue.push(ts[0]);
+      this->trajectory_pub->publish(ts[0]);
 
       // Set move_group_state to the last state of planned trajectory (planning of next trajectory starts there)
       robot_trajectory::RobotTrajectory rt(move_group.getRobotModel());
@@ -471,6 +477,27 @@ public:
   }
 };
 
+class TrajectoryExecutor : public rclcpp::Node
+{
+  public:
+    //trajectory_pub = this->create_publisher<moveit_msgs::msg::RobotTrajectory>("lite6_trajectory", 10);
+    rclcpp::Subscription<moveit_msgs::msg::RobotTrajectory>::SharedPtr subscription;
+    moveit::planning_interface::MoveGroupInterface move_group;
+    TrajectoryExecutor()
+      : Node("lite6_trajectory_executor"),
+        move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+    {
+      subscription = this->create_subscription<moveit_msgs::msg::RobotTrajectory>(
+        "lite6_trajectory", 100, std::bind(&TrajectoryExecutor::trajectory_callback, this, std::placeholders::_1));
+    }
+    void trajectory_callback(const moveit_msgs::msg::RobotTrajectory msg)
+    {
+      RCLCPP_INFO(this->get_logger(), "Received trajectory, executing");
+      move_group.execute(msg);
+      RCLCPP_INFO(this->get_logger(), "Finished executing trajectory");
+    }
+};
+
 /**
  * Starts lite6_controller
  */
@@ -491,6 +518,7 @@ int main(int argc, char ** argv)
   //rclcpp::executors::SingleThreadedExecutor executor;
   rclcpp::executors::MultiThreadedExecutor executor;
   executor.add_node(lite6);
+  //executor.add_node(trajectory_executor);
   executor.spin();
 
   rclcpp::shutdown();