From bff98bfefdba363368a821506041b23d014d24c8 Mon Sep 17 00:00:00 2001 From: Guillaume Sarthou Date: Mon, 26 Jun 2023 16:54:12 +0200 Subject: [PATCH 01/17] [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/17] [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/17] [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/17] 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/17] [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/17] [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/17] [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/17] [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/17] [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/17] [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/17] [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/17] [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/17] [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/17] [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/17] [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/17] [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/17] [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)