From 05909444d61e52ef5aadc77a89574f30fa7e9538 Mon Sep 17 00:00:00 2001
From: Nicolas Hiillos <nicolashiillos@gmail.com>
Date: Wed, 15 Feb 2023 16:22:57 +0200
Subject: [PATCH] Remove excessive calls to getCurrentState()

---
 src/lite6_controller/src/lite6_controller.cpp | 27 +++++++++++--------
 1 file changed, 16 insertions(+), 11 deletions(-)

diff --git a/src/lite6_controller/src/lite6_controller.cpp b/src/lite6_controller/src/lite6_controller.cpp
index a66fcb1..f8cfbc7 100644
--- a/src/lite6_controller/src/lite6_controller.cpp
+++ b/src/lite6_controller/src/lite6_controller.cpp
@@ -214,7 +214,7 @@ public:
   /**
    * Creates a trajectory for a pose and appends it to a given trajectory
    */
-  bool addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory)
+  bool addPoseToTrajectory(geometry_msgs::msg::PoseStamped pose, moveit_msgs::msg::RobotTrajectory *trajectory, moveit::core::RobotStatePtr state)
   {
     pose = translatePose(pose);
     move_group.setPoseTarget(pose);
@@ -224,21 +224,21 @@ public:
     //move_group.setPlanningPipelineId("PTP");
     move_group.setPlannerId("PTP");
 
-    robot_trajectory::RobotTrajectory previous_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
-    previous_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), *trajectory);
+    robot_trajectory::RobotTrajectory previous_trajectory(state->getRobotModel(), move_group.getName());
+    previous_trajectory.setRobotTrajectoryMsg(*state, *trajectory);
 
 
     moveit::planning_interface::MoveGroupInterface::Plan plan;
     bool success = (move_group.plan(plan) == moveit::core::MoveItErrorCode::SUCCESS);
-    RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED");
+    RCLCPP_INFO(this->get_logger(), "Plan (pose goal) %s", success ? "SUCCEEDED" : "FAILED, trying approximate method (if enabled)");
 
     if (success)
     {
-      robot_trajectory::RobotTrajectory next_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
-      next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
+      robot_trajectory::RobotTrajectory next_trajectory(state->getRobotModel(), move_group.getName());
+      next_trajectory.setRobotTrajectoryMsg(*state, plan.trajectory_);
 
       // append trajectory, with time step of 2.0, not skipping any points
-      previous_trajectory.append(next_trajectory, 0.01, 0);
+      previous_trajectory.append(next_trajectory, 0.000001, 0);
       *trajectory = moveit_msgs::msg::RobotTrajectory();
       previous_trajectory.getRobotTrajectoryMsg(*trajectory);
 
@@ -251,7 +251,9 @@ public:
     }
     else
     {
-      success = addPoseToTrajectoryApproximate(pose, trajectory);
+      // Unsafe approximate solution
+      // Likely to break pens
+      //success = addPoseToTrajectoryApproximate(pose, trajectory);
     }
     move_group.clearPoseTarget();
     return success;
@@ -280,7 +282,7 @@ public:
       next_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), plan.trajectory_);
 
       // append trajectory, with time step of 2.0, not skipping any points
-      previous_trajectory.append(next_trajectory, 0.01, 0);
+      previous_trajectory.append(next_trajectory, 0.0001, 0);
       *trajectory = moveit_msgs::msg::RobotTrajectory();
       previous_trajectory.getRobotTrajectoryMsg(*trajectory);
 
@@ -332,18 +334,21 @@ public:
     robot_trajectory::RobotTrajectory single_trajectory(move_group.getCurrentState()->getRobotModel(), move_group.getName());
 
     move_group.setStartStateToCurrentState();
+    moveit::core::RobotStatePtr move_group_state = move_group.getCurrentState();
     for (auto p : goal->motion.path)
     {
       //RCLCPP_INFO(this->get_logger(), "Planning trajectory");
 
       // Append next pose to trajectory
-      if (!addPoseToTrajectory(p, &multi_trajectory)) continue;
+      if (!addPoseToTrajectory(p, &multi_trajectory, move_group_state)) continue;
 
       // set move_group start state to final pose of trajectory
       //RCLCPP_INFO(this->get_logger(), "setRobotTrajectoryMsg");
-      single_trajectory.setRobotTrajectoryMsg(*move_group.getCurrentState(), multi_trajectory);
+      single_trajectory.setRobotTrajectoryMsg(*move_group_state, multi_trajectory);
       //rt.setRobotTrajectoryMsg(rt.getLastWayPoint(), trajectory);
       //RCLCPP_INFO(this->get_logger(), "getLastWayPoint");
+      //
+      //moveit::core::RobotState robot_state = single_trajectory.getLastWayPoint();
       moveit::core::RobotState robot_state = single_trajectory.getLastWayPoint();
       //RCLCPP_INFO(this->get_logger(), "eef");
       //const Eigen::Isometry3d& eef_transform = robot_state.getGlobalLinkTransform(move_group.getEndEffectorLink());