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