diff --git a/.catkin_tools/profiles/default/devel_collisions.txt b/.catkin_tools/profiles/default/devel_collisions.txt
index b7bf02b8284aa2b2cb28399b9d43e30082b16dbc..cef6793205b81f7a69dd392904303a17fbbe18bf 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 8726
+/home/matteo/reachability/devel/./cmake.lock 8732
/home/matteo/reachability/devel/lib/libmoveit_grasps.so 66
/home/matteo/reachability/devel/lib/libmoveit_grasps_filter.so 66
diff --git a/src/mtc/include/impl/abstract_mediator.h b/src/mtc/include/impl/abstract_mediator.h
index 759969f76b5feb081f8d0283668d364a94eacd09..2b84eda571bf7794e26bc3c267c25d494f3e422d 100644
--- a/src/mtc/include/impl/abstract_mediator.h
+++ b/src/mtc/include/impl/abstract_mediator.h
@@ -3,16 +3,57 @@
#include "ros/ros.h"
#include "impl/abstract_robot.h"
+#include "impl/abstract_robot_element.h"
+#include "impl/robot.h"
class Abstract_mediator{
protected:
std::vector<Abstract_robot*> robots_;
std::vector<std::vector<tf2::Vector3>> objects_;
+ std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_;
+ ros::Publisher* pub_;
+
public:
- Abstract_mediator(std::vector<std::vector<tf2::Vector3>> objects) : objects_(objects){};
- virtual void connect_robots(Abstract_robot* robot) {robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());};
+ Abstract_mediator(std::vector<std::vector<tf2::Vector3>> objects, ros::Publisher* pub) : objects_(objects), pub_(pub){}
+ inline void connect_robots(Abstract_robot* robot) {
+ robots_.push_back(robot); ROS_INFO("%s connected...", robot->name().c_str());
+ Robot* ceti = dynamic_cast<Robot*>(robot);
+ std::vector<tf2::Transform> plane;
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.4f, 0.4f, ceti->tf().getOrigin().getZ())));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, 0.4f, ceti->tf().getOrigin().getZ())));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(0.4f, -0.4f, ceti->tf().getOrigin().getZ())));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(-0.4f, -0.4f, ceti->tf().getOrigin().getZ())));
+ std::vector<std::vector<tf2::Transform>> planes;
+
+ for(Abstract_robot_element* wing : ceti->observers()){
+ plane.clear();
+ if (wing->relative_tf().getOrigin().getY() > 0){
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(right.getX()/2.f, right.getY()/2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(right.getX()/2.f, right.getY()/-2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(right.getX()/-2.f, right.getY()/2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(right.getX()/-2.f, right.getY()/-2.f,0)));
+ } else
+ if (wing->relative_tf().getOrigin().getY() < 0){
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(left.getX()/2.f, left.getY()/2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(left.getX()/2.f, left.getY()/-2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(left.getX()/-2.f, left.getY()/2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(left.getX()/-2.f, left.getY()/-2.f,0)));
+ } else
+ if (wing->relative_tf().getOrigin().getY() == 0){
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(mid.getX()/2.f, mid.getY()/2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(mid.getX()/2.f, mid.getY()/-2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(mid.getX()/-2.f, mid.getY()/2.f,0)));
+ plane.push_back(tf2::Transform(tf2::Quaternion(0,0,0,1), tf2::Vector3(mid.getX()/-2.f, mid.getY()/-2.f,0)));
+ }
+ planes.push_back(plane);
+ }
+ relative_bounds_.push_back(planes);
+ }
+
+ virtual void mediate()=0;
+
};
diff --git a/src/mtc/include/impl/abstract_robot.h b/src/mtc/include/impl/abstract_robot.h
index 350814df4c83fda5d846417470f7d0200899213b..059745c3abc2a5ac9c56082ed8a31bc22b6a4775 100644
--- a/src/mtc/include/impl/abstract_robot.h
+++ b/src/mtc/include/impl/abstract_robot.h
@@ -7,6 +7,9 @@
#include "impl/abstract_strategy.h"
#include "impl/abstract_robot_element.h"
+#define right tf2::Vector3(0.7f, 0.5f, 0.01f)
+#define left tf2::Vector3(0.7f, 0.5f, 0.01f)
+#define mid tf2::Vector3(0.5f, 0.7f, 0.01f)
class Abstract_robot {
protected:
diff --git a/src/mtc/include/impl/mediator.h b/src/mtc/include/impl/mediator.h
index 73e1896824e6137200ba9359cec40076b719a228..7548dca02346db2e04a4eb4410b917e6d8228044 100644
--- a/src/mtc/include/impl/mediator.h
+++ b/src/mtc/include/impl/mediator.h
@@ -3,10 +3,25 @@
#include "ros/ros.h"
#include "impl/abstract_mediator.h"
+#include "impl/abstract_robot.h"
+#include "impl/robot.h"
class Mediator : public Abstract_mediator{
public:
- Mediator(std::vector<std::vector<tf2::Vector3>> objects) : Abstract_mediator(objects){};
+ Mediator(std::vector<std::vector<tf2::Vector3>> objects, ros::Publisher* pub) : Abstract_mediator(objects, pub){};
+ void mediate() override {
+ ros::Rate loop_rate(1);
+ for (Abstract_robot* robot : robots_){
+ Robot* ceti = dynamic_cast<Robot*>(robot);
+ while (ros::ok()){
+ for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers()) markers->markers.clear();
+ ceti->rotate(-M_PI/4);
+ ceti->notify();
+ for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub_->publish(*markers);
+ loop_rate.sleep();
+ }
+ }
+ }
};
diff --git a/src/mtc/src/base_routine.cpp b/src/mtc/src/base_routine.cpp
index 814cc751b5d358374c8f1e67a7e0d5e7df75c7f6..9bf48965bd09aac3a0cab5eee13e05f466c67f52 100644
--- a/src/mtc/src/base_routine.cpp
+++ b/src/mtc/src/base_routine.cpp
@@ -19,9 +19,6 @@
#include "impl/wing_rviz_decorator.cpp"
#include "impl/field_rviz_decorator.cpp"
-#define right tf2::Vector3(0.7f, 0.5f, 0.01f)
-#define left tf2::Vector3(0.7f, 0.5f, 0.01f)
-#define mid tf2::Vector3(0.5f, 0.7f, 0.01f)
@@ -99,20 +96,9 @@ int main(int argc, char **argv){
map_loader->base_calculation();
*/
- Abstract_mediator* mediator = new Mediator(map_loader->task_grasps());
+ Abstract_mediator* mediator = new Mediator(map_loader->task_grasps(), new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1)));
mediator->connect_robots(robo);
-
- ros::Rate loop_rate(1);
- ros::Publisher* pub = new ros::Publisher(n.advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1));
-
- while (ros::ok()){
- fields->markers.clear();
- wings->markers.clear();
- ceti->rotate(-M_PI/4);
- ceti->notify();
- for (visualization_msgs::MarkerArray* markers : ceti->rviz_markers())pub->publish(*markers);
- loop_rate.sleep();
- }
+ mediator->mediate();
free(rviz_right);
free(rviz_mid);
@@ -121,7 +107,6 @@ int main(int argc, char **argv){
// free(map_loader);
// free(strategy);
- free(pub);
free(robo);
free(right_wing);
free(left_wing);