From e3a68e95c3126a9c013e261b726bf9313d13549f Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Wed, 11 Jan 2023 11:12:18 +0200
Subject: [PATCH] Add pilz

---
 src/lite6_controller/CMakeLists.txt           |  2 ++
 src/lite6_controller/src/lite6_controller.cpp | 15 ++++++++++++++-
 2 files changed, 16 insertions(+), 1 deletion(-)

diff --git a/src/lite6_controller/CMakeLists.txt b/src/lite6_controller/CMakeLists.txt
index e792711..1ac8c21 100644
--- a/src/lite6_controller/CMakeLists.txt
+++ b/src/lite6_controller/CMakeLists.txt
@@ -13,6 +13,7 @@ find_package(rclcpp_action REQUIRED)
 find_package(robot_interfaces REQUIRED)
 find_package(robot_controller REQUIRED)
 find_package(moveit REQUIRED)
+find_package(pilz_industrial_motion_planner REQUIRED)
 find_package(moveit_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 
@@ -38,6 +39,7 @@ ament_target_dependencies(lite6_controller
   "rclcpp"
   "rclcpp_action"
   "Eigen3"
+  "pilz_industrial_motion_planner"
   "robot_controller"
   "moveit_ros_planning_interface"
   "robot_interfaces")
diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp
index 80afddc..db7420d 100644
--- a/src/lite6_controller/src/lite6_controller.cpp
+++ b/src/lite6_controller/src/lite6_controller.cpp
@@ -8,6 +8,8 @@
 #include <moveit/planning_scene_interface/planning_scene_interface.h>
 #include <moveit_msgs/msg/collision_object.hpp>
 
+#include <pilz_industrial_motion_planner/command_list_manager.h>
+
 const std::string MOVE_GROUP = "lite6";
 
 //
@@ -18,11 +20,21 @@ public:
   moveit::planning_interface::MoveGroupInterface move_group;
 
   //bool moved = false;
+  //
+  // TODO get pilz working
+  //std::unique_ptr<pilz_industrial_motion_planner::CommandListManager> command_list_manager_;
+  // command_list_manager_ = std::make_unique<pilz_industrial_motion_planner::CommandListManager>(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
+  pilz_industrial_motion_planner::CommandListManager command_list_manager;
+  //pilz_industrial_motion_planner::CommandListManager command_list_manager(*std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
 
   Lite6Controller(const rclcpp::NodeOptions & options = rclcpp::NodeOptions())
     : RobotController(options),
-      move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP)
+      move_group(std::shared_ptr<rclcpp::Node>(std::move(this)), MOVE_GROUP),
+      command_list_manager(std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel())
   {
+
+    //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(this->move_group.getNodeHandle(), this->move_group.getRobotModel());
+    //command_list_manager = new pilz_industrial_motion_planner::CommandListManager(std::shared_ptr<rclcpp::Node>(std::move(this)), this->move_group.getRobotModel());
     // Use upper joint velocity and acceleration limits
     //this->move_group.setMaxAccelerationScalingFactor(1.0);
     //this->move_group.setMaxVelocityScalingFactor(1.0);
@@ -46,6 +58,7 @@ public:
   /// Callback that executes path on robot
   virtual void executePath(const std::shared_ptr<rclcpp_action::ServerGoalHandle<ExecuteMotion>> goal_handle)
   {
+
     RCLCPP_INFO(this->get_logger(), "Executing goal");
     rclcpp::Rate loop_rate(20);
     const auto goal = goal_handle->get_goal();