diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index 2c19e6d6e7fcbe551cdf453c64fc5cb6c0b53375..35d97a11fafa59e6bc88b583aefdff3f1c5e2f54 100644
--- a/.catkin_tools/profiles/default/devel_collisions.txt
+++ b/.catkin_tools/profiles/default/devel_collisions.txt
@@ -1,4 +1,4 @@
/home/matteo/ws_panda/devel/./cmake.lock 42
-/home/matteo/reachability/devel/./cmake.lock 26636
+/home/matteo/reachability/devel/./cmake.lock 26851
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json
index 73da41b81424f1f5c153e7a6e175259059509353..5b82495cc6e55e63e3bcfa1e71c2c6169ad76d8f 100644
--- a/src/.vscode/c_cpp_properties.json
+++ b/src/.vscode/c_cpp_properties.json
@@ -1,26 +1,26 @@
{
- "configurations": [
- {
- "browse": {
- "databaseFilename": "${default}",
- "limitSymbolsToIncludedHeaders": false
- },
- "includePath": [
- "/home/matteo/reachability/devel/include/**",
- "/opt/ros/noetic/include/**",
- "/home/matteo/reachability/src/moveit_task_constructor/core/include/**",
- "/home/matteo/reachability/src/moveit_task_constructor/demo/include/**",
- "/home/matteo/reachability/src/mtc/include/**",
- "/home/matteo/reachability/src/reachability/include/**",
- "/home/matteo/reachability/src/moveit_task_constructor/rviz_marker_tools/include/**",
- "/usr/include/**"
- ],
- "name": "ROS",
- "intelliSenseMode": "gcc-x64",
- "compilerPath": "/usr/bin/gcc",
- "cStandard": "gnu11",
- "cppStandard": "c++14"
- }
- ],
- "version": 4
+ "configurations": [
+ {
+ "browse": {
+ "databaseFilename": "${default}",
+ "limitSymbolsToIncludedHeaders": false
+ },
+ "includePath": [
+ "/opt/ros/noetic/include/**",
+ "/home/matteo/reachability/devel/include/**",
+ "/home/matteo/reachability/src/moveit_task_constructor/core/include/**",
+ "/home/matteo/reachability/src/moveit_task_constructor/demo/include/**",
+ "/home/matteo/reachability/src/mtc/include/**",
+ "/home/matteo/reachability/src/reachability/include/**",
+ "/home/matteo/reachability/src/moveit_task_constructor/rviz_marker_tools/include/**",
+ "/usr/include/**"
+ ],
+ "name": "ROS",
+ "intelliSenseMode": "gcc-x64",
+ "compilerPath": "/usr/bin/gcc",
+ "cStandard": "gnu11",
+ "cppStandard": "c++14"
+ }
+ ],
+ "version": 4
}
\ No newline at end of file
diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt
index 5175af5543b34a2048896c671f6c0b4830437eec..9dd7d2e01bb38e82e25e13560819b53b50e5f576 100644
--- a/src/mtc/CMakeLists.txt
+++ b/src/mtc/CMakeLists.txt
@@ -81,6 +81,12 @@ src/impl/wing_moveit_decorator.cpp
src/impl/wing_rviz_decorator.cpp
src/impl/collision_helper.cpp
+src/reader/abstract_param_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/wing_reader.cpp
+
+
+
)
add_executable(cell_routine src/cell_routine.cpp
@@ -94,6 +100,10 @@ src/impl/mediator.cpp
src/impl/wing_moveit_decorator.cpp
src/impl/wing_rviz_decorator.cpp
src/impl/collision_helper.cpp
+
+src/reader/abstract_param_reader.cpp
+src/reader/robot_reader.cpp
+src/reader/wing_reader.cpp
)
@@ -101,6 +111,7 @@ src/impl/collision_helper.cpp
add_executable(mtc2taskspace src/mtc2taskspace.cpp
src/reader/abstract_param_reader.cpp
src/reader/task_space_reader.cpp
+src/reader/robot_reader.cpp
)
diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h
index 091b26360a96a3a22ed5bb8b701ae97591ce12ea..c143c0aaa656726f7670be343a4638dc441696be 100644
--- a/src/mtc/include/impl/abstract_mediator.h
+++ b/src/mtc/include/impl/abstract_mediator.h
@@ -5,6 +5,8 @@
#include "impl/abstract_robot.h"
#include "impl/abstract_robot_element.h"
#include "impl/robot.h"
+#include "reader/wing_reader.h"
+
#include <ros/package.h>
#include <yaml-cpp/yaml.h>
@@ -14,6 +16,8 @@
#include <pcl/point_cloud.h>
#include <pcl/octree/octree.h>
+
+
#define box_size tf2::Vector3(0.0318f, 0.0636f, 0.091f)
#define R_POS tf2::Vector3(0,0.6525f,0.4425f -0.005f)
#define L_POS tf2::Vector3(0,-0.6525f,0.4425f -0.005f)
@@ -59,7 +63,7 @@ class Abstract_mediator {
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 void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& 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;
diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h
index 28f9003152f7c25bbdc867d155df2e787decc4f0..1716c7ea81b3459eb37d85b5653b324a097c14b6 100644
--- a/src/mtc/include/impl/mediator.h
+++ b/src/mtc/include/impl/mediator.h
@@ -22,7 +22,7 @@ class Mediator : public Abstract_mediator{
void setup_rviz();
void calculate(std::vector<tf2::Transform>& ground_per_robot);
- void set_wings(std::vector<std::vector<wing_BP>>& wbp) override;
+ void set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
bool check_collision(const int& robot) override;
void mediate() override;
void build_wings(std::bitset<3>& wings, int& robot) override;
diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h
index 98eb16068243ed90721cb9c21b92b5645ef4a0d5..1d0949e01296a864b17cb14329fa03521c18fc10 100644
--- a/src/mtc/include/impl/moveit_mediator.h
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -93,7 +93,7 @@ class Moveit_mediator : public Abstract_mediator{
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 set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp) override;
void merge_ps(moveit_msgs::PlanningScene& out, moveit_msgs::PlanningScene* in, Moveit_robot* mr);
void publish_tables();
diff --git a/src/mtc/include/reader/abstract_param_reader.h b/src/mtc/include/reader/abstract_param_reader.h
index fb85893833d5bafde1992a9b33e446c976d40e50..9524d639944c3050243d61b43e36f82f6444d523 100644
--- a/src/mtc/include/reader/abstract_param_reader.h
+++ b/src/mtc/include/reader/abstract_param_reader.h
@@ -4,12 +4,17 @@
#include "ros/ros.h"
#include <ros/package.h>
#include <xmlrpcpp/XmlRpc.h>
-#include <tf/transform_broadcaster.h>
+#include <tf2/LinearMath/Transform.h>
#include <yaml-cpp/yaml.h>
#include <fstream>
#include <regex>
+struct object_data {
+ std::string name_;
+ tf2::Transform pose_;
+ tf2::Vector3 size_;
+};
class Abstract_param_reader{
protected:
diff --git a/src/mtc/include/reader/robot_reader.h b/src/mtc/include/reader/robot_reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..29ad3ea815bbeeb718d544f36426fa3046c76800
--- /dev/null
+++ b/src/mtc/include/reader/robot_reader.h
@@ -0,0 +1,23 @@
+#ifndef ROBOT_READER_
+#define ROBOT_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Robot_reader : public Abstract_param_reader{
+ protected:
+ std::vector<object_data> robot_data_;
+
+ public:
+ Robot_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){};
+
+ inline void set_robot_data(std::vector<object_data>& robot_data) {robot_data_ = robot_data;}
+ inline std::vector<object_data> robot_data() {return robot_data_;}
+
+ void read() override;
+};
+#endif
diff --git a/src/mtc/include/reader/wing_reader.h b/src/mtc/include/reader/wing_reader.h
new file mode 100644
index 0000000000000000000000000000000000000000..ec5956e45ee239af3ce7c5d3d281e1eaddd2f3f7
--- /dev/null
+++ b/src/mtc/include/reader/wing_reader.h
@@ -0,0 +1,23 @@
+#ifndef WING_READER_
+#define WING_READER_
+
+#include "ros/ros.h"
+#include <ros/package.h>
+#include <xmlrpcpp/XmlRpc.h>
+
+#include "reader/abstract_param_reader.h"
+
+
+class Wing_reader : public Abstract_param_reader{
+ protected:
+ std::vector<std::pair<std::vector<object_data>, int>> wing_data_;
+
+ public:
+ Wing_reader(std::shared_ptr<ros::NodeHandle> const& d) : Abstract_param_reader(d){};
+
+ inline void set_wing_data(std::vector<std::pair<std::vector<object_data>, int>>& wing_data) {wing_data_ = wing_data;}
+ inline std::vector<std::pair<std::vector<object_data>, int>> wing_data() {return wing_data_;}
+
+ void read() override;
+};
+#endif
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
index aac714e9c77f1484ba995cf16cdf1add28b0f065..91bcb4c106ce1feedc08ce9d7f515534f61cc030 100644
--- a/src/mtc/launch/cell_routine.launch
+++ b/src/mtc/launch/cell_routine.launch
@@ -1,5 +1,5 @@
<launch>
- <arg name="result" default="dummy/-474510554.yaml" />
+ <arg name="result" default="dummy/-211114391.yaml" />
<!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
<!-- this is to change-->
diff --git a/src/mtc/results/dummy/1543924248.yaml b/src/mtc/results/dummy/-1003092792.yaml
similarity index 100%
rename from src/mtc/results/dummy/1543924248.yaml
rename to src/mtc/results/dummy/-1003092792.yaml
diff --git a/src/mtc/results/dummy/1601455593.yaml b/src/mtc/results/dummy/-1010460506.yaml
similarity index 100%
rename from src/mtc/results/dummy/1601455593.yaml
rename to src/mtc/results/dummy/-1010460506.yaml
diff --git a/src/mtc/results/dummy/1587832961.yaml b/src/mtc/results/dummy/-1024001513.yaml
similarity index 100%
rename from src/mtc/results/dummy/1587832961.yaml
rename to src/mtc/results/dummy/-1024001513.yaml
diff --git a/src/mtc/results/dummy/1575731070.yaml b/src/mtc/results/dummy/-1036064907.yaml
similarity index 100%
rename from src/mtc/results/dummy/1575731070.yaml
rename to src/mtc/results/dummy/-1036064907.yaml
diff --git a/src/mtc/results/dummy/-1890202883.yaml b/src/mtc/results/dummy/-211114391.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1890202883.yaml
rename to src/mtc/results/dummy/-211114391.yaml
diff --git a/src/mtc/results/dummy/-1901373430.yaml b/src/mtc/results/dummy/-222544024.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1901373430.yaml
rename to src/mtc/results/dummy/-222544024.yaml
diff --git a/src/mtc/results/dummy/-1951250192.yaml b/src/mtc/results/dummy/-258879761.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1951250192.yaml
rename to src/mtc/results/dummy/-258879761.yaml
diff --git a/src/mtc/results/dummy/-1962818280.yaml b/src/mtc/results/dummy/-270419468.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1962818280.yaml
rename to src/mtc/results/dummy/-270419468.yaml
diff --git a/src/mtc/results/dummy/1850935177.yaml b/src/mtc/results/dummy/-652201118.yaml
similarity index 100%
rename from src/mtc/results/dummy/1850935177.yaml
rename to src/mtc/results/dummy/-652201118.yaml
diff --git a/src/mtc/results/dummy/1895656920.yaml b/src/mtc/results/dummy/-696648853.yaml
similarity index 100%
rename from src/mtc/results/dummy/1895656920.yaml
rename to src/mtc/results/dummy/-696648853.yaml
diff --git a/src/mtc/results/dummy/1821310512.yaml b/src/mtc/results/dummy/-742232139.yaml
similarity index 100%
rename from src/mtc/results/dummy/1821310512.yaml
rename to src/mtc/results/dummy/-742232139.yaml
diff --git a/src/mtc/results/dummy/1807829100.yaml b/src/mtc/results/dummy/-755774193.yaml
similarity index 100%
rename from src/mtc/results/dummy/1807829100.yaml
rename to src/mtc/results/dummy/-755774193.yaml
diff --git a/src/mtc/results/dummy/1795913128.yaml b/src/mtc/results/dummy/-767674851.yaml
similarity index 100%
rename from src/mtc/results/dummy/1795913128.yaml
rename to src/mtc/results/dummy/-767674851.yaml
diff --git a/src/mtc/results/dummy/1868843792.yaml b/src/mtc/results/dummy/-789859288.yaml
similarity index 100%
rename from src/mtc/results/dummy/1868843792.yaml
rename to src/mtc/results/dummy/-789859288.yaml
diff --git a/src/mtc/results/dummy/1855511004.yaml b/src/mtc/results/dummy/-803422225.yaml
similarity index 100%
rename from src/mtc/results/dummy/1855511004.yaml
rename to src/mtc/results/dummy/-803422225.yaml
diff --git a/src/mtc/results/dummy/1843511501.yaml b/src/mtc/results/dummy/-815321487.yaml
similarity index 100%
rename from src/mtc/results/dummy/1843511501.yaml
rename to src/mtc/results/dummy/-815321487.yaml
diff --git a/src/mtc/results/dummy/1656942183.yaml b/src/mtc/results/dummy/-846087811.yaml
similarity index 100%
rename from src/mtc/results/dummy/1656942183.yaml
rename to src/mtc/results/dummy/-846087811.yaml
diff --git a/src/mtc/results/dummy/1711393564.yaml b/src/mtc/results/dummy/-852264234.yaml
similarity index 100%
rename from src/mtc/results/dummy/1711393564.yaml
rename to src/mtc/results/dummy/-852264234.yaml
diff --git a/src/mtc/results/dummy/1697794188.yaml b/src/mtc/results/dummy/-865767525.yaml
similarity index 100%
rename from src/mtc/results/dummy/1697794188.yaml
rename to src/mtc/results/dummy/-865767525.yaml
diff --git a/src/mtc/results/dummy/1685895957.yaml b/src/mtc/results/dummy/-877673981.yaml
similarity index 100%
rename from src/mtc/results/dummy/1685895957.yaml
rename to src/mtc/results/dummy/-877673981.yaml
diff --git a/src/mtc/results/dummy/1701273998.yaml b/src/mtc/results/dummy/-890048247.yaml
similarity index 100%
rename from src/mtc/results/dummy/1701273998.yaml
rename to src/mtc/results/dummy/-890048247.yaml
diff --git a/src/mtc/results/dummy/1759072325.yaml b/src/mtc/results/dummy/-899976593.yaml
similarity index 100%
rename from src/mtc/results/dummy/1759072325.yaml
rename to src/mtc/results/dummy/-899976593.yaml
diff --git a/src/mtc/results/dummy/1745566468.yaml b/src/mtc/results/dummy/-914020264.yaml
similarity index 100%
rename from src/mtc/results/dummy/1745566468.yaml
rename to src/mtc/results/dummy/-914020264.yaml
diff --git a/src/mtc/results/dummy/1733626611.yaml b/src/mtc/results/dummy/-925946624.yaml
similarity index 100%
rename from src/mtc/results/dummy/1733626611.yaml
rename to src/mtc/results/dummy/-925946624.yaml
diff --git a/src/mtc/results/dummy/1567654444.yaml b/src/mtc/results/dummy/-934521686.yaml
similarity index 100%
rename from src/mtc/results/dummy/1567654444.yaml
rename to src/mtc/results/dummy/-934521686.yaml
diff --git a/src/mtc/results/dummy/1555239444.yaml b/src/mtc/results/dummy/-947074197.yaml
similarity index 100%
rename from src/mtc/results/dummy/1555239444.yaml
rename to src/mtc/results/dummy/-947074197.yaml
diff --git a/src/mtc/results/dummy/1588704657.yaml b/src/mtc/results/dummy/-958226134.yaml
similarity index 100%
rename from src/mtc/results/dummy/1588704657.yaml
rename to src/mtc/results/dummy/-958226134.yaml
diff --git a/src/mtc/results/dummy/1648958842.yaml b/src/mtc/results/dummy/-962936586.yaml
similarity index 100%
rename from src/mtc/results/dummy/1648958842.yaml
rename to src/mtc/results/dummy/-962936586.yaml
diff --git a/src/mtc/results/dummy/1635476033.yaml b/src/mtc/results/dummy/-976487161.yaml
similarity index 100%
rename from src/mtc/results/dummy/1635476033.yaml
rename to src/mtc/results/dummy/-976487161.yaml
diff --git a/src/mtc/results/dummy/1612469426.yaml b/src/mtc/results/dummy/-979308652.yaml
similarity index 100%
rename from src/mtc/results/dummy/1612469426.yaml
rename to src/mtc/results/dummy/-979308652.yaml
diff --git a/src/mtc/results/dummy/1623703517.yaml b/src/mtc/results/dummy/-988375807.yaml
similarity index 100%
rename from src/mtc/results/dummy/1623703517.yaml
rename to src/mtc/results/dummy/-988375807.yaml
diff --git a/src/mtc/results/dummy/1599871509.yaml b/src/mtc/results/dummy/-991922905.yaml
similarity index 100%
rename from src/mtc/results/dummy/1599871509.yaml
rename to src/mtc/results/dummy/-991922905.yaml
diff --git a/src/mtc/results/dummy/-542667557.yaml b/src/mtc/results/dummy/1183415122.yaml
similarity index 100%
rename from src/mtc/results/dummy/-542667557.yaml
rename to src/mtc/results/dummy/1183415122.yaml
diff --git a/src/mtc/results/dummy/-587344676.yaml b/src/mtc/results/dummy/1227695304.yaml
similarity index 100%
rename from src/mtc/results/dummy/-587344676.yaml
rename to src/mtc/results/dummy/1227695304.yaml
diff --git a/src/mtc/results/dummy/-453685788.yaml b/src/mtc/results/dummy/1272105605.yaml
similarity index 100%
rename from src/mtc/results/dummy/-453685788.yaml
rename to src/mtc/results/dummy/1272105605.yaml
diff --git a/src/mtc/results/dummy/-442405442.yaml b/src/mtc/results/dummy/1283302731.yaml
similarity index 100%
rename from src/mtc/results/dummy/-442405442.yaml
rename to src/mtc/results/dummy/1283302731.yaml
diff --git a/src/mtc/results/dummy/-429778748.yaml b/src/mtc/results/dummy/1295853078.yaml
similarity index 100%
rename from src/mtc/results/dummy/-429778748.yaml
rename to src/mtc/results/dummy/1295853078.yaml
diff --git a/src/mtc/results/dummy/-498149190.yaml b/src/mtc/results/dummy/1316801078.yaml
similarity index 100%
rename from src/mtc/results/dummy/-498149190.yaml
rename to src/mtc/results/dummy/1316801078.yaml
diff --git a/src/mtc/results/dummy/-487005315.yaml b/src/mtc/results/dummy/1328208922.yaml
similarity index 100%
rename from src/mtc/results/dummy/-487005315.yaml
rename to src/mtc/results/dummy/1328208922.yaml
diff --git a/src/mtc/results/dummy/-474510554.yaml b/src/mtc/results/dummy/1340801804.yaml
similarity index 100%
rename from src/mtc/results/dummy/-474510554.yaml
rename to src/mtc/results/dummy/1340801804.yaml
diff --git a/src/mtc/results/dummy/-1564063432.yaml b/src/mtc/results/dummy/144778443.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1564063432.yaml
rename to src/mtc/results/dummy/144778443.yaml
diff --git a/src/mtc/results/dummy/-1553074118.yaml b/src/mtc/results/dummy/155931149.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1553074118.yaml
rename to src/mtc/results/dummy/155931149.yaml
diff --git a/src/mtc/results/dummy/-1540350554.yaml b/src/mtc/results/dummy/168492950.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1540350554.yaml
rename to src/mtc/results/dummy/168492950.yaml
diff --git a/src/mtc/results/dummy/-1608866404.yaml b/src/mtc/results/dummy/189371734.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1608866404.yaml
rename to src/mtc/results/dummy/189371734.yaml
diff --git a/src/mtc/results/dummy/-1597568458.yaml b/src/mtc/results/dummy/203146650.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1597568458.yaml
rename to src/mtc/results/dummy/203146650.yaml
diff --git a/src/mtc/results/dummy/-1585194105.yaml b/src/mtc/results/dummy/215783882.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1585194105.yaml
rename to src/mtc/results/dummy/215783882.yaml
diff --git a/src/mtc/results/dummy/-1358218541.yaml b/src/mtc/results/dummy/254319817.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1358218541.yaml
rename to src/mtc/results/dummy/254319817.yaml
diff --git a/src/mtc/results/dummy/-1405464563.yaml b/src/mtc/results/dummy/301530140.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1405464563.yaml
rename to src/mtc/results/dummy/301530140.yaml
diff --git a/src/mtc/results/dummy/-1572377916.yaml b/src/mtc/results/dummy/40651904.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1572377916.yaml
rename to src/mtc/results/dummy/40651904.yaml
diff --git a/src/mtc/results/dummy/-1180235424.yaml b/src/mtc/results/dummy/434875766.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1180235424.yaml
rename to src/mtc/results/dummy/434875766.yaml
diff --git a/src/mtc/results/dummy/-1227713112.yaml b/src/mtc/results/dummy/482378175.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1227713112.yaml
rename to src/mtc/results/dummy/482378175.yaml
diff --git a/src/mtc/results/dummy/-1060422961.yaml b/src/mtc/results/dummy/560822576.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1060422961.yaml
rename to src/mtc/results/dummy/560822576.yaml
diff --git a/src/mtc/results/dummy/-1107549484.yaml b/src/mtc/results/dummy/608106095.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1107549484.yaml
rename to src/mtc/results/dummy/608106095.yaml
diff --git a/src/mtc/results/dummy/-1006227179.yaml b/src/mtc/results/dummy/702701396.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1006227179.yaml
rename to src/mtc/results/dummy/702701396.yaml
diff --git a/src/mtc/results/dummy/-1039705736.yaml b/src/mtc/results/dummy/714102535.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1039705736.yaml
rename to src/mtc/results/dummy/714102535.yaml
diff --git a/src/mtc/results/dummy/-1027180942.yaml b/src/mtc/results/dummy/726842647.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1027180942.yaml
rename to src/mtc/results/dummy/726842647.yaml
diff --git a/src/mtc/results/dummy/-1051277393.yaml b/src/mtc/results/dummy/748047042.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1051277393.yaml
rename to src/mtc/results/dummy/748047042.yaml
diff --git a/src/mtc/results/dummy/-995076880.yaml b/src/mtc/results/dummy/759537023.yaml
similarity index 100%
rename from src/mtc/results/dummy/-995076880.yaml
rename to src/mtc/results/dummy/759537023.yaml
diff --git a/src/mtc/results/dummy/-982538817.yaml b/src/mtc/results/dummy/772496165.yaml
similarity index 100%
rename from src/mtc/results/dummy/-982538817.yaml
rename to src/mtc/results/dummy/772496165.yaml
diff --git a/src/mtc/results/dummy/-772783050.yaml b/src/mtc/results/dummy/832741068.yaml
similarity index 100%
rename from src/mtc/results/dummy/-772783050.yaml
rename to src/mtc/results/dummy/832741068.yaml
diff --git a/src/mtc/results/dummy/-1619843661.yaml b/src/mtc/results/dummy/88011482.yaml
similarity index 100%
rename from src/mtc/results/dummy/-1619843661.yaml
rename to src/mtc/results/dummy/88011482.yaml
diff --git a/src/mtc/results/dummy/-827862775.yaml b/src/mtc/results/dummy/880346008.yaml
similarity index 100%
rename from src/mtc/results/dummy/-827862775.yaml
rename to src/mtc/results/dummy/880346008.yaml
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
index e462fcf3abc9daa562bf3fe1d6f408c50c2b9940..f63e1d405fcc5ccb8c40a5526d6852c7de134669 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/src/mtc/src/base_routine.cpp
@@ -14,28 +14,12 @@
#include "impl/base_by_rotation.h"
#include <xmlrpcpp/XmlRpc.h>
#include <filesystem>
+#include "reader/abstract_param_reader.h"
+#include "reader/robot_reader.h"
+#include "reader/wing_reader.h"
+
+
-float value_caster(XmlRpc::XmlRpcValue& val){
- float t = 0;
- try{
- switch (val.getType()){
- case XmlRpc::XmlRpcValue::TypeInt:
- t = static_cast<int>(val);
- break;
- case XmlRpc::XmlRpcValue::TypeDouble:
- t = static_cast<double>(val);
- break;
- case XmlRpc::XmlRpcValue::TypeString:
- t = std::stof(val);
- break;
- default:
- ROS_INFO("value type is some unknown type");
- }
- } catch (XmlRpc::XmlRpcException){
- ROS_INFO("Something went wrong, returning 0");
- }
- return t;
-}
int main(int argc, char **argv){
@@ -64,316 +48,24 @@ int main(int argc, char **argv){
mediator->set_result_vector(results);
mediator->set_dirname(filename);
- float a = 0; float b = 0;
- std::vector<std::vector<wing_BP>> blueprints_per_robot(2);
- std::vector<wing_BP> blueprints(3);
-
- blueprints[0] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
- blueprints[1] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
- blueprints[2] = {std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
- blueprints_per_robot[0] = blueprints;
- blueprints_per_robot[1] = blueprints;
- for (int i = 0; i < resources.size(); i++){
- if(resources[i]["id"] == "table1_table_top"){
- ROS_INFO("=> Robot: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
+ std::unique_ptr<Abstract_param_reader> robot_reader(new Robot_reader(n));
+ robot_reader->read();
+ std::vector<object_data> rd = static_cast<Robot_reader*>(robot_reader.get())->robot_data();
+ for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Robot(rd[i].name_, rd[i].pose_, rd[i].size_));
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
+ std::unique_ptr<Abstract_param_reader> wing_reader(new Wing_reader(n));
+ wing_reader->read();
+ auto wd = static_cast<Wing_reader*>(wing_reader.get())->wing_data();
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- std::string name("panda_arm1");
- mediator->connect_robots(new Robot(name, tf2::Transform(rot, pos), size));
- ROS_INFO("=> Robot:loading SUCCESS");
- }
-
- if(resources[i]["id"] == "table2_table_top"){
- ROS_INFO("=> Robot: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- std::string name("panda_arm2");
- mediator->connect_robots(new Robot(name, tf2::Transform(rot, pos), size));
- ROS_INFO("=> Robot:loading SUCCESS");
+ for (int i = 0; i < mediator->robots().size(); i++){
+ for(object_data& w : wd[i].first){
+ w.pose_ = mediator->robots()[i]->tf().inverse() * w.pose_;
}
+ mediator->robots()[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
}
-
- for (int i = 0; i < resources.size(); i++){
- if(resources[i]["id"] == "table1_right_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- /*
- Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[0][0]);
- w->set_name("table1_right_panel");
- w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
- w->set_set(size);
-
- blueprints_per_robot[0][0] = new Wing_rviz_decorator(w);
- */
-
- blueprints_per_robot[0][0].name_ = "table1_right_panel";
- blueprints_per_robot[0][0].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[0][0].size_ = size;
-
-
- a += std::pow(2, 0);
- ROS_INFO("=> WING:loading SUCCESS");
- }
-
- if(resources[i]["id"] == "table1_front_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- /*
- Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
- Wing* w = dynamic_cast<Wing*>(are);
- w->set_name("table1_front_panel");
- w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
- w->set_set(size);
-
- Abstract_robot_element* wrd = new Wing_rviz_decorator(w);
- blueprints_per_robot[0][1] = wrd;
-
- */
- a += std::pow(2, 1);
-
- blueprints_per_robot[0][1].name_ = std::string("table1_front_panel");
- ROS_INFO("uzguzg");
- blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[0][1].size_ = size;
- ROS_INFO("=> WING:loading SUCCESS");
- }
-
- if(resources[i]["id"] == "table1_left_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- /*
- Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
- Wing* w = dynamic_cast<Wing*>(are);
- w->set_name("table1_left_panel");
- w->set_relative_tf(mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
- w->set_set(size);
-
- Abstract_robot_element* wrd = new Wing_rviz_decorator(are);
- blueprints_per_robot[0][2] = wrd;
- */
- blueprints_per_robot[0][2].name_ = "table1_left_panel";
- blueprints_per_robot[0][2].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[0][2].size_ = size;
- a += std::pow(2, 2);
- ROS_INFO("=> WING:loading SUCCESS");
-
- }
-
- if(resources[i]["id"] == "table2_right_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- /*
- Abstract_robot_element* are = new Wing(std::string(""), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0));
- Wing* w = dynamic_cast<Wing*>(are);
- w->set_name("table2_right_panel");
- w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
- w->set_set(size);
-
- blueprints_per_robot[1][0] = new Wing_rviz_decorator(w);
- */
- blueprints_per_robot[1][0].name_ = "table2_right_panel";
- blueprints_per_robot[1][0].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[1][0].size_ = size;
- b += std::pow(2, 0);
- ROS_INFO("=> WING:loading SUCCESS");
- }
-
- if(resources[i]["id"] == "table2_front_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- /*
- Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[1][1]);
- w->set_name("table2_front_panel");
- w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
- w->set_set(size);
- blueprints_per_robot[1][1] = new Wing_rviz_decorator(w);
- */
- blueprints_per_robot[1][1].name_ = "table2_front_panel";
- blueprints_per_robot[1][1].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[1][1].size_ = size;
- ROS_INFO("=> WING:loading SUCCESS");
- b += std::pow(2, 1);
-
- }
-
- if(resources[i]["id"] == "table2_left_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
- /*
- Wing* w = dynamic_cast<Wing*>(blueprints_per_robot[1][2]);
- w->set_name("table2_left_panel");
- w->set_relative_tf(mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos));
- w->set_set(size);
- blueprints_per_robot[1][2] = new Wing_rviz_decorator(w);
- */
-
- blueprints_per_robot[1][2].name_ = "table2_left_panel";
- blueprints_per_robot[1][2].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[1][2].size_ = size;
- ROS_INFO("=> WING:loading SUCCESS");
- b += std::pow(2, 2);
- }
- }
- mediator->robots()[0]->set_observer_mask(static_cast<wing_config>(a));
- mediator->robots()[1]->set_observer_mask(static_cast<wing_config>(b));
-
-
- ROS_INFO("ist durch");
- mediator->set_wings(blueprints_per_robot);
+ mediator->set_wings(wd);
mediator->mediate();
diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp
index b9a4c3387b9ac9857ee84112dc98d60a0b68ab5d..e56dc02f4144c36b12638e2f8635f5e685e09749 100644
--- a/src/mtc/src/cell_routine.cpp
+++ b/src/mtc/src/cell_routine.cpp
@@ -10,28 +10,10 @@
#include "impl/moveit_robot.h"
#include "impl/collision_helper.h"
#include <xmlrpcpp/XmlRpc.h>
+#include "reader/abstract_param_reader.h"
+#include "reader/robot_reader.h"
+#include "reader/wing_reader.h"
-float value_caster(XmlRpc::XmlRpcValue& val){
- float t = 0;
- try{
- switch (val.getType()){
- case XmlRpc::XmlRpcValue::TypeInt:
- t = static_cast<int>(val);
- break;
- case XmlRpc::XmlRpcValue::TypeDouble:
- t = static_cast<double>(val);
- break;
- case XmlRpc::XmlRpcValue::TypeString:
- t = std::stof(val);
- break;
- default:
- ROS_INFO("value type is some unknown type");
- }
- } catch (XmlRpc::XmlRpcException){
- ROS_INFO("Something went wrong, returning 0");
- }
- return t;
-}
int main(int argc, char **argv){
ros::init(argc, argv, "cell_routine");
@@ -57,280 +39,34 @@ int main(int argc, char **argv){
moveit_mediator->load_robot_description();
- std::vector<std::vector<wing_BP>> blueprints_per_robot(2);
- std::vector<wing_BP> blueprints(3);
- blueprints[0] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
- blueprints[1] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
- blueprints[2] = {"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)};
- blueprints_per_robot[0] = blueprints;
- blueprints_per_robot[1] = blueprints;
- int a = 0; int b = 0;
-
-for (int i = 0; i < resources.size(); i++){
- if(resources[i]["id"] == "table1_table_top"){
- ROS_INFO("=> Robot: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- std::string name("panda_arm1");
- moveit_mediator->task_map().insert({name, std::vector<moveit::task_constructor::Task>()});
- Abstract_robot* robot1 = new Moveit_robot(name, tf2::Transform(rot, pos), size);
- Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(robot1);
- ceti1->set_observer_mask(wing_config::RML);
- mediator->connect_robots(robot1);
- ROS_INFO("=> Robot:loading SUCCESS");
- }
-
- if(resources[i]["id"] == "table2_table_top"){
- ROS_INFO("=> Robot: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ((value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+ std::unique_ptr<Abstract_param_reader> robot_reader(new Robot_reader(nnh));
+ robot_reader->read();
+ std::vector<object_data> rd = static_cast<Robot_reader*>(robot_reader.get())->robot_data();
+ for (int i = 0; i < rd.size() ;i++) mediator->connect_robots(new Moveit_robot(rd[i].name_, rd[i].pose_, rd[i].size_));
- std::string name("panda_arm2");
- moveit_mediator->task_map().insert({name, std::vector<moveit::task_constructor::Task>()});
- Abstract_robot* robot2 = new Moveit_robot(name, tf2::Transform(rot, pos), size);
- Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robot2);
- ceti2->set_observer_mask(wing_config::RML);
- mediator->connect_robots(robot2);
+ std::unique_ptr<Abstract_param_reader> wing_reader(new Wing_reader(nnh));
+ wing_reader->read();
+ auto wd = static_cast<Wing_reader*>(wing_reader.get())->wing_data();
- ROS_INFO("=> Robot:loading SUCCESS");
+ for (int i = 0; i < mediator->robots().size(); i++){
+ for(object_data& w : wd[i].first){
+ w.pose_ = mediator->robots()[i]->tf().inverse() * w.pose_;
}
+ mediator->robots()[i]->set_observer_mask(static_cast<wing_config>(wd[i].second));
}
-
- for (int i = 0; i < resources.size(); i++){
- if(resources[i]["id"] == "table1_right_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- blueprints_per_robot[0][0].name_ = "table1_right_panel";
- blueprints_per_robot[0][0].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[0][0].size_ = size;
-
-
- a += std::pow(2, 0);
- ROS_INFO("=> WING:loading SUCCESS");
- }
-
- if(resources[i]["id"] == "table1_front_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- a += std::pow(2, 1);
-
- blueprints_per_robot[0][1].name_ = std::string("table1_front_panel");
- blueprints_per_robot[0][1].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[0][1].size_ = size;
- ROS_INFO("=> WING:loading SUCCESS");
- }
-
- if(resources[i]["id"] == "table1_left_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- blueprints_per_robot[0][2].name_ = "table1_left_panel";
- blueprints_per_robot[0][2].pos_ = mediator->robots()[0]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[0][2].size_ = size;
- a += std::pow(2, 2);
- ROS_INFO("=> WING:loading SUCCESS");
-
- }
-
- if(resources[i]["id"] == "table2_right_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- blueprints_per_robot[1][0].name_ = "table2_right_panel";
- blueprints_per_robot[1][0].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[1][0].size_ = size;
- b += std::pow(2, 0);
- ROS_INFO("=> WING:loading SUCCESS");
- }
-
- if(resources[i]["id"] == "table2_front_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- blueprints_per_robot[1][1].name_ = "table2_front_panel";
- blueprints_per_robot[1][1].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[1][1].size_ = size;
- ROS_INFO("=> WING:loading SUCCESS");
- b += std::pow(2, 1);
-
- }
-
- if(resources[i]["id"] == "table2_left_panel" ) {
- ROS_INFO("=> WING: description found, loading...");
- tf2::Vector3 pos;
- tf2::Vector3 size;
- tf2::Quaternion rot;
-
- (resources[i]["size"].hasMember("length")) ? size.setX(value_caster(resources[i]["size"]["length"])) :size.setX(0);
- (resources[i]["size"].hasMember("width")) ? size.setY(value_caster(resources[i]["size"]["width"])) :size.setY(0);
- (resources[i]["size"].hasMember("height")) ? size.setZ(value_caster(resources[i]["size"]["height"])) :size.setZ(0);
-
-
- (resources[i]["pos"].hasMember("x")) ? pos.setX(value_caster(resources[i]["pos"]["x"])) :pos.setX(0);
- (resources[i]["pos"].hasMember("y")) ? pos.setY(value_caster(resources[i]["pos"]["y"])) :pos.setY(0);
- (resources[i]["pos"].hasMember("z")) ? pos.setZ(value_caster(resources[i]["pos"]["z"])+ size.getZ()/2) :pos.setZ(0);
- (resources[i]["orientation"].hasMember("x")) ? rot.setX(value_caster(resources[i]["orientation"]["x"])) :rot.setX(0);
- (resources[i]["orientation"].hasMember("y")) ? rot.setY(value_caster(resources[i]["orientation"]["y"])) :rot.setY(0);
- (resources[i]["orientation"].hasMember("z")) ? rot.setZ(value_caster(resources[i]["orientation"]["z"])) :rot.setZ(0);
- (resources[i]["orientation"].hasMember("w")) ? rot.setW(value_caster(resources[i]["orientation"]["w"])) :rot.setW(0);
-
+ moveit_mediator->set_wings(wd);
+ Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(mediator->robots()[0]);
+ Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(mediator->robots()[1]);
+
- ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
- ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
- ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
-
- blueprints_per_robot[1][2].name_ = "table2_left_panel";
- blueprints_per_robot[1][2].pos_ = mediator->robots()[1]->tf().inverse() * tf2::Transform(rot.normalized(), pos);
- blueprints_per_robot[1][2].size_ = size;
- ROS_INFO("=> WING:loading SUCCESS");
- b += std::pow(2, 2);
- }
+ for (int i = 0; i < mediator->robots().size(); i++){
+ std::bitset<3> bs = wd[i].second;
+ moveit_mediator->build_wings(bs, i);
}
-
- moveit_mediator->set_wings(blueprints_per_robot);
- Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(mediator->robots()[0]);
- Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(mediator->robots()[1]);
-
- std::bitset<3> ia = a;
- std::bitset<3> ib = b;
- int r1 = 0; int r2 = 1;
- moveit_mediator->build_wings(ia, r1);
- moveit_mediator->build_wings(ib, r2);
//moveit::planning_interface::PlanningSceneInterface::applyCollisionObject()
ceti1->notify();
ceti2->notify();
diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp
index a530e8ac37415b6b4b0e3a67e478b5bf686c4cfa..b3c1ffcd5599c1c37734ccf9cdabca84cf386297 100644
--- a/src/mtc/src/impl/mediator.cpp
+++ b/src/mtc/src/impl/mediator.cpp
@@ -37,12 +37,12 @@ void Mediator::setup_rviz(){
//pub_->publish(ma);
}
-void Mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
+void Mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
for (int i =0; i < wbp.size(); i++){
std::vector<Abstract_robot_element*> v;
- for (int j =0; j < wbp[i].size(); j++){
- Abstract_robot_element* are = new Wing_rviz_decorator( new Wing(wbp[i][j].name_, wbp[i][j].pos_,wbp[i][j].size_));
+ for (int j =0; j < wbp[i].first.size(); j++){
+ Abstract_robot_element* are = new Wing_rviz_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
v.push_back(are);
}
wings_.push_back(v);
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
index 0f1e9ce05f75b4d85e2f83fb60bddc10f0eb8475..2071a411b7239548b442ec985705d6d7f28ee78e 100644
--- a/src/mtc/src/impl/moveit_mediator.cpp
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -71,18 +71,20 @@ void Moveit_mediator::mediate(){
void Moveit_mediator::build_wings(std::bitset<3>& wing, int& robot){
std::bitset<3> result = robots_[robot]->observer_mask() & wing;
Robot* ceti = dynamic_cast<Robot*>(robots_[robot]);
+
for (std::size_t i = 0; i < result.size(); i++){
if (result[i]){
+ ROS_INFO("%i", i);
ceti->register_observers(wings_[robot][i]);
}
}
}
-void Moveit_mediator::set_wings(std::vector<std::vector<wing_BP>>& wbp){
+void Moveit_mediator::set_wings(std::vector<std::pair<std::vector<object_data>, int>>& wbp){
for (int i =0; i < wbp.size(); i++){
std::vector<Abstract_robot_element*> v;
- for (int j =0; j < wbp[i].size(); j++){
- Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i][j].name_, wbp[i][j].pos_,wbp[i][j].size_));
+ for (int j =0; j < wbp[i].first.size(); j++){
+ Abstract_robot_element* are = new Wing_moveit_decorator( new Wing(wbp[i].first[j].name_, wbp[i].first[j].pose_,wbp[i].first[j].size_));
v.push_back(are);
}
wings_.push_back(v);
diff --git a/src/mtc/src/mtc2taskspace.cpp b/src/mtc/src/mtc2taskspace.cpp
index bff2f439a519fe1549833d4f1879b108f8826e14..e1b78f574d5fff8c1ecafe3b0f77c75f8e21390d 100644
--- a/src/mtc/src/mtc2taskspace.cpp
+++ b/src/mtc/src/mtc2taskspace.cpp
@@ -1,12 +1,13 @@
#include "reader/abstract_param_reader.h"
#include "reader/task_space_reader.h"
+#include "reader/robot_reader.h"
#include <regex>
int main(int argc, char **argv){
ros::init(argc, argv, "mtc2taskspace");
std::shared_ptr<ros::NodeHandle> n(new ros::NodeHandle);
- std::unique_ptr<Abstract_param_reader> task_space_reader(new Task_space_reader(n));
- task_space_reader->read();
+ std::unique_ptr<Abstract_param_reader> ts_reader(new Task_space_reader(n));
+ ts_reader->read();
return 0;
}
\ No newline at end of file
diff --git a/src/mtc/src/reader/robot_reader.cpp b/src/mtc/src/reader/robot_reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..a2e3b5703c95169828120e8137bdc08049b75680
--- /dev/null
+++ b/src/mtc/src/reader/robot_reader.cpp
@@ -0,0 +1,43 @@
+#include "reader/robot_reader.h"
+
+void Robot_reader::read(){
+ std::string filename = "dummy.yaml";
+ //nh_->getParam("/resource_name", filename);
+
+ ROS_INFO("--- ROBOT_READER ---");
+ XmlRpc::XmlRpcValue resources;
+ nh_->getParam("/objects", resources);
+ std::regex rx("table([0-9]+)_table_top");
+ std::smatch match;
+
+ for (int i = 0; i < resources.size(); i++){
+ std::string str = resources[i]["id"];
+ if(std::regex_match(str, match, rx)){
+ ROS_INFO("=> Robot: description found, loading...");
+ std::stringstream ss;
+ ss << "panda_arm" << match[1];
+ tf2::Vector3 pos;
+ tf2::Vector3 size;
+ tf2::Quaternion rot;
+
+ (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
+ (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
+ (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
+
+ (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
+ (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
+ (resources[i]["pos"].hasMember("z")) ? pos.setZ((float_of(resources[i]["pos"]["z"]) + size.getZ()/2) /2) :pos.setZ(0);
+ (resources[i]["orientation"].hasMember("x")) ? rot.setX(float_of(resources[i]["orientation"]["x"])) :rot.setX(0);
+ (resources[i]["orientation"].hasMember("y")) ? rot.setY(float_of(resources[i]["orientation"]["y"])) :rot.setY(0);
+ (resources[i]["orientation"].hasMember("z")) ? rot.setZ(float_of(resources[i]["orientation"]["z"])) :rot.setZ(0);
+ (resources[i]["orientation"].hasMember("w")) ? rot.setW(float_of(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+ ROS_INFO("--- Robot: %s ---", ss.str().c_str());
+ ROS_INFO("=> Robot: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+ ROS_INFO("=> Robot: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+ ROS_INFO("=> Robot: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+ robot_data_.push_back({ss.str().c_str(), tf2::Transform(rot,pos), size});
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/mtc/src/reader/task_space_reader.cpp b/src/mtc/src/reader/task_space_reader.cpp
index fa623b8e0d553370b46865466d4b6ec1078af2bc..5fef734010a5cbc5e5210ae31dc38c0012cf576f 100644
--- a/src/mtc/src/reader/task_space_reader.cpp
+++ b/src/mtc/src/reader/task_space_reader.cpp
@@ -2,7 +2,7 @@
void Task_space_reader::read(){
- std::string filename = "penis";
+ std::string filename = "dummy.yaml";
//nh_->getParam("/resource_name", filename);
XmlRpc::XmlRpcValue resources;
diff --git a/src/mtc/src/reader/wing_reader.cpp b/src/mtc/src/reader/wing_reader.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..0af4e425971000a8c8b77b799f2896cc4930ed92
--- /dev/null
+++ b/src/mtc/src/reader/wing_reader.cpp
@@ -0,0 +1,133 @@
+#include "reader/wing_reader.h"
+
+void Wing_reader::read(){
+ XmlRpc::XmlRpcValue resources;
+ nh_->getParam("/objects", resources);
+
+ std::regex rx("table([0-9]+)_table_top");
+ std::smatch match;
+
+ ROS_INFO("--- WING_READER ---");
+ for(int i = 0; i < resources.size(); i++){
+ std::string str = resources[i]["id"];
+ if(std::regex_match(str, match, rx)){
+ std::vector<object_data> wd;
+ wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
+ wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
+ wd.push_back({"", tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0)), tf2::Vector3(0,0,0)});
+ std::pair<std::vector<object_data>, int> pair;
+ pair.first = wd;
+ pair.second = 0;
+ wing_data_.push_back(pair);
+ }
+ }
+
+
+ rx.assign("table([0-9]+)_right_panel");
+ for(int i = 0; i < resources.size(); i++){
+ std::string str = resources[i]["id"];
+ if (std::regex_match(str, match, rx)){
+ ROS_INFO("=> WING: description found, loading...");
+ tf2::Vector3 pos;
+ tf2::Vector3 size;
+ tf2::Quaternion rot;
+
+
+ (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
+ (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
+ (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
+
+ (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
+ (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
+ (resources[i]["pos"].hasMember("z")) ? pos.setZ(float_of(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
+ (resources[i]["orientation"].hasMember("x")) ? rot.setX(float_of(resources[i]["orientation"]["x"])) :rot.setX(0);
+ (resources[i]["orientation"].hasMember("y")) ? rot.setY(float_of(resources[i]["orientation"]["y"])) :rot.setY(0);
+ (resources[i]["orientation"].hasMember("z")) ? rot.setZ(float_of(resources[i]["orientation"]["z"])) :rot.setZ(0);
+ (resources[i]["orientation"].hasMember("w")) ? rot.setW(float_of(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+ ROS_INFO("--- Wing %s ---", str.c_str());
+ ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+ ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+ ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[0].name_ = str.c_str();
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[0].pose_ = tf2::Transform(rot.normalized(), pos); // mediator->robots()[0]->tf().inverse() *
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[0].size_ = size;
+ wing_data_[std::atoi(match[1].str().c_str())-1].second += std::pow(2, 0);
+ }
+ }
+
+ rx.assign("table([0-9]+)_front_panel");
+ for(int i = 0; i < resources.size(); i++){
+ std::string str = resources[i]["id"];
+ if (std::regex_match(str, match, rx)){
+ ROS_INFO("=> WING: description found, loading...");
+ tf2::Vector3 pos;
+ tf2::Vector3 size;
+ tf2::Quaternion rot;
+
+
+ (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
+ (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
+ (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
+
+ (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
+ (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
+ (resources[i]["pos"].hasMember("z")) ? pos.setZ(float_of(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
+ (resources[i]["orientation"].hasMember("x")) ? rot.setX(float_of(resources[i]["orientation"]["x"])) :rot.setX(0);
+ (resources[i]["orientation"].hasMember("y")) ? rot.setY(float_of(resources[i]["orientation"]["y"])) :rot.setY(0);
+ (resources[i]["orientation"].hasMember("z")) ? rot.setZ(float_of(resources[i]["orientation"]["z"])) :rot.setZ(0);
+ (resources[i]["orientation"].hasMember("w")) ? rot.setW(float_of(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+ ROS_INFO("--- Wing %s ---", str.c_str());
+ ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+ ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+ ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[1].name_ = str.c_str();
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[1].pose_ = tf2::Transform(rot.normalized(), pos); // mediator->robots()[0]->tf().inverse() *
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[1].size_ = size;
+ wing_data_[std::atoi(match[1].str().c_str())-1].second += std::pow(2, 1);
+
+
+ }
+ }
+
+ rx.assign("table([0-9]+)_left_panel");
+ for(int i = 0; i < resources.size(); i++){
+ std::string str = resources[i]["id"];
+ if (std::regex_match(str, match, rx)){
+ ROS_INFO("=> WING: description found, loading...");
+ tf2::Vector3 pos;
+ tf2::Vector3 size;
+ tf2::Quaternion rot;
+
+
+ (resources[i]["size"].hasMember("length")) ? size.setX(float_of(resources[i]["size"]["length"])) :size.setX(0);
+ (resources[i]["size"].hasMember("width")) ? size.setY(float_of(resources[i]["size"]["width"])) :size.setY(0);
+ (resources[i]["size"].hasMember("height")) ? size.setZ(float_of(resources[i]["size"]["height"])) :size.setZ(0);
+
+ (resources[i]["pos"].hasMember("x")) ? pos.setX(float_of(resources[i]["pos"]["x"])) :pos.setX(0);
+ (resources[i]["pos"].hasMember("y")) ? pos.setY(float_of(resources[i]["pos"]["y"])) :pos.setY(0);
+ (resources[i]["pos"].hasMember("z")) ? pos.setZ(float_of(resources[i]["pos"]["z"]) + size.getZ()/2) :pos.setZ(0);
+ (resources[i]["orientation"].hasMember("x")) ? rot.setX(float_of(resources[i]["orientation"]["x"])) :rot.setX(0);
+ (resources[i]["orientation"].hasMember("y")) ? rot.setY(float_of(resources[i]["orientation"]["y"])) :rot.setY(0);
+ (resources[i]["orientation"].hasMember("z")) ? rot.setZ(float_of(resources[i]["orientation"]["z"])) :rot.setZ(0);
+ (resources[i]["orientation"].hasMember("w")) ? rot.setW(float_of(resources[i]["orientation"]["w"])) :rot.setW(0);
+
+ ROS_INFO("--- Wing %s ---", str.c_str());
+ ROS_INFO("=> WING: pos('%f', '%f', '%f')", pos.getX(), pos.getY(), pos.getZ());
+ ROS_INFO("=> WING: orientation('%f', '%f', '%f', '%f')", rot.getX(), rot.getY(), rot.getZ(), rot.getW());
+ ROS_INFO("=> WING: size('%f', '%f', '%f')", size.getX(), size.getY(), size.getZ());
+
+
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[2].name_ = str.c_str();
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[2].pose_ = tf2::Transform(rot.normalized(), pos); // mediator->robots()[0]->tf().inverse() *
+ wing_data_[std::atoi(match[1].str().c_str())-1].first[2].size_ = size;
+ wing_data_[std::atoi(match[1].str().c_str())-1].second += std::pow(2, 2);
+
+ }
+ }
+}