From 0ff1a065a4bf1968ae0c69f3bd6b4c80928186f0 Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Tue, 5 Sep 2023 15:45:59 +0200
Subject: [PATCH] moveit_interface.cpp works

---
 CMakeLists.txt           |  6 +++---
 src/moveit_interface.cpp | 18 +++++++++---------
 2 files changed, 12 insertions(+), 12 deletions(-)

diff --git a/CMakeLists.txt b/CMakeLists.txt
index df65fa0..3d638c5 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -24,10 +24,10 @@ find_package(moveit_ros_planning_interface)
 find_package(std_msgs REQUIRED)
 find_package(geometry_msgs REQUIRED)
 
-add_executable(moveit_interface src/moveit_interface.cpp)
+add_executable(franka_moveit_interface src/moveit_interface.cpp)
 
 ament_target_dependencies(
-  moveit_interface
+  franka_moveit_interface
   rclcpp
   moveit
   moveit_msgs
@@ -38,7 +38,7 @@ ament_target_dependencies(
 )
 
 install(
-  TARGETS moveit_interface
+  TARGETS franka_moveit_interface
   DESTINATION lib/${PROJECT_NAME}
 )
 
diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp
index 6ffca5c..fe1a6ce 100644
--- a/src/moveit_interface.cpp
+++ b/src/moveit_interface.cpp
@@ -33,13 +33,13 @@ class MoveitInterface : public rclcpp::Node
       // Connecting to moveit
       static const std::string PLANNING_GROUP = "panda_arm";
 
-      robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>(std::shared_ptr<rclcpp::Node>(shared_this), "robot_description");
-      const moveit::core::RobotModelPtr& robot_model = robot_model_loader->getModel();
-      robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
-      const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
-
-      planning_scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
-      planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
+//      robot_model_loader = std::make_shared<robot_model_loader::RobotModelLoader>(std::shared_ptr<rclcpp::Node>(shared_this), "robot_description");
+//      const moveit::core::RobotModelPtr& robot_model = robot_model_loader->getModel();
+//      robot_state = std::make_shared<moveit::core::RobotState>(robot_model);
+//      const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(PLANNING_GROUP);
+//
+//      planning_scene = std::make_shared<planning_scene::PlanningScene>(robot_model);
+//      planning_scene->getCurrentStateNonConst().setToDefaultValues(joint_model_group, "ready");
 
       move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(shared_this), PLANNING_GROUP);
       // const moveit::core::JointModelGroup* joint_model_group = move_group_->getCurrentState()->getJointModelGroup(PLANNING_GROUP);
@@ -54,8 +54,8 @@ class MoveitInterface : public rclcpp::Node
   private:
     void task_space_callback(const geometry_msgs::msg::PoseArray::SharedPtr msg)
     {
-      moveit::core::RobotState start_state(*move_group_->getCurrentState());
-      move_group_->setStartState(start_state);
+      // moveit::core::RobotState start_state(*move_group_->getCurrentState());
+      // move_group_->setStartState(start_state);
       std::vector<geometry_msgs::msg::Pose> waypoints = msg->poses;