[ApproachSender] WIP constraintToTree

CMakeLists.txt

@@ -200,7 +200,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    #
@@ -291,4 +292,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)

include/overworld/Senders/ApproachSender.h

@@ -41,6 +41,8 @@ class ApproachSender
 
   void setRobotName(const std::string& robot_name);
 
+  LogicalAlgebraNode constraintToTree(std::string constraint);
+
 private:
   ros::NodeHandle* n_;
   std::string robot_name_;
@@ -49,6 +51,8 @@ class ApproachSender
   PerceptionManagers* managers_;
   onto::OntologyManipulator* onto_;
 
+  //LogicalAlgebraNode constraintToTree(std::string constraint);
+  size_t getIn(size_t begin, std::string& in_text, const std::string& text, char symbol_in, char symbol_out);
 };
 
 } // namespace owds

src/Senders/ApproachSender.cpp

@@ -60,4 +60,56 @@ void ApproachSender::setRobotName(const std::string& robot_name)
   robot_ = managers_->robots_manager_.getAgent(robot_name_);
 }
 
+LogicalAlgebraNode ApproachSender::constraintToTree(std::string constraint)
+{
+  std::cout << "in = " << constraint << std::endl;
+
+  std::unordered_map<std::string, LogicalAlgebraNode> braquet_nodes;
+  size_t braquet_cpt = 0;
+  size_t braquet_pose = constraint.find("(");
+  while(braquet_pose != std::string::npos)
+  {
+    std::string in_braquet;
+    size_t out_pose = getIn(braquet_pose, in_braquet, constraint, '(', ')');
+    constraint.replace(braquet_pose, out_pose - braquet_pose + 1, "$" + std::to_string(braquet_cpt));
+    braquet_nodes.insert(std::make_pair<std::string,LogicalAlgebraNode>("$" + std::to_string(braquet_cpt),
+                                                                        constraintToTree(in_braquet)));
+    braquet_cpt++;
+    braquet_pose = constraint.find("(");
+  }
+
+  std::cout << "out = " << constraint << std::endl;
+
+  return LogicalAlgebraNode(logical_none);
+}
+
+size_t ApproachSender::getIn(size_t begin, std::string& in_text, const std::string& text, char symbol_in, char symbol_out)
+{
+  size_t pose = begin;
+
+  if(text.at(pose) == symbol_in)
+  {
+    size_t first_pose = pose;
+    int cpt = 1;
+    while((cpt != 0) && (pose+1 < text.length()))
+    {
+      ++pose;
+      if(text.at(pose) == symbol_in)
+        cpt++;
+      else if(text.at(pose) == symbol_out)
+        cpt--;
+
+    }
+
+    in_text = text.substr(first_pose+1, pose-first_pose-1);
+
+    if(cpt == 0)
+      return pose;
+    else
+      return std::string::npos;
+  }
+  else
+    return begin;
+}
+
 } // namespace owds

src/TestFiles/approach_sender.cpp

@@ -0,0 +1,19 @@
+#include "overworld/Senders/ApproachSender.h"
+
+int main()
+{
+    owds::ApproachSender sender(nullptr, nullptr);
+
+    sender.constraintToTree("a & b | c");
+    std::cout << "-----" << std::endl;
+    sender.constraintToTree("a & (b | c)");
+    std::cout << "-----" << std::endl;
+    sender.constraintToTree("a & (b | c) & !(d | e)");
+    std::cout << "-----" << std::endl;
+    sender.constraintToTree("a & (b | (c & e))");
+    std::cout << "-----" << std::endl;
+    sender.constraintToTree("a&(b|(c&e))");
+    std::cout << "-----" << std::endl;
+
+    return 0;
+}