Add get_trail service for entities

mpavezb · Nov 19, 2018 · b412778 · b412778
1 parent d2df126
commit b412778
Show file tree

Hide file tree

Showing 4 changed files with 47 additions and 0 deletions.
diff --git a/include/ltm/db/entity_collection.h b/include/ltm/db/entity_collection.h
@@ -65,6 +65,7 @@ namespace ltm {
  std::string ltm_get_type();
  std::string ltm_get_collection_name();
  std::string ltm_get_log_collection_name();
+ std::string ltm_get_diff_collection_name();
  std::string ltm_get_status();
  std::string ltm_get_db_name();
 

diff --git a/include/ltm/db/impl/entity_collection_impl.hxx b/include/ltm/db/impl/entity_collection_impl.hxx
@@ -22,6 +22,11 @@ namespace ltm {
  return _log_collection_name;
  }
 
+ template<class EntityMsg>
+ std::string EntityCollectionManager<EntityMsg>::ltm_get_diff_collection_name() {
+ return _diff_collection_name;
+ } 
+
  template<class EntityMsg>
  std::string EntityCollectionManager<EntityMsg>::ltm_get_db_name() {
  return _db_name;

diff --git a/include/ltm/plugin/entity_ros.h b/include/ltm/plugin/entity_ros.h
@@ -33,6 +33,7 @@ namespace ltm {
  ros::ServiceServer _add_entity_service;
  ros::ServiceServer _get_entity_service;
  ros::ServiceServer _get_entity_logs_service;
+ ros::ServiceServer _get_entity_trail_service;
  ros::ServiceServer _delete_entity_service;
 
  public:
@@ -53,6 +54,8 @@ namespace ltm {
 
  bool get_logs_service(ltm::GetEntityLogs::Request &req, ltm::GetEntityLogs::Response &res);
 
+ bool get_trail_service(EntitySrvRequest &req, EntitySrvResponse &res);
+
  };
  }
 }

diff --git a/include/ltm/plugin/impl/entity_ros_impl.hxx b/include/ltm/plugin/impl/entity_ros_impl.hxx
@@ -29,6 +29,7 @@ namespace ltm {
  _get_entity_service = priv.advertiseService(ns + "get", &EntityROS<EntityMsg, EntitySrv>::get_service, this);
  _delete_entity_service = priv.advertiseService(ns + "delete", &EntityROS<EntityMsg, EntitySrv>::delete_service, this);
  _get_entity_logs_service = priv.advertiseService(ns + "get_logs", &EntityROS<EntityMsg, EntitySrv>::get_logs_service, this);
+ _get_entity_trail_service = priv.advertiseService(ns + "get_trail", &EntityROS<EntityMsg, EntitySrv>::get_trail_service, this);
  }
 
  template<class EntityMsg, class EntitySrv>
@@ -101,6 +102,7 @@ namespace ltm {
  // ROS_WARN_STREAM("RETRACE: FOUND!");
  res.msgs.push_back(entity);
  }
+ res.not_found = not_found;
  ROS_WARN_STREAM_COND(not_found.size() > 0, this->_log_prefix
  << "GET: The following requested entities were not found: " << ltm::util::vector_to_str(not_found));
  return true;
@@ -143,6 +145,42 @@ namespace ltm {
  }
  return true; 
  }
+
+ template<class EntityMsg, class EntitySrv>
+ bool EntityROS<EntityMsg, EntitySrv>::get_trail_service(EntitySrvRequest &req, EntitySrvResponse &res) {
+ ROS_INFO_STREAM(this->_log_prefix << "Retrieving entity trails from collection '" << this->ltm_get_diff_collection_name() << "': " << ltm::util::vector_to_str(req.uids));
+ res.msgs.clear();
+
+ // check
+ if (!req.stamps.empty()) {
+ ROS_WARN_STREAM("Get Trail Service: stamps are not considered for this service.");
+ }
+
+ // search
+ std::vector<uint32_t> visited, not_found;
+ std::vector<uint32_t>::const_iterator it;
+ std::vector<ros::Time>::const_iterator t_it;
+ for (it = req.uids.begin(); it != req.uids.end(); ++it) {
+ uint32_t log_uid = *it;
+
+ // do not seek repeated msgs
+ if (visited.end() != std::find(visited.begin(), visited.end(), log_uid)) continue;
+ visited.push_back(log_uid);
+
+ // lookup
+ EntityWithMetadataPtr diff;
+ if (!this->ltm_get_diff(log_uid, diff)) {
+ ROS_WARN_STREAM(this->_log_prefix << "Could not find trail with uid (" << log_uid << ") in collection '" << this->ltm_get_diff_collection_name() << "'");
+ not_found.push_back(log_uid);
+ continue;
+ }
+ res.msgs.push_back(*diff);
+ }
+ res.not_found = not_found;
+ ROS_WARN_STREAM_COND(not_found.size() > 0, this->_log_prefix
+ << "GET: The following requested entities were not found: " << ltm::util::vector_to_str(not_found));
+ return true;
+ }
  }
 }