Merge branch 'egocentric-facts' into dev

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)
@@ -75,6 +75,7 @@ add_service_files(
   FILES
   StartStopModules.srv
   BoundingBox.srv
+  GetAgents.srv
   GetPose.srv
   GetRelations.srv
 )
@@ -258,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

include/overworld/BasicTypes/FieldOfView.h

@@ -17,14 +17,18 @@ 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_; }
     double getClipNear() const { return clip_near_; }
     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 
@@ -33,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
@@ -52,6 +56,7 @@ class FieldOfView
     double width_; // degrees
     double clip_near_; // meters
     double clip_far_; // meters
+    double opengl_ratio_;
 };
 
 } // namespace owds

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;

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; }
 

include/overworld/Perception/Managers/AgentPerceptionManager.h

@@ -21,7 +21,7 @@ class AgentPerceptionManager : public EntitiesPerceptionManager<BodyPart>
   Agent* getAgent(const std::string& agent_name, AgentType_e type);
 
   std::map<std::string, Agent*>::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<std::string>& vect, double default_value);
 };

include/overworld/Perception/Managers/EntitiesPerceptionManager.h

@@ -23,7 +23,7 @@ class EntitiesPerceptionManager : public BasePerceptionManager<T>
     static_assert(std::is_base_of<Entity,T>::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<T>
     BulletClient* bullet_client_;
 
     std::string myself_agent_name_;
-    OntologiesManipulator ontos_;
-    OntologyManipulator* onto_;
+    onto::OntologiesManipulator ontos_;
+    onto::OntologyManipulator* onto_;
 
     virtual void getPercepts( std::map<std::string, T>& percepts);
     virtual void reasoningOnUpdate() {}

include/overworld/Perception/Managers/ObjectsPerceptionManager.h

@@ -45,7 +45,7 @@ class ObjectsPerceptionManager : public EntitiesPerceptionManager<Object>
   void stopSimulation(Object* object, bool erase = true);
 
   std::vector<PointOfInterest> getPoisInFov(Object* object);
-  bool isObjectsInFovAabb(std::vector<Object*> objects);
+  std::vector<Object*> isObjectsInFovAabb(std::vector<Object*> objects);
   bool shouldBeSeen(Object* object, const std::vector<PointOfInterest>& pois);
   std::unordered_set<int> getObjectsInCamera();
 

include/overworld/Perception/Modules/HumansModules/FakeHumanPerceptionModule.h

@@ -26,8 +26,8 @@ class FakeHumanPerceptionModule : public PerceptionModuleRosBase<BodyPart, overw
 
     BodyPart createBodyPart(const std::string& human_name, const std::string& part_name);
 
-    OntologiesManipulator* ontologies_manipulator_;
-    OntologyManipulator* onto_;
+    onto::OntologiesManipulator* ontologies_manipulator_;
+    onto::OntologyManipulator* onto_;
 
     tf2_ros::Buffer tf_buffer_;
     tf2_ros::TransformListener tf2_listener_;

include/overworld/Perception/Modules/HumansModules/StampedPosePerceptionModule.h

@@ -22,8 +22,8 @@ class StampedPosePerceptionModule : public PerceptionModuleRosBase<BodyPart, geo
   protected:
     bool perceptionCallback(const geometry_msgs::PoseStamped& msg) override;
 
-    OntologiesManipulator* ontologies_manipulator_;
-    OntologyManipulator* onto_;
+    onto::OntologiesManipulator* ontologies_manipulator_;
+    onto::OntologyManipulator* onto_;
 
     std::string human_name_;
     std::string head_name_;

include/overworld/Perception/Modules/ObjectsModules/FakeObjectPerceptionModule.h

@@ -19,8 +19,8 @@ class FakeObjectPerceptionModule : public PerceptionModuleRosBase<Object, overwo
   bool closeInitialization() override;
 
 private:
-  OntologiesManipulator* ontologies_manipulator_;
-  OntologyManipulator* onto_;
+  onto::OntologiesManipulator* ontologies_manipulator_;
+  onto::OntologyManipulator* onto_;
 
   tf2_ros::Buffer tf_buffer_;
   tf2_ros::TransformListener tf2_listener_;

include/overworld/Perception/Modules/ObjectsModules/StaticObjectsPerceptionModule.h

@@ -17,8 +17,8 @@ class StaticObjectsPerceptionModule : public PerceptionModuleBase_<Object>
     virtual bool closeInitialization() override;
 
 private:
-    OntologiesManipulator* ontologies_manipulator_;
-    OntologyManipulator* onto_;
+    onto::OntologiesManipulator* ontologies_manipulator_;
+    onto::OntologyManipulator* onto_;
 
     std::string config_file_;
 

include/overworld/Perception/Modules/RobotsModules/JointStatePerceptionModule.h

@@ -44,8 +44,8 @@ class JointStatePerceptionModule : public owds::PerceptionModuleRosBase<owds::Bo
     tf2_ros::Buffer tf_buffer_;
     tf2_ros::TransformListener tf2_listener_;
 
-    OntologiesManipulator* ontologies_manipulator_;
-    OntologyManipulator* onto_;
+    onto::OntologiesManipulator* ontologies_manipulator_;
+    onto::OntologyManipulator* onto_;
 
     bool updateBasePose(const ros::Time& stamp = ros::Time(0));
     void loadRobotModel();

include/overworld/SituationAssessor.h

@@ -19,6 +19,7 @@
 
 #include <overworld/StartStopModules.h>
 #include <overworld/BoundingBox.h>
+#include <overworld/GetAgents.h>
 
 namespace owds {
 
@@ -74,6 +75,8 @@ 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<bool> run_;
   double time_step_; // in second
   double simu_step_;
@@ -108,6 +111,7 @@ class SituationAssessor
   template<typename T>
   bool stopModule(BasePerceptionManager<T>& 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);
 
 };
 

include/overworld/Utility/Ontology.h

@@ -12,15 +12,15 @@ namespace owds {
 
 namespace ontology {
 
-std::array<double, 3> getEntityColor(OntologyManipulator* onto, const std::string& indiv_name, const std::array<double, 3>& default_value = {0.8,0.8,0.8});
+std::array<double, 3> getEntityColor(onto::OntologyManipulator* onto, const std::string& indiv_name, const std::array<double, 3>& 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
 

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<Fact> FactsCalculator::computeAgentsFacts(const std::map<std::string
 
   for(auto& agent_from : agents)
   {
-    isInHand(agent_from.second);
+    //isInHand(agent_from.second); // objects already in agent's hand
     for (auto& obj: objects)
     {
       if(isValid(obj.second) == false)
         continue;
 
-      if (agent_from.second->getType() == 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<int>& 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;
         }

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<ObjectPropertyBranch_t*> 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<ObjectPropertyBranch_t*> 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()

src/Perception/Managers/AgentPerceptionManager.cpp

@@ -35,7 +35,7 @@ std::map<std::string, Agent*>::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())
   {

src/Perception/Managers/HumansPerceptionManager.cpp

@@ -25,7 +25,7 @@ void HumansPerceptionManager::getPercepts( std::map<std::string, BodyPart>& 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());