From bff98bfefdba363368a821506041b23d014d24c8 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 26 Jun 2023 16:54:12 +0200 Subject: [PATCH 01/37] [ReasonerEgocentric] preReason with boolean return --- .../OntologeniusPlugins/ReasonerEgocentric.h | 2 +- src/OntologeniusPlugins/ReasonerEgocentric.cpp | 12 +++++++----- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h b/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h index ef6eea43..e7ebb515 100644 --- a/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h +++ b/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h @@ -15,7 +15,7 @@ class ReasonerEgocentric : public ReasonerInterface virtual void initialize() override; - virtual void preReason(const QueryInfo_t& query_info) override; + virtual bool preReason(const QueryInfo_t& query_info) override; virtual bool implementPreReasoning() override { return true; } diff --git a/src/OntologeniusPlugins/ReasonerEgocentric.cpp b/src/OntologeniusPlugins/ReasonerEgocentric.cpp index 91899841..81341386 100644 --- a/src/OntologeniusPlugins/ReasonerEgocentric.cpp +++ b/src/OntologeniusPlugins/ReasonerEgocentric.cpp @@ -21,7 +21,7 @@ void ReasonerEgocentric::initialize() } } -void ReasonerEgocentric::preReason(const QueryInfo_t& query_info) +bool ReasonerEgocentric::preReason(const QueryInfo_t& query_info) { if((query_info.query_origin == query_origin_individual) && (query_info.query_type == query_relation)) @@ -32,14 +32,14 @@ void ReasonerEgocentric::preReason(const QueryInfo_t& query_info) { property_ptr = isComputableProperty(query_info.predicate); if(property_ptr == nullptr) - return; + return false; } if(query_info.subject != "") { std::set valid_properties = isInDomain(query_info.subject, property_ptr); if(valid_properties.size() == 0) - return; + return false; to_compute_properties = valid_properties; } @@ -48,7 +48,7 @@ void ReasonerEgocentric::preReason(const QueryInfo_t& query_info) { std::set valid_properties = isInRange(query_info.object, property_ptr); if(valid_properties.size() == 0) - return; + return false; if(to_compute_properties.size() == 0) to_compute_properties = valid_properties; @@ -62,7 +62,7 @@ void ReasonerEgocentric::preReason(const QueryInfo_t& query_info) } if(to_compute_properties.size() == 0) - return; + return false; } if(to_compute_properties.size() == 0) @@ -76,6 +76,8 @@ void ReasonerEgocentric::preReason(const QueryInfo_t& query_info) if(call(srv)) updateOntology(srv.response.to_add, srv.response.to_delete); } + + return true; } std::string ReasonerEgocentric::getName() From 6313ac5e77bbe7d2eb563d8cedad63d813dd34ae Mon Sep 17 00:00:00 2001 From: Adrien Vigne Date: Tue, 27 Jun 2023 16:46:45 +0200 Subject: [PATCH 02/37] [SituationAssessor] add publisher on new assessor --- include/overworld/SituationAssessor.h | 1 + src/SituationAssessor.cpp | 14 ++++++++++++++ 2 files changed, 15 insertions(+) diff --git a/include/overworld/SituationAssessor.h b/include/overworld/SituationAssessor.h index 6fc37e44..210893b0 100644 --- a/include/overworld/SituationAssessor.h +++ b/include/overworld/SituationAssessor.h @@ -74,6 +74,7 @@ class SituationAssessor ros::ServiceServer start_modules_service_; ros::ServiceServer stop_modules_service_; ros::ServiceServer bounding_box_service_; + ros::Publisher new_assessor_publisher_; std::atomic run_; double time_step_; // in second double simu_step_; diff --git a/src/SituationAssessor.cpp b/src/SituationAssessor.cpp index 5403fb03..de21c70d 100644 --- a/src/SituationAssessor.cpp +++ b/src/SituationAssessor.cpp @@ -29,6 +29,11 @@ SituationAssessor::SituationAssessor(const std::string& agent_name, { n_.setCallbackQueue(&callback_queue_); + if(is_robot_) + { + new_assessor_publisher_ = n_.advertise("/overworld/new_assessor",5); + } + if (is_robot_) { bullet_client_ = PhysicsServers::connectPhysicsServer(owds::CONNECT_GUI); @@ -87,6 +92,12 @@ SituationAssessor::SituationAssessor(const std::string& agent_name, start_modules_service_ = n_.advertiseService(agent_name_ + "/startPerceptionModules", &SituationAssessor::startModules, this); stop_modules_service_ = n_.advertiseService(agent_name_ + "/stopPerceptionModules", &SituationAssessor::stopModules, this); bounding_box_service_ = n_.advertiseService(agent_name_ + "/getBoundingBox", &SituationAssessor::getBoundingBox, this); + if(is_robot_) + { + auto msg = std_msgs::String(); + msg.data = "ADD|"+agent_name_; + new_assessor_publisher_.publish(msg); + } } SituationAssessor::~SituationAssessor() @@ -323,6 +334,9 @@ std::map::iterator SituationAssessor::createHumanA assessor->second.assessor->addAreaPerceptionModule("emulated_areas", assessor->second.areas_module); std::thread th(&SituationAssessor::run, assessor->second.assessor); assessor->second.thread = std::move(th); + auto msg = std_msgs::String(); + msg.data = "ADD|"+human_name; + new_assessor_publisher_.publish(msg); return assessor; } From 1a294af7c5f605684323329d398e94d5253a6a17 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 24 Jul 2023 23:12:59 +0200 Subject: [PATCH 03/37] [ontologenius] make compatible with version 0.3.0 --- .../Facts/Publisher/OntologeniusFactsPublisher.h | 4 ++-- .../Perception/Managers/EntitiesPerceptionManager.h | 6 +++--- .../Modules/HumansModules/FakeHumanPerceptionModule.h | 4 ++-- .../HumansModules/StampedPosePerceptionModule.h | 4 ++-- .../ObjectsModules/FakeObjectPerceptionModule.h | 4 ++-- .../ObjectsModules/StaticObjectsPerceptionModule.h | 4 ++-- .../Modules/RobotsModules/JointStatePerceptionModule.h | 4 ++-- include/overworld/Utility/Ontology.h | 10 +++++----- .../HumansModules/FakeHumanPerceptionModule.cpp | 2 +- .../HumansModules/StampedPosePerceptionModule.cpp | 2 +- .../ObjectsModules/FakeObjectPerceptionModule.cpp | 2 +- .../ObjectsModules/StaticObjectsPerceptionModule.cpp | 2 +- .../RobotsModules/JointStatePerceptionModule.cpp | 2 +- src/Utility/Ontology.cpp | 10 +++++----- 14 files changed, 30 insertions(+), 30 deletions(-) diff --git a/include/overworld/Facts/Publisher/OntologeniusFactsPublisher.h b/include/overworld/Facts/Publisher/OntologeniusFactsPublisher.h index b25688ba..5ab89f13 100644 --- a/include/overworld/Facts/Publisher/OntologeniusFactsPublisher.h +++ b/include/overworld/Facts/Publisher/OntologeniusFactsPublisher.h @@ -14,8 +14,8 @@ class OntologeniusFactsPublisher : public FactsPublisher private: std::string agent_name_; - OntologiesManipulator ontologies_manipulator_; - OntologyManipulator* onto_; + onto::OntologiesManipulator ontologies_manipulator_; + onto::OntologyManipulator* onto_; void addToKb(const Fact& fact) override; void removeFromKb(const Fact& fact) override; diff --git a/include/overworld/Perception/Managers/EntitiesPerceptionManager.h b/include/overworld/Perception/Managers/EntitiesPerceptionManager.h index ff556d7d..e793c8b8 100644 --- a/include/overworld/Perception/Managers/EntitiesPerceptionManager.h +++ b/include/overworld/Perception/Managers/EntitiesPerceptionManager.h @@ -23,7 +23,7 @@ class EntitiesPerceptionManager : public BasePerceptionManager static_assert(std::is_base_of::value, "T must be derived from Entity"); public: explicit EntitiesPerceptionManager(ros::NodeHandle* nh): bullet_client_(nullptr), - ontos_(OntologiesManipulator(nh)), + ontos_(onto::OntologiesManipulator(nh)), onto_(nullptr) {} virtual ~EntitiesPerceptionManager(); @@ -42,8 +42,8 @@ class EntitiesPerceptionManager : public BasePerceptionManager BulletClient* bullet_client_; std::string myself_agent_name_; - OntologiesManipulator ontos_; - OntologyManipulator* onto_; + onto::OntologiesManipulator ontos_; + onto::OntologyManipulator* onto_; virtual void getPercepts( std::map& percepts); virtual void reasoningOnUpdate() {} diff --git a/include/overworld/Perception/Modules/HumansModules/FakeHumanPerceptionModule.h b/include/overworld/Perception/Modules/HumansModules/FakeHumanPerceptionModule.h index 8825f44e..3bbfbfa6 100644 --- a/include/overworld/Perception/Modules/HumansModules/FakeHumanPerceptionModule.h +++ b/include/overworld/Perception/Modules/HumansModules/FakeHumanPerceptionModule.h @@ -26,8 +26,8 @@ class FakeHumanPerceptionModule : public PerceptionModuleRosBase virtual bool closeInitialization() override; private: - OntologiesManipulator* ontologies_manipulator_; - OntologyManipulator* onto_; + onto::OntologiesManipulator* ontologies_manipulator_; + onto::OntologyManipulator* onto_; std::string config_file_; diff --git a/include/overworld/Perception/Modules/RobotsModules/JointStatePerceptionModule.h b/include/overworld/Perception/Modules/RobotsModules/JointStatePerceptionModule.h index 61176f3e..0a685a04 100644 --- a/include/overworld/Perception/Modules/RobotsModules/JointStatePerceptionModule.h +++ b/include/overworld/Perception/Modules/RobotsModules/JointStatePerceptionModule.h @@ -44,8 +44,8 @@ class JointStatePerceptionModule : public owds::PerceptionModuleRosBase getEntityColor(OntologyManipulator* onto, const std::string& indiv_name, const std::array& default_value = {0.8,0.8,0.8}); +std::array getEntityColor(onto::OntologyManipulator* onto, const std::string& indiv_name, const std::array& default_value = {0.8,0.8,0.8}); -Shape_t getEntityShape(OntologyManipulator* onto, const std::string& indiv_name); +Shape_t getEntityShape(onto::OntologyManipulator* onto, const std::string& indiv_name); -double getEntityMass(OntologyManipulator* onto, const std::string& indiv_name); +double getEntityMass(onto::OntologyManipulator* onto, const std::string& indiv_name); -void addColor(OntologyManipulator* onto, const std::string& color_name, const std::string& rgb_value = ""); +void addColor(onto::OntologyManipulator* onto, const std::string& color_name, const std::string& rgb_value = ""); -void addColorToEntity(OntologyManipulator* onto, const std::string& indiv_name, const std::string& color_name); +void addColorToEntity(onto::OntologyManipulator* onto, const std::string& indiv_name, const std::string& color_name); } // namespace ontology diff --git a/src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp b/src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp index 80563b8a..11ab4248 100644 --- a/src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp +++ b/src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp @@ -14,7 +14,7 @@ FakeHumanPerceptionModule::FakeHumanPerceptionModule() : PerceptionModuleRosBase bool FakeHumanPerceptionModule::closeInitialization() { - ontologies_manipulator_ = new OntologiesManipulator(n_); + ontologies_manipulator_ = new onto::OntologiesManipulator(n_); ontologies_manipulator_->waitInit(); ontologies_manipulator_->add(robot_agent_->getId()); onto_ = ontologies_manipulator_->get(robot_agent_->getId()); diff --git a/src/Perception/Modules/HumansModules/StampedPosePerceptionModule.cpp b/src/Perception/Modules/HumansModules/StampedPosePerceptionModule.cpp index 778c8918..e865581c 100644 --- a/src/Perception/Modules/HumansModules/StampedPosePerceptionModule.cpp +++ b/src/Perception/Modules/HumansModules/StampedPosePerceptionModule.cpp @@ -27,7 +27,7 @@ bool StampedPosePerceptionModule::closeInitialization() return false; } - ontologies_manipulator_ = new OntologiesManipulator(n_); + ontologies_manipulator_ = new onto::OntologiesManipulator(n_); ontologies_manipulator_->waitInit(); ontologies_manipulator_->add(robot_agent_->getId()); onto_ = ontologies_manipulator_->get(robot_agent_->getId()); diff --git a/src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp b/src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp index da84184a..4978c836 100644 --- a/src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp +++ b/src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp @@ -12,7 +12,7 @@ FakeObjectPerceptionModule::FakeObjectPerceptionModule() : PerceptionModuleRosBa bool FakeObjectPerceptionModule::closeInitialization() { - ontologies_manipulator_ = new OntologiesManipulator(n_); + ontologies_manipulator_ = new onto::OntologiesManipulator(n_); ontologies_manipulator_->waitInit(); std::string robot_name = robot_agent_->getId(); ontologies_manipulator_->add(robot_name); diff --git a/src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp b/src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp index bb120261..7ffd9be4 100644 --- a/src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp +++ b/src/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.cpp @@ -25,7 +25,7 @@ void StaticObjectsPerceptionModule::setParameter(const std::string& parameter_na bool StaticObjectsPerceptionModule::closeInitialization() { - ontologies_manipulator_ = new OntologiesManipulator(n_); + ontologies_manipulator_ = new onto::OntologiesManipulator(n_); std::string robot_name = robot_agent_->getId(); if(ontologies_manipulator_->add(robot_name) == false) { diff --git a/src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp b/src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp index 2b04a5b9..2156479c 100644 --- a/src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp +++ b/src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp @@ -33,7 +33,7 @@ bool JointStatePerceptionModule::closeInitialization() return false; } - ontologies_manipulator_ = new OntologiesManipulator(n_); + ontologies_manipulator_ = new onto::OntologiesManipulator(n_); ontologies_manipulator_->waitInit(); ontologies_manipulator_->add(robot_name_); onto_ = ontologies_manipulator_->get(robot_name_); diff --git a/src/Utility/Ontology.cpp b/src/Utility/Ontology.cpp index 831ce907..2ad91906 100644 --- a/src/Utility/Ontology.cpp +++ b/src/Utility/Ontology.cpp @@ -7,7 +7,7 @@ namespace owds { namespace ontology { -std::array getEntityColor(OntologyManipulator* onto, const std::string& indiv_name, const std::array& default_value) +std::array getEntityColor(onto::OntologyManipulator* onto, const std::string& indiv_name, const std::array& default_value) { auto color = onto->individuals.getOn(indiv_name, "hasColor"); if(color.size() != 0) @@ -24,7 +24,7 @@ std::array getEntityColor(OntologyManipulator* onto, const std::strin return default_value; } -Shape_t getEntityShape(OntologyManipulator* onto, const std::string& indiv_name) +Shape_t getEntityShape(onto::OntologyManipulator* onto, const std::string& indiv_name) { auto visual_meshes = onto->individuals.getOn(indiv_name, "hasVisualMesh"); auto collision_meshes = onto->individuals.getOn(indiv_name, "hasCollisionMesh"); @@ -77,7 +77,7 @@ Shape_t getEntityShape(OntologyManipulator* onto, const std::string& indiv_name) return shape; } -double getEntityMass(OntologyManipulator* onto, const std::string& indiv_name) +double getEntityMass(onto::OntologyManipulator* onto, const std::string& indiv_name) { auto masses = onto->individuals.getOn(indiv_name, "hasMass"); @@ -90,7 +90,7 @@ double getEntityMass(OntologyManipulator* onto, const std::string& indiv_name) return 0; } -void addColor(OntologyManipulator* onto, const std::string& color_name, const std::string& rgb_value) +void addColor(onto::OntologyManipulator* onto, const std::string& color_name, const std::string& rgb_value) { if(onto->individuals.exist(color_name) == false) { @@ -101,7 +101,7 @@ void addColor(OntologyManipulator* onto, const std::string& color_name, const st } } -void addColorToEntity(OntologyManipulator* onto, const std::string& indiv_name, const std::string& color_name) +void addColorToEntity(onto::OntologyManipulator* onto, const std::string& indiv_name, const std::string& color_name) { onto->feeder.addProperty(indiv_name, "hasColor", color_name); onto->feeder.waitUpdate(1000); From a100875d3787e9857e188876dfe7cea52893beef Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 24 Jul 2023 23:14:38 +0200 Subject: [PATCH 04/37] require ontologenius 0.3.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 933fe58e..892494b5 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge pluginlib ) -find_package(ontologenius 0.2.12 REQUIRED) +find_package(ontologenius 0.3.0 REQUIRED) find_package(Eigen3 REQUIRED NO_MODULE) find_package(Threads REQUIRED) find_package(CURL REQUIRED) From 22d7171cbb6eb3fd8c191fb2e1917c6ac09859c2 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 16 Aug 2023 15:43:44 +0200 Subject: [PATCH 05/37] [Services] add GetAgents service file --- CMakeLists.txt | 1 + srv/GetAgents.srv | 2 ++ 2 files changed, 3 insertions(+) create mode 100644 srv/GetAgents.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 892494b5..a275013d 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -75,6 +75,7 @@ add_service_files( FILES StartStopModules.srv BoundingBox.srv + GetAgents.srv GetPose.srv GetRelations.srv ) diff --git a/srv/GetAgents.srv b/srv/GetAgents.srv new file mode 100644 index 00000000..279e92a2 --- /dev/null +++ b/srv/GetAgents.srv @@ -0,0 +1,2 @@ +--- +string[] agents \ No newline at end of file From 8078cf609a19fae3fa20eb48570732b4a050a342 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 16 Aug 2023 15:44:35 +0200 Subject: [PATCH 06/37] [SituationAssessor] create getAgents service --- include/overworld/SituationAssessor.h | 3 +++ src/SituationAssessor.cpp | 9 +++++++++ 2 files changed, 12 insertions(+) diff --git a/include/overworld/SituationAssessor.h b/include/overworld/SituationAssessor.h index 210893b0..cbd62f11 100644 --- a/include/overworld/SituationAssessor.h +++ b/include/overworld/SituationAssessor.h @@ -19,6 +19,7 @@ #include #include +#include namespace owds { @@ -74,6 +75,7 @@ class SituationAssessor ros::ServiceServer start_modules_service_; ros::ServiceServer stop_modules_service_; ros::ServiceServer bounding_box_service_; + ros::ServiceServer agents_list_service_; ros::Publisher new_assessor_publisher_; std::atomic run_; double time_step_; // in second @@ -109,6 +111,7 @@ class SituationAssessor template bool stopModule(BasePerceptionManager& manager, const std::string& module_name, int& status); bool getBoundingBox(overworld::BoundingBox::Request &req, overworld::BoundingBox::Response &res); + bool getAgents(overworld::GetAgents::Request &req, overworld::GetAgents::Response &res); }; diff --git a/src/SituationAssessor.cpp b/src/SituationAssessor.cpp index de21c70d..32758810 100644 --- a/src/SituationAssessor.cpp +++ b/src/SituationAssessor.cpp @@ -92,6 +92,7 @@ SituationAssessor::SituationAssessor(const std::string& agent_name, start_modules_service_ = n_.advertiseService(agent_name_ + "/startPerceptionModules", &SituationAssessor::startModules, this); stop_modules_service_ = n_.advertiseService(agent_name_ + "/stopPerceptionModules", &SituationAssessor::stopModules, this); bounding_box_service_ = n_.advertiseService(agent_name_ + "/getBoundingBox", &SituationAssessor::getBoundingBox, this); + agents_list_service_ = n_.advertiseService(agent_name_ + "/getAgents", &SituationAssessor::getAgents, this); if(is_robot_) { auto msg = std_msgs::String(); @@ -393,4 +394,12 @@ bool SituationAssessor::getBoundingBox(overworld::BoundingBox::Request &req, ove return true; } +bool SituationAssessor::getAgents(overworld::GetAgents::Request &req, overworld::GetAgents::Response &res) +{ + (void)req; + for(auto& assessor : humans_assessors_) + res.agents.push_back(assessor.first); + return true; +} + } \ No newline at end of file From 3e483d8826630f378d5bc87b15261e774b797996 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 16 Aug 2023 15:45:42 +0200 Subject: [PATCH 07/37] [PerceptionModules] return false on tf exception --- .../Modules/HumansModules/FakeHumanPerceptionModule.cpp | 1 + .../Modules/ObjectsModules/FakeObjectPerceptionModule.cpp | 1 + .../Modules/RobotsModules/JointStatePerceptionModule.cpp | 1 + 3 files changed, 3 insertions(+) diff --git a/src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp b/src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp index 11ab4248..91ce33e2 100644 --- a/src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp +++ b/src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp @@ -51,6 +51,7 @@ bool FakeHumanPerceptionModule::perceptionCallback(const overworld::AgentPose& m } catch (const tf2::TransformException& ex) { ShellDisplay::error("[FakeHumanPerceptionModule]" + std::string(ex.what())); + return false; } } diff --git a/src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp b/src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp index 4978c836..aca5b96d 100644 --- a/src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp +++ b/src/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.cpp @@ -46,6 +46,7 @@ bool FakeObjectPerceptionModule::perceptionCallback(const overworld::EntitiesPos } catch (const tf2::TransformException& ex) { ShellDisplay::error("[FakeObjectPerceptionModule]" + std::string(ex.what())); + return false; } } diff --git a/src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp b/src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp index 2156479c..d3ff19b0 100644 --- a/src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp +++ b/src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp @@ -135,6 +135,7 @@ bool JointStatePerceptionModule::updateBasePose(const ros::Time& stamp) } catch (const tf2::TransformException& ex){ ShellDisplay::error("[JointStatePerceptionModule]" + std::string(ex.what())); + return false; } catch(...) { return false; From 3a11d6b48252db7dd45a173513eb0b386000ab22 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 16 Aug 2023 15:46:53 +0200 Subject: [PATCH 08/37] [AgentsPerceptionManager] rename UpdateAgent to updateAgent --- include/overworld/Perception/Managers/AgentPerceptionManager.h | 2 +- src/Perception/Managers/AgentPerceptionManager.cpp | 2 +- src/Perception/Managers/HumansPerceptionManager.cpp | 2 +- src/Perception/Managers/RobotsPerceptionManager.cpp | 2 +- 4 files changed, 4 insertions(+), 4 deletions(-) diff --git a/include/overworld/Perception/Managers/AgentPerceptionManager.h b/include/overworld/Perception/Managers/AgentPerceptionManager.h index b352c335..03266505 100644 --- a/include/overworld/Perception/Managers/AgentPerceptionManager.h +++ b/include/overworld/Perception/Managers/AgentPerceptionManager.h @@ -21,7 +21,7 @@ class AgentPerceptionManager : public EntitiesPerceptionManager Agent* getAgent(const std::string& agent_name, AgentType_e type); std::map::iterator createAgent(const std::string& name, AgentType_e type); - Agent* UpdateAgent(BodyPart* body_part, AgentType_e type); + Agent* updateAgent(BodyPart* body_part, AgentType_e type); FieldOfView getFov(const std::string& agent_name); double getOntoValue(const std::vector& vect, double default_value); }; diff --git a/src/Perception/Managers/AgentPerceptionManager.cpp b/src/Perception/Managers/AgentPerceptionManager.cpp index 95c5ab4d..04f41ddf 100644 --- a/src/Perception/Managers/AgentPerceptionManager.cpp +++ b/src/Perception/Managers/AgentPerceptionManager.cpp @@ -35,7 +35,7 @@ std::map::iterator AgentPerceptionManager::createAgent(cons return it; } -Agent* AgentPerceptionManager::UpdateAgent(BodyPart* body_part, AgentType_e type) +Agent* AgentPerceptionManager::updateAgent(BodyPart* body_part, AgentType_e type) { if(body_part->isAgentKnown()) { diff --git a/src/Perception/Managers/HumansPerceptionManager.cpp b/src/Perception/Managers/HumansPerceptionManager.cpp index 5ff26628..9d6b29e6 100644 --- a/src/Perception/Managers/HumansPerceptionManager.cpp +++ b/src/Perception/Managers/HumansPerceptionManager.cpp @@ -25,7 +25,7 @@ void HumansPerceptionManager::getPercepts( std::map& perc if(it->second->bulletId() != -1) addToBullet(it->second, it->second->bulletId()); } - UpdateAgent(it->second, AgentType_e::HUMAN); + updateAgent(it->second, AgentType_e::HUMAN); } updateEntityPose(it->second, percept.second.pose(), percept.second.lastStamp()); diff --git a/src/Perception/Managers/RobotsPerceptionManager.cpp b/src/Perception/Managers/RobotsPerceptionManager.cpp index e3bb828b..f4002dce 100644 --- a/src/Perception/Managers/RobotsPerceptionManager.cpp +++ b/src/Perception/Managers/RobotsPerceptionManager.cpp @@ -23,7 +23,7 @@ void RobotsPerceptionManager::getPercepts( std::map& perc if(it->second->bulletId() != -1) addToBullet(it->second, it->second->bulletId()); } - UpdateAgent(it->second, AgentType_e::ROBOT); + updateAgent(it->second, AgentType_e::ROBOT); } if(percept.second.isLocated()) From 0aba6c36ce5afdffe7e47ce01e21855efa44a223 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 16 Aug 2023 15:48:01 +0200 Subject: [PATCH 09/37] [PerceptionManagers] update humans before objects --- src/Perception/PerceptionManagers.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/Perception/PerceptionManagers.cpp b/src/Perception/PerceptionManagers.cpp index a6754408..6a2c0f62 100644 --- a/src/Perception/PerceptionManagers.cpp +++ b/src/Perception/PerceptionManagers.cpp @@ -31,8 +31,8 @@ PerceptionManagers::~PerceptionManagers() void PerceptionManagers::update() { robots_manager_.update(); - objects_manager_.update(); humans_manager_.update(); + objects_manager_.update(); areas_manager_.update(); } From 36bd407aa688ebc4e686721b62c4033260148ec4 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 16 Aug 2023 16:07:32 +0200 Subject: [PATCH 10/37] [SituationAssessor] unique getAgent service with robot itself --- src/SituationAssessor.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/SituationAssessor.cpp b/src/SituationAssessor.cpp index 32758810..f378beee 100644 --- a/src/SituationAssessor.cpp +++ b/src/SituationAssessor.cpp @@ -32,6 +32,7 @@ SituationAssessor::SituationAssessor(const std::string& agent_name, if(is_robot_) { new_assessor_publisher_ = n_.advertise("/overworld/new_assessor",5); + agents_list_service_ = n_.advertiseService("/overworld/getAgents", &SituationAssessor::getAgents, this); } if (is_robot_) @@ -92,7 +93,6 @@ SituationAssessor::SituationAssessor(const std::string& agent_name, start_modules_service_ = n_.advertiseService(agent_name_ + "/startPerceptionModules", &SituationAssessor::startModules, this); stop_modules_service_ = n_.advertiseService(agent_name_ + "/stopPerceptionModules", &SituationAssessor::stopModules, this); bounding_box_service_ = n_.advertiseService(agent_name_ + "/getBoundingBox", &SituationAssessor::getBoundingBox, this); - agents_list_service_ = n_.advertiseService(agent_name_ + "/getAgents", &SituationAssessor::getAgents, this); if(is_robot_) { auto msg = std_msgs::String(); @@ -397,6 +397,7 @@ bool SituationAssessor::getBoundingBox(overworld::BoundingBox::Request &req, ove bool SituationAssessor::getAgents(overworld::GetAgents::Request &req, overworld::GetAgents::Response &res) { (void)req; + res.agents.push_back(agent_name_); for(auto& assessor : humans_assessors_) res.agents.push_back(assessor.first); return true; From b314deaa1579c4a04daf7a84f05170f3e53110ae Mon Sep 17 00:00:00 2001 From: sarthou Date: Thu, 17 Aug 2023 15:43:46 +0200 Subject: [PATCH 11/37] [ObjectsPerceptionManager] use MAX_UNSEEN for entities with no poi --- src/Perception/Managers/ObjectsPerceptionManager.cpp | 9 +++++++++ 1 file changed, 9 insertions(+) diff --git a/src/Perception/Managers/ObjectsPerceptionManager.cpp b/src/Perception/Managers/ObjectsPerceptionManager.cpp index 3cdaf4af..ea4c8e75 100644 --- a/src/Perception/Managers/ObjectsPerceptionManager.cpp +++ b/src/Perception/Managers/ObjectsPerceptionManager.cpp @@ -226,8 +226,17 @@ void ObjectsPerceptionManager::reasoningOnUpdate() for(auto no_data_obj : no_data_objects) { + // equivalent to shouldBeSeen if(objects_in_camera.find(no_data_obj->bulletId()) != objects_in_camera.end()) + { + auto it_unseen = lost_objects_nb_frames_.find(no_data_obj->id()); + if(it_unseen == lost_objects_nb_frames_.end()) + it_unseen = lost_objects_nb_frames_.insert({no_data_obj->id(), 0}).first; + + it_unseen->second++; + if(it_unseen->second > MAX_UNSEEN) objects_to_remove.push_back(no_data_obj); + } else objects_to_simulate.push_back(no_data_obj); } From d14fa71f87cde5dd2fd919bcf50138b1a9294c50 Mon Sep 17 00:00:00 2001 From: sarthou Date: Thu, 17 Aug 2023 15:44:53 +0200 Subject: [PATCH 12/37] [FactsCalculator] clean hasInHand --- src/Facts/FactsCalculator.cpp | 39 ++++++++++++++++++----------------- 1 file changed, 20 insertions(+), 19 deletions(-) diff --git a/src/Facts/FactsCalculator.cpp b/src/Facts/FactsCalculator.cpp index 256bd2b3..aa346216 100644 --- a/src/Facts/FactsCalculator.cpp +++ b/src/Facts/FactsCalculator.cpp @@ -1,5 +1,7 @@ #include "overworld/Facts/FactsCalculator.h" +#define IN_HAND_DIST 0.08 + namespace owds { FactsCalculator::FactsCalculator(const std::string& agent_name) @@ -57,13 +59,14 @@ std::vector FactsCalculator::computeAgentsFacts(const std::mapgetType() == AgentType_e::HUMAN){ + if (agent_from.second->getType() == AgentType_e::HUMAN) + { hasInHand(agent_from.second, obj.second); isHandMovingTowards(agent_from.second, obj.second); } @@ -264,46 +267,44 @@ bool FactsCalculator::isLookingAt(Agent* agent, const std::unordered_set& s bool FactsCalculator::hasInHand(Agent* agent, Object* object) { - Hand *leftHand, *rightHand; - if ((leftHand = agent->getLeftHand()) != nullptr) + Hand *left_hand, *right_hand; + if ((left_hand = agent->getLeftHand()) != nullptr) { - const auto inLeftHand = leftHand->getInHand(); - if (std::find(inLeftHand.begin(), inLeftHand.end(), object->id()) != inLeftHand.end()) + if (left_hand->isInHand(object->id())) { facts_.emplace_back(agent->getId(), "hasInLeftHand", object->id()); - return true; + return true; // cannot be in two hands at time } - else if (leftHand->isLocated() && object->isLocated()) + else if (left_hand->isEmpty() && left_hand->isLocated() && object->isLocated()) // allows only one object in hand { - if (leftHand->pose().distanceTo(object->pose()) < 0.08) + if (left_hand->pose().distanceTo(object->pose()) < IN_HAND_DIST) { if(object->isA("Pickable")) { - object->setInHand(leftHand); - leftHand->putInHand(object); + object->setInHand(left_hand); + left_hand->putInHand(object); facts_.emplace_back(agent->getId(), "hasInLeftHand", object->id()); - return true; + return true; // cannot be in two hands at time } } } } - if ((rightHand = agent->getRightHand()) != nullptr) + if ((right_hand = agent->getRightHand()) != nullptr) { - const auto inRightHand = rightHand->getInHand(); - if (std::find(inRightHand.begin(), inRightHand.end(), object->id()) != inRightHand.end()) + if (right_hand->isInHand(object->id())) { facts_.emplace_back(agent->getId(), "hasInRightHand", object->id()); return true; } - else if (rightHand->isLocated() && object->isLocated()) + else if (right_hand->isEmpty() && right_hand->isLocated() && object->isLocated()) // allows only one object in hand { - if (rightHand->pose().distanceTo(object->pose()) < 0.08) + if (right_hand->pose().distanceTo(object->pose()) < IN_HAND_DIST) { if(object->isA("Pickable")) { - object->setInHand(rightHand); - rightHand->putInHand(object); + object->setInHand(right_hand); + right_hand->putInHand(object); facts_.emplace_back(agent->getId(), "hasInRightHand", object->id()); return true; } From ce290d7c1c89743ac41355a108df28d0f1df8939 Mon Sep 17 00:00:00 2001 From: sarthou Date: Tue, 22 Aug 2023 10:48:15 +0200 Subject: [PATCH 13/37] [SituationAssessor] publish tf of humans --- src/SituationAssessor.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/SituationAssessor.cpp b/src/SituationAssessor.cpp index f378beee..f5576121 100644 --- a/src/SituationAssessor.cpp +++ b/src/SituationAssessor.cpp @@ -239,11 +239,15 @@ void SituationAssessor::assess() if(is_robot_) { ros_sender_->sendEntitiesToTFAndRViz(myself_agent_->getId() + "/objects_markers", objects); + ros_sender_->sendEntitiesToTFAndRViz(myself_agent_->getId() + "/humans_markers", body_parts); //bernie_sender_->sendBernie(); } else + { ros_sender_->sendEntitiesToRViz(myself_agent_->getId() + "/objects_markers", objects); ros_sender_->sendEntitiesToRViz(myself_agent_->getId() + "/humans_markers", body_parts); + } + if(is_robot_) humans_process.join(); From 179a52e1bc85c438eb9e83576bffec7343957daf Mon Sep 17 00:00:00 2001 From: sarthou Date: Tue, 22 Aug 2023 10:49:04 +0200 Subject: [PATCH 14/37] [computeProjectionMatrix] always use Z forward --- src/Perception/Managers/ObjectsPerceptionManager.cpp | 7 +------ src/SituationAssessor.cpp | 2 +- 2 files changed, 2 insertions(+), 7 deletions(-) diff --git a/src/Perception/Managers/ObjectsPerceptionManager.cpp b/src/Perception/Managers/ObjectsPerceptionManager.cpp index ea4c8e75..7a6a6feb 100644 --- a/src/Perception/Managers/ObjectsPerceptionManager.cpp +++ b/src/Perception/Managers/ObjectsPerceptionManager.cpp @@ -445,12 +445,7 @@ std::unordered_set ObjectsPerceptionManager::getObjectsInCamera() myself_agent_->getFieldOfView().getRatio(), myself_agent_->getFieldOfView().getClipNear(), myself_agent_->getFieldOfView().getClipFar()); - - std::array axes = {1,0,0}; - if(myself_agent_->getType() == AgentType_e::ROBOT) - axes = {0,0,1}; - - Pose target_pose = myself_agent_->getHead()->pose() * Pose(axes, {0,0,0,1}); + Pose target_pose = myself_agent_->getHead()->pose() * Pose({0,0,1}, {0,0,0,1}); auto head_pose_trans = myself_agent_->getHead()->pose().arrays().first; auto target_pose_trans = target_pose.arrays().first; auto view_matrix = bullet_client_->computeViewMatrix({(float)head_pose_trans[0], (float)head_pose_trans[1], (float)head_pose_trans[2]}, diff --git a/src/SituationAssessor.cpp b/src/SituationAssessor.cpp index f5576121..a0190fdf 100644 --- a/src/SituationAssessor.cpp +++ b/src/SituationAssessor.cpp @@ -276,7 +276,7 @@ void SituationAssessor::processHumans(std::mapgetFieldOfView().getRatio(), human.second->getFieldOfView().getClipNear(), human.second->getFieldOfView().getClipFar()); - Pose target_pose = human.second->getHead()->pose() * Pose({1,0,0}, {0,0,0,1}); + Pose target_pose = human.second->getHead()->pose() * Pose({0,0,1}, {0,0,0,1}); auto head_pose_trans = human.second->getHead()->pose().arrays().first; auto target_pose_trans = target_pose.arrays().first; auto view_matrix = bullet_client_->computeViewMatrix({(float)head_pose_trans[0], (float)head_pose_trans[1], (float)head_pose_trans[2]}, From e41574bcb8820e88d748c8d1d68cea8cea9d97ac Mon Sep 17 00:00:00 2001 From: sarthou Date: Thu, 24 Aug 2023 13:17:44 +0200 Subject: [PATCH 15/37] [FieldOfView] dedicated ratio for onpengl --- CMakeLists.txt | 9 ++ include/overworld/BasicTypes/FieldOfView.h | 7 +- .../Managers/ObjectsPerceptionManager.cpp | 4 +- src/SituationAssessor.cpp | 4 +- src/TestFiles/fov.cpp | 122 ++++++++++++++++++ 5 files changed, 141 insertions(+), 5 deletions(-) create mode 100644 src/TestFiles/fov.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index a275013d..9e29c292 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -259,6 +259,15 @@ target_link_libraries(${PROJECT_NAME}_bullet_test PRIVATE ${PROJECT_NAME}_utility_lib ${catkin_LIBRARIES}) +add_executable(${PROJECT_NAME}_fov_test src/TestFiles/fov.cpp ) +target_link_libraries(${PROJECT_NAME}_fov_test PRIVATE + ${PROJECT_NAME}_bullet_lib + ${PROJECT_NAME}_types_lib + ${PROJECT_NAME}_perception_lib + ${PROJECT_NAME}_utility_lib + ${PROJECT_NAME}_sender_lib + ${catkin_LIBRARIES}) + add_executable(${PROJECT_NAME}_multi_server_test src/TestFiles/multi_server_test.cpp ) target_link_libraries(${PROJECT_NAME}_multi_server_test PRIVATE ${PROJECT_NAME}_bullet_lib diff --git a/include/overworld/BasicTypes/FieldOfView.h b/include/overworld/BasicTypes/FieldOfView.h index 6826735d..171d990e 100644 --- a/include/overworld/BasicTypes/FieldOfView.h +++ b/include/overworld/BasicTypes/FieldOfView.h @@ -17,7 +17,9 @@ class FieldOfView width_(width), clip_near_(clip_near), clip_far_(clip_far) - {} + { + opengl_ratio_ = std::sqrt(2) * std::tan(width_ / height_ * 0.6116); + } double getHeight() const { return height_; } double getWidth() const { return width_; } @@ -25,6 +27,8 @@ class FieldOfView double getClipFar() const { return clip_far_; } double getRatio() const { return width_ / height_; } + double getRatioOpenGl() const { return opengl_ratio_ } + //double getRatioOpenGl() const { return 1.4227 * std::tan(width_ / height_ * 0.6105); } /** * @brief @@ -52,6 +56,7 @@ class FieldOfView double width_; // degrees double clip_near_; // meters double clip_far_; // meters + double opengl_ratio_; }; } // namespace owds diff --git a/src/Perception/Managers/ObjectsPerceptionManager.cpp b/src/Perception/Managers/ObjectsPerceptionManager.cpp index 7a6a6feb..66d767ae 100644 --- a/src/Perception/Managers/ObjectsPerceptionManager.cpp +++ b/src/Perception/Managers/ObjectsPerceptionManager.cpp @@ -442,7 +442,7 @@ std::unordered_set ObjectsPerceptionManager::getObjectsInCamera() return {}; auto proj_matrix = bullet_client_->computeProjectionMatrix(myself_agent_->getFieldOfView().getHeight(), - myself_agent_->getFieldOfView().getRatio(), + myself_agent_->getFieldOfView().getRatioOpenGl(), myself_agent_->getFieldOfView().getClipNear(), myself_agent_->getFieldOfView().getClipFar()); Pose target_pose = myself_agent_->getHead()->pose() * Pose({0,0,1}, {0,0,0,1}); @@ -451,7 +451,7 @@ std::unordered_set ObjectsPerceptionManager::getObjectsInCamera() auto view_matrix = bullet_client_->computeViewMatrix({(float)head_pose_trans[0], (float)head_pose_trans[1], (float)head_pose_trans[2]}, {(float)target_pose_trans[0], (float)target_pose_trans[1], (float)target_pose_trans[2]}, {0.,0.,1.}); - auto images = bullet_client_->getCameraImage(100*myself_agent_->getFieldOfView().getRatio(), 100, view_matrix, proj_matrix, owds::BULLET_HARDWARE_OPENGL); + auto images = bullet_client_->getCameraImage(100*myself_agent_->getFieldOfView().getRatioOpenGl(), 100, view_matrix, proj_matrix, owds::BULLET_HARDWARE_OPENGL); return bullet_client_->getSegmentationIds(images); } diff --git a/src/SituationAssessor.cpp b/src/SituationAssessor.cpp index a0190fdf..09fcddd5 100644 --- a/src/SituationAssessor.cpp +++ b/src/SituationAssessor.cpp @@ -273,7 +273,7 @@ void SituationAssessor::processHumans(std::mapcomputeProjectionMatrix(human.second->getFieldOfView().getHeight(), - human.second->getFieldOfView().getRatio(), + human.second->getFieldOfView().getRatioOpenGl(), human.second->getFieldOfView().getClipNear(), human.second->getFieldOfView().getClipFar()); Pose target_pose = human.second->getHead()->pose() * Pose({0,0,1}, {0,0,0,1}); @@ -282,7 +282,7 @@ void SituationAssessor::processHumans(std::mapcomputeViewMatrix({(float)head_pose_trans[0], (float)head_pose_trans[1], (float)head_pose_trans[2]}, {(float)target_pose_trans[0], (float)target_pose_trans[1], (float)target_pose_trans[2]}, {0.,0.,1.}); - auto images = bullet_client_->getCameraImage(300*human.second->getFieldOfView().getRatio(), 300, view_matrix, proj_matrix, owds::BULLET_HARDWARE_OPENGL); + auto images = bullet_client_->getCameraImage(300*human.second->getFieldOfView().getRatioOpenGl(), 300, view_matrix, proj_matrix, owds::BULLET_HARDWARE_OPENGL); ros_sender_->sendImage(human.first + "/view", images); agents_segmentation_ids[human.first] = bullet_client_->getSegmentationIds(images); diff --git a/src/TestFiles/fov.cpp b/src/TestFiles/fov.cpp new file mode 100644 index 00000000..955bf236 --- /dev/null +++ b/src/TestFiles/fov.cpp @@ -0,0 +1,122 @@ +#include "overworld/Bullet/PhysicsServers.h" +#include "overworld/Geometry/Pose.h" +#include "overworld/BasicTypes/Object.h" +#include "overworld/BasicTypes/FieldOfView.h" +#include "overworld/Senders/ROSSender.h" + +#include +#include +#include +#include + +#include + +#include "overworld/Utility/ShellDisplay.h" + +using namespace std::chrono_literals; + +static volatile std::sig_atomic_t flag; + +void sigHandler(int signal) +{ + if(signal == SIGINT) + flag = true; +} + +void createCube(owds::BulletClient* client, const std::array& pose, const std::array& color) +{ + int visual_id = client->createVisualShapeBox({0.5,0.5,0.5}, color); + int collision_id = client->createCollisionShapeBox({0.5,0.5,0.5}); + int obj_id = client->createMultiBody(0, collision_id, visual_id, pose, {0,0,0,1}); +} + +void createScreen(owds::BulletClient* client, double height, double width) +{ + double dist = 6; + auto w = width*TO_HALF_RAD; + auto h = height*TO_HALF_RAD; + auto left = dist*std::tan(w); + auto top = dist*std::tan(h); + + int visual_top_id = client->createVisualShapeBox({0.05,left,0.05}, {0,0,0,1}); + int collision_top_id = client->createCollisionShapeBox({0.05,left,0.05}); + int obj_top_id = client->createMultiBody(0, collision_top_id, visual_top_id, {dist,0,top}, {0,0,0,1}); + + int visual_bot_id = client->createVisualShapeBox({0.05,left,0.05}, {0,0,0,1}); + int collision_bot_id = client->createCollisionShapeBox({0.05,left,0.05}); + int obj_bot_id = client->createMultiBody(0, collision_bot_id, visual_bot_id, {dist,0,-top}, {0,0,0,1}); + + int visual_left_id = client->createVisualShapeBox({0.05,0.05,top}, {0,0,0,1}); + int collision_left_id = client->createCollisionShapeBox({0.05,0.05, top}); + int obj_left_id = client->createMultiBody(0, collision_left_id, visual_left_id, {dist,left,0}, {0,0,0,1}); + + int visual_right_id = client->createVisualShapeBox({0.05,0.05,top}, {0,0,0,1}); + int collision_right_id = client->createCollisionShapeBox({0.05,0.05,top}); + int obj_right_id = client->createMultiBody(0, collision_right_id, visual_right_id, {dist,-left,0}, {0,0,0,1}); +} + +int main(int argc, char** argv) +{ + ros::init(argc, argv, "bullet_fov"); + ros::NodeHandle n; + owds::ROSSender ros_sender(&n); + flag = false; + + owds::BulletClient* client = owds::PhysicsServers::connectPhysicsServer(owds::CONNECT_GUI); + + client->configureDebugVisualizer(COV_ENABLE_DEPTH_BUFFER_PREVIEW, false); + client->configureDebugVisualizer(COV_ENABLE_SHADOWS, false); + + createCube(client, {5,0,0}, {1,0,0,1}); + createCube(client, {5,-2,0}, {0,1,0,1}); + createCube(client, {5,2,0}, {0,1,0,1}); + createCube(client, {5,0,-2}, {0,0,1,1}); + createCube(client, {5,0,2}, {0,0,1,1}); + + owds::FieldOfView fov(70, 102.4, 0.1, 100); + owds::FieldOfView fov_1_1(70, 70, 0.1, 100); + + createScreen(client, fov.getHeight(), fov.getWidth()); + + owds::Pose head_pose({0,0,0}, {0.5,0.5,0.5,0.5}); + owds::Pose target_pose = head_pose * owds::Pose({0,0,1}, {0,0,0,1}); + + double ratio = fov.getRatioOpenGl(); + std::cout << "ratio = " << ratio << std::endl; + std::cout << "ratio 1x1 = " << fov_1_1.getRatioOpenGl() << std::endl; + + auto proj_matrix = client->computeProjectionMatrix(fov.getHeight(), + ratio, // fov.getRatio(), + fov.getClipNear(), + fov.getClipFar()); + /*auto w = fov.getWidth()*TO_HALF_RAD; + auto h = fov.getHeight()*TO_HALF_RAD; + auto left = std::tan(w); + auto top = std::tan(h); + std::cout << "w :" << w << "h : " << h << " left : " << left << "top : " << top << std::endl; + auto proj_matrix = client->computeProjectionMatrix(left, + -left, + -top, + top, + fov.getClipNear(), + fov.getClipFar());*/ + auto head_pose_trans = head_pose.arrays().first; + auto target_pose_trans = target_pose.arrays().first; + auto view_matrix = client->computeViewMatrix({(float)head_pose_trans[0], (float)head_pose_trans[1], (float)head_pose_trans[2]}, + {(float)target_pose_trans[0], (float)target_pose_trans[1], (float)target_pose_trans[2]}, + {0.,0.,1.}); + auto images = client->getCameraImage(1000*ratio, 1000, view_matrix, proj_matrix, owds::BULLET_HARDWARE_OPENGL); + + std::cout << "image pixels: " << images.m_pixelWidth << "x" << images.m_pixelHeight << std::endl; + + while(flag == false && ros::ok()) + { + std::this_thread::sleep_for(100ms); + ros::spinOnce(); + ros_sender.sendImage("overworld/view", images); + } + + delete client; + + return 0; +} From 2d8d20700d85dd3553400bf5db796ffeff6677f9 Mon Sep 17 00:00:00 2001 From: sarthou Date: Thu, 24 Aug 2023 15:47:52 +0200 Subject: [PATCH 16/37] [FieldOfView] margin on hasIn --- include/overworld/BasicTypes/FieldOfView.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/include/overworld/BasicTypes/FieldOfView.h b/include/overworld/BasicTypes/FieldOfView.h index 171d990e..26f274a2 100644 --- a/include/overworld/BasicTypes/FieldOfView.h +++ b/include/overworld/BasicTypes/FieldOfView.h @@ -27,7 +27,7 @@ class FieldOfView double getClipFar() const { return clip_far_; } double getRatio() const { return width_ / height_; } - double getRatioOpenGl() const { return opengl_ratio_ } + double getRatioOpenGl() const { return opengl_ratio_; } //double getRatioOpenGl() const { return 1.4227 * std::tan(width_ / height_ * 0.6105); } /** @@ -37,10 +37,10 @@ class FieldOfView * @return true * @return false */ - inline bool hasIn(const Pose& pose) const + inline bool hasIn(const Pose& pose, double margin = 0.) const { - return pose.getZ() <= clip_far_ && std::abs(pose.getOriginTilt()) <= height_ * TO_HALF_RAD && - std::abs(pose.getOriginPan()) <= width_ * TO_HALF_RAD; + return pose.getZ() <= clip_far_ && std::abs(pose.getOriginTilt()) <= (height_ - margin*2) * TO_HALF_RAD && + std::abs(pose.getOriginPan()) <= (width_ - margin*2) * TO_HALF_RAD; } std::string toString() const From 9ccc1f17717e9e3be50ef485406176877c6742e5 Mon Sep 17 00:00:00 2001 From: sarthou Date: Thu, 24 Aug 2023 15:48:33 +0200 Subject: [PATCH 17/37] [ObjectsPerceptionManager] isObjectsInFovAabb returns objects --- .../Managers/ObjectsPerceptionManager.h | 2 +- .../Managers/ObjectsPerceptionManager.cpp | 19 +++++++++++++------ 2 files changed, 14 insertions(+), 7 deletions(-) diff --git a/include/overworld/Perception/Managers/ObjectsPerceptionManager.h b/include/overworld/Perception/Managers/ObjectsPerceptionManager.h index 9fa8530d..eca17d0c 100644 --- a/include/overworld/Perception/Managers/ObjectsPerceptionManager.h +++ b/include/overworld/Perception/Managers/ObjectsPerceptionManager.h @@ -45,7 +45,7 @@ class ObjectsPerceptionManager : public EntitiesPerceptionManager void stopSimulation(Object* object, bool erase = true); std::vector getPoisInFov(Object* object); - bool isObjectsInFovAabb(std::vector objects); + std::vector isObjectsInFovAabb(std::vector objects); bool shouldBeSeen(Object* object, const std::vector& pois); std::unordered_set getObjectsInCamera(); diff --git a/src/Perception/Managers/ObjectsPerceptionManager.cpp b/src/Perception/Managers/ObjectsPerceptionManager.cpp index 66d767ae..c93de739 100644 --- a/src/Perception/Managers/ObjectsPerceptionManager.cpp +++ b/src/Perception/Managers/ObjectsPerceptionManager.cpp @@ -220,11 +220,12 @@ void ObjectsPerceptionManager::reasoningOnUpdate() if(no_data_objects.size()) { - if(isObjectsInFovAabb(no_data_objects)) + auto objects_in_fov = isObjectsInFovAabb(no_data_objects); + if(objects_in_fov.size()) { auto objects_in_camera = getObjectsInCamera(); - for(auto no_data_obj : no_data_objects) + for(auto no_data_obj : objects_in_fov) { // equivalent to shouldBeSeen if(objects_in_camera.find(no_data_obj->bulletId()) != objects_in_camera.end()) @@ -373,10 +374,12 @@ std::vector ObjectsPerceptionManager::getPoisInFov(Object* obje return pois_in_fov; } -bool ObjectsPerceptionManager::isObjectsInFovAabb(std::vector objects) +std::vector ObjectsPerceptionManager::isObjectsInFovAabb(std::vector objects) { + std::vector res; for(auto object : objects) { + size_t nb_in_fov = 0; std::array points = { Pose({object->getAabb().min[0], object->getAabb().min[1], object->getAabb().min[2]}, {0,0,0,1}), Pose({object->getAabb().min[0], object->getAabb().min[1], object->getAabb().max[2]}, {0,0,0,1}), Pose({object->getAabb().min[0], object->getAabb().max[1], object->getAabb().min[2]}, {0,0,0,1}), @@ -389,12 +392,16 @@ bool ObjectsPerceptionManager::isObjectsInFovAabb(std::vector objects) for(const auto& point : points) { auto point_in_head = point.transformIn(myself_agent_->getHead()->pose()); - if (myself_agent_->getFieldOfView().hasIn(point_in_head)) - return true; + if (myself_agent_->getFieldOfView().hasIn(point_in_head, 2)) + { + nb_in_fov ++; + if(nb_in_fov >= 2) + res.push_back(object); + } } } - return false; + return res; } bool ObjectsPerceptionManager::shouldBeSeen(Object* object, const std::vector& pois) From 72e88b3cb6d35e752022aa04992a5e85b5a67dff Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Sun, 27 Aug 2023 18:25:35 +0200 Subject: [PATCH 18/37] [FieldOfView] true aspectRatio formula --- include/overworld/BasicTypes/FieldOfView.h | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/include/overworld/BasicTypes/FieldOfView.h b/include/overworld/BasicTypes/FieldOfView.h index 26f274a2..ed20fd01 100644 --- a/include/overworld/BasicTypes/FieldOfView.h +++ b/include/overworld/BasicTypes/FieldOfView.h @@ -18,7 +18,7 @@ class FieldOfView clip_near_(clip_near), clip_far_(clip_far) { - opengl_ratio_ = std::sqrt(2) * std::tan(width_ / height_ * 0.6116); + opengl_ratio_ = std::tan(width_ * TO_HALF_RAD) / std::tan(height_ * TO_HALF_RAD); } double getHeight() const { return height_; } @@ -28,7 +28,6 @@ class FieldOfView double getRatio() const { return width_ / height_; } double getRatioOpenGl() const { return opengl_ratio_; } - //double getRatioOpenGl() const { return 1.4227 * std::tan(width_ / height_ * 0.6105); } /** * @brief From ba2aeae9c59479384d9a70a1715d363d368ca95d Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Tue, 24 Oct 2023 13:57:51 +0200 Subject: [PATCH 19/37] [srv] add GetApproachPoint service --- CMakeLists.txt | 1 + srv/GetApproachPoint.srv | 4 ++++ 2 files changed, 5 insertions(+) create mode 100644 srv/GetApproachPoint.srv diff --git a/CMakeLists.txt b/CMakeLists.txt index 9e29c292..e8358453 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -76,6 +76,7 @@ add_service_files( StartStopModules.srv BoundingBox.srv GetAgents.srv + GetApproachPoint.srv GetPose.srv GetRelations.srv ) diff --git a/srv/GetApproachPoint.srv b/srv/GetApproachPoint.srv new file mode 100644 index 00000000..7bd8869c --- /dev/null +++ b/srv/GetApproachPoint.srv @@ -0,0 +1,4 @@ +string area_constraints +string entity +--- +geometry_msgs/PoseStamped pose \ No newline at end of file From 6c95073bc373212c0c07aa795861cb9257e17511 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Tue, 24 Oct 2023 14:05:42 +0200 Subject: [PATCH 20/37] [SituationAssessor] rename object_pose_sender --- include/overworld/SituationAssessor.h | 2 +- src/SituationAssessor.cpp | 8 ++++---- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/include/overworld/SituationAssessor.h b/include/overworld/SituationAssessor.h index cbd62f11..128d8c87 100644 --- a/include/overworld/SituationAssessor.h +++ b/include/overworld/SituationAssessor.h @@ -88,7 +88,7 @@ class SituationAssessor OntologeniusFactsPublisher facts_publisher_; ROSSender* ros_sender_; - PoseSender* motion_planning_pose_sender_; + PoseSender* objetcs_pose_sender_; BernieSenders* bernie_sender_; std::map humans_assessors_; diff --git a/src/SituationAssessor.cpp b/src/SituationAssessor.cpp index 09fcddd5..98a331d5 100644 --- a/src/SituationAssessor.cpp +++ b/src/SituationAssessor.cpp @@ -82,12 +82,12 @@ SituationAssessor::SituationAssessor(const std::string& agent_name, ros_sender_ = new ROSSender(&n_); if(is_robot_) { - motion_planning_pose_sender_ = new PoseSender(&n_, perception_manager_.objects_manager_); + objetcs_pose_sender_ = new PoseSender(&n_, perception_manager_.objects_manager_); bernie_sender_ = new BernieSenders(&n_); } else { - motion_planning_pose_sender_ = nullptr; + objetcs_pose_sender_ = nullptr; bernie_sender_ = nullptr; } start_modules_service_ = n_.advertiseService(agent_name_ + "/startPerceptionModules", &SituationAssessor::startModules, this); @@ -105,8 +105,8 @@ SituationAssessor::~SituationAssessor() { if(ros_sender_ != nullptr) delete ros_sender_; - if(motion_planning_pose_sender_ != nullptr) - delete motion_planning_pose_sender_; + if(objetcs_pose_sender_ != nullptr) + delete objetcs_pose_sender_; if (bernie_sender_ != nullptr) delete bernie_sender_; if(bullet_client_ != nullptr) From e06991f48bff0a2e269af867ef2147a1356b256f Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Wed, 25 Oct 2023 10:55:58 +0200 Subject: [PATCH 21/37] [ApproachSender] LogicalAlgebraNode --- CMakeLists.txt | 1 + include/overworld/Senders/ApproachSender.h | 56 +++++++++++++++++++ src/Senders/ApproachSender.cpp | 63 ++++++++++++++++++++++ 3 files changed, 120 insertions(+) create mode 100644 include/overworld/Senders/ApproachSender.h create mode 100644 src/Senders/ApproachSender.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e8358453..f57b1681 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -190,6 +190,7 @@ target_link_libraries(${PROJECT_NAME}_facts_lib PUBLIC ${PROJECT_NAME}_types_lib) add_library(${PROJECT_NAME}_sender_lib STATIC + src/Senders/ApproachSender.cpp src/Senders/ROSSender.cpp src/Senders/PoseSender.cpp src/Senders/Bernie.cpp diff --git a/include/overworld/Senders/ApproachSender.h b/include/overworld/Senders/ApproachSender.h new file mode 100644 index 00000000..3650018c --- /dev/null +++ b/include/overworld/Senders/ApproachSender.h @@ -0,0 +1,56 @@ +#ifndef OWDS_APPROACHSENDER_H +#define OWDS_APPROACHSENDER_H + +#include "ontologenius/OntologyManipulator.h" + +#include "overworld/Perception/PerceptionManagers.h" +#include "overworld/BasicTypes/Agent.h" + +namespace owds { + +enum LogicalAlgebraOperator_e +{ + logical_and, + logical_or, + logical_not, + logical_none +}; + +class LogicalAlgebraNode +{ +public: + LogicalAlgebraNode(LogicalAlgebraOperator_e op) : operator_(op) {} + + void insert(const std::string& value) { values_.push_back(value); } + void insert(const LogicalAlgebraNode& node) { nodes_.push_back(node); } + + bool evaluate(const std::string& entity, onto::IndividualClient* client); + +private: + LogicalAlgebraOperator_e operator_; + std::vector values_; + std::vector nodes_; + + bool evaluate(const std::string& entity, const std::string& value, onto::IndividualClient* client); +}; + +class ApproachSender +{ +public: + ApproachSender(ros::NodeHandle* n, PerceptionManagers* managers); + + void setRobotName(const std::string& robot_name); + +private: + ros::NodeHandle* n_; + std::string robot_name_; + Agent* robot_; + + PerceptionManagers* managers_; + onto::OntologyManipulator* onto_; + +}; + +} // namespace owds + +#endif // OWDS_APPROACHSENDER_H \ No newline at end of file diff --git a/src/Senders/ApproachSender.cpp b/src/Senders/ApproachSender.cpp new file mode 100644 index 00000000..f6659ce9 --- /dev/null +++ b/src/Senders/ApproachSender.cpp @@ -0,0 +1,63 @@ +#include "overworld/Senders/ApproachSender.h" + +#include "ontologenius/OntologiesManipulator.h" + +namespace owds { + +bool LogicalAlgebraNode::evaluate(const std::string& entity, onto::IndividualClient* client) +{ + switch (operator_) + { + case logical_and: + for(auto& value : values_) + if(!evaluate(entity, value, client)) + return false; + for(auto& node : nodes_) + if(!node.evaluate(entity, client)) + return false; + return true; + case logical_or: + for(auto& value : values_) + if(evaluate(entity, value, client)) + return true; + for(auto& node : nodes_) + if(node.evaluate(entity, client)) + return true; + return false; + case logical_not: + if(values_.size()) + return !evaluate(entity, values_.front(), client); + else if(nodes_.size()) + return !nodes_.front().evaluate(entity, client); + else + return false; + default: + return false; + } +} + +bool LogicalAlgebraNode::evaluate(const std::string& entity, const std::string& value, onto::IndividualClient* client) +{ + auto res = client->getOn(entity, "isInArea"); + return (std::find(res.begin(), res.end(), value) != res.end()); +} + +ApproachSender::ApproachSender(ros::NodeHandle* n, PerceptionManagers* managers) : n_(n), + robot_(nullptr), + managers_(managers), + onto_(nullptr) +{} + +void ApproachSender::setRobotName(const std::string& robot_name) +{ + robot_name_ = robot_name; + auto ontos = onto::OntologiesManipulator(n_); + ontos.waitInit(); + ontos.add(robot_name_); + onto_ = ontos.get(robot_name_); + onto_->close(); + + robot_ = managers_->robots_manager_.getAgent(robot_name_); +} + +} // namespace owds \ No newline at end of file From 2d66aa29a0e9f05501bc80aa273821b928306506 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 25 Oct 2023 10:59:51 +0200 Subject: [PATCH 22/37] [SituationAssessor] setSimulation service --- include/overworld/SituationAssessor.h | 4 ++++ src/SituationAssessor.cpp | 24 +++++++++++++++++++++--- 2 files changed, 25 insertions(+), 3 deletions(-) diff --git a/include/overworld/SituationAssessor.h b/include/overworld/SituationAssessor.h index 128d8c87..1b315f2b 100644 --- a/include/overworld/SituationAssessor.h +++ b/include/overworld/SituationAssessor.h @@ -20,6 +20,7 @@ #include #include #include +#include namespace owds { @@ -56,6 +57,7 @@ class SituationAssessor void run(); void stop(); bool isRunning() { return run_; } + void setSimulation(bool simulate); void addObjectPerceptionModule(const std::string& module_name, PerceptionModuleBase_* module); void addHumanPerceptionModule(const std::string& module_name, PerceptionModuleBase_* module); @@ -74,6 +76,7 @@ class SituationAssessor ros::CallbackQueue callback_queue_; ros::ServiceServer start_modules_service_; ros::ServiceServer stop_modules_service_; + ros::ServiceServer set_simulation_service_; ros::ServiceServer bounding_box_service_; ros::ServiceServer agents_list_service_; ros::Publisher new_assessor_publisher_; @@ -112,6 +115,7 @@ class SituationAssessor bool stopModule(BasePerceptionManager& manager, const std::string& module_name, int& status); bool getBoundingBox(overworld::BoundingBox::Request &req, overworld::BoundingBox::Response &res); bool getAgents(overworld::GetAgents::Request &req, overworld::GetAgents::Response &res); + bool setSimulation(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res); }; diff --git a/src/SituationAssessor.cpp b/src/SituationAssessor.cpp index 98a331d5..c6db5a31 100644 --- a/src/SituationAssessor.cpp +++ b/src/SituationAssessor.cpp @@ -33,6 +33,7 @@ SituationAssessor::SituationAssessor(const std::string& agent_name, { new_assessor_publisher_ = n_.advertise("/overworld/new_assessor",5); agents_list_service_ = n_.advertiseService("/overworld/getAgents", &SituationAssessor::getAgents, this); + set_simulation_service_ = n_.advertiseService("/overworld/setSimulation", &SituationAssessor::setSimulation, this); } if (is_robot_) @@ -96,7 +97,7 @@ SituationAssessor::SituationAssessor(const std::string& agent_name, if(is_robot_) { auto msg = std_msgs::String(); - msg.data = "ADD|"+agent_name_; + msg.data = "ADD|" + agent_name_; new_assessor_publisher_.publish(msg); } } @@ -120,6 +121,13 @@ void SituationAssessor::stop() callback_queue_.clear(); } + +void SituationAssessor::setSimulation(bool simulate) +{ + simulate_ = simulate; + perception_manager_.objects_manager_.setSimulation(simulate_); +} + void SituationAssessor::run() { std::thread assessment_thread(&SituationAssessor::assessmentLoop, this); @@ -245,7 +253,7 @@ void SituationAssessor::assess() else { ros_sender_->sendEntitiesToRViz(myself_agent_->getId() + "/objects_markers", objects); - ros_sender_->sendEntitiesToRViz(myself_agent_->getId() + "/humans_markers", body_parts); + ros_sender_->sendEntitiesToRViz(myself_agent_->getId() + "/humans_markers", body_parts); } @@ -321,8 +329,8 @@ void SituationAssessor::updateHumansPerspective(const std::string& human_name, std::transform(areas.cbegin(), areas.cend(), std::back_inserter(seen_areas), [](const auto& it) { return it.second; }); - assessor_it->second.objects_module->sendPerception(seen_objects); assessor_it->second.humans_module->sendPerception(seen_humans); + assessor_it->second.objects_module->sendPerception(seen_objects); assessor_it->second.areas_module->sendPerception(seen_areas); } @@ -407,4 +415,14 @@ bool SituationAssessor::getAgents(overworld::GetAgents::Request &req, overworld: return true; } +bool SituationAssessor::setSimulation(std_srvs::SetBool::Request &req, std_srvs::SetBool::Response &res) +{ + std::cout << "[SituationAssessor] set simulation to " << (int)(req.data) << std::endl; + setSimulation(req.data); + for(auto& assessor : humans_assessors_) + assessor.second.assessor->setSimulation(req.data); + res.success = true; + return true; +} + } \ No newline at end of file From 9f445bb36528a628c2398b6511c66907a6430699 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 25 Oct 2023 11:00:32 +0200 Subject: [PATCH 23/37] [Entity] hsaMoved with time stamp --- include/overworld/BasicTypes/Entity.h | 3 +- src/BasicTypes/Entity.cpp | 42 ++++++++++++++++++++++----- 2 files changed, 36 insertions(+), 9 deletions(-) diff --git a/include/overworld/BasicTypes/Entity.h b/include/overworld/BasicTypes/Entity.h index 199735dc..a1575418 100644 --- a/include/overworld/BasicTypes/Entity.h +++ b/include/overworld/BasicTypes/Entity.h @@ -39,6 +39,7 @@ class Entity const Pose& pose(const ros::Time& stamp) const; ros::Time lastStamp() const { return last_poses_.back().stamp; } bool hasMoved() const; + bool hasMoved(const ros::Time& stamp) const; std::array computeTranslationSpeed() const; @@ -81,7 +82,7 @@ class Entity bool is_true_id_; std::unordered_set false_ids_; - CircularBuffer last_poses_; + CircularBuffer last_poses_; bool is_located_; int bullet_id_; int bullet_link_id_; diff --git a/src/BasicTypes/Entity.cpp b/src/BasicTypes/Entity.cpp index 896fd4a8..daed93ea 100644 --- a/src/BasicTypes/Entity.cpp +++ b/src/BasicTypes/Entity.cpp @@ -59,26 +59,38 @@ const Pose& Entity::pose(unsigned int id) const return last_poses_.at(last_poses_.size() - id - 1).pose; } +// TODO optimize when increase => shortcut const Pose& Entity::pose(const ros::Time& stamp) const { if (!is_located_) throw UnlocatedEntityError(id_); + auto duration_zero = ros::Duration(0); + ros::Duration min_err = stamp - last_poses_.back().stamp; + if(min_err < duration_zero) + min_err = last_poses_.back().stamp - stamp; + + int min_i = -1; - for(size_t i = last_poses_.size() - 2; i >= 0; i--) + for(size_t i = 0; i < last_poses_.size(); i++) { auto delta = stamp - last_poses_.at(i).stamp; + if(delta < duration_zero) + delta = last_poses_.at(i).stamp - stamp; if(delta < min_err) { min_err = delta; - std::cout << "take pose at -" << i << std::endl; + min_i = i; } - else - return last_poses_.at(i + 1).pose; } - return last_poses_.back().pose; + if(min_i < 0) + return last_poses_.back().pose; + else if(min_i) + return last_poses_.at(min_i - 1).pose; //-1 bug + else + return last_poses_.at(min_i).pose; } bool Entity::hasMoved() const @@ -91,17 +103,31 @@ bool Entity::hasMoved() const { return true; } - if (pose().distanceTo(last_poses_.at(last_poses_.size() - 2).pose) > 0.005) // 5mm - { + + if(pose().similarTo(last_poses_.at(last_poses_.size() - 2).pose, 0.001, 0.00349066) == false) // 5mm // 0.2 degree// 0.5degree = 0.00872665 return true; + + return false; +} + +bool Entity::hasMoved(const ros::Time& stamp) const +{ + if (!is_located_) + { + throw UnlocatedEntityError(id_); } - if (pose().angularDistance(last_poses_.at(last_poses_.size() - 2).pose) > 0.0174533) // 1degree + if (last_poses_.size() <= 1) { return true; } + + if(pose().similarTo(pose(stamp), 0.001, 0.00349066) == false) // 5mm // 0.2 degree// 0.5degree = 0.00872665 + return true; + return false; } + std::array Entity::computeTranslationSpeed() const { if (!hasMoved()) From 196dda4828f8197e8a57555cf67321ba37ef61e7 Mon Sep 17 00:00:00 2001 From: sarthou Date: Wed, 25 Oct 2023 11:23:45 +0200 Subject: [PATCH 24/37] [ApproachSender] WIP constraintToTree --- CMakeLists.txt | 9 +++- include/overworld/Senders/ApproachSender.h | 4 ++ src/Senders/ApproachSender.cpp | 52 ++++++++++++++++++++++ src/TestFiles/approach_sender.cpp | 19 ++++++++ 4 files changed, 82 insertions(+), 2 deletions(-) create mode 100644 src/TestFiles/approach_sender.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index f57b1681..e7fa8bc0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -200,7 +200,8 @@ target_link_libraries(${PROJECT_NAME}_sender_lib PUBLIC ${catkin_LIBRARIES} Bullet3 # For b3CameraImage ${CURL_LIBRARIES} - ${PROJECT_NAME}_types_lib) + ${PROJECT_NAME}_types_lib + ${PROJECT_NAME}_perception_lib) ################# # Plugins # @@ -291,4 +292,8 @@ add_dependencies(${PROJECT_NAME}_gravity_test overworld_gencpp) add_executable(${PROJECT_NAME}_area_test src/TestFiles/area.cpp ) target_link_libraries(${PROJECT_NAME}_area_test PRIVATE - ${PROJECT_NAME}_types_lib) \ No newline at end of file + ${PROJECT_NAME}_types_lib) + +add_executable(${PROJECT_NAME}_approach_test src/TestFiles/approach_sender.cpp ) +target_link_libraries(${PROJECT_NAME}_approach_test PRIVATE + ${PROJECT_NAME}_sender_lib) \ No newline at end of file diff --git a/include/overworld/Senders/ApproachSender.h b/include/overworld/Senders/ApproachSender.h index 3650018c..cb0f4475 100644 --- a/include/overworld/Senders/ApproachSender.h +++ b/include/overworld/Senders/ApproachSender.h @@ -41,6 +41,8 @@ class ApproachSender void setRobotName(const std::string& robot_name); + LogicalAlgebraNode constraintToTree(std::string constraint); + private: ros::NodeHandle* n_; std::string robot_name_; @@ -49,6 +51,8 @@ class ApproachSender PerceptionManagers* managers_; onto::OntologyManipulator* onto_; + //LogicalAlgebraNode constraintToTree(std::string constraint); + size_t getIn(size_t begin, std::string& in_text, const std::string& text, char symbol_in, char symbol_out); }; } // namespace owds diff --git a/src/Senders/ApproachSender.cpp b/src/Senders/ApproachSender.cpp index f6659ce9..fc673682 100644 --- a/src/Senders/ApproachSender.cpp +++ b/src/Senders/ApproachSender.cpp @@ -60,4 +60,56 @@ void ApproachSender::setRobotName(const std::string& robot_name) robot_ = managers_->robots_manager_.getAgent(robot_name_); } +LogicalAlgebraNode ApproachSender::constraintToTree(std::string constraint) +{ + std::cout << "in = " << constraint << std::endl; + + std::unordered_map braquet_nodes; + size_t braquet_cpt = 0; + size_t braquet_pose = constraint.find("("); + while(braquet_pose != std::string::npos) + { + std::string in_braquet; + size_t out_pose = getIn(braquet_pose, in_braquet, constraint, '(', ')'); + constraint.replace(braquet_pose, out_pose - braquet_pose + 1, "$" + std::to_string(braquet_cpt)); + braquet_nodes.insert(std::make_pair("$" + std::to_string(braquet_cpt), + constraintToTree(in_braquet))); + braquet_cpt++; + braquet_pose = constraint.find("("); + } + + std::cout << "out = " << constraint << std::endl; + + return LogicalAlgebraNode(logical_none); +} + +size_t ApproachSender::getIn(size_t begin, std::string& in_text, const std::string& text, char symbol_in, char symbol_out) +{ + size_t pose = begin; + + if(text.at(pose) == symbol_in) + { + size_t first_pose = pose; + int cpt = 1; + while((cpt != 0) && (pose+1 < text.length())) + { + ++pose; + if(text.at(pose) == symbol_in) + cpt++; + else if(text.at(pose) == symbol_out) + cpt--; + + } + + in_text = text.substr(first_pose+1, pose-first_pose-1); + + if(cpt == 0) + return pose; + else + return std::string::npos; + } + else + return begin; +} + } // namespace owds \ No newline at end of file diff --git a/src/TestFiles/approach_sender.cpp b/src/TestFiles/approach_sender.cpp new file mode 100644 index 00000000..0e3bf018 --- /dev/null +++ b/src/TestFiles/approach_sender.cpp @@ -0,0 +1,19 @@ +#include "overworld/Senders/ApproachSender.h" + +int main() +{ + owds::ApproachSender sender(nullptr, nullptr); + + sender.constraintToTree("a & b | c"); + std::cout << "-----" << std::endl; + sender.constraintToTree("a & (b | c)"); + std::cout << "-----" << std::endl; + sender.constraintToTree("a & (b | c) & !(d | e)"); + std::cout << "-----" << std::endl; + sender.constraintToTree("a & (b | (c & e))"); + std::cout << "-----" << std::endl; + sender.constraintToTree("a&(b|(c&e))"); + std::cout << "-----" << std::endl; + + return 0; +} \ No newline at end of file From 5366abe29dcefeef62df85f8daabe404e81f2365 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Thu, 26 Oct 2023 15:23:38 +0200 Subject: [PATCH 25/37] [ApproachSender] constraintToTree --- include/overworld/Senders/ApproachSender.h | 13 +++- src/Senders/ApproachSender.cpp | 90 +++++++++++++++++++++- src/TestFiles/approach_sender.cpp | 31 +++++--- 3 files changed, 117 insertions(+), 17 deletions(-) diff --git a/include/overworld/Senders/ApproachSender.h b/include/overworld/Senders/ApproachSender.h index cb0f4475..675abdb4 100644 --- a/include/overworld/Senders/ApproachSender.h +++ b/include/overworld/Senders/ApproachSender.h @@ -19,13 +19,15 @@ enum LogicalAlgebraOperator_e class LogicalAlgebraNode { public: - LogicalAlgebraNode(LogicalAlgebraOperator_e op) : operator_(op) {} + explicit LogicalAlgebraNode(LogicalAlgebraOperator_e op) : operator_(op) {} void insert(const std::string& value) { values_.push_back(value); } void insert(const LogicalAlgebraNode& node) { nodes_.push_back(node); } bool evaluate(const std::string& entity, onto::IndividualClient* client); + void print(size_t level = 0); + private: LogicalAlgebraOperator_e operator_; std::vector values_; @@ -41,9 +43,9 @@ class ApproachSender void setRobotName(const std::string& robot_name); - LogicalAlgebraNode constraintToTree(std::string constraint); - +#ifndef OWDS_TESTS private: +#endif ros::NodeHandle* n_; std::string robot_name_; Agent* robot_; @@ -51,8 +53,11 @@ class ApproachSender PerceptionManagers* managers_; onto::OntologyManipulator* onto_; - //LogicalAlgebraNode constraintToTree(std::string constraint); + LogicalAlgebraNode constraintToTree(std::string constraint); + LogicalAlgebraNode createAndNode(std::string constraint, const std::unordered_map& braquet_nodes); + void fillNode(LogicalAlgebraNode& node, std::string constraint, const std::unordered_map& braquet_nodes); size_t getIn(size_t begin, std::string& in_text, const std::string& text, char symbol_in, char symbol_out); + std::vector split(const std::string& text, const std::string& delim); }; } // namespace owds diff --git a/src/Senders/ApproachSender.cpp b/src/Senders/ApproachSender.cpp index fc673682..db3abbf2 100644 --- a/src/Senders/ApproachSender.cpp +++ b/src/Senders/ApproachSender.cpp @@ -2,6 +2,9 @@ #include "ontologenius/OntologiesManipulator.h" +#include +#include + namespace owds { bool LogicalAlgebraNode::evaluate(const std::string& entity, onto::IndividualClient* client) @@ -42,6 +45,23 @@ bool LogicalAlgebraNode::evaluate(const std::string& entity, const std::string& return (std::find(res.begin(), res.end(), value) != res.end()); } +void LogicalAlgebraNode::print(size_t level) +{ + std::string tabs; + for(size_t i = 0; i < level; i++) + tabs += " "; + if(operator_ == logical_and) + std::cout << tabs << "-AND" << std::endl; + else if(operator_ == logical_or) + std::cout << tabs << "-OR" << std::endl; + else if(operator_ == logical_not) + std::cout << tabs << "-NOT" << std::endl; + for(auto& value : values_) + std::cout << tabs << "-->" << value << std::endl; + for(auto& node : nodes_) + node.print(level+1); +} + ApproachSender::ApproachSender(ros::NodeHandle* n, PerceptionManagers* managers) : n_(n), robot_(nullptr), managers_(managers), @@ -62,6 +82,7 @@ void ApproachSender::setRobotName(const std::string& robot_name) LogicalAlgebraNode ApproachSender::constraintToTree(std::string constraint) { + constraint.erase(std::remove_if(constraint.begin(), constraint.end(), ::isspace),constraint.end()); std::cout << "in = " << constraint << std::endl; std::unordered_map braquet_nodes; @@ -79,8 +100,59 @@ LogicalAlgebraNode ApproachSender::constraintToTree(std::string constraint) } std::cout << "out = " << constraint << std::endl; + auto or_statments = split(constraint, "|"); + if(or_statments.size() > 1) + { + auto or_node = LogicalAlgebraNode(logical_or); + for(auto& statment : or_statments) + { + if(statment.find("&") == std::string::npos) + { + if(statment[0] == '!') + { + auto not_node = LogicalAlgebraNode(logical_not); + fillNode(not_node, statment.substr(1), braquet_nodes); + or_node.insert(not_node); + } + else + fillNode(or_node, statment, braquet_nodes); + } + else + or_node.insert(createAndNode(statment, braquet_nodes)); + } + + return or_node; + } + else + return createAndNode(constraint, braquet_nodes); +} + +LogicalAlgebraNode ApproachSender::createAndNode(std::string constraint, const std::unordered_map& braquet_nodes) +{ + auto and_node = LogicalAlgebraNode(logical_and); + + auto and_statments = split(constraint, "&"); + for(auto& statment : and_statments) + { + if(statment[0] == '!') + { + auto not_node = LogicalAlgebraNode(logical_not); + fillNode(not_node, statment.substr(1), braquet_nodes); + and_node.insert(not_node); + } + else + fillNode(and_node, statment, braquet_nodes); + } + + return and_node; +} - return LogicalAlgebraNode(logical_none); +void ApproachSender::fillNode(LogicalAlgebraNode& node, std::string constraint, const std::unordered_map& braquet_nodes) +{ + if(constraint[0] == '$') + node.insert(braquet_nodes.at(constraint)); + else + node.insert(constraint); } size_t ApproachSender::getIn(size_t begin, std::string& in_text, const std::string& text, char symbol_in, char symbol_out) @@ -112,4 +184,20 @@ size_t ApproachSender::getIn(size_t begin, std::string& in_text, const std::stri return begin; } +std::vector ApproachSender::split(const std::string& text, const std::string& delim) +{ + std::vector res; + std::string tmp_text = text; + while(tmp_text.find(delim) != std::string::npos) + { + size_t pos = tmp_text.find(delim); + std::string part = tmp_text.substr(0, pos); + tmp_text = tmp_text.substr(pos + delim.size(), tmp_text.size() - pos - delim.size()); + if(part != "") + res.emplace_back(part); + } + res.emplace_back(tmp_text); + return res; +} + } // namespace owds \ No newline at end of file diff --git a/src/TestFiles/approach_sender.cpp b/src/TestFiles/approach_sender.cpp index 0e3bf018..d83a7a79 100644 --- a/src/TestFiles/approach_sender.cpp +++ b/src/TestFiles/approach_sender.cpp @@ -1,19 +1,26 @@ +#define OWDS_TESTS + #include "overworld/Senders/ApproachSender.h" int main() { - owds::ApproachSender sender(nullptr, nullptr); + owds::ApproachSender sender(nullptr, nullptr); - sender.constraintToTree("a & b | c"); - std::cout << "-----" << std::endl; - sender.constraintToTree("a & (b | c)"); - std::cout << "-----" << std::endl; - sender.constraintToTree("a & (b | c) & !(d | e)"); - std::cout << "-----" << std::endl; - sender.constraintToTree("a & (b | (c & e))"); - std::cout << "-----" << std::endl; - sender.constraintToTree("a&(b|(c&e))"); - std::cout << "-----" << std::endl; + auto node = sender.constraintToTree("a & b | c"); + node.print(); + std::cout << "-----" << std::endl; + node = sender.constraintToTree("a & (b | c)"); + node.print(); + std::cout << "-----" << std::endl; + node = sender.constraintToTree("a & (b | c) & !(d | e)"); + node.print(); + std::cout << "-----" << std::endl; + node = sender.constraintToTree("a & (b | (c & e))"); + node.print(); + std::cout << "-----" << std::endl; + node = sender.constraintToTree("a&(b|(!c&!e))"); + node.print(); + std::cout << "---END---" << std::endl; - return 0; + return 0; } \ No newline at end of file From 62f74c46d25938d0af069f32fa0e7c3a074cc6cb Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 30 Oct 2023 09:28:06 +0100 Subject: [PATCH 26/37] [ApproachSender] add srv --- include/overworld/Senders/ApproachSender.h | 2 ++ src/Senders/ApproachSender.cpp | 17 +++++++++++++---- 2 files changed, 15 insertions(+), 4 deletions(-) diff --git a/include/overworld/Senders/ApproachSender.h b/include/overworld/Senders/ApproachSender.h index 675abdb4..8101f4d1 100644 --- a/include/overworld/Senders/ApproachSender.h +++ b/include/overworld/Senders/ApproachSender.h @@ -52,6 +52,8 @@ class ApproachSender PerceptionManagers* managers_; onto::OntologyManipulator* onto_; + + ros::ServiceServer get_pose_service_; LogicalAlgebraNode constraintToTree(std::string constraint); LogicalAlgebraNode createAndNode(std::string constraint, const std::unordered_map& braquet_nodes); diff --git a/src/Senders/ApproachSender.cpp b/src/Senders/ApproachSender.cpp index db3abbf2..dacca220 100644 --- a/src/Senders/ApproachSender.cpp +++ b/src/Senders/ApproachSender.cpp @@ -1,4 +1,5 @@ #include "overworld/Senders/ApproachSender.h" +#include "overworld/GetApproachPoint.h" #include "ontologenius/OntologiesManipulator.h" @@ -35,7 +36,7 @@ bool LogicalAlgebraNode::evaluate(const std::string& entity, onto::IndividualCli else return false; default: - return false; + return true; } } @@ -66,7 +67,9 @@ ApproachSender::ApproachSender(ros::NodeHandle* n, PerceptionManagers* managers) robot_(nullptr), managers_(managers), onto_(nullptr) -{} +{ + get_pose_service_ = n_->advertiseService("/overworld/getApproachPose", &ApproachSender::onGetApproachPoint, this); +} void ApproachSender::setRobotName(const std::string& robot_name) { @@ -80,10 +83,17 @@ void ApproachSender::setRobotName(const std::string& robot_name) robot_ = managers_->robots_manager_.getAgent(robot_name_); } +bool PoseSender::onGetApproachPoint(overworld::GetApproachPoint::Request& req, overworld::GetApproachPoint::Response& res) +{ + auto constraint = LogicalAlgebraNode(logical_none); + if(req.area_constraints) + constraint = constraintToTree(req.area_constraints); + return true; +} + LogicalAlgebraNode ApproachSender::constraintToTree(std::string constraint) { constraint.erase(std::remove_if(constraint.begin(), constraint.end(), ::isspace),constraint.end()); - std::cout << "in = " << constraint << std::endl; std::unordered_map braquet_nodes; size_t braquet_cpt = 0; @@ -99,7 +109,6 @@ LogicalAlgebraNode ApproachSender::constraintToTree(std::string constraint) braquet_pose = constraint.find("("); } - std::cout << "out = " << constraint << std::endl; auto or_statments = split(constraint, "|"); if(or_statments.size() > 1) { From 17a4e9cdf8850fdbfe2b13e1f635c2ff8a1f63af Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 30 Oct 2023 09:51:59 +0100 Subject: [PATCH 27/37] [Area] isInside with pose --- include/overworld/BasicTypes/Area.h | 1 + src/BasicTypes/Area.cpp | 23 +++++++++++++++++++++++ 2 files changed, 24 insertions(+) diff --git a/include/overworld/BasicTypes/Area.h b/include/overworld/BasicTypes/Area.h index b64f56e0..2678e01e 100644 --- a/include/overworld/BasicTypes/Area.h +++ b/include/overworld/BasicTypes/Area.h @@ -38,6 +38,7 @@ class Area double getHalfHeight() const { return half_height_; } const Pose& getCenterPose() const { return center_; } + bool isInside(const Pose& pose, bool hysteresis = true); bool isInside(Entity* entity); bool setOut(Entity* entity); void clearInsideEntities(); diff --git a/src/BasicTypes/Area.cpp b/src/BasicTypes/Area.cpp index 82b0107d..0920f3e1 100644 --- a/src/BasicTypes/Area.cpp +++ b/src/BasicTypes/Area.cpp @@ -38,6 +38,29 @@ bool Area::isEmpty() const return (nb_entity == 0); } +bool Area::isInside(const Pose& pose, bool hysteresis) +{ + if(hysteresis) + { + Entity entity_wrapper("plop"); + entity_wrapper.updatePose(pose); + return isInside(&entity_wrapper); + } + else + { + if(is_circle_) + { + double entity_distance = pose.distanceTo(center_); + return (entity_distance <= radius_); + } + else + { + point_t entity_point(pose.getX(), pose.getY()); + return polygon_.isInside(entity_point); + } + } +} + bool Area::isInside(Entity* entity) { if(is_circle_) From b76561f2964517aec880c5e3cf0927b590f8f9bf Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 30 Oct 2023 09:52:15 +0100 Subject: [PATCH 28/37] [AreasPerceptionManager] getArea --- .../Perception/Managers/AreasPerceptionManager.h | 1 + src/Perception/Managers/AreasPerceptionManager.cpp | 9 +++++++++ 2 files changed, 10 insertions(+) diff --git a/include/overworld/Perception/Managers/AreasPerceptionManager.h b/include/overworld/Perception/Managers/AreasPerceptionManager.h index 7ce937dc..38c8a815 100644 --- a/include/overworld/Perception/Managers/AreasPerceptionManager.h +++ b/include/overworld/Perception/Managers/AreasPerceptionManager.h @@ -28,6 +28,7 @@ class AreasPerceptionManager : public BasePerceptionManager void registerBodyPartsManager(EntitiesPerceptionManager* manager) { bodyparts_managers_.insert(manager); } const std::map& getEntities() const { return areas_; } + Area* getArea(const std::string& area_id); bool update(); diff --git a/src/Perception/Managers/AreasPerceptionManager.cpp b/src/Perception/Managers/AreasPerceptionManager.cpp index 046aebd1..a3ae4a47 100644 --- a/src/Perception/Managers/AreasPerceptionManager.cpp +++ b/src/Perception/Managers/AreasPerceptionManager.cpp @@ -11,6 +11,15 @@ AreasPerceptionManager::~AreasPerceptionManager() areas_.clear(); } +Area* AreasPerceptionManager::getArea(const std::string& area_id) +{ + auto it = areas_.find(area_id); + if(it != areas_.end()) + return it->second; + else + return nullptr; +} + bool AreasPerceptionManager::update() { if((pending_percepts_.size() == 0) && !this->shouldRun()) From 81eb1efe1a3ad3e87becf217a29e856c9ca31799 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 30 Oct 2023 09:52:56 +0100 Subject: [PATCH 29/37] [LogicalAlgebraNode] evaluate nodes with pose --- include/overworld/Senders/ApproachSender.h | 4 ++-- src/Senders/ApproachSender.cpp | 23 ++++++++++++---------- 2 files changed, 15 insertions(+), 12 deletions(-) diff --git a/include/overworld/Senders/ApproachSender.h b/include/overworld/Senders/ApproachSender.h index 8101f4d1..81231f73 100644 --- a/include/overworld/Senders/ApproachSender.h +++ b/include/overworld/Senders/ApproachSender.h @@ -24,7 +24,7 @@ class LogicalAlgebraNode void insert(const std::string& value) { values_.push_back(value); } void insert(const LogicalAlgebraNode& node) { nodes_.push_back(node); } - bool evaluate(const std::string& entity, onto::IndividualClient* client); + bool evaluate(const Pose& pose, AreasPerceptionManager* manager); void print(size_t level = 0); @@ -33,7 +33,7 @@ class LogicalAlgebraNode std::vector values_; std::vector nodes_; - bool evaluate(const std::string& entity, const std::string& value, onto::IndividualClient* client); + bool evaluate(const Pose& pose, const std::string& value, AreasPerceptionManager* manager); }; class ApproachSender diff --git a/src/Senders/ApproachSender.cpp b/src/Senders/ApproachSender.cpp index dacca220..eb313bd7 100644 --- a/src/Senders/ApproachSender.cpp +++ b/src/Senders/ApproachSender.cpp @@ -8,31 +8,31 @@ namespace owds { -bool LogicalAlgebraNode::evaluate(const std::string& entity, onto::IndividualClient* client) +bool LogicalAlgebraNode::evaluate(const Pose& pose, AreasPerceptionManager* manager) { switch (operator_) { case logical_and: for(auto& value : values_) - if(!evaluate(entity, value, client)) + if(!evaluate(pose, value, manager)) return false; for(auto& node : nodes_) - if(!node.evaluate(entity, client)) + if(!node.evaluate(pose, manager)) return false; return true; case logical_or: for(auto& value : values_) - if(evaluate(entity, value, client)) + if(evaluate(pose, value, manager)) return true; for(auto& node : nodes_) - if(node.evaluate(entity, client)) + if(node.evaluate(pose, manager)) return true; return false; case logical_not: if(values_.size()) - return !evaluate(entity, values_.front(), client); + return !evaluate(pose, values_.front(), manager); else if(nodes_.size()) - return !nodes_.front().evaluate(entity, client); + return !nodes_.front().evaluate(pose, manager); else return false; default: @@ -40,10 +40,13 @@ bool LogicalAlgebraNode::evaluate(const std::string& entity, onto::IndividualCli } } -bool LogicalAlgebraNode::evaluate(const std::string& entity, const std::string& value, onto::IndividualClient* client) +bool LogicalAlgebraNode::evaluate(const Pose& pose, const std::string& value, AreasPerceptionManager* manager) { - auto res = client->getOn(entity, "isInArea"); - return (std::find(res.begin(), res.end(), value) != res.end()); + Area* area = manager->getArea(value); + if(area == nullptr) + return false; + + return area->isInside(pose); } void LogicalAlgebraNode::print(size_t level) From d15d1b8183b6981c1c280e094f11587507e7e156 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 30 Oct 2023 09:53:11 +0100 Subject: [PATCH 30/37] [ApproachSender] fix service --- include/overworld/Senders/ApproachSender.h | 4 ++++ src/Senders/ApproachSender.cpp | 5 ++--- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/include/overworld/Senders/ApproachSender.h b/include/overworld/Senders/ApproachSender.h index 81231f73..c03c36d1 100644 --- a/include/overworld/Senders/ApproachSender.h +++ b/include/overworld/Senders/ApproachSender.h @@ -6,6 +6,8 @@ #include "overworld/Perception/PerceptionManagers.h" #include "overworld/BasicTypes/Agent.h" +#include "overworld/GetApproachPoint.h" + namespace owds { enum LogicalAlgebraOperator_e @@ -54,6 +56,8 @@ class ApproachSender onto::OntologyManipulator* onto_; ros::ServiceServer get_pose_service_; + + bool onGetApproachPoint(overworld::GetApproachPoint::Request& req, overworld::GetApproachPoint::Response& res); LogicalAlgebraNode constraintToTree(std::string constraint); LogicalAlgebraNode createAndNode(std::string constraint, const std::unordered_map& braquet_nodes); diff --git a/src/Senders/ApproachSender.cpp b/src/Senders/ApproachSender.cpp index eb313bd7..0f9cd7f3 100644 --- a/src/Senders/ApproachSender.cpp +++ b/src/Senders/ApproachSender.cpp @@ -1,5 +1,4 @@ #include "overworld/Senders/ApproachSender.h" -#include "overworld/GetApproachPoint.h" #include "ontologenius/OntologiesManipulator.h" @@ -86,10 +85,10 @@ void ApproachSender::setRobotName(const std::string& robot_name) robot_ = managers_->robots_manager_.getAgent(robot_name_); } -bool PoseSender::onGetApproachPoint(overworld::GetApproachPoint::Request& req, overworld::GetApproachPoint::Response& res) +bool ApproachSender::onGetApproachPoint(overworld::GetApproachPoint::Request& req, overworld::GetApproachPoint::Response& res) { auto constraint = LogicalAlgebraNode(logical_none); - if(req.area_constraints) + if(req.area_constraints != "") constraint = constraintToTree(req.area_constraints); return true; } From 17ed2f129b8f1692559e5d897d258f5eaf418ef8 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 11 Dec 2023 11:51:54 +0100 Subject: [PATCH 31/37] [ReasonerEgocentric] add ros include --- include/overworld/OntologeniusPlugins/ReasonerEgocentric.h | 2 ++ 1 file changed, 2 insertions(+) diff --git a/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h b/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h index e7ebb515..c8185545 100644 --- a/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h +++ b/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h @@ -5,6 +5,8 @@ #include "overworld/GetRelations.h" +#include + namespace ontologenius { class ReasonerEgocentric : public ReasonerInterface From 1e872eac9caa9acaa9402900ff01bf0b13c53d90 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 11 Dec 2023 11:56:58 +0100 Subject: [PATCH 32/37] [ApproachSender] comment WIP functions --- src/Senders/ApproachSender.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/Senders/ApproachSender.cpp b/src/Senders/ApproachSender.cpp index dacca220..73f02cc3 100644 --- a/src/Senders/ApproachSender.cpp +++ b/src/Senders/ApproachSender.cpp @@ -68,7 +68,7 @@ ApproachSender::ApproachSender(ros::NodeHandle* n, PerceptionManagers* managers) managers_(managers), onto_(nullptr) { - get_pose_service_ = n_->advertiseService("/overworld/getApproachPose", &ApproachSender::onGetApproachPoint, this); + //get_pose_service_ = n_->advertiseService("/overworld/getApproachPose", &ApproachSender::onGetApproachPoint, this); } void ApproachSender::setRobotName(const std::string& robot_name) @@ -83,13 +83,13 @@ void ApproachSender::setRobotName(const std::string& robot_name) robot_ = managers_->robots_manager_.getAgent(robot_name_); } -bool PoseSender::onGetApproachPoint(overworld::GetApproachPoint::Request& req, overworld::GetApproachPoint::Response& res) +/*bool PoseSender::onGetApproachPoint(overworld::GetApproachPoint::Request& req, overworld::GetApproachPoint::Response& res) { auto constraint = LogicalAlgebraNode(logical_none); if(req.area_constraints) constraint = constraintToTree(req.area_constraints); return true; -} +}*/ LogicalAlgebraNode ApproachSender::constraintToTree(std::string constraint) { From 9c9354de30688763876a35c7f250f86d1f046044 Mon Sep 17 00:00:00 2001 From: Adrien Vigne Date: Fri, 19 Jan 2024 14:44:32 +0100 Subject: [PATCH 33/37] [AgentsPose] add AgentsPose msg and module to publish simultaneous multiple fake agent pose --- CMakeLists.txt | 2 + .../FakeHumansPerceptionModule.h | 38 ++++++ modules_plugins.xml | 9 ++ msg/AgentsPose.msg | 1 + .../FakeHumansPerceptionModule.cpp | 109 ++++++++++++++++++ 5 files changed, 159 insertions(+) create mode 100644 include/overworld/Perception/Modules/HumansModules/FakeHumansPerceptionModule.h create mode 100644 msg/AgentsPose.msg create mode 100644 src/Perception/Modules/HumansModules/FakeHumansPerceptionModule.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index e7fa8bc0..7052358e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -68,6 +68,7 @@ add_message_files( EntityPose.msg EntitiesPoses.msg AgentPose.msg + AgentsPose.msg Pose.msg ) @@ -213,6 +214,7 @@ add_library(overworld_modules_plugin MODULE src/Perception/Modules/RobotsModules/JointStatePerceptionModule.cpp src/Perception/Modules/HumansModules/StampedPosePerceptionModule.cpp src/Perception/Modules/HumansModules/FakeHumanPerceptionModule.cpp + src/Perception/Modules/HumansModules/FakeHumansPerceptionModule.cpp src/Perception/Modules/AreasModules/ObjAreasPerceptionModule.cpp ) target_link_libraries(overworld_modules_plugin PUBLIC diff --git a/include/overworld/Perception/Modules/HumansModules/FakeHumansPerceptionModule.h b/include/overworld/Perception/Modules/HumansModules/FakeHumansPerceptionModule.h new file mode 100644 index 00000000..40c01bde --- /dev/null +++ b/include/overworld/Perception/Modules/HumansModules/FakeHumansPerceptionModule.h @@ -0,0 +1,38 @@ +#ifndef OWDS_FAKEHUMANPERCEPTIONMODULE_H +#define OWDS_FAKEHUMANPERCEPTIONMODULE_H + +#include "overworld/BasicTypes/BodyPart.h" +#include "overworld/Perception/Modules/PerceptionModuleBase.h" + +#include "overworld/AgentsPose.h" + +#include "ontologenius/OntologiesManipulator.h" + +#include +#include + +namespace owds { + + +class FakeHumansPerceptionModule : public PerceptionModuleRosBase +{ + public: + FakeHumansPerceptionModule(); + + virtual bool closeInitialization() override; + + private: + bool perceptionCallback(const overworld::AgentsPose& msg); + + BodyPart createBodyPart(const std::string& human_name, const std::string& part_name); + + onto::OntologiesManipulator* ontologies_manipulator_; + onto::OntologyManipulator* onto_; + + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf2_listener_; +}; + +} // namespace owds + +#endif // OWDS_FAKEHUMANPERCEPTIONMODULE_H diff --git a/modules_plugins.xml b/modules_plugins.xml index 71f969d4..5ef0fb62 100644 --- a/modules_plugins.xml +++ b/modules_plugins.xml @@ -15,6 +15,7 @@ The module subscribe to "/peopleTrack". Remap your topic if needed. + A human perception module providing fake body parts from a ROS topic. @@ -22,12 +23,20 @@ + + + A humans perception module providing fake body parts from a ROS topic. + Does not take any parameter. + + + An object perception module providing static objects. This module requires a 'file' parameter being a yaml description of the assets to be loaded. + An object perception module providing fake objects from a ROS topic. diff --git a/msg/AgentsPose.msg b/msg/AgentsPose.msg new file mode 100644 index 00000000..81977545 --- /dev/null +++ b/msg/AgentsPose.msg @@ -0,0 +1 @@ +AgentPose[] agents \ No newline at end of file diff --git a/src/Perception/Modules/HumansModules/FakeHumansPerceptionModule.cpp b/src/Perception/Modules/HumansModules/FakeHumansPerceptionModule.cpp new file mode 100644 index 00000000..672bd1a7 --- /dev/null +++ b/src/Perception/Modules/HumansModules/FakeHumansPerceptionModule.cpp @@ -0,0 +1,109 @@ +#include "overworld/Perception/Modules/HumansModules/FakeHumansPerceptionModule.h" + +#include "overworld/Utility/ShellDisplay.h" + +#include + +namespace owds { + +FakeHumansPerceptionModule::FakeHumansPerceptionModule() : PerceptionModuleRosBase("/overworld/fake_humans_poses"), + ontologies_manipulator_(nullptr), + onto_(nullptr), + tf2_listener_(tf_buffer_) +{} + +bool FakeHumansPerceptionModule::closeInitialization() +{ + ontologies_manipulator_ = new onto::OntologiesManipulator(); + ontologies_manipulator_->waitInit(); + ontologies_manipulator_->add(robot_agent_->getId()); + onto_ = ontologies_manipulator_->get(robot_agent_->getId()); + onto_->close(); + + return true; +} + +bool FakeHumansPerceptionModule::perceptionCallback(const overworld::AgentsPose& msg) +{ + if (msg.agents.size() == 0) + return false; + for(auto& agent : msg.agents) + for(auto& part : agent.parts) + { + auto it_percept = percepts_.find(part.id); + if(it_percept == percepts_.end()) + it_percept = percepts_.insert(std::make_pair(part.id, createBodyPart(agent.id, part.id))).first; + + std::string frame_id = part.pose.header.frame_id; + if (frame_id[0] == '/') + frame_id = frame_id.substr(1); + + try { + geometry_msgs::PoseStamped part_in_map; + if(part.pose.header.frame_id != "/map") + { + geometry_msgs::TransformStamped to_map = tf_buffer_.lookupTransform("map", frame_id, part.pose.header.stamp, ros::Duration(1.0)); + tf2::doTransform(part.pose, part_in_map, to_map); + } + else + part_in_map = part.pose; + it_percept->second.updatePose(part_in_map); + } + catch (const tf2::TransformException& ex) { + ShellDisplay::error("[FakeHumanPerceptionModule]" + std::string(ex.what())); + return false; + } + } + + return true; +} + +BodyPart FakeHumansPerceptionModule::createBodyPart(const std::string& human_name, const std::string& part_name) +{ + BodyPartType_e part_type = BodyPartType_e::BODY_PART_UNKNOW; + + auto types = onto_->individuals.getUp(part_name); + if(std::find(types.begin(), types.end(), "Head") != types.end()) + part_type = BodyPartType_e::BODY_PART_HEAD; + else if(std::find(types.begin(), types.end(), "LeftHand") != types.end()) + part_type = BodyPartType_e::BODY_PART_LEFT_HAND; + else if(std::find(types.begin(), types.end(), "RightHand") != types.end()) + part_type = BodyPartType_e::BODY_PART_RIGHT_HAND; + else if(std::find(types.begin(), types.end(), "Base") != types.end()) + part_type = BodyPartType_e::BODY_PART_BASE; + else if(std::find(types.begin(), types.end(), "Torso") != types.end()) + part_type = BodyPartType_e::BODY_PART_TORSO; + + BodyPart part(part_name); + part.setAgentName(human_name); + part.setType(part_type); + + Shape_t shape = ontology::getEntityShape(onto_, part_name); + if(shape.type == SHAPE_NONE) + { + if(part_type == BodyPartType_e::BODY_PART_HEAD) + { + shape.type = SHAPE_SPEHERE; + shape.scale = {0.12, 0.15, 0.2}; + } + else if((part_type == BodyPartType_e::BODY_PART_LEFT_HAND) || (part_type == BodyPartType_e::BODY_PART_RIGHT_HAND)) + { + shape.type = SHAPE_CUBE; + shape.scale = {0.10, 0.03, 0.18}; + } + else if((part_type == BodyPartType_e::BODY_PART_TORSO) || (part_type == BodyPartType_e::BODY_PART_BASE)) + { + shape.type = SHAPE_CUBE; + shape.scale = {0.5, 0.2, 0.85}; + } + + shape.color = ontology::getEntityColor(onto_, part_name, {0.976470588, 0.894117647, 0.717647059}); + } + part.setShape(shape); + + return part; +} + +} // namespace owds + +PLUGINLIB_EXPORT_CLASS(owds::FakeHumansPerceptionModule, owds::PerceptionModuleBase_) From d4c0ab0004515f200500c8ddbd7fefa2db2053eb Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Fri, 23 Feb 2024 15:23:36 +0100 Subject: [PATCH 34/37] [ReasonerEgocentric] put under overworld namespace --- .../OntologeniusPlugins/ReasonerEgocentric.h | 20 +++++++++---------- reasoners_plugins.xml | 2 +- .../ReasonerEgocentric.cpp | 10 ++++++---- 3 files changed, 17 insertions(+), 15 deletions(-) diff --git a/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h b/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h index c8185545..f9690bee 100644 --- a/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h +++ b/include/overworld/OntologeniusPlugins/ReasonerEgocentric.h @@ -7,9 +7,9 @@ #include -namespace ontologenius { +namespace overworld { -class ReasonerEgocentric : public ReasonerInterface +class ReasonerEgocentric : public ontologenius::ReasonerInterface { public: ReasonerEgocentric() {} @@ -17,26 +17,26 @@ class ReasonerEgocentric : public ReasonerInterface virtual void initialize() override; - virtual bool preReason(const QueryInfo_t& query_info) override; + virtual bool preReason(const ontologenius::QueryInfo_t& query_info) override; virtual bool implementPreReasoning() override { return true; } virtual std::string getName() override; - virtual std::string getDesciption() override; + virtual std::string getDescription() override; private: - std::unordered_set computable_properties_; + std::unordered_set computable_properties_; ros::NodeHandle n_; ros::ServiceClient overworld_client_; - ObjectPropertyBranch_t* isComputableProperty(const std::string& property); - std::set isInRange(const std::string& indiv, ObjectPropertyBranch_t* property); - std::set isInDomain(const std::string& indiv, ObjectPropertyBranch_t* property); + ontologenius::ObjectPropertyBranch_t* isComputableProperty(const std::string& property); + std::set isInRange(const std::string& indiv, ontologenius::ObjectPropertyBranch_t* property); + std::set isInDomain(const std::string& indiv, ontologenius::ObjectPropertyBranch_t* property); - overworld::GetRelations createRequest(const std::string& subject, const std::set& predicates, const std::string& object); + overworld::GetRelations createRequest(const std::string& subject, const std::set& predicates, const std::string& object); bool call(overworld::GetRelations& srv); void updateOntology(const std::vector& to_add, const std::vector& to_delete); }; -} // namespace ontologenius +} // namespace overworld #endif // OVERWORLD_REASONEREGOCENTRIC_H diff --git a/reasoners_plugins.xml b/reasoners_plugins.xml index 3dec6fd8..12771a0e 100644 --- a/reasoners_plugins.xml +++ b/reasoners_plugins.xml @@ -1,5 +1,5 @@ - + This reasoner is provided by Overworld. It computes egocentric relations on query demand. diff --git a/src/OntologeniusPlugins/ReasonerEgocentric.cpp b/src/OntologeniusPlugins/ReasonerEgocentric.cpp index 81341386..63784e45 100644 --- a/src/OntologeniusPlugins/ReasonerEgocentric.cpp +++ b/src/OntologeniusPlugins/ReasonerEgocentric.cpp @@ -5,7 +5,9 @@ #include -namespace ontologenius { +namespace overworld { + +using namespace ontologenius; void ReasonerEgocentric::initialize() { @@ -85,7 +87,7 @@ std::string ReasonerEgocentric::getName() return "reasoner egocentric"; } -std::string ReasonerEgocentric::getDesciption() +std::string ReasonerEgocentric::getDescription() { return "This reasoner is provided by Overworld. It computes egocentric relations on query demand."; } @@ -233,6 +235,6 @@ void ReasonerEgocentric::updateOntology(const std::vector& t } } -PLUGINLIB_EXPORT_CLASS(ReasonerEgocentric, ReasonerInterface) +} // namespace overworld -} // namespace ontologenius +PLUGINLIB_EXPORT_CLASS(overworld::ReasonerEgocentric, ontologenius::ReasonerInterface) \ No newline at end of file From a9f1570f84c32f8870f786507661bde645917fd2 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Fri, 23 Feb 2024 15:31:41 +0100 Subject: [PATCH 35/37] [Readme] fix build badge --- Readme.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Readme.md b/Readme.md index 319cb7cf..c8c13c77 100644 --- a/Readme.md +++ b/Readme.md @@ -79,5 +79,5 @@ More information about this link and tutorials to show its use will come soon. [Bullet-Dependency-Image]: https://img.shields.io/badge/dependencies-bullet3-yellow [Bullet-Dependency-Url]: https://github.com/bulletphysics/bullet3 -[Build-Status-Image]: https://github.com/sarthou/overworld/actions/workflows/main.yml/badge.svg +[Build-Status-Image]: https://github.com/sarthou/overworld/actions/workflows/overworld.yml/badge.svg [Build-Status-Url]: https://github.com/sarthou/overworld/actions \ No newline at end of file From 86469478f47c832d9add22aa1071aadbbe39a294 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Fri, 23 Feb 2024 15:42:38 +0100 Subject: [PATCH 36/37] set to 0.1.5 --- Readme.md | 4 ++-- docs/AvailableModules.html | 2 +- docs/Download.html | 27 ++++++++++++++++++++++++++- docs/InstallOverworld.html | 2 +- docs/Publications.html | 2 +- docs/Tutorials/Tutorials.html | 2 +- docs/index.html | 6 +++--- docs/overview/Configuration.html | 2 +- docs/overview/Plugins.html | 2 +- docs/overview/bulletGUI.html | 2 +- docs/overview/launchers.html | 2 +- package.xml | 2 +- 12 files changed, 40 insertions(+), 15 deletions(-) diff --git a/Readme.md b/Readme.md index c8c13c77..296a4b83 100644 --- a/Readme.md +++ b/Readme.md @@ -71,8 +71,8 @@ The knowledge base Ontologenius can be used as a source of information for Overw More information about this link and tutorials to show its use will come soon. -[Release-Url]: https://github.com/sarthou/overworld/releases/tag/v0.1.4 -[Release-Image]: http://img.shields.io/badge/release-v0.1.4-blue +[Release-Url]: https://github.com/sarthou/overworld/releases/tag/v0.1.5 +[Release-Image]: http://img.shields.io/badge/release-v0.1.5-blue [Ontologenius-Dependency-Image]: https://img.shields.io/badge/dependencies-ontologenius-yellow [Ontologenius-Dependency-Url]: https://github.com/sarthou/ontologenius diff --git a/docs/AvailableModules.html b/docs/AvailableModules.html index d7930b2b..0ace4088 100644 --- a/docs/AvailableModules.html +++ b/docs/AvailableModules.html @@ -1,7 +1,7 @@ - Available Modules | Overworld 0.1.4 + Available Modules | Overworld 0.1.5 diff --git a/docs/Download.html b/docs/Download.html index 75c8dc83..80be1fbb 100644 --- a/docs/Download.html +++ b/docs/Download.html @@ -1,7 +1,7 @@ - Download | Overworld 0.1.4 + Download | Overworld 0.1.5 @@ -88,6 +88,31 @@

Download

You have below the archives of the major versions of Overworld:

+

V0.1.5

+

23 / 02 / 2024

+ + +

V0.1.4

23 / 05 / 2023

diff --git a/docs/InstallOverworld.html b/docs/InstallOverworld.html index d69cd26a..43fdc873 100644 --- a/docs/InstallOverworld.html +++ b/docs/InstallOverworld.html @@ -1,7 +1,7 @@ - Install | Overworld 0.1.4 + Install | Overworld 0.1.5 diff --git a/docs/Publications.html b/docs/Publications.html index 235dfdea..4735e799 100644 --- a/docs/Publications.html +++ b/docs/Publications.html @@ -1,7 +1,7 @@ - Publications | Overworld 0.1.4 + Publications | Overworld 0.1.5 diff --git a/docs/Tutorials/Tutorials.html b/docs/Tutorials/Tutorials.html index e0b0525e..864234d5 100644 --- a/docs/Tutorials/Tutorials.html +++ b/docs/Tutorials/Tutorials.html @@ -1,7 +1,7 @@ - Tutorials | Overworld 0.1.4 + Tutorials | Overworld 0.1.5 diff --git a/docs/index.html b/docs/index.html index dcf6ab27..6fa06849 100644 --- a/docs/index.html +++ b/docs/index.html @@ -1,7 +1,7 @@ - Overworld 0.1.4 + Overworld 0.1.5 @@ -96,7 +96,7 @@

Assessing the geometry of the world for Human-Robot Interaction

- Download (0.1.4) + Download (0.1.5) github View on Github

@@ -158,7 +158,7 @@

What's New

Define areas

-

From Overworld 0.1.4 you can now define areas in the form of polygons and cylinders.

+

From Overworld 0.1.5 you can now define areas in the form of polygons and cylinders.

Static or attached to other entities, Overworld will allow you to know the entities in them with a newly introduced semantic fact!

Being implemented using perception modules you can adapt it to your need and even perceive them dynamically.

diff --git a/docs/overview/Configuration.html b/docs/overview/Configuration.html index 6f3714b7..d1ffd5df 100644 --- a/docs/overview/Configuration.html +++ b/docs/overview/Configuration.html @@ -1,7 +1,7 @@ - Configuration | Overworld 0.1.4 + Configuration | Overworld 0.1.5 diff --git a/docs/overview/Plugins.html b/docs/overview/Plugins.html index 1ac5ca6e..fe3297fa 100644 --- a/docs/overview/Plugins.html +++ b/docs/overview/Plugins.html @@ -1,7 +1,7 @@ - Plugins | Overworld 0.1.4 + Plugins | Overworld 0.1.5 diff --git a/docs/overview/bulletGUI.html b/docs/overview/bulletGUI.html index 50ee02a8..4898ca2a 100644 --- a/docs/overview/bulletGUI.html +++ b/docs/overview/bulletGUI.html @@ -1,7 +1,7 @@ - Bullet GUI | Overworld 0.1.4 + Bullet GUI | Overworld 0.1.5 diff --git a/docs/overview/launchers.html b/docs/overview/launchers.html index f9a32347..35131ab7 100644 --- a/docs/overview/launchers.html +++ b/docs/overview/launchers.html @@ -1,7 +1,7 @@ - Launchers | Overworld 0.1.4 + Launchers | Overworld 0.1.5 diff --git a/package.xml b/package.xml index 7e81d1c7..8972afb7 100644 --- a/package.xml +++ b/package.xml @@ -1,7 +1,7 @@ overworld - 0.1.4 + 0.1.5 The overworld package Guillaume Sarthou From 47942ac9a5d13ef01b81c2e7f87ea83618b84082 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Fri, 23 Feb 2024 15:42:53 +0100 Subject: [PATCH 37/37] requires ontologenius 0.4.1 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index 7052358e..0f025760 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -27,7 +27,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge pluginlib ) -find_package(ontologenius 0.3.0 REQUIRED) +find_package(ontologenius 0.4.1 REQUIRED) find_package(Eigen3 REQUIRED NO_MODULE) find_package(Threads REQUIRED) find_package(CURL REQUIRED)