From 876119a26858eec91f5ff95fe6759935ee241119 Mon Sep 17 00:00:00 2001
From: KingMaZito <matteo.aneddama@icloud.com>
Date: Thu, 9 Feb 2023 17:05:35 +0100
Subject: [PATCH] some changes

---
 include/mediator/abstract_mediator.h       |  4 +-
 src/mediator/base_calculation_mediator.cpp | 55 ++++++++++++----------
 src/reader/ts_reader.cpp                   | 15 +++---
 src/robot/ceti_robot.cpp                   |  3 +-
 4 files changed, 39 insertions(+), 38 deletions(-)

diff --git a/include/mediator/abstract_mediator.h b/include/mediator/abstract_mediator.h
index d2ac279..be6ad78 100644
--- a/include/mediator/abstract_mediator.h
+++ b/include/mediator/abstract_mediator.h
@@ -34,7 +34,7 @@ class Abstract_mediator {
     protected:
     std::shared_ptr<ros::NodeHandle> nh_; //!< Ros nodehandle object
     std::unique_ptr<Ts_reader> task_space_reader_; //!< Task_space reader which provides drop off positions
-    std::vector<std::unique_ptr<Abstract_robot_decorator>> robots_; //!< Robots agents
+    std::map<std::string, std::unique_ptr<Abstract_robot_decorator>> robots_; //!< Robots agents
     std::vector<std::vector<std::vector<tf2::Transform>>> relative_bounds_; //!< total bound a  workspace
 
     std::map<const std::string, std::vector<pcl::PointXYZ>> result_vector_; //!< Result_vector of base positions linked to robot
@@ -71,7 +71,7 @@ class Abstract_mediator {
     /*!
         \return robots as decorators
     */
-    inline std::vector<std::unique_ptr<Abstract_robot_decorator>>& robots(){return robots_;}
+    inline std::map<std::string, std::unique_ptr<Abstract_robot_decorator>>& robots(){return robots_;}
 
     //! Set dirname
     /*!
diff --git a/src/mediator/base_calculation_mediator.cpp b/src/mediator/base_calculation_mediator.cpp
index 4162b3c..6296764 100644
--- a/src/mediator/base_calculation_mediator.cpp
+++ b/src/mediator/base_calculation_mediator.cpp
@@ -7,36 +7,36 @@ Base_calculation_mediator::Base_calculation_mediator(std::shared_ptr<ros::NodeHa
     , pub_(std::make_unique<ros::Publisher>(nh->advertise< visualization_msgs::MarkerArray >("visualization_marker_array", 1))){};
 
 void Base_calculation_mediator::generate_grounds(const tf2::Vector3 origin, const float diameter, float resolution){
-  for(int i = 0; i < robots_.size();i++){
+  for(const auto& s_r : robots_){
     std::vector<pcl::PointXYZ> ground_plane; 
     for (float x = origin.getX() - diameter * 1.5; x <= origin.getX() + diameter * 1.5; x += resolution){
         for (float y = origin.getY() - diameter * 1.5; y <= origin.getY() + diameter * 1.5; y += resolution){
-        ground_plane.push_back(pcl::PointXYZ(x,y, robots_[i]->tf().getOrigin().getZ() - 0.0025f));
+        ground_plane.push_back(pcl::PointXYZ(x,y, s_r.second->tf().getOrigin().getZ() - 0.0025f));
         }
     }
-    grounds_.insert(std::pair<const std::string, std::vector<pcl::PointXYZ>>(robots_[i]->name(), ground_plane));
+    grounds_.insert(std::pair<const std::string, std::vector<pcl::PointXYZ>>(s_r.first, ground_plane));
   }
 }
 
 void Base_calculation_mediator::connect_robots(std::unique_ptr<Abstract_robot_decorator> robot) {
-    robots_.push_back(std::move(robot)); 
-    ROS_INFO("%s connected...", robots_.back()->name().c_str());
+    ROS_INFO("connecting %s...", robot->name().c_str());
+    robots_.insert_or_assign(robot->name(), std::move(robot)); 
 }
 
 void Base_calculation_mediator::set_panel(){
     auto wd = wing_reader_->wing_data();
 
-    for (int i = 0; i < robots_.size(); i++){
+    for (const auto& s_r : robots_){
         try{
-            auto ceti_bot = dynamic_cast<Ceti_robot*>(robots_[i]->next());
-            ceti_bot->set_observer_mask(static_cast<wing_config>(wd.at(robots_[i]->name()).second));
+            auto ceti_bot = dynamic_cast<Ceti_robot*>(s_r.second->next());
+            ceti_bot->set_observer_mask(static_cast<wing_config>(wd.at(s_r.first).second));
         } catch(const std::out_of_range& oor) {
-            ROS_INFO("No mask data for %s", robots_[i]->name().c_str());
+            ROS_INFO("No mask data for %s", s_r.first.c_str());
         }
     }
     
-    for (int i = 0; i < robots_.size(); i++){
-        auto ceti_bot = dynamic_cast<Ceti_robot*>(robots_[i]->next());
+    for (const auto& s_r : robots_){
+        auto ceti_bot = dynamic_cast<Ceti_robot*>(s_r.second->next());
         std::vector<std::unique_ptr<Abstract_robot_element_decorator>> panels(3);
             
         for (std::size_t j = 0; j < ceti_bot->observer_mask().size(); j++){
@@ -44,7 +44,7 @@ void Base_calculation_mediator::set_panel(){
                 try{
                     tf2::Transform relative_tf = ceti_bot->tf().inverse() * wd.at(ceti_bot->name()).first[j].pose_;
 
-                    std::unique_ptr<Abstract_robot_element> panel = std::make_unique<Rviz_panel>(wd.at(robots_[i]->name()).first[j].name_, relative_tf, wd.at(robots_[i]->name()).first[j].size_);
+                    std::unique_ptr<Abstract_robot_element> panel = std::make_unique<Rviz_panel>(wd.at(s_r.first).first[j].name_, relative_tf, wd.at(s_r.first).first[j].size_);
                     std::unique_ptr<Abstract_robot_element_decorator> log = std::make_unique<Log_decorator>(std::move(panel));
                     panels[j] = std::move(log);
                 } catch (std::out_of_range &oor){
@@ -63,14 +63,12 @@ bool Base_calculation_mediator::check_collision(const std::string& robot, std::b
     std::stringstream ns;
     ns << "Targets " << robot;
 
-    // Get Robot reference by name (which is ugly)
+    // Get Robot reference by name 
     Ceti_robot* ceti1;
-    for(int i =0; i < robots_.size();i++){
-        if(robots_[i]->name() == robot) {
-            ceti1 = dynamic_cast<Ceti_robot*>(robots_[i]->next());
-        }
-    }
-    
+    try{
+        ceti1 = dynamic_cast<Ceti_robot*>(robots_.at(robot)->next());
+    } catch (std::out_of_range& oor){}
+   
     // prepare map for workload check
     std::map<std::string, int> workload_map = {{"base_1", 0}};
     for (std::size_t i = 0; i < panel_mask.size(); i++){
@@ -101,7 +99,9 @@ bool Base_calculation_mediator::check_collision(const std::string& robot, std::b
             marker.pose.orientation.w = ts.at(robot)[i].pose_.getRotation().getW();
             marker.scale.x = ts.at(robot)[i].size_.getX();
             marker.scale.y = ts.at(robot)[i].size_.getY();
-            marker.scale.z = ts.at(robot)[i].size_.getZ();
+            marker.scale.z = 0.2f;
+
+            ROS_INFO("%f, %f, %f", ts.at(robot)[i].pose_.getOrigin().getX(), ts.at(robot)[i].pose_.getOrigin().getY(), ts.at(robot)[i].pose_.getOrigin().getZ());
             if(ceti1->check_single_object_collision(ts.at(robot)[i].pose_, str, panel_mask)){
                 try {
                     workload_map.at(str)++;
@@ -127,6 +127,7 @@ bool Base_calculation_mediator::check_collision(const std::string& robot, std::b
     }
     pub_->publish(ma);
 
+
     workload = true; 
     for (auto& s_i: workload_map) {
         if (s_i.second == 0) {
@@ -219,12 +220,10 @@ void Base_calculation_mediator::calculate(std::map<const std::string, std::vecto
     visualization_msgs::MarkerArray ma;
     for (auto& s_pos : workcell){
         Ceti_robot* ceti1;
-        for(int i =0; i < robots_.size();i++){
-            if(robots_[i]->name() == s_pos.first) {
-                ceti1 = dynamic_cast<Ceti_robot*>(robots_[i]->next());
-            }
-        }
-
+        try{
+            ceti1 = dynamic_cast<Ceti_robot*>(robots_.at(s_pos.first)->next());
+        } catch(std::out_of_range& oor){}
+       
         visualization_msgs::Marker m;
         m.header.frame_id = "map";
         m.header.stamp = ros::Time();
@@ -301,6 +300,7 @@ void Base_calculation_mediator::calculate(std::map<const std::string, std::vecto
 }
 
 void Base_calculation_mediator::approximation(Ceti_robot* robot){
+    /*
     auto ceti1 = dynamic_cast<Ceti_robot*>(robots_[1].get());
     int r1 = 1;
 
@@ -373,6 +373,7 @@ void Base_calculation_mediator::approximation(Ceti_robot* robot){
         pub_->publish(ma);
         ma.markers.clear();
     }
+    */
 }
 
 
@@ -500,6 +501,7 @@ void Base_calculation_mediator::write_file(Ceti_robot* A, Ceti_robot* B){
 }
 
 void Base_calculation_mediator::build_wings(std::bitset<3>& wing, int& robot){
+    /*
     auto ceti = dynamic_cast<Ceti_robot*>(robots_[robot].get());
     std::bitset<3> result = ceti->observer_mask() & wing;
     
@@ -516,6 +518,7 @@ void Base_calculation_mediator::build_wings(std::bitset<3>& wing, int& robot){
         ma.markers.push_back(wrd->marker());
     }
     pub_->publish(ma);
+    */
 }
 
 void Base_calculation_mediator::publish(Ceti_robot* ar){
diff --git a/src/reader/ts_reader.cpp b/src/reader/ts_reader.cpp
index a827566..7a7c8f6 100644
--- a/src/reader/ts_reader.cpp
+++ b/src/reader/ts_reader.cpp
@@ -1,9 +1,6 @@
 #include "reader/ts_reader.h"
 
 void Ts_reader::read(){
-    std::string filename = "dummy.yaml";
-    //nh_->getParam("/resource_name", filename);
-
     XmlRpc::XmlRpcValue resources;
     nh_->getParam("/objects", resources);
 
@@ -48,16 +45,16 @@ void Ts_reader::read(){
           (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"])) :pos.setZ(0);
 
-          (resources[i]["orientation"].hasMember("x")) ? pos.setZ(float_of(resources[i]["orientation"]["x"])) :pos.setX(0);
-          (resources[i]["orientation"].hasMember("y")) ? pos.setZ(float_of(resources[i]["orientation"]["y"])) :pos.setY(0);
-          (resources[i]["orientation"].hasMember("z")) ? pos.setZ(float_of(resources[i]["orientation"]["z"])) :pos.setZ(0);
-          (resources[i]["orientation"].hasMember("w")) ? pos.setZ(float_of(resources[i]["orientation"]["w"])) :pos.setW(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);
+          
           robot_pointer->push_back({"", tf2::Transform(rot, pos), size});
         }
       }
diff --git a/src/robot/ceti_robot.cpp b/src/robot/ceti_robot.cpp
index 25231c5..07550c5 100644
--- a/src/robot/ceti_robot.cpp
+++ b/src/robot/ceti_robot.cpp
@@ -68,8 +68,9 @@ bool Ceti_robot::check_single_object_collision(tf2::Transform& obj, std::string&
     }
 
     // 3. panel collsision check
-    for(std::size_t i = 0; i < panel_mask.size(); i++){
+    for(std::size_t i = 0; i < observers_.size(); i++){
         if(observer_mask_[i] & panel_mask[i]){
+            ROS_INFO("%i und %i", observer_mask_[i], panel_mask[i]);
             tf2::Transform A = observers_[i]->world_tf() * observers_[i]->bounds()[0];
             tf2::Transform B = observers_[i]->world_tf() * observers_[i]->bounds()[1];
             tf2::Transform C = observers_[i]->world_tf() * observers_[i]->bounds()[2];
-- 
GitLab