From a0ee92d53a0d62840aef33587854f79a9c901c9a Mon Sep 17 00:00:00 2001
From: Thomas <thomasmuller336@gmail.com>
Date: Tue, 22 Mar 2022 20:38:54 -0400
Subject: [PATCH] AAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAAA

---
 src/ar3_config/launch/dev.launch.py           |  2 --
 .../src/move_group_interface_node.cpp         | 36 +------------------
 2 files changed, 1 insertion(+), 37 deletions(-)

diff --git a/src/ar3_config/launch/dev.launch.py b/src/ar3_config/launch/dev.launch.py
index fc29818..5f55bdb 100644
--- a/src/ar3_config/launch/dev.launch.py
+++ b/src/ar3_config/launch/dev.launch.py
@@ -134,8 +134,6 @@ def generate_launch_description():
                 get_yaml(join(config_root, 'trajectory.yaml')),
                 get_yaml(join(config_root, 'planning_scene.yaml')),
                 get_yaml(join(config_root, "ar3_controllers.yaml")),
-                { "default_planning_pipeline": "ompl" },
-                { "planning_pipelines": {"pipeline_names":["ompl"], "ompl": {"planning_plugin": "ompl_interface/OMPLPlanner"} } }
             ],
         ),
 
diff --git a/src/move_group_interface/src/move_group_interface_node.cpp b/src/move_group_interface/src/move_group_interface_node.cpp
index 9f70e36..e3a9337 100644
--- a/src/move_group_interface/src/move_group_interface_node.cpp
+++ b/src/move_group_interface/src/move_group_interface_node.cpp
@@ -35,18 +35,16 @@ public:
         pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
             "move_pose",
             rclcpp::QoS(1),
-            std::bind(&ScanNPlan::moveCallbackHowItShouldWork, this, std::placeholders::_1),
+            std::bind(&ScanNPlan::moveCallback, this, std::placeholders::_1),
             options
         );
 
-        /*
         relative_pose_sub_ = this->create_subscription<geometry_msgs::msg::PoseStamped>(
             "move_relative_pose",
             rclcpp::QoS(1),
             std::bind(&ScanNPlan::relativeMoveCallback, this, std::placeholders::_1),
             options
         );
-        */
     }
 
     void init() {
@@ -58,38 +56,6 @@ private:
     void moveCallback(geometry_msgs::msg::PoseStamped::SharedPtr p) {
     	RCLCPP_INFO(this->get_logger(), "GOT MSG");
 
-        move_group_->setStartStateToCurrentState();
-        move_group_->setPlanningTime(20);
-        move_group_->setNumPlanningAttempts(50);
-        move_group_->setMaxVelocityScalingFactor(1.0);
-        move_group_->setMaxAccelerationScalingFactor(1.0);
-
-        std::vector<geometry_msgs::msg::Pose> waypoints;
-        waypoints.push_back(p->pose);
-        moveit_msgs::msg::RobotTrajectory trajectory;
-        double fraction = move_group_->computeCartesianPath(waypoints, 0.01, 0.0, trajectory, true);
-        if(fraction >= 1.0) {
-            RCLCPP_INFO(this->get_logger(), "Did the carts %f%%", fraction * 100.);
-
-            robot_trajectory::RobotTrajectory rt(move_group_->getRobotModel(), move_group_->getName());
-            rt.setRobotTrajectoryMsg(*move_group_->getCurrentState(10), trajectory);
-            trajectory_processing::IterativeParabolicTimeParameterization iptp;
-            bool success =
-                iptp.computeTimeStamps(rt, 0.1, 0.1);
-            RCLCPP_INFO(this->get_logger(), "Got stamps %s", success ? "YES" : "NO");
-
-            moveit::planning_interface::MoveGroupInterface::Plan plan;
-            rt.getRobotTrajectoryMsg(plan.trajectory_);
-
-            move_group_->execute(plan);
-        }
-
-
-        //move_group_->move();
-    }
-    void moveCallbackHowItShouldWork(geometry_msgs::msg::PoseStamped::SharedPtr p) {
-    	RCLCPP_INFO(this->get_logger(), "GOT MSG");
-
         move_group_->setStartStateToCurrentState();
         move_group_->setPoseTarget(p->pose);