diff --git a/.catkin_tools/profiles/default/build.yaml b/.catkin_tools/profiles/default/build.yaml
index 910ef428350b2f977ff36bed501d87b46cb53802..122835594a26db730f6f4ac3160d4daf5d186ce0 100644
--- a/.catkin_tools/profiles/default/build.yaml
+++ b/.catkin_tools/profiles/default/build.yaml
@@ -10,7 +10,8 @@ extends: null
install: false
install_space: install
isolate_install: false
-jobs_args: []
+jobs_args:
+- -j2
licenses:
- TODO
log_space: logs
diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index d9a9bc8c771fcb9d5dd6d9dbcdf9924cbb791b1e..78c3c1b805dec2b7496ff8f136ae97520bd8b3ec 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 10354
+/home/matteo/reachability/devel/./cmake.lock 11015
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 79
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 79
diff --git a/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt b/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt
index 10aa091b063542d59b565e0b28acd741836a2dba..d505503fe7d34fdd0fc03488deecadb7aaf57276 100644
--- a/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt
+++ b/.catkin_tools/profiles/default/packages/mtc/devel_manifest.txt
@@ -3,8 +3,10 @@ mtc
/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig-version.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig-version.cmake
/home/matteo/reachability/devel/.private/mtc/share/mtc/cmake/mtcConfig.cmake /home/matteo/reachability/devel/share/mtc/cmake/mtcConfig.cmake
/home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps_filter.so /home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so
+/home/matteo/reachability/devel/.private/mtc/lib/liblib.so /home/matteo/reachability/devel/lib/liblib.so
/home/matteo/reachability/devel/.private/mtc/lib/libmoveit_grasps.so /home/matteo/reachability/devel/lib/libmoveit_grasps.so
/home/matteo/reachability/devel/.private/mtc/lib/pkgconfig/mtc.pc /home/matteo/reachability/devel/lib/pkgconfig/mtc.pc
/home/matteo/reachability/devel/.private/mtc/lib/mtc/mtc2taskspace /home/matteo/reachability/devel/lib/mtc/mtc2taskspace
/home/matteo/reachability/devel/.private/mtc/lib/mtc/base_routine /home/matteo/reachability/devel/lib/mtc/base_routine
/home/matteo/reachability/devel/.private/mtc/lib/mtc/moveit_grasps_grasp_pipeline /home/matteo/reachability/devel/lib/mtc/moveit_grasps_grasp_pipeline
+/home/matteo/reachability/devel/.private/mtc/lib/mtc/cell_routine /home/matteo/reachability/devel/lib/mtc/cell_routine
diff --git a/src/.vscode/c_cpp_properties.json b/src/.vscode/c_cpp_properties.json
index 060596d6214c5aa77b28e1fc301552d48a0df3b1..f67badc18cf5ae6175e1331b920da00ffe2a1159 100644
--- a/src/.vscode/c_cpp_properties.json
+++ b/src/.vscode/c_cpp_properties.json
@@ -33,7 +33,9 @@
"/home/matteo/ws_moveit/src/srdfdom/include/**",
"/home/matteo/ws_moveit/src/warehouse_ros/include/**",
"/usr/include/**",
- "${workspaceFolder}/mtc/include"
+ "${workspaceFolder}/mtc/include",
+ "${workspaceFolder}/mtc/include/impl",
+ "${workspaceFolder}/mtc/include/impl/ho"
],
"name": "ROS"
}
diff --git a/src/ceti_double/config/kinematics.yaml b/src/ceti_double/config/kinematics.yaml
index 9e26dfeeb6e641a33dae4961196235bdb965b21b..67d5a8281019b69e68c11d008f9bfeb0047aefc7 100644
--- a/src/ceti_double/config/kinematics.yaml
+++ b/src/ceti_double/config/kinematics.yaml
@@ -1 +1,8 @@
-{}
\ No newline at end of file
+panda_arm1:
+ kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
+panda_arm2:
+ kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin
+ kinematics_solver_search_resolution: 0.005
+ kinematics_solver_timeout: 0.05
diff --git a/src/ceti_double/config/panda.srdf b/src/ceti_double/config/panda.srdf
index 360074c6423824fe171e6a0819fd139bd13bdfc9..65e62271ee82b52b7be7922a4f27e75e7b2fd82b 100644
--- a/src/ceti_double/config/panda.srdf
+++ b/src/ceti_double/config/panda.srdf
@@ -48,7 +48,8 @@
<end_effector name="hand1" parent_link="panda_1_link8" group="hand_1"/>
<end_effector name="hand2" parent_link="panda_2_link8" group="hand_2"/>
<!--VIRTUAL JOINT: Purpose: this element defines a virtual joint between a robot link and an external frame of reference (considered fixed with respect to the robot)-->
- <virtual_joint name="virtual_joint" type="fixed" parent_frame="world" child_link="world"/>
+ <virtual_joint name="virtual_joint1" type="fixed" parent_frame="world" child_link="base_1"/>
+ <virtual_joint name="virtual_joint2" type="fixed" parent_frame="world" child_link="base_2"/>
<!--DISABLE COLLISIONS: By default it is assumed that any link of the robot could potentially come into collision with any other link in the robot. This tag disables collision checking between a specified pair of links. -->
<disable_collisions link1="base_1" link2="base_2" reason="Adjacent"/>
<disable_collisions link1="base_1" link2="panda_1_link0" reason="Adjacent"/>
diff --git a/src/ceti_double/launch/demo.launch b/src/ceti_double/launch/demo.launch
index ff84767aca1541eec72f92b13d4ef363f04d40ae..e9c08c838edcb5dca7cc1608e384f3a30b383ab5 100644
--- a/src/ceti_double/launch/demo.launch
+++ b/src/ceti_double/launch/demo.launch
@@ -24,7 +24,8 @@
<arg name="use_rviz" default="true" />
<!-- If needed, broadcast static tf for robot root -->
- <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world world" />
+ <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_0" args="0 0 0 0 0 0 world base1" />
+ <node pkg="tf2_ros" type="static_transform_publisher" name="virtual_joint_broadcaster_1" args="0 0 0 0 0 0 world base2" />
diff --git a/src/mtc/CMakeLists.txt b/src/mtc/CMakeLists.txt
index 9ca717b8e2483cc95400708ace2f7da4e1947996..1b18ded04128a426f68dc9b5cf24d16a4cd922d1 100644
--- a/src/mtc/CMakeLists.txt
+++ b/src/mtc/CMakeLists.txt
@@ -51,6 +51,7 @@ catkin_package(
trajectory_msgs
INCLUDE_DIRS
include
+ include/impl
DEPENDS
EIGEN3
)
@@ -61,11 +62,38 @@ catkin_package(
include_directories(
include
+ include/impl
${catkin_INCLUDE_DIRS}
${EIGEN3_INCLUDE_DIR}
${PCL_INCLUDE_DIR}
)
+# Demo grasp pipeline
+add_executable(base_routine src/base_routine.cpp
+src/impl/abstract_mediator.cpp
+src/impl/base_by_rotation.cpp
+src/impl/field_rviz_decorator.cpp
+src/impl/map_loader.cpp
+src/impl/mediator.cpp
+src/impl/moveit_mediator.cpp
+src/impl/robot.cpp
+src/impl/wing_moveit_decorator.cpp
+src/impl/wing_rviz_decorator.cpp
+)
+
+add_executable(cell_routine src/cell_routine.cpp
+src/impl/abstract_mediator.cpp
+src/impl/moveit_mediator.cpp
+src/impl/wing_moveit_decorator.cpp
+src/impl/robot.cpp
+
+src/impl/mediator.cpp
+src/impl/wing_moveit_decorator.cpp
+src/impl/wing_rviz_decorator.cpp
+)
+
+
+
# Grasp Library
add_library(moveit_grasps
src/grasp_candidate.cpp
@@ -98,13 +126,14 @@ set_target_properties(moveit_grasps_filter PROPERTIES COMPILE_FLAGS "${CMAKE_CXX
set_target_properties(moveit_grasps_filter PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}")
-# Demo grasp pipeline
-add_executable(base_routine src/base_routine.cpp)
+
+
add_executable(mtc2taskspace src/mtc2taskspace.cpp)
add_executable(moveit_grasps_grasp_pipeline src/grasp_pipeline.cpp)
add_dependencies(base_routine ${base_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+add_dependencies(cell_routine ${cell_routine_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
add_dependencies(mtc2taskspace ${mtc2taskspace_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
@@ -118,6 +147,12 @@ target_link_libraries(base_routine
${PCL_LIBRARY_DIRS}
yaml-cpp
)
+target_link_libraries(cell_routine
+ ${catkin_LIBRARIES}
+ ${OCTOMAP_LIBRARIES}
+ ${PCL_LIBRARY_DIRS}
+ yaml-cpp
+)
target_link_libraries(mtc2taskspace
${catkin_LIBRARIES}
diff --git a/src/mtc/include/impl/abstract_map_loader.h b/src/mtc/include/impl/abstract_map_loader.h
index dfb04241392a4804a5fc003b0cdac8ff814e4f56..e98aaf6406d17ae09612977900a44b2a5d7952ef 100644
--- a/src/mtc/include/impl/abstract_map_loader.h
+++ b/src/mtc/include/impl/abstract_map_loader.h
@@ -11,6 +11,7 @@
#include "impl/abstract_strategy.h"
+class Abstract_strategy;
class Abstract_map_loader{
protected:
@@ -26,8 +27,9 @@ class Abstract_map_loader{
Abstract_map_loader() = default;
~Abstract_map_loader() = default;
- virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0;
-
+ inline void set_strategy(Abstract_strategy* some_stratergy) {strategy_ = some_stratergy;}
+ inline Abstract_strategy* strategy() {return strategy_;}
+
inline std::vector<tf2::Transform>& inv_map() {return inv_map_;}
inline std::vector<tf2::Transform>& map() {return map_;}
inline void set_inv_map(std::vector<tf2::Transform>& list) {inv_map_ = list;}
@@ -39,12 +41,9 @@ class Abstract_map_loader{
inline void set_target_cloud(std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& cloud) {target_cloud_ = cloud;}
inline std::vector<std::vector<pcl::PointCloud< pcl::PointXYZ >::Ptr>>& target_cloud () { return target_cloud_;}
- inline void set_strategy(Abstract_strategy* some_stratergy) {strategy_ = some_stratergy;}
- inline Abstract_strategy* strategy() {return strategy_;}
static std::vector<pcl::PointXYZ> create_pcl_box();
+ virtual std::vector<std::vector<pcl::PointXYZ>> base_calculation()=0;
};
-
-
#endif
diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h
index eae099d568d03f4499689f6cd928cdee33082822..247274582b22e329e50cde91d47dbd1c6b9bcfe5 100644
--- a/src/mtc/include/impl/abstract_mediator.h
+++ b/src/mtc/include/impl/abstract_mediator.h
@@ -11,46 +11,49 @@
#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)
+#define M_POS tf2::Vector3(0.6525f,0,0.4425f-0.005f)
+
+struct wing_BP {
+ tf2::Transform pos_;
+ tf2::Vector3 size_;
+ wing_BP(tf2::Transform pos, tf2::Vector3 size) : pos_(pos), size_(size){};
+};
+
-class Abstract_mediator{
+class Abstract_mediator {
protected:
std::vector<Abstract_robot*> robots_;
std::vector<std::vector<tf2::Transform>> objects_;
std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
ros::Publisher* pub_;
std::vector<std::vector<pcl::PointXYZ>> result_vector_;
- std::vector<Abstract_robot_element*> wings_;
+ std::vector<wing_BP> wings_;
+
public:
Abstract_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){
- wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right));
- wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left));
- wings_.push_back(new Wing(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid));
+ wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.6525f,0.4425f -0.005f)), right));
+ wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.6525f,0.4425f -0.005f)), left));
+ wings_.push_back(wing_BP(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.6525f,0,0.4425f-0.005f)), mid));
}
+
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<wing_BP> wings() {return wings_;}
inline std::vector<std::vector<pcl::PointXYZ>>& result_vector() {return result_vector_;}
+
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);
- void add_wing(Robot* robot) {
- if (robot->observers().size() == 0){
- Abstract_robot_element* rviz_right = new Wing_rviz_decorator(wings_[0], robot->rviz_markers()[0]);
- robot->register_observers(rviz_right);
- } else if (robot->observers().size() == 1){
- Abstract_robot_element* rviz_left = new Wing_rviz_decorator(wings_[1], robot->rviz_markers()[0]);
- robot->register_observers(rviz_left);
- } else if (robot->observers().size() == 2){
- Abstract_robot_element* rviz_mid = new Wing_rviz_decorator(wings_[2], robot->rviz_markers()[0]);
- robot->register_observers(rviz_mid);
- } else {
- ROS_INFO("[Warning] %s got to many wings", robot->name().c_str());
- }
- }
+
virtual bool check_collision( const int& robot)=0;
virtual void mediate()=0;
+ virtual void build_wings(std::bitset<3>& wing,Robot* robot)=0;
+
};
diff --git a/src/mtc/include/impl/abstract_robot.h b/src/mtc/include/impl/abstract_robot.h
index d8ab4d874fe319b91686d42ecc687dd759679e8e..9b41c207260bec0bd70b76fbf448a59cde68ab8a 100644
--- a/src/mtc/include/impl/abstract_robot.h
+++ b/src/mtc/include/impl/abstract_robot.h
@@ -11,6 +11,17 @@
#define left tf2::Vector3(0.7f, 0.5f, 0.01f)
#define mid tf2::Vector3(0.5f, 0.7f, 0.01f)
+enum wing_config{
+ RML = 7,
+ RM_ = 6,
+ R_L = 5,
+ R__ = 4,
+ _ML = 3,
+ _M_ = 2,
+ __L = 1,
+ ___ = 0
+};
+
class Abstract_robot {
protected:
std::string name_;
@@ -18,6 +29,7 @@ class Abstract_robot {
tf2::Transform root_tf_;
std::vector<tf2::Transform> bounds_;
std::vector<tf2::Transform> robot_root_bounds_;
+ std::bitset<3> observer_mask_;
@@ -35,7 +47,10 @@ class Abstract_robot {
robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.113f, -0.095f, 0)));
robot_root_bounds_.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.113f, -0.095f, 0)));
+ observer_mask_ = std::bitset<3>(0);
+
};
+
inline std::string& name() { return name_;}
inline tf2::Transform& tf() { return tf_;}
inline tf2::Transform& root_tf() { return root_tf_;}
@@ -43,6 +58,9 @@ class Abstract_robot {
inline std::vector<tf2::Transform>& robot_root_bounds() { return robot_root_bounds_;}
inline void rotate(float deg) {tf2::Quaternion rot; rot.setRPY(0,0,deg); tf2::Transform t(rot, tf2::Vector3(0,0,0)); tf_= tf_* t;}
inline void translate(tf2::Vector3 t) {tf2::Transform tf(tf2::Quaternion(0,0,0,1), t); tf_*=tf;}
+ inline std::bitset<3> observer_mask() {return observer_mask_;}
+ inline void set_observer_mask(int i) {observer_mask_ = std::bitset<3>(i);}
+
virtual void notify()= 0;
virtual bool check_single_object_collision(tf2::Transform& obj)= 0;
diff --git a/src/mtc/include/impl/abstract_robot_element.h b/src/mtc/include/impl/abstract_robot_element.h
index 7d77c0578e53b46026d521148a6b1fd2117dd025..b10e1c25443fdadb6cabae74647a5bff1b41c567 100644
--- a/src/mtc/include/impl/abstract_robot_element.h
+++ b/src/mtc/include/impl/abstract_robot_element.h
@@ -13,6 +13,7 @@ class Abstract_robot_element {
public:
Abstract_robot_element() = default;
+
inline void set_relative_tf(tf2::Transform tf) { relative_tf_= tf;}
inline tf2::Transform& relative_tf(){ return relative_tf_;}
inline void calc_world_tf(tf2::Transform& tf) {world_tf_= tf * relative_tf_;}
diff --git a/src/mtc/include/impl/abstract_robot_element_decorator.h b/src/mtc/include/impl/abstract_robot_element_decorator.h
index eb16b2b1ffc230d7d0fb37c9e1eb136cf2346bd1..e7845413efb32618c641d7a79e343e3681df7a6f 100644
--- a/src/mtc/include/impl/abstract_robot_element_decorator.h
+++ b/src/mtc/include/impl/abstract_robot_element_decorator.h
@@ -11,9 +11,10 @@ class Abstract_robot_element_decorator : public Abstract_robot_element{
public:
Abstract_robot_element_decorator(Abstract_robot_element* next) : next_(next){};
+
inline Abstract_robot_element* wing() { return next_;}
- void update(tf2::Transform& tf) override {next_->update(tf);}
+ void update(tf2::Transform& tf) override {next_->update(tf);}
virtual void input_filter(tf2::Transform& tf)=0;
virtual void output_filter()=0;
};
diff --git a/src/mtc/include/impl/field.h b/src/mtc/include/impl/field.h
index 606ef6f80446b71e77b6a6492822e2355ebe5603..9fbec3090d7cb1465d4af68f0b72e1550f7c6074 100644
--- a/src/mtc/include/impl/field.h
+++ b/src/mtc/include/impl/field.h
@@ -7,6 +7,7 @@
class Field : public Abstract_robot_element{
public:
Field(tf2::Transform tf){ relative_tf_ = tf;}
+
void update(tf2::Transform& tf) override {this->calc_world_tf(tf);}
};
diff --git a/src/mtc/include/impl/field_rviz_decorator.h b/src/mtc/include/impl/field_rviz_decorator.h
index ef352eeea4225c24cc435b7215bb407ea227a752..254c6cc0938f09c068da879dac6a81fa8b71b61d 100644
--- a/src/mtc/include/impl/field_rviz_decorator.h
+++ b/src/mtc/include/impl/field_rviz_decorator.h
@@ -13,8 +13,10 @@ class Field_rviz_decorator : public Abstract_robot_element_decorator{
public:
Field_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_;}
+
void update(tf2::Transform& tf) override;
void input_filter(tf2::Transform& tf) override;
void output_filter() override;
diff --git a/src/mtc/include/impl/map_loader.h b/src/mtc/include/impl/map_loader.h
index e73ab705b922020788a05ddd088f5e3a4690db34..2e9c208049322f55792e027130b1149ceb86c83f 100644
--- a/src/mtc/include/impl/map_loader.h
+++ b/src/mtc/include/impl/map_loader.h
@@ -2,7 +2,7 @@
#define MAP_LOADER_
#include "ros/ros.h"
-#include "abstract_map_loader.h"
+#include "impl/abstract_map_loader.h"
#include <regex>
diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h
index 87eacd85b74bb582049b56f373d814999c171c12..c1389289e7dc1482293d26627f8629b6d7e446f3 100644
--- a/src/mtc/include/impl/mediator.h
+++ b/src/mtc/include/impl/mediator.h
@@ -1,7 +1,10 @@
#ifndef MEDIATOR_
#define MEDIATOR_
-#include "ros/ros.h"
+#include <ros/ros.h>
+#include <ros/package.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
#include "impl/abstract_mediator.h"
#include "impl/abstract_robot.h"
#include "impl/robot.h"
@@ -12,12 +15,15 @@ class Mediator : public Abstract_mediator{
public:
Mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
- bool check_collision(const int& robot) override;
- void mediate() override;
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);
- void approximation(Robot* ceti1);
+ void approximation(Robot* robot, int& wings, std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict);
+ void write_file(std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict);
+
+ bool check_collision(const int& robot) override;
+ void mediate() override;
+ void build_wings(std::bitset<3>& wings, Robot* robot) override;
};
diff --git a/src/mtc/include/impl/moveit_mediator.h b/src/mtc/include/impl/moveit_mediator.h
new file mode 100644
index 0000000000000000000000000000000000000000..766473719a260f56b0143def624782762f42f35f
--- /dev/null
+++ b/src/mtc/include/impl/moveit_mediator.h
@@ -0,0 +1,43 @@
+#ifndef MOVEIT_MEDIATOR_
+#define MOVEIT_MEDIATOR_
+
+#include <ros/ros.h>
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/kinematic_constraints/utils.h>
+
+#include "impl/abstract_mediator.h"
+#include "impl/abstract_robot.h"
+#include "impl/robot.h"
+#include "impl/moveit_robot.h"
+
+class Moveit_mediator : public Abstract_mediator{
+ protected:
+ ros::NodeHandle nh_;
+ ros::Publisher collision_object_publisher_;
+ ros::Publisher planning_scene_diff_publisher_;
+ robot_model_loader::RobotModelLoader* robot_model_loader_;
+ robot_model::RobotModelPtr kinematic_model_;
+ planning_scene::PlanningScene* ps_;
+
+ public:
+ Moveit_mediator(std::vector<std::vector<tf2::Transform>> objects, ros::Publisher* pub, ros::NodeHandle nh) : Abstract_mediator(objects, pub), nh_(nh){
+ collision_object_publisher_ = nh.advertise<moveit_msgs::CollisionObject>("collision_object", 1);
+ planning_scene_diff_publisher_ = nh.advertise<moveit_msgs::PlanningScene>("planning_scene", 1);
+ };
+
+ bool check_collision(const int& robot) override;
+ void mediate() override;
+ void build_wings(std::bitset<3>& wing,Robot* robot) override;
+
+ void publish_tables();
+ void load_robot_description();
+
+
+};
+
+#endif
\ No newline at end of file
diff --git a/src/mtc/include/impl/moveit_robot.h b/src/mtc/include/impl/moveit_robot.h
new file mode 100644
index 0000000000000000000000000000000000000000..2b3aa8c2cf0a7dff6587653031162e33e467ed05
--- /dev/null
+++ b/src/mtc/include/impl/moveit_robot.h
@@ -0,0 +1,20 @@
+#ifndef MOVEIT_ROBOT_
+#define MOVEIT_ROBOT_
+
+#include <ros/ros.h>
+#include "impl/robot.h"
+
+
+class Moveit_robot : public Robot{
+ private:
+ moveit::planning_interface::MoveGroupInterface* mgi_;
+
+ public:
+ Moveit_robot(std::string name, tf2::Transform tf) : Robot(name, tf){
+ mgi_ = new moveit::planning_interface::MoveGroupInterface(name);
+ }
+
+
+};
+
+#endif
\ No newline at end of file
diff --git a/src/mtc/include/impl/robot.h b/src/mtc/include/impl/robot.h
index e4c347187620c820f092be3a9d2d4dfd3f34684d..1a51167293167190d195c28ebed3ad899461dbea 100644
--- a/src/mtc/include/impl/robot.h
+++ b/src/mtc/include/impl/robot.h
@@ -9,147 +9,43 @@
#include "impl/wing_rviz_decorator.h"
#include "impl/field.h"
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/robot_model_loader/robot_model_loader.h>
+#include <moveit/planning_scene/planning_scene.h>
+#include <moveit/kinematic_constraints/utils.h>
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) : Abstract_robot(name, tf){
generate_access_fields();
}
-
- void generate_access_fields(){
- for (int i = 0; i <= 2; i++){
- for (int j = 0; j <= 2; j++){
- if(i == 0 && j == 0) {continue;}
- if(i == 2 && j == 2) {continue;}
- if(i == 0) {
-
- access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.8f*j,0))));
- access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.8f*j,0))));
- } else if (j == 0){
- access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,0,0))));
- access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,0,0))));
- } else {
- access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,0.8f*j,0))));
- access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,0.8f*j,0))));
- access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,-0.8f*j,0))));
- access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,-0.8f*j,0))));
- }
-
- };
- };
- }
-
+
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 float area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C){
- return std::abs(
- (B.getOrigin().getX() * A.getOrigin().getY()) - (A.getOrigin().getX() * B.getOrigin().getY()) +
- (C.getOrigin().getX() * B.getOrigin().getY()) - (B.getOrigin().getX() * C.getOrigin().getY()) +
- (A.getOrigin().getX() * C.getOrigin().getY()) - (C.getOrigin().getX() * A.getOrigin().getY()))*0.5f;
- }
-
-
- void notify() override {for(Abstract_robot_element* wing : observers_) wing->update(tf_); for(Abstract_robot_element* field : access_fields_) field->update(tf_);}
- bool check_single_object_collision(tf2::Transform& obj) override {
-
- tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0];
- tf2::Transform B = tf_ * root_tf_ * robot_root_bounds_[1];
- tf2::Transform C = tf_ * root_tf_ * robot_root_bounds_[2];
- tf2::Transform D = tf_ * root_tf_ * robot_root_bounds_[3];
-
- float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
- float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
- if ((std::floor(sum*100)/100.f) <= full_area) {return false; }
-
-
- for (int i = 0; i < bounds_.size(); i++){
- tf2::Transform A = tf_ * bounds_[0];
- tf2::Transform B = tf_ * bounds_[1];
- tf2::Transform C = tf_ * bounds_[2];
- tf2::Transform D = tf_ * bounds_[3];
-
- full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
- sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+ inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
+ inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
+ inline void add_coll_markers(moveit_msgs::CollisionObject* marker) {coll_markers_.push_back(marker);}
+ inline std::vector<moveit_msgs::CollisionObject*> coll_markers(){ return coll_markers_;}
- if ((std::floor(sum*100)/100.f) <= full_area) return true;
- }
+ void register_observers(Abstract_robot_element* wd);
+ void reset();
+ void generate_access_fields();
+ float area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C);
- if (!observers_.empty()){
- std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();
- while (it != observers_.end()) {
- Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
- Wing* ceti = dynamic_cast<Wing*>(deco->wing());
- tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
- tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
- tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
- tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
-
- full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
- sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+ bool check_single_object_collision(tf2::Transform& obj) override;
+ void notify() override;
- if ((std::floor(sum*100)/100.f) <= full_area) {
- return true;
- } else {
- ++it;
- }
- }
- }
- return false;
- }
- void reset(){
- observers_.clear();
- access_fields_.clear();
- generate_access_fields();
- tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
- }
-
-
- inline std::vector<Abstract_robot_element*>& observers() { return observers_;}
- inline std::vector<Abstract_robot_element*>& access_fields() { return access_fields_;}
- void register_observers(Abstract_robot_element* wd){
- Abstract_robot_element_decorator* decorator = dynamic_cast<Abstract_robot_element_decorator*>(wd);
- observers_.push_back(wd);
- std::vector<tf2::Transform> plane;
- bool found = false;
- int index = 0;
- if (decorator->wing()->relative_tf().getOrigin().getY()>0){
- for(int i = 0; i < access_fields_.size(); i++){
- if ((access_fields_[i]->relative_tf().getOrigin().getY() > 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
- found = true;
- index = i;
- break;
- }
- }
- if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
- } else if (decorator->wing()->relative_tf().getOrigin().getY()<0) {
- for(int i = 0; i < access_fields_.size(); i++){
- if ((access_fields_[i]->relative_tf().getOrigin().getY() < 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
- found = true;
- index = i;
- break;
- }
- }
- if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
- } else {
- for(int i = 0; i < access_fields_.size(); i++){
- if (access_fields_[i]->relative_tf().getOrigin().getX() > 0){
- found = true;
- index = i;
- break;
- }
- }
- if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
- }
- };
};
diff --git a/src/mtc/include/impl/wing_moveit_decorator.h b/src/mtc/include/impl/wing_moveit_decorator.h
new file mode 100644
index 0000000000000000000000000000000000000000..0db53ef50ad7b92747a87dea705277a658db2aa5
--- /dev/null
+++ b/src/mtc/include/impl/wing_moveit_decorator.h
@@ -0,0 +1,29 @@
+#ifndef WING_MOVEIT_DECORATOR_
+#define WING_MOVEIT_DECORATOR_
+
+#include <ros/ros.h>
+#include "impl/wing.h"
+#include "impl/abstract_robot_element_decorator.h"
+
+#include <moveit/planning_scene_interface/planning_scene_interface.h>
+#include <moveit/move_group_interface/move_group_interface.h>
+#include <moveit_msgs/ApplyPlanningScene.h>
+#include <moveit/planning_scene/planning_scene.h>
+
+class Wing_moveit_decorator : public Abstract_robot_element_decorator{
+ private:
+ moveit_msgs::CollisionObject* markers_;
+
+
+ public:
+ Wing_moveit_decorator(Abstract_robot_element* next, moveit_msgs::CollisionObject* markers) : Abstract_robot_element_decorator(next), markers_(markers){};
+ inline void set_markers(moveit_msgs::CollisionObject* markers) { markers_= markers;}
+ inline moveit_msgs::CollisionObject* markers() { return markers_;}
+
+ void update(tf2::Transform& tf) override;
+ void input_filter(tf2::Transform& tf) override;
+ void output_filter() override;
+};
+
+
+#endif
\ No newline at end of file
diff --git a/src/mtc/include/mtc2taskspace.h b/src/mtc/include/mtc2taskspace.h
index af645f13e38a7d4bd29c52fbe38366ada9f65d9c..9a7327ded050e740b181d9bf4cabfd602f72e808 100644
--- a/src/mtc/include/mtc2taskspace.h
+++ b/src/mtc/include/mtc2taskspace.h
@@ -1,14 +1,16 @@
#ifndef MTC2TASKSPACE_H
#define MTC2TASKSPACE_H
-#include "ros/ros.h"
+#include <ros/ros.h>
#include <ros/package.h>
+#include <yaml-cpp/yaml.h>
+#include <fstream>
+
+
#include "std_msgs/String.h"
#include <tf/transform_broadcaster.h>
#include <regex>
-#include "yaml-cpp/yaml.h"
#include <xmlrpcpp/XmlRpc.h>
-#include <fstream>
#endif
diff --git a/src/mtc/launch/cell_routine.launch b/src/mtc/launch/cell_routine.launch
new file mode 100644
index 0000000000000000000000000000000000000000..d394fd582456818ddba30e976e7927fbd34243b3
--- /dev/null
+++ b/src/mtc/launch/cell_routine.launch
@@ -0,0 +1,7 @@
+<launch>
+ <!--<include file="$(find panda_moveit_config)/launch/demo.launch"></include> -->
+ <include file="$(find ceti_double)/launch/demo.launch">
+ </include>
+
+ <node pkg="mtc" type="cell_routine" name="cell_routine" output="screen" required="true"/>
+</launch>
\ No newline at end of file
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
index 1933442b8ecfea7adbe294fd7e27c1d8cb0d1e84..532c2c16d2aadf449f455fc18116be38ab2ca08a 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/src/mtc/src/base_routine.cpp
@@ -14,13 +14,7 @@
#include "impl/base_by_rotation.h"
#include <xmlrpcpp/XmlRpc.h>
-#include "impl/abstract_mediator.cpp"
-#include "impl/base_by_rotation.cpp"
-#include "impl/field_rviz_decorator.cpp"
-#include "impl/map_loader.cpp"
-#include "impl/mediator.cpp"
-#include "impl/wing_rviz_decorator.cpp"
-
+// cpp inputs ,,,
int main(int argc, char **argv){
ros::init(argc, argv, "base_routine");
@@ -50,6 +44,7 @@ int main(int argc, char **argv){
Abstract_robot* robo = new Robot(std::string("Robot_arm1"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f)));
Robot* ceti = dynamic_cast<Robot*>(robo);
+ ceti->set_observer_mask(wing_config::RML);
ceti->add_rviz_markers(wings1);
ceti->add_rviz_markers(fields1);
diff --git a/src/mtc/src/cell_routine.cpp b/src/mtc/src/cell_routine.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..752f3d6d9f19cb8acf08895a703ba27345142361
--- /dev/null
+++ b/src/mtc/src/cell_routine.cpp
@@ -0,0 +1,54 @@
+#include "impl/abstract_robot.h"
+#include "impl/abstract_robot_element.h"
+#include "impl/abstract_robot_element_decorator.h"
+#include "impl/abstract_mediator.h"
+#include "impl/moveit_mediator.h"
+#include "impl/wing_moveit_decorator.h"
+#include "impl/wing.h"
+#include "impl/moveit_robot.h"
+#include <xmlrpcpp/XmlRpc.h>
+
+int main(int argc, char **argv){
+ ros::init(argc, argv, "cell_routine");
+ ros::NodeHandle nh;
+ ros::AsyncSpinner spinner(1);
+ spinner.start();
+
+ std::vector<std::vector<tf2::Transform>> objects; //in progress
+ ros::Publisher* pub = new ros::Publisher(nh.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)); //refractor
+
+ Abstract_mediator* mediator = new Moveit_mediator(objects, pub, nh);
+ Moveit_mediator* moveit_mediator = dynamic_cast<Moveit_mediator*>(mediator);
+
+ moveit_mediator->load_robot_description();
+
+ Abstract_robot* robo1 = new Moveit_robot(std::string("panda_arm1"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(1,0,0.4425f)));
+ Abstract_robot* robo2 = new Moveit_robot(std::string("panda_arm2"), tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-1,0,0.4425f)));
+ robo1->set_observer_mask(wing_config::RML);
+ robo2->set_observer_mask(wing_config::RML);
+
+
+
+ mediator->connect_robots(robo1);
+ mediator->connect_robots(robo2);
+
+
+
+ Moveit_robot* ceti1 = dynamic_cast<Moveit_robot*>(robo1);
+ Moveit_robot* ceti2 = dynamic_cast<Moveit_robot*>(robo2);
+
+
+ std::bitset<3> i = 7;
+ moveit_mediator->build_wings(i, ceti1);
+ moveit_mediator->build_wings(i, ceti2);
+
+ ceti1->notify();
+ ceti2->notify();
+ moveit_mediator->publish_tables();
+
+
+
+ while (ros::ok()){
+ ros::spinOnce();
+ }
+}
\ No newline at end of file
diff --git a/src/mtc/src/impl/abstract_mediator.cpp b/src/mtc/src/impl/abstract_mediator.cpp
index fc6b6c21a5c92da41d4378c5da7f9f6c4a3b67e0..1b169411b60a4f69f6a49a210a13303b90eb01c8 100644
--- a/src/mtc/src/impl/abstract_mediator.cpp
+++ b/src/mtc/src/impl/abstract_mediator.cpp
@@ -19,4 +19,4 @@ pcl::PointCloud< pcl::PointXYZ >::Ptr Abstract_mediator::vector_to_cloud(std::ve
task_voxel->push_back(point);
return task_voxel;
-}
\ No newline at end of file
+}
diff --git a/src/mtc/src/impl/map_loader.cpp b/src/mtc/src/impl/map_loader.cpp
index 268208bbbce9cf81fb03af505c95bcc16eedaced..785247636fb9e9b7ad21923a4523f5c5c6a589a8 100644
--- a/src/mtc/src/impl/map_loader.cpp
+++ b/src/mtc/src/impl/map_loader.cpp
@@ -1,5 +1,3 @@
-#include <ros/ros.h>
-#include "impl/abstract_map_loader.h"
#include "impl/map_loader.h"
diff --git a/src/mtc/src/impl/mediator.cpp b/src/mtc/src/impl/mediator.cpp
index 90a54184effe09e1a3a50b1575dea952d72b2ac4..60f09ad52946e5ef95eaeade84120384afb49f00 100644
--- a/src/mtc/src/impl/mediator.cpp
+++ b/src/mtc/src/impl/mediator.cpp
@@ -1,5 +1,6 @@
#include "impl/mediator.h"
-#include "impl/abstract_mediator.h"
+
+#include "impl/wing_rviz_decorator.h"
@@ -64,54 +65,16 @@ void Mediator::mediate(){
if(objects_[0][i].getOrigin().distance(objects_[1].back().getOrigin()) == 0) objects_[1].pop_back();
}
- /*
- for (int i = 0; i < result_vector_.size();i++){
- result_clouds.push_back(pcl::octree::OctreePointCloudSearch< pcl::PointXYZ >(0.4f));
- result_clouds[i].setInputCloud(Abstract_mediator::vector_to_cloud(result_vector_[i]));
- result_clouds[i].addPointsFromInputCloud();
- std::vector<tf2::Transform> ground_per_robot;
- for(pcl::PointXYZ& p : grCenter){
- double min_x, min_y, min_z, max_x, max_y, max_z;
- result_clouds[i].getBoundingBox(min_x, min_y, min_z, max_x, max_y, max_z);
- bool isInBox = (p.x >= min_x && p.x <= max_x) && (p.y >= min_y && p.y <= max_y) && (p.z >= min_z && p.z <= max_z);
- if (isInBox && result_clouds[i].isVoxelOccupiedAtPoint(p)) {
- std::vector< int > pointIdxVec;
- if (result_clouds[i].voxelSearch(p, pointIdxVec)) ground_per_robot.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(p.x, p.y, p.z)));
- }
- }
- filter_per_robot.push_back(ground_per_robot);
- }
+ std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>> dict;
ros::Rate loop_rate(10);
- for (int x = 0; x < robots_.size(); x++){
- Robot* ceti = dynamic_cast<Robot*>(robots_[x]);
- for (int i = 0; i < filter_per_robot[x].size(); i++){
- robots_[x]->set_tf(filter_per_robot[x][i]);
- for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
- for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear();
- rviz_markers_->markers.clear();
- ceti->rotate(0.0872665f);
- ceti->notify();
- if (check_collision(x)) {
- ROS_INFO("robot %i found its position", x);
-
- } else {
- continue;
- }
- for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers);
- pub_->publish(*rviz_markers_);
- loop_rate.sleep();
- }
+ Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
- }
- }
- */
+ for(int j = 0; j <= 7; j++){
+ std::bitset<3> wing_config(j);
+ build_wings(wing_config, ceti1);
-
- ros::Rate loop_rate(10);
- Robot* ceti1 = dynamic_cast<Robot*>(robots_[0]);
- for(int j = 0; j <= 3; j++){
for (int i = 0; i < ground_per_robot.size(); i++){
robots_[0]->set_tf(ground_per_robot[i]);
for ( float p = 0; p < 2*M_PI; p += 0.0872665f){
@@ -119,20 +82,32 @@ void Mediator::mediate(){
rviz_markers_->markers.clear();
ceti1->rotate(0.0872665f);
ceti1->notify();
+ bool temp = check_collision(0);
+ /*
if (check_collision(0)){
- approximation(ceti1);
+ approximation(ceti1, j, dict);
} else {
continue;
}
+ */
for (visualization_msgs::MarkerArray* markers : ceti1->rviz_markers())pub_->publish(*markers);
pub_->publish(*rviz_markers_);
}
}
- add_wing(ceti1);
+ ceti1->reset();
+ }
+
+ if (dict.empty()){
+ ROS_INFO("unfortunately, no result is found...");
+ return;
+ } else {
+ ROS_INFO("write data to file...");
+
}
+
}
-void Mediator::approximation(Robot* robot){
+void Mediator::approximation(Robot* robot, int& wings, std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict){
ros::Rate loop_rate(1);
std::vector<pcl::PointXYZ> relative_ground;
pcl::octree::OctreePointCloudSearch< pcl::PointXYZ > cloud(0.4f);
@@ -140,7 +115,12 @@ void Mediator::approximation(Robot* robot){
cloud.addPointsFromInputCloud();
Robot* ceti = dynamic_cast<Robot*>(robots_[1]);
- for (int i = 0; i <= 3; i++){
+ std::vector<std::pair<tf2::Transform, int>> second_robot_data;
+
+ for (int i = 0; i <= 7; i++){
+ std::bitset<3> wing_config(i);
+ build_wings(wing_config, ceti);
+
for (Abstract_robot_element* fields : robot->access_fields()) {
ceti->set_tf(fields->world_tf());
for ( float p = 0; p < 2*M_PI; p += M_PI/2){
@@ -148,6 +128,7 @@ void Mediator::approximation(Robot* robot){
ceti->notify();
if (check_collision(1)) {
ROS_INFO("should work");
+ second_robot_data.push_back(std::pair<tf2::Transform, int>(ceti->tf(), i));
} else {
continue;
}
@@ -155,9 +136,49 @@ void Mediator::approximation(Robot* robot){
}
//relative_ground.push_back(pcl::PointXYZ(fields->world_tf().getOrigin().getX(), fields->world_tf().getOrigin().getY(), fields->world_tf().getOrigin().getZ()));
}
- add_wing(ceti);
+ ceti->reset();
}
+ std::pair<tf2::Transform, int> ceti1(robot->tf(), wings);
+ dict.push_back(std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>(ceti1, second_robot_data));
ceti->reset();
}
+void Mediator::write_file(std::vector<std::pair<std::pair<tf2::Transform, int>, std::vector<std::pair<tf2::Transform, int>>>>& dict){
+ /*
+
+ std::ofstream o(ros::package::getPath("mtc") + "/results/dummy.yaml");
+ YAML::Node node;
+
+
+
+
+ for(tf2::Vector3& vec : robot_1){
+ node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
+ }
+
+ ROS_INFO("writing items for robot2...");
+ for(tf2::Vector3& vec : robot_2){
+ node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));
+ }
+
+ ROS_INFO("writing intersection...");
+ for(tf2::Vector3& vec : robot_middle){
+ node["task"]["groups"]["robot_x"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
+ node["task"]["groups"]["robot_y"].push_back(std::to_string((float)vec.getX())+ " " + std::to_string((float) vec.getY()) + " " + std::to_string((float)vec.getZ()));;
+ }
+ o << node;
+ */
+}
+
+void Mediator::build_wings(std::bitset<3>& wing, Robot* robot){
+
+ std::bitset<3> result = robot->observer_mask() & wing;
+ for (int i = 0; i < result.size(); i++){
+ ROS_INFO( "%i", i);
+ if (result[i]){
+ Abstract_robot_element* moveit_right = new Wing_rviz_decorator(new Wing(wings_[i].pos_, wings_[i].size_), robot->rviz_markers()[0]);
+ robot->register_observers(moveit_right);
+ }
+ }
+}
diff --git a/src/mtc/src/impl/moveit_mediator.cpp b/src/mtc/src/impl/moveit_mediator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..5e344327d44739c22b40a54d927c474771ffcc80
--- /dev/null
+++ b/src/mtc/src/impl/moveit_mediator.cpp
@@ -0,0 +1,43 @@
+#include "impl/moveit_mediator.h"
+#include "impl/wing_moveit_decorator.h"
+
+
+void Moveit_mediator::publish_tables(){
+ ros::WallDuration sleep_time(1.0);
+
+ moveit_msgs::PlanningScene planning_scene;
+ planning_scene.is_diff = true;
+
+ for(int i = 0; i < robots_.size();i++){
+ Moveit_robot* ceti = dynamic_cast<Moveit_robot*>(robots_[i]);
+ for (moveit_msgs::CollisionObject* markers : ceti->coll_markers()) {
+ planning_scene.world.collision_objects.push_back(*markers);
+ sleep_time.sleep();
+ }
+ }
+
+
+ planning_scene_diff_publisher_.publish(planning_scene);
+
+}
+
+void Moveit_mediator::load_robot_description(){
+ robot_model_loader_ = new robot_model_loader::RobotModelLoader("robot_description");
+ kinematic_model_ = robot_model_loader_->getModel();
+ ps_ = new planning_scene::PlanningScene(kinematic_model_);
+}
+
+bool Moveit_mediator::check_collision(const int& robot){return true;};
+void Moveit_mediator::mediate(){};
+
+void Moveit_mediator::build_wings(std::bitset<3>& wing, Robot* robot){
+ std::bitset<3> result = robot->observer_mask() & wing;
+ for (int i = 0; i < result.size(); i++){
+ if (result[i]){
+ moveit_msgs::CollisionObject* marker = new moveit_msgs::CollisionObject();
+ robot->add_coll_markers(marker);
+ Abstract_robot_element* moveit_right = new Wing_moveit_decorator(new Wing(wings_[i].pos_, wings_[i].size_), marker);
+ robot->register_observers(moveit_right);
+ }
+ }
+}
\ No newline at end of file
diff --git a/src/mtc/src/impl/robot.cpp b/src/mtc/src/impl/robot.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..409862028cbb68ddc6bd5493028f237526a070f3
--- /dev/null
+++ b/src/mtc/src/impl/robot.cpp
@@ -0,0 +1,129 @@
+#include "impl/robot.h"
+
+void Robot::generate_access_fields(){
+ for (int i = 0; i <= 2; i++){
+ for (int j = 0; j <= 2; j++){
+ if(i == 0 && j == 0) {continue;}
+ if(i == 2 && j == 2) {continue;}
+ if(i == 0) {
+
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0.8f*j,0))));
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,-0.8f*j,0))));
+ } else if (j == 0){
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,0,0))));
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,0,0))));
+ } else {
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,0.8f*j,0))));
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,0.8f*j,0))));
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.8f*i,-0.8f*j,0))));
+ access_fields_.push_back(new Field(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.8f*i,-0.8f*j,0))));
+ }
+
+ };
+ };
+}
+
+float Robot::area_calculation(tf2::Transform& A, tf2::Transform& B, tf2::Transform& C){
+ return std::abs(
+ (B.getOrigin().getX() * A.getOrigin().getY()) - (A.getOrigin().getX() * B.getOrigin().getY()) +
+ (C.getOrigin().getX() * B.getOrigin().getY()) - (B.getOrigin().getX() * C.getOrigin().getY()) +
+ (A.getOrigin().getX() * C.getOrigin().getY()) - (C.getOrigin().getX() * A.getOrigin().getY()))*0.5f;
+}
+
+bool Robot::check_single_object_collision(tf2::Transform& obj){
+
+ tf2::Transform A = tf_ * root_tf_ * robot_root_bounds_[0];
+ tf2::Transform B = tf_ * root_tf_ * robot_root_bounds_[1];
+ tf2::Transform C = tf_ * root_tf_ * robot_root_bounds_[2];
+ tf2::Transform D = tf_ * root_tf_ * robot_root_bounds_[3];
+
+ float full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+ float sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+ if ((std::floor(sum*100)/100.f) <= full_area) {return false; }
+
+
+ for (int i = 0; i < bounds_.size(); i++){
+ tf2::Transform A = tf_ * bounds_[0];
+ tf2::Transform B = tf_ * bounds_[1];
+ tf2::Transform C = tf_ * bounds_[2];
+ tf2::Transform D = tf_ * bounds_[3];
+
+ full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+ sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+
+
+ if ((std::floor(sum*100)/100.f) <= full_area) return true;
+ }
+
+ if (!observers_.empty()){
+ std::vector<Abstract_robot_element*>::const_iterator it = observers_.begin();
+ while (it != observers_.end()) {
+ Abstract_robot_element_decorator *deco = dynamic_cast<Abstract_robot_element_decorator*>(*it);
+ Wing* ceti = dynamic_cast<Wing*>(deco->wing());
+ tf2::Transform A = ceti->world_tf() * ceti->bounds()[0];
+ tf2::Transform B = ceti->world_tf() * ceti->bounds()[1];
+ tf2::Transform C = ceti->world_tf() * ceti->bounds()[2];
+ tf2::Transform D = ceti->world_tf() * ceti->bounds()[3];
+
+ full_area = area_calculation(A,B,C) + area_calculation(A,D,C);
+ sum = area_calculation(A,obj,D) + area_calculation(D,obj,C) + area_calculation(C,obj,B) + area_calculation(obj, B, A);
+
+ if ((std::floor(sum*100)/100.f) <= full_area) {
+ return true;
+ } else {
+ ++it;
+ }
+ }
+ }
+ return false;
+}
+
+void Robot::reset(){
+ observers_.clear();
+ access_fields_.clear();
+
+ generate_access_fields();
+ tf_ = tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0,0,0.4425f));
+}
+
+void Robot::notify(){
+ for(Abstract_robot_element* wing : observers_) wing->update(tf_); for(Abstract_robot_element* field : access_fields_) field->update(tf_);
+}
+
+
+void Robot::register_observers(Abstract_robot_element* wd){
+ Abstract_robot_element_decorator* decorator = dynamic_cast<Abstract_robot_element_decorator*>(wd);
+ observers_.push_back(wd);
+ std::vector<tf2::Transform> plane;
+ bool found = false;
+ int index = 0;
+
+ if (decorator->wing()->relative_tf().getOrigin().getY()>0){
+ for(int i = 0; i < access_fields_.size(); i++){
+ if ((access_fields_[i]->relative_tf().getOrigin().getY() > 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
+ found = true;
+ index = i;
+ break;
+ }
+ }
+ if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
+ } else if (decorator->wing()->relative_tf().getOrigin().getY()<0) {
+ for(int i = 0; i < access_fields_.size(); i++){
+ if ((access_fields_[i]->relative_tf().getOrigin().getY() < 0) && (access_fields_[i]->relative_tf().getOrigin().getX() == 0)){
+ found = true;
+ index = i;
+ break;
+ }
+ }
+ if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
+ } else {
+ for(int i = 0; i < access_fields_.size(); i++){
+ if (access_fields_[i]->relative_tf().getOrigin().getX() > 0){
+ found = true;
+ index = i;
+ break;
+ }
+ }
+ if(found) {access_fields_.erase(access_fields_.begin() + index); return;}
+ }
+};
diff --git a/src/mtc/src/impl/wing_moveit_decorator.cpp b/src/mtc/src/impl/wing_moveit_decorator.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6943b43df32ac3ff4ac9053742f4ed3edac9df0a
--- /dev/null
+++ b/src/mtc/src/impl/wing_moveit_decorator.cpp
@@ -0,0 +1,37 @@
+#include "impl/wing_moveit_decorator.h"
+
+void Wing_moveit_decorator::update(tf2::Transform& tf) {
+ Abstract_robot_element_decorator::update(tf);
+ output_filter();
+}
+
+void Wing_moveit_decorator::input_filter(tf2::Transform& tf) {}
+
+void Wing_moveit_decorator::output_filter() {
+ Wing* wing = dynamic_cast<Wing*> (next_);
+ tf2::Vector3 world_origin = wing->world_tf().getOrigin();
+ tf2::Quaternion world_quat = wing->world_tf().getRotation().normalized();
+
+
+ markers_->id = std::to_string(*((int*)(&next_)));
+ markers_->header.frame_id = "world";
+
+ markers_->primitives.resize(1);
+ markers_->primitives[0].type = markers_->primitives[0].BOX;
+ markers_->primitives[0].dimensions.resize(3);
+ markers_->primitives[0].dimensions[0] = wing->size().getX();
+ markers_->primitives[0].dimensions[1] = wing->size().getY();
+ markers_->primitives[0].dimensions[2] = wing->size().getZ();
+
+
+ markers_->primitive_poses.resize(1);
+ markers_->primitive_poses[0].position.x = world_origin.getX();
+ markers_->primitive_poses[0].position.y = world_origin.getY();
+ markers_->primitive_poses[0].position.z = world_origin.getZ();
+ markers_->primitive_poses[0].orientation.x = world_quat.getX();
+ markers_->primitive_poses[0].orientation.y = world_quat.getY();
+ markers_->primitive_poses[0].orientation.z = world_quat.getZ();
+ markers_->primitive_poses[0].orientation.w = world_quat.getW();
+
+ markers_->operation = markers_->ADD;
+}
\ No newline at end of file