From 0b5b6175b9c7f5f2314f16b3df3aa13c2606d0cc Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Tue, 5 Sep 2023 13:35:50 +0200
Subject: [PATCH] moveit_interface.cpp update

---
 src/moveit_interface.cpp | 13 +++++++++++++
 1 file changed, 13 insertions(+)

diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp
index 3d20c2d..6ffca5c 100644
--- a/src/moveit_interface.cpp
+++ b/src/moveit_interface.cpp
@@ -13,6 +13,7 @@
 
 #include "moveit/move_group_interface/move_group_interface.h"
 #include "moveit_visual_tools/moveit_visual_tools.h"
+#include "moveit/robot_model_loader/robot_model_loader.h"
 
 using namespace std::chrono_literals;
 
@@ -31,6 +32,15 @@ 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");
+
       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);
 
@@ -80,6 +90,9 @@ class MoveitInterface : public rclcpp::Node
     rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr execute_sub_;
     std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
     std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
+    std::shared_ptr<robot_model_loader::RobotModelLoader> robot_model_loader;
+    std::shared_ptr<moveit::core::RobotState> robot_state;
+    std::shared_ptr<planning_scene::PlanningScene> planning_scene;
 
     Eigen::Isometry3d text_pose;
     moveit_msgs::msg::RobotTrajectory trajectory;