From 14f1bc45fc7d2479afcaf660e29d23a3cbb1eb7f Mon Sep 17 00:00:00 2001
From: Niko <nikolaus.feith@unileoben.ac.at>
Date: Thu, 14 Sep 2023 21:15:21 +0200
Subject: [PATCH] finishing iml experiment

---
 src/moveit_interface.cpp | 48 ++++++++++++++++++++++++++++++++++++++--
 src/scene_interface.cpp  |  0
 2 files changed, 46 insertions(+), 2 deletions(-)
 delete mode 100644 src/scene_interface.cpp

diff --git a/src/moveit_interface.cpp b/src/moveit_interface.cpp
index 5c5b74c..dca3ca0 100644
--- a/src/moveit_interface.cpp
+++ b/src/moveit_interface.cpp
@@ -7,6 +7,10 @@
 
 #include <rclcpp/rclcpp.hpp>
 
+#include <moveit_msgs/msg/attached_collision_object.hpp>
+#include <moveit_msgs/msg/collision_object.hpp>
+#include <moveit_msgs/msg/display_robot_state.hpp>
+#include <moveit_msgs/msg/display_trajectory.hpp>
 #include "moveit_msgs/msg/robot_trajectory.hpp"
 #include "geometry_msgs/msg/pose_array.hpp"
 #include "geometry_msgs/msg/pose_stamped.hpp"
@@ -15,7 +19,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"
+#include "moveit/planning_scene_interface/planning_scene_interface.h"
 
 #include "tf2_ros/transform_listener.h"
 
@@ -31,6 +35,7 @@ class MoveitInterface : public rclcpp::Node
       pose_publisher_ = this->create_publisher<geometry_msgs::msg::PoseStamped>("moveit_interface/current_pose", 10);
       subscriber_ = this->create_subscription<geometry_msgs::msg::PoseArray>("moveit_interface/task_space_trajectory", 10, std::bind(&MoveitInterface::task_space_callback, this, std::placeholders::_1));
       execute_sub_ = this->create_subscription<std_msgs::msg::Empty>("moveit_interface/execution", 10, std::bind(&MoveitInterface::execution_callback, this, std::placeholders::_1));
+      box_sub_ = this->create_subscription<std_msgs::msg::Empty>("moveit_interface/box", 10, std::bind(&MoveitInterface::box_callback, this, std::placeholders::_1));
       
       // Create a shared instance of the current node
       auto shared_this = std::shared_ptr<rclcpp::Node>(this);
@@ -39,6 +44,8 @@ class MoveitInterface : public rclcpp::Node
       static const std::string PLANNING_GROUP = "panda_arm";
       move_group_ = std::make_shared<moveit::planning_interface::MoveGroupInterface>(std::shared_ptr<rclcpp::Node>(shared_this), PLANNING_GROUP);
       move_group_->setStartStateToCurrentState();
+      planning_scene_interface_ = std::make_shared<moveit::planning_interface::PlanningSceneInterface>();
+
       // Connection to Rviz
       visual_tools_ = std::make_shared<moveit_visual_tools::MoveItVisualTools>(std::shared_ptr<rclcpp::Node>(shared_this), "panda_link0", "move_group", move_group_->getRobotModel());
       Eigen::Isometry3d text_pose = Eigen::Isometry3d::Identity();
@@ -49,7 +56,6 @@ class MoveitInterface : public rclcpp::Node
       tf2_ros::Buffer tf_buffer_{this->get_clock()};
       tf2_ros::TransformListener tf_listener_{tf_buffer_};
       tf2_timer_ = this->create_wall_timer(std::chrono::milliseconds(200), std::bind(&MoveitInterface::compute_endeffector_pose, this));
-
     }
 
   private:
@@ -111,20 +117,58 @@ class MoveitInterface : public rclcpp::Node
         RCLCPP_WARN(logger, "Trajectroy is not set. Unable to execute.");
       }
     }
+
+    //Just for the IML experiment
+    void box_callback(const std_msgs::msg::Empty::SharedPtr /*msg*/)
+    {
+      moveit_msgs::msg::CollisionObject visual_object;
+      visual_object.header.frame_id = move_group_->getPlanningFrame();
+
+      visual_object.id = "box1";
+
+      shape_msgs::msg::SolidPrimitive primitive;
+      primitive.type = primitive.BOX;
+      primitive.dimensions.resize(3);
+      primitive.dimensions[primitive.BOX_X] = 0.2;
+      primitive.dimensions[primitive.BOX_Y] = 0.2;
+      primitive.dimensions[primitive.BOX_Z] = 0.4;
+
+      geometry_msgs::msg::Pose box_pose;
+      box_pose.orientation.w = 1.0;
+      box_pose.position.x = 0.5;
+      box_pose.position.y = 0.0;
+      box_pose.position.z = 0.2;
+
+      visual_object.primitives.push_back(primitive);
+      visual_object.primitive_poses.push_back(box_pose);
+      visual_object.operation = visual_object.ADD;
+
+      std::vector<moveit_msgs::msg::CollisionObject> visual_objects;
+      visual_objects.push_back(visual_object);
+
+      planning_scene_interface_->addCollisionObjects(visual_objects);
+
+    }
+
     rclcpp::Publisher<moveit_msgs::msg::RobotTrajectory>::SharedPtr publisher_;
     rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_publisher_;
     rclcpp::Subscription<geometry_msgs::msg::PoseArray>::SharedPtr subscriber_;
     rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr execute_sub_;
+    rclcpp::Subscription<std_msgs::msg::Empty>::SharedPtr box_sub_;
     std::shared_ptr<moveit::planning_interface::MoveGroupInterface> move_group_;
     std::shared_ptr<moveit_visual_tools::MoveItVisualTools> visual_tools_;
+    std::shared_ptr<moveit::planning_interface::PlanningSceneInterface> planning_scene_interface_;
     tf2_ros::Buffer tf_buffer_;
     tf2_ros::TransformListener tf_listener_;
     rclcpp::TimerBase::SharedPtr tf2_timer_;
+    
 
     Eigen::Isometry3d text_pose;
     moveit_msgs::msg::RobotTrajectory trajectory;
     rclcpp::Logger logger = this->get_logger();
     bool trajectory_set;
+
+
 };
 
 int main(int argc, char * argv[])
diff --git a/src/scene_interface.cpp b/src/scene_interface.cpp
deleted file mode 100644
index e69de29..0000000