searching for double planning solutions

.catkin_tools/profiles/default/devel_collisions.txt

 /home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 17431
+/home/matteo/reachability/devel/./cmake.lock 21138
 /home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
 /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79

.catkin_tools/profiles/default/packages/mtc/package.xml

@@ -35,11 +35,14 @@
   <build_depend>moveit_msgs</build_depend>
   <build_depend>geometry_msgs</build_depend>
   <build_depend>YAML_CPP</build_depend> 
+  <build_depend>filesystem</build_depend> 
+
+
   <build_depend>pcl_ros</build_depend>
   <build_depend>yaml_to_mtc</build_depend>
 
   
-
+  <run_depend>filesystem</run_depend> 
   <run_depend>YAML_CPP</run_depend> 
   <run_depend>pcl_ros</run_depend>
   <run_depend>std_msgs</run_depend>

.catkin_tools/profiles/default/packages/reachability/package.xml

@@ -35,6 +35,7 @@
   <exec_depend>interactive_markers</exec_depend>
   <exec_depend>eigen_conversions</exec_depend>
   <exec_depend>YAML_CPP</exec_depend>
+  
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>

src/.vscode/c_cpp_properties.json

@@ -41,7 +41,9 @@
                 "${workspaceFolder}/moveit_task_constructor/core/include",
                 "${workspaceFolder}/moveit_task_constructor/"
             ],
-            "name": "ROS"
+            "name": "ROS",
+            "cStandard": "c17",
+            "cppStandard": "c++17"
         }
     ],
     "version": 4

src/.vscode/settings.json

@@ -79,6 +79,8 @@
         "typeindex": "cpp",
         "typeinfo": "cpp",
         "variant": "cpp",
-        "any": "cpp"
+        "any": "cpp",
+        "filesystem": "cpp",
+        "codecvt": "cpp"
     }
 }
\ No newline at end of file

src/mtc/CMakeLists.txt

 cmake_minimum_required(VERSION 3.0.2)
 project(mtc)
 # C++ 11
-add_compile_options(-std=c++14)
+add_compile_options(-std=c++17)
 
 # Warnings
 add_definitions(-W -Wall -Wextra

src/mtc/include/impl/abstract_mediator.h

@@ -33,7 +33,8 @@ class Abstract_mediator {
     std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
     ros::Publisher* pub_;
     std::vector<std::vector<pcl::PointXYZ>> result_vector_;
-    std::vector<std::vector<wing_BP>> wings_;
+    std::vector<std::vector<Abstract_robot_element*>> wings_;
+    std::string dirname_;
 
 
 
@@ -41,25 +42,24 @@ class Abstract_mediator {
 
     public:
     Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
-        std::vector<wing_BP> wings;
-        wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
-        wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
-        wings.push_back({"right_panel", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right_size});
-        wings_.push_back(wings);
-        wings_.push_back(wings);
     }
 
     inline void connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());}
     inline void set_result_vector(std::vector<std::vector<pcl::PointXYZ>>& res) {result_vector_ = res;}
-    inline std::vector<std::vector<wing_BP>> wings() {return wings_;}
-    inline void set_wings(std::vector<std::vector<wing_BP>>& w) {wings_ = w;}
+    inline std::vector<std::vector<Abstract_robot_element*>> wings() {return wings_;}
     inline std::vector<std::vector<pcl::PointXYZ>>& result_vector() {return result_vector_;}
     inline std::vector<Abstract_robot*> robots(){return robots_;}
 
 
+    inline void set_dirname(std::string& dirn) {dirname_ = dirn;}
+    inline std::string& dirname() {return dirname_;}
+
+
+
     pcl::PointCloud< pcl::PointXYZ >::Ptr vector_to_cloud(std::vector<pcl::PointXYZ>& vector);
     std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
     
+    virtual void set_wings(std::vector<std::vector<wing_BP>>& wbp)=0;
     virtual bool check_collision( const int& robot)=0;
     virtual void mediate()=0;
     virtual void build_wings(std::bitset<3>& wing, int& robot)=0;

src/mtc/include/impl/abstract_robot.h

@@ -35,7 +35,7 @@ class Abstract_robot {
 
 
     public:
-    Abstract_robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
+    Abstract_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : name_(name), tf_(tf), size_(size){
         root_tf_= tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.22f, 0, tf.getOrigin().getZ()));
 
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3( 0.4f, 0.4f, tf.getOrigin().getZ()))); // ++

src/mtc/include/impl/mediator.h

@@ -10,9 +10,6 @@
 #include "impl/robot.h"
 
 class Mediator : public Abstract_mediator{
-    protected:
-    visualization_msgs::MarkerArray* rviz_markers_ = new visualization_msgs::MarkerArray;
-
     public:
     Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
 
@@ -20,8 +17,12 @@ class Mediator : public Abstract_mediator{
     std::vector<pcl::PointXYZ> generate_Ground(const tf2::Vector3 origin, const float diameter, float resolution);
     void approximation(Robot* robot);
     void write_file(Robot* A, Robot* B);
+    void publish(Robot* r);
 
-
+    void setup_rviz();
+    void calculate(std::vector<tf2::Transform>& ground_per_robot);
+    
+    void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
     bool check_collision(const int& robot) override;
     void mediate() override;
     void build_wings(std::bitset<3>& wings, int& robot) override;

src/mtc/include/impl/moveit_mediator.h

@@ -23,6 +23,7 @@ class Moveit_mediator : public Abstract_mediator{
     robot_model_loader::RobotModelLoader* robot_model_loader_;
     robot_model::RobotModelPtr kinematic_model_;
     planning_scene::PlanningScene* ps_;
+    XmlRpc::XmlRpcValue properties_;
 
     public:
     Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh){
@@ -30,13 +31,17 @@ class Moveit_mediator : public Abstract_mediator{
         planning_scene_diff_publisher_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
     };
 
+    void setup_task();
+
     bool check_collision(const int& robot) override;
     void mediate() override;
     void build_wings(std::bitset<3>& wing,int& robot) override;
+    void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
 
+    
     void publish_tables(moveit::planning_interface::PlanningSceneInterface& psi);
     void load_robot_description();
-    void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& target);
+    void rewrite_task_template(Abstract_robot* r, moveit_msgs::CollisionObject& source, tf2::Transform& target);
 };
 
 #endif
\ No newline at end of file

src/mtc/include/impl/moveit_robot.h

@@ -6,15 +6,37 @@
 
 
 class Moveit_robot : public Robot{
-    private:
+    protected:
     moveit::planning_interface::MoveGroupInterface* mgi_;
+    std::map<std::string, std::pair <std::string, std::string>>  map_; 
+
+
+
 
     public:
-    Moveit_robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
+    Moveit_robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Robot(name, tf, size){
         mgi_ = new moveit::planning_interface::MoveGroupInterface(name);
-        ROS_INFO("planning frame: %s", mgi_->getPlanningFrame().c_str());
+        std::stringstream group, eef, hand_grasping_frame, ik_frame, hand, hand_n, ik_frame_n, name_n;
+        name_n << name;
+        group << "group_" << name;
+        eef << "eef_" << name;
+        hand_grasping_frame << "hand_grasping_frame_" << name;
+        ik_frame << "ik_frame_" << name;
+        hand << "hand_" << name;
+        hand_n << "hand_" << name.back();
+        ik_frame_n << "panda_" << name.back() << "_link8";
+
+
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("group", std::make_pair<std::string, std::string>(group.str(), name_n.str())));
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("eef", std::make_pair<std::string, std::string>(eef.str(), hand_n.str())));
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("hand_grasping_frame", std::make_pair<std::string, std::string>(hand_grasping_frame.str(), ik_frame_n.str())));
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("ik_frame", std::make_pair<std::string, std::string>(ik_frame.str(), ik_frame_n.str())));
+        map_.insert(std::make_pair<std::string, std::pair<std::string, std::string>>("hand", std::make_pair<std::string, std::string>(hand.str(), hand_n.str())));
     }
 
+    inline moveit::planning_interface::MoveGroupInterface* mgi() {return mgi_;}
+    inline std::map<std::string, std::pair <std::string, std::string>>&  map(){return map_;};
+
 
 };
 

src/mtc/include/impl/robot.h

@@ -20,17 +20,12 @@ class Robot : public Abstract_robot{
     protected:
     std::vector<Abstract_robot_element*> observers_;
     std::vector<Abstract_robot_element*> access_fields_;
-    std::vector<visualization_msgs::MarkerArray*> rviz_markers_;
     std::vector<moveit_msgs::CollisionObject*> coll_markers_;
 
 
     public:
-    Robot(std::string name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){ 
-        generate_access_fields();
-    }
+    Robot(std::string& name, tf2::Transform tf, tf2::Vector3 size) : Abstract_robot(name, tf, size){generate_access_fields();}
     
-    inline void add_rviz_markers(visualization_msgs::MarkerArray* marker) {rviz_markers_.push_back(marker);}
-    inline std::vector<visualization_msgs::MarkerArray*> rviz_markers(){return rviz_markers_;}
     inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
     inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
 

src/mtc/include/impl/wing.h

@@ -11,16 +11,19 @@ class Wing  : public Abstract_robot_element{
     std::vector<tf2::Transform> bounds_;
 
     public:
-    Wing(std::string& name, tf2::Transform& tf, tf2::Vector3& size) : size_(size), name_(name){ 
+    Wing(std::string name, tf2::Transform tf, tf2::Vector3 size) : size_(size), name_(name){ 
         relative_tf_ = tf;
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/2.f,0))); //++
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/2.f, size_.getY()/-2.f,0))); //+-
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/-2.f,0))); //--
         bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(size_.getX()/-2.f, size_.getY()/2.f,0))); //-+
         }
+    virtual ~Wing() {}
         
     inline std::string& name() {return name_;}
-    inline void set_name(std::string& str) {name_ = str;}
+    inline void set_name(std::string str) {name_ = str;}
+    inline void set_set(tf2::Vector3& vec) {size_ = vec;}
+
     inline tf2::Vector3 size(){ return size_;}
     inline std::vector<tf2::Transform>& bounds() {return bounds_;}
     void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}

src/mtc/include/impl/wing_moveit_decorator.h

@@ -12,11 +12,11 @@
 
 class Wing_moveit_decorator  : public Abstract_robot_element_decorator{
     private:
-    moveit_msgs::CollisionObject* markers_;
+    moveit_msgs::CollisionObject* markers_ = new moveit_msgs::CollisionObject();
 
 
     public:
-    Wing_moveit_decorator(Abstract_robot_element* next, moveit_msgs::CollisionObject* markers) : Abstract_robot_element_decorator(next), markers_(markers){};
+    Wing_moveit_decorator(Abstract_robot_element* next) : Abstract_robot_element_decorator(next){};
     inline void set_markers(moveit_msgs::CollisionObject* markers) { markers_= markers;}
     inline moveit_msgs::CollisionObject* markers() { return markers_;}
     

src/mtc/include/impl/wing_rviz_decorator.h

@@ -8,13 +8,13 @@
 
 class Wing_rviz_decorator  : public Abstract_robot_element_decorator{
     private:
-    visualization_msgs::MarkerArray* markers_;
+    visualization_msgs::Marker* marker_ = new visualization_msgs::Marker();
 
 
     public:
-    Wing_rviz_decorator(Abstract_robot_element* next, visualization_msgs::MarkerArray* markers) : Abstract_robot_element_decorator(next), markers_(markers){};
-    inline void set_markers(visualization_msgs::MarkerArray* markers) { markers_= markers;}
-    inline visualization_msgs::MarkerArray* markers() { return markers_;}
+    Wing_rviz_decorator(Abstract_robot_element* next) : Abstract_robot_element_decorator(next){};
+    inline void set_markers(visualization_msgs::Marker* markers) { marker_= markers;}
+    inline visualization_msgs::Marker* markers() { return marker_;}
     void update(tf2::Transform& tf) override;
     void input_filter(tf2::Transform& tf) override;
     void output_filter() override;

src/mtc/launch/base_routine.launch

 <launch>
-    <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/resources/dummy.yaml"/>
+    <arg name="map" default="dummy" />
+    <arg name="description" default="dummy" />
+    <arg name="resource" default="dummy" />
 
 
+    <rosparam command="load" file="$(find mtc)/maps/$(arg map).yaml"/>
+    <rosparam command="load" file="$(find mtc)/descriptions/$(arg description).yaml"/>
+    <rosparam command="load" file="$(find mtc)/resources/$(arg resource).yaml"/>
+
+    <rosparam param="resource_name" subst_value="True"> $(arg resource)</rosparam>
     <node pkg="mtc" type="base_routine" name="base_routine" output="screen" required="true"/>
 </launch>
\ No newline at end of file

src/mtc/launch/cell_routine.launch

 <launch>    
     <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
     <rosparam command="load" file="$(find mtc)/results/dummy.yaml"/>
-    <rosparam command="load" file="$(find mtc)/mtc_task_file/dummy.yaml" />
+    <rosparam command="load" file="$(find mtc)/mtc_templates/dummy.yaml" />
     <rosparam command="load" file="$(find mtc)/maps/dummy.yaml"/>
     <rosparam command="load" file="$(find mtc)/descriptions/dummy.yaml"/>