Review all agent monitor

agent_monitor/CMakeLists.txt

@@ -1,5 +1,6 @@
 cmake_minimum_required(VERSION 2.8.3)
 project(agent_monitor)
+set(CMAKE_CXX_FLAGS "-std=c++11 ${CMAKE_CXX_FLAGS}")
 
 ## Find catkin macros and libraries
 ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
@@ -86,7 +87,7 @@ generate_dynamic_reconfigure_options(
 ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 ## DEPENDS: system dependencies of this project that dependent projects also need
 catkin_package(
- # INCLUDE_DIRS include
+  INCLUDE_DIRS include
 #  LIBRARIES agent_monitor
   CATKIN_DEPENDS toaster_msgs
 #  DEPENDS system_lib
@@ -98,7 +99,7 @@ catkin_package(
 
 ## Specify additional locations of header files
 ## Your package locations should be listed before other locations
-# include_directories(include)
+include_directories(include)
 include_directories(
   ${catkin_INCLUDE_DIRS}
   ${Boost_INCLUDE_DIRS}  $ENV{TOASTERLIB_DIR}/include
@@ -112,7 +113,14 @@ include_directories(
 # )
 
 ## Declare a cpp executable
-add_executable(agent_monitor src/main.cpp)
+set(${PROJECT_NAME}_SOURCES
+    src/AgentManager.cpp
+    src/LookingFact.cpp
+    src/Distances.cpp
+    src/Motion2D.cpp
+    src/FactCreator.cpp
+)
+add_executable(agent_monitor ${${PROJECT_NAME}_SOURCES} src/main.cpp)
 
 ## Add cmake target dependencies of the executable/library
 ## as an example, message headers may need to be generated before nodes

agent_monitor/include/AgentManager.h

@@ -0,0 +1,75 @@
+#include <string>
+#include <map>
+#include <vector>
+
+#include <ros/ros.h>
+
+#include "toaster_msgs/MonitorAll.h"
+#include "toaster_msgs/Empty.h"
+#include "toaster_msgs/RemoveJointToAgent.h"
+#include "toaster_msgs/RemoveAllJointsToAgent.h"
+#include "toaster_msgs/AddAgent.h"
+#include "toaster_msgs/AddJointToAgent.h"
+#include "toaster_msgs/RemoveAgent.h"
+
+using namespace std;
+
+class AgentManager
+{
+public:
+  AgentManager();
+  ~AgentManager() {};
+
+  void init(ros::NodeHandle* node);
+
+  bool monitorAllHumans() { return monitorAllHumans_; }
+  bool monitorAllRobots() { return monitorAllRobots_; }
+  vector<string> getMonitoredAgents() { return agentsMonitored_; }
+  map<string, vector<string> > getMonitoredJoints() { return mapAgentToJointsMonitored_;}
+
+private:
+  bool monitorAllHumans_;
+  bool monitorAllRobots_;
+
+  vector<string> agentsMonitored_;
+  map<string, vector<string> > mapAgentToJointsMonitored_;
+
+  ros::NodeHandle* node_;
+
+  ros::ServiceServer serviceAdd_;
+  ros::ServiceServer serviceAddJoint_;
+  bool addMonitoredAgent(string id);
+  bool addAgent(toaster_msgs::AddAgent::Request &req,
+                toaster_msgs::AddAgent::Response & res);
+  bool addJointToAgent(toaster_msgs::AddJointToAgent::Request &req,
+                      toaster_msgs::AddJointToAgent::Response & res);
+
+  ros::ServiceServer serviceRemove_;
+  ros::ServiceServer serviceRemoves_;
+  bool removeMonitoredAgent(string id);
+  bool removeAgent(toaster_msgs::RemoveAgent::Request &req,
+                  toaster_msgs::RemoveAgent::Response & res);
+  bool removeAllAgents(toaster_msgs::Empty::Request &req,
+                      toaster_msgs::Empty::Response & res);
+
+  ros::ServiceServer serviceRemoveJts_;
+  ros::ServiceServer serviceRemoveJt_;
+  bool removeJointToAgent(toaster_msgs::RemoveJointToAgent::Request &req,
+                          toaster_msgs::RemoveJointToAgent::Response & res);
+  bool removeAllJointsToAgent(toaster_msgs::RemoveAllJointsToAgent::Request &req,
+                              toaster_msgs::RemoveAllJointsToAgent::Response & res);
+
+  ros::ServiceServer servicePrintMonitored_;
+  bool printAllMonitoredAgents(toaster_msgs::Empty::Request &req,
+                              toaster_msgs::Empty::Response & res);
+
+  ros::ServiceServer serviceMonitorAllAgents_;
+  ros::ServiceServer serviceMonitorAllHumans_;
+  ros::ServiceServer serviceMonitorAllRobots_;
+  bool monitorAllAgents(toaster_msgs::MonitorAll::Request &req,
+                        toaster_msgs::MonitorAll::Response & res);
+  bool monitorAllHumans(toaster_msgs::MonitorAll::Request &req,
+                        toaster_msgs::MonitorAll::Response & res);
+  bool monitorAllRobots(toaster_msgs::MonitorAll::Request &req,
+                        toaster_msgs::MonitorAll::Response & res);
+};

agent_monitor/include/Distances.h

@@ -0,0 +1,18 @@
+#include <string>
+#include <map>
+
+#include "toaster-lib/TRBuffer.h"
+#include "toaster-lib/Entity.h"
+
+using namespace std;
+
+class Distances
+{
+public:
+  static map<string, double> computeDeltaDist(map<string, TRBuffer < Entity* > > mapEnts,
+                                              string agentMonitored, unsigned long timelapse);
+
+  static map<string, double> computeJointDeltaDist(map<string, TRBuffer < Entity* > > mapEnts,
+                                                  string agentMonitored, string jointName,
+                                                  unsigned long timelapse);
+};

agent_monitor/include/FactCreator.h

@@ -0,0 +1,21 @@
+#include "toaster_msgs/Fact.h"
+#include "toaster-lib/Joint.h"
+#include "toaster-lib/Entity.h"
+#include "toaster-lib/TRBuffer.h"
+
+#include <map>
+#include <vector>
+#include <string>
+
+using namespace std;
+
+class FactCreator
+{
+public:
+  static toaster_msgs::Fact setFactBase(Joint* joint);
+  static toaster_msgs::Fact setFactBase(string agent, map<string, TRBuffer < Entity* > >& mapTRBEntity);
+
+  static toaster_msgs::Fact setMotionFact(toaster_msgs::Fact baseFact, double speed, double confidence, string type = "agent");
+  static toaster_msgs::Fact setDirectionFact(toaster_msgs::Fact baseFact, string target, double confidence);
+  static toaster_msgs::Fact setDistanceFact(toaster_msgs::Fact baseFact, string target, double confidence);
+};

agent_monitor/include/LookingFact.h

@@ -0,0 +1,36 @@
+#include <map>
+#include <string>
+
+#include "toaster-lib/TRBuffer.h"
+#include "toaster-lib/Entity.h"
+#include "toaster_msgs/FactList.h"
+
+using namespace std;
+
+class LookingFact
+{
+public:
+  /**
+   * @brief This function compute the map required by the fact "IsLookingToward"
+   *        by testing if an entity is lying in the 3D cone of agent visual attention
+   * @param Entity map containing all entity involved in the joint action & their
+   *        respective ids
+   * @param Monitored agent id
+   * @param Distance between center of basement circle and agent head position
+   * @param Angular aperture of the cone in radians
+   * @return Map required by the fact "IsLookingToward" containing all entities
+   *         lying in the cone and all normalized angles beetween entities and cone axis
+   */
+  static map<string, double> compute(map<string, TRBuffer < Entity* > > mapEnts,
+                                      string agentMonitored, double deltaDist,
+                                      double angularAperture);
+
+  static void createTowardFact(map<string, double> mapIdValue, toaster_msgs::FactList &factList_msg,
+                                double angle, string subjectId, Entity* entity);
+
+  static void createAtFact(map<string, double> mapIdValue, toaster_msgs::FactList &factList_msg,
+                          double angle, string subjectId, Entity* entity);
+
+  static void createFact(map<string, double> mapIdValue, toaster_msgs::FactList &factList_msg,
+                        double angle, string subjectId, Entity* entity, string property);
+};

agent_monitor/include/Motion2D.h

@@ -0,0 +1,31 @@
+#include <string>
+#include <map>
+
+#include "toaster-lib/TRBuffer.h"
+#include "toaster-lib/Entity.h"
+
+using namespace std;
+
+class Motion2D
+{
+public:
+  static map<string, double> computeToward(map<string, TRBuffer < Entity* > > mapEnts,
+                                          string agentMonitored,
+                                          double towardAngle, double angleThreshold,
+                                          string jointName = "");
+
+  /*Compute Motion angle for agents (jointName == "") or joints (jointName != "")*/
+  static double computeDirection(TRBuffer< Entity* > confBuffer, unsigned long timelapse,
+                                string jointName = "");
+
+  /*Compute Motion speed for agents (jointName == "") or joints (jointName != "")*/
+  static double compute(TRBuffer< Entity* > confBuffer, unsigned long timelapse,
+                        string jointName = "");
+
+  /*Compute if is moving for agents (jointName == "") or joints (jointName != "")*/
+  static bool computeIsMoving(TRBuffer< Entity* > confBuffer, unsigned long timelapse,
+                              double distanceThreshold, string jointName = "");
+
+private:
+  static Entity* getEntityOrJoint(Entity* entity, string jointName);
+};