Merge branch '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.4.1 REQUIRED)
 find_package(Eigen3 REQUIRED NO_MODULE)
 find_package(Threads REQUIRED)
 find_package(CURL REQUIRED)
@@ -68,13 +68,16 @@ add_message_files(
   EntityPose.msg
   EntitiesPoses.msg
   AgentPose.msg
+  AgentsPose.msg
   Pose.msg
 )
 
 add_service_files(
   FILES
   StartStopModules.srv
   BoundingBox.srv
+  GetAgents.srv
+  GetApproachPoint.srv
   GetPose.srv
   GetRelations.srv
 )
@@ -188,6 +191,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
@@ -197,7 +201,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    #
@@ -209,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
@@ -258,6 +264,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
@@ -279,4 +294,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)
+                      ${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)

Readme.md

@@ -71,13 +71,13 @@ 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
 [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

docs/AvailableModules.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Available Modules | Overworld 0.1.4</title>
+  <title>Available Modules | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="images/favicon.ico" rel="shortcut icon">

docs/Download.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Download | Overworld 0.1.4</title>
+  <title>Download | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="images/favicon.ico" rel="shortcut icon">
@@ -88,6 +88,31 @@ <h1 class="title">Download</h1>
 
         <p>You have below the archives of the major versions of Overworld:</p>
 
+        <h2> V0.1.5 </h2>
+        <p>23 / 02 / 2024</p>
+
+        <ul>
+          <li>
+            <a href="https://github.com/sarthou/overworld/archive/v0.1.5.zip" rel="nofollow">
+              <svg class="octicon octicon-file-zip flex-shrink-0 text-gray" width="16" height="16" viewBox="0 0 12 16" version="1.1" aria-hidden="true">
+                <path fill-rule="evenodd" d="M8.5 1H1a1 1 0 0 0-1 1v12a1 1 0 0 0 1 1h10a1 1 0 0 0 1-1V4.5L8.5 1zM11 14H1V2h3v1h1V2h3l3 3v9zM5 4V3h1v1H5zM4 4h1v1H4V4zm1 2V5h1v1H5zM4 6h1v1H4V6zm1 2V7h1v1H5zM4 9.28A2 2 0 0 0 3 11v1h4v-1a2 2 0 0 0-2-2V8H4v1.28zM6 10v1H4v-1h2z">
+                </path>
+              </svg>
+              <strong class="px-1">Source code</strong> (zip)
+            </a>
+          </li>
+
+          <li>
+            <a href="https://github.com/sarthou/overworld/archive/v0.1.5.tar.gz" rel="nofollow">
+              <svg class="octicon octicon-file-zip flex-shrink-0 text-gray" width="16" height="16" viewBox="0 0 12 16" version="1.1" aria-hidden="true">
+                <path fill-rule="evenodd" d="M8.5 1H1a1 1 0 0 0-1 1v12a1 1 0 0 0 1 1h10a1 1 0 0 0 1-1V4.5L8.5 1zM11 14H1V2h3v1h1V2h3l3 3v9zM5 4V3h1v1H5zM4 4h1v1H4V4zm1 2V5h1v1H5zM4 6h1v1H4V6zm1 2V7h1v1H5zM4 9.28A2 2 0 0 0 3 11v1h4v-1a2 2 0 0 0-2-2V8H4v1.28zM6 10v1H4v-1h2z">
+                </path>
+              </svg>
+              <strong class="px-1">Source code</strong> (tar.gz)
+            </a>
+          </li>
+        </ul>
+
         <h2> V0.1.4 </h2>
         <p>23 / 05 / 2023</p>
 

docs/InstallOverworld.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Install | Overworld 0.1.4</title>
+  <title>Install | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="images/favicon.ico" rel="shortcut icon">

docs/Publications.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Publications | Overworld 0.1.4</title>
+  <title>Publications | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="images/favicon.ico" rel="shortcut icon">

docs/Tutorials/Tutorials.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Tutorials | Overworld 0.1.4</title>
+  <title>Tutorials | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="../style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="../images/favicon.ico" rel="shortcut icon">

docs/index.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Overworld 0.1.4</title>
+  <title>Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <meta charset="utf-8">
 	<meta name="viewport" content="width=device-width, initial-scale=1.0">
@@ -96,7 +96,7 @@ <h2>Assessing the geometry of the world for Human-Robot Interaction</h2>
             <div class="row">
               <div class="text-center">
                 <p>
-                  <a href="https://github.com/sarthou/overworld/archive/v0.1.4.zip" class="btn btn-primary btn-lg anchorLink jumbo-button" role="button"><span class="glyphicon glyphicon-download-alt"></span> Download <small>(0.1.4)</small></a>
+                  <a href="https://github.com/sarthou/overworld/archive/v0.1.5.zip" class="btn btn-primary btn-lg anchorLink jumbo-button" role="button"><span class="glyphicon glyphicon-download-alt"></span> Download <small>(0.1.5)</small></a>
                   <a href="https://github.com/sarthou/overworld" target="_blank" rel="noopener noreferrer" class="btn btn-custom btn-lg anchorLink jumbo-button" role="button"><span><img src="https://upload.wikimedia.org/wikipedia/commons/thumb/9/91/Octicons-mark-github.svg/1200px-Octicons-mark-github.svg.png" alt="github" class="github-icon"></span> View on Github</a>
                 </p>
               </div> <!-- col-md-6 text-center -->
@@ -158,7 +158,7 @@ <h1>What's New</h1>
               <img src="images/index_areas.png" width="50%" style="float: right; padding-left: 20px;"/>
               <h3>Define areas</h3>
               <div>
-                <p>From Overworld 0.1.4 you can now define areas in the form of polygons and cylinders.</p>
+                <p>From Overworld 0.1.5 you can now define areas in the form of polygons and cylinders.</p>
                 <p>Static or attached to other entities, Overworld will allow you to know the entities in them with a newly introduced semantic fact!</p>
                 <p>Being implemented using perception modules you can adapt it to your need and even perceive them dynamically.</p>
               </div>

docs/overview/Configuration.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Configuration | Overworld 0.1.4</title>
+  <title>Configuration | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="../style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="../images/favicon.ico" rel="shortcut icon">

docs/overview/Plugins.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Plugins | Overworld 0.1.4</title>
+  <title>Plugins | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="../style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="../images/favicon.ico" rel="shortcut icon">

docs/overview/bulletGUI.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Bullet GUI | Overworld 0.1.4</title>
+  <title>Bullet GUI | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="../style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="../images/favicon.ico" rel="shortcut icon">

docs/overview/launchers.html

@@ -1,7 +1,7 @@
 <html lang="en_US">
 
 <head>
-  <title>Launchers | Overworld 0.1.4</title>
+  <title>Launchers | Overworld 0.1.5</title>
   <meta content="text/html; charset=utf-8" http-equiv="Content-Type">
   <link href="../style/overworld_api.css" rel="stylesheet" type="text/css">
   <link href="../images/favicon.ico" rel="shortcut icon">

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();

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<double, 3> computeTranslationSpeed() const;
 
@@ -81,7 +82,7 @@ class Entity
     bool is_true_id_;
     std::unordered_set<std::string> false_ids_;
 
-    CircularBuffer<PoseStamped_s, 10> last_poses_;
+    CircularBuffer<PoseStamped_s, 30> last_poses_;
     bool is_located_;
     int bullet_id_;
     int bullet_link_id_;

include/overworld/BasicTypes/FieldOfView.h

@@ -17,14 +17,17 @@ class FieldOfView
                                    width_(width),
                                    clip_near_(clip_near),
                                    clip_far_(clip_far)
-    {}
+    {
+        opengl_ratio_ = std::tan(width_ * TO_HALF_RAD) / std::tan(height_ * TO_HALF_RAD);
+    }
 
     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_; }
 
     /**
      * @brief 
@@ -33,10 +36,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 +55,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

@@ -10,12 +10,12 @@ namespace owds {
 class OntologeniusFactsPublisher : public FactsPublisher
 {
 public:
-  OntologeniusFactsPublisher(ros::NodeHandle* n, const std::string& agent_name);
+  OntologeniusFactsPublisher(const std::string& agent_name);
 
 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

@@ -5,36 +5,38 @@
 
 #include "overworld/GetRelations.h"
 
-namespace ontologenius {
+#include <ros/ros.h>
 
-class ReasonerEgocentric : public ReasonerInterface
+namespace overworld {
+
+class ReasonerEgocentric : public ontologenius::ReasonerInterface
 {
 public:
   ReasonerEgocentric() {}
   virtual ~ReasonerEgocentric() = default;
 
   virtual void initialize() override;
 
-  virtual void 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<ObjectPropertyBranch_t*> computable_properties_;
+  std::unordered_set<ontologenius::ObjectPropertyBranch_t*> computable_properties_;
   ros::NodeHandle n_;
   ros::ServiceClient overworld_client_;
 
-  ObjectPropertyBranch_t* isComputableProperty(const std::string& property);
-  std::set<ObjectPropertyBranch_t*> isInRange(const std::string& indiv, ObjectPropertyBranch_t* property);
-  std::set<ObjectPropertyBranch_t*> isInDomain(const std::string& indiv, ObjectPropertyBranch_t* property);
+  ontologenius::ObjectPropertyBranch_t* isComputableProperty(const std::string& property);
+  std::set<ontologenius::ObjectPropertyBranch_t*> isInRange(const std::string& indiv, ontologenius::ObjectPropertyBranch_t* property);
+  std::set<ontologenius::ObjectPropertyBranch_t*> isInDomain(const std::string& indiv, ontologenius::ObjectPropertyBranch_t* property);
 
-  overworld::GetRelations createRequest(const std::string& subject, const std::set<ObjectPropertyBranch_t*>& predicates, const std::string& object);
+  overworld::GetRelations createRequest(const std::string& subject, const std::set<ontologenius::ObjectPropertyBranch_t*>& predicates, const std::string& object);
   bool call(overworld::GetRelations& srv);
   void updateOntology(const std::vector<overworld::Triplet>& to_add, const std::vector<overworld::Triplet>& to_delete);
 };
 
-} // namespace ontologenius
+} // namespace overworld
 
 #endif // OVERWORLD_REASONEREGOCENTRIC_H

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/AreasPerceptionManager.h

@@ -28,6 +28,7 @@ class AreasPerceptionManager : public BasePerceptionManager<Area>
   void registerBodyPartsManager(EntitiesPerceptionManager<BodyPart>* manager) { bodyparts_managers_.insert(manager); }
 
   const std::map<std::string, Area*>& getEntities() const { return areas_; }
+  Area* getArea(const std::string& area_id);
 
   bool update();