Merge pull request #15 from IIIM-IS/setup-simplification

Setup simplification

controllers/hand_grab_sphere_UR_3D_controller/aera_controller.cpp → controllers/AERA_controller_base/aera_controller.cpp

@@ -16,6 +16,47 @@ AERAController::~AERAController()
   tcp_connection_->stop();
 }
 
+std::map<std::string, std::map<std::string, tcp_io_device::MetaData>> AERAController::setup(std::string settings_file_name)
+{
+  toml_parser::TOMLParser parser;
+  parser.parse(settings_file_name);
+
+  std::vector<tcp_io_device::MetaData> objects;
+  std::map<std::string, std::map<std::string, tcp_io_device::MetaData> > objects_map;
+  std::vector<tcp_io_device::MetaData> commands;
+
+
+  std::vector<std::string> entity_names = parser.entityNames();
+  std::vector<std::string> property_names = parser.propertyNames();
+  std::vector<std::string> command_names = parser.commandNames();
+
+  std::vector<std::string> object_names;
+  object_names.resize(entity_names.size() + property_names.size() + command_names.size());
+  // Add entities to the vector.
+  object_names.insert(object_names.end(), entity_names.begin(), entity_names.end());
+  // Add properties to the vector
+  object_names.insert(object_names.end(), property_names.begin(), property_names.end());
+  // Add commands to the vector
+  object_names.insert(object_names.end(), command_names.begin(), command_names.end());
+
+  // Generate communication ids by filling the string_id_mapping_
+  fillIdStringMaps(object_names);
+
+  std::map<std::string, toml_parser::entity> entity_map = parser.entities();
+  for (auto e_it = entity_map.begin(); e_it != entity_map.end(); ++e_it) {
+    auto e = e_it->second;
+    for (auto p_it = e.properties.begin(); p_it != e.properties.end(); ++p_it) {
+      objects.push_back(tcp_io_device::MetaData(string_id_mapping_[e_it->first], string_id_mapping_[p_it->name], p_it->data_type, p_it->dimensions, p_it->opcode_handle));
+      objects_map[e_it->first].insert(std::make_pair(p_it->name, objects.back()));
+    }
+    for (auto c_it = e.commands.begin(); c_it != e.commands.end(); ++c_it) {
+      commands.push_back(tcp_io_device::MetaData(string_id_mapping_[e_it->first], string_id_mapping_[c_it->name], c_it->data_type, c_it->dimensions, c_it->opcode_handle));
+    }
+  }
+  sendSetupMessage(objects, commands);
+  return objects_map;
+}
+
 int AERAController::startConnection()
 {
   std::cout << "Establishing TCP connection" << std::endl;
@@ -115,10 +156,6 @@ tcp_io_device::MsgData AERAController::createMsgData(tcp_io_device::MetaData met
   return tcp_io_device::MsgData::createNewMsgData(meta_data, data);
 }
 
-tcp_io_device::MsgData AERAController::createMsgData(tcp_io_device::MetaData meta_data, std::string data) {
-  return tcp_io_device::MsgData::createNewMsgData(meta_data, data);
-}
-
 void AERAController::sendDataMessage(std::vector<tcp_io_device::MsgData> msg_data) {
   std::unique_ptr<tcp_io_device::TCPMessage> msg = std::make_unique<tcp_io_device::TCPMessage>();
   msg->set_messagetype(tcp_io_device::TCPMessage::DATA);

controllers/hand_grab_sphere_NED_1D_controller/aera_controller.h → controllers/AERA_controller_base/aera_controller.h

@@ -10,6 +10,7 @@
 #include <webots/Supervisor.hpp>
 #include <webots/Motor.hpp>
 #include <webots/PositionSensor.hpp>
+#include "toml_parser.h"
 
 using communication_id_t = int64_t;
 
@@ -22,6 +23,8 @@ class AERAController
 
   virtual int start() = 0;
 
+  std::map<std::string, std::map<std::string, tcp_io_device::MetaData> > setup(std::string settings_file_name);
+
   int startConnection();
 
   void stop();

controllers/hand_grab_sphere_NED_1D_controller/hand_grab_sphere_NED_1D_controller.cpp

@@ -1,5 +1,5 @@
 #include "hand_grab_sphere_NED_1D_controller.h"
-#include "toml_parser.h"
+#include "../AERA_controller_base/toml_parser.h"
 
 #define DEBUG 1
 
@@ -45,40 +45,10 @@ HandGrabSphereController::~HandGrabSphereController() {
 
 int HandGrabSphereController::start() {
 
-  toml_parser::TOMLParser parser;
-  parser.parse("settings.toml");
+  std::map<std::string, std::map<std::string, tcp_io_device::MetaData> > objects_map;
 
-  std::vector<tcp_io_device::MetaData> objects;
-  std::vector<tcp_io_device::MetaData> commands;
+  objects_map = setup("settings.toml");
 
-
-  std::vector<std::string> entity_names = parser.entityNames();
-  std::vector<std::string> property_names = parser.propertyNames();
-  std::vector<std::string> command_names = parser.commandNames();
-
-  std::vector<std::string> object_names;
-  object_names.resize(entity_names.size() + property_names.size() + command_names.size());
-  // Add entities to the vector.
-  object_names.insert(object_names.end(), entity_names.begin(), entity_names.end());
-  // Add properties to the vector
-  object_names.insert(object_names.end(), property_names.begin(), property_names.end());
-  // Add commands to the vector
-  object_names.insert(object_names.end(), command_names.begin(), command_names.end());
-
-  // Generate communication ids by filling the string_id_mapping_
-  fillIdStringMaps(object_names);
-
-  std::map<std::string, toml_parser::entity> entity_map = parser.entities();
-  for (auto e_it = entity_map.begin(); e_it != entity_map.end(); ++e_it) {
-    auto e = e_it->second;
-    for (auto p_it = e.properties.begin(); p_it != e.properties.end(); ++p_it) {
-      objects.push_back(tcp_io_device::MetaData(string_id_mapping_[e_it->first], string_id_mapping_[p_it->name], p_it->data_type, p_it->dimensions, p_it->opcode_handle));
-    }
-    for (auto c_it = e.commands.begin(); c_it != e.commands.end(); ++c_it) {
-      commands.push_back(tcp_io_device::MetaData(string_id_mapping_[e_it->first], string_id_mapping_[c_it->name], c_it->data_type, c_it->dimensions, c_it->opcode_handle));
-    }
-  }
-  sendSetupMessage(objects, commands);
   waitForStartMsg();
 
   init();
@@ -95,10 +65,10 @@ int HandGrabSphereController::start() {
   std::cout << "Sphere Position: " << s_position << std::endl;
 
   std::vector<tcp_io_device::MsgData> data_to_send;
-  data_to_send.push_back(createMsgData<double>(objects[0], { c_position }));
-  data_to_send.push_back(createMsgData<double>(objects[1], { h_position }));
-  data_to_send.push_back(createMsgData<communication_id_t>(objects[2], { -1 }));
-  data_to_send.push_back(createMsgData<double>(objects[3], { s_position }));
+  data_to_send.push_back(createMsgData<double>(objects_map["h"].at("position"), { h_position }));
+  data_to_send.push_back(createMsgData<communication_id_t>(objects_map["h"].at("holding"), { -1 }));
+  data_to_send.push_back(createMsgData<double>(objects_map["c"].at("position"), { c_position }));
+  data_to_send.push_back(createMsgData<double>(objects_map["s"].at("position"), { s_position }));
   sendDataMessage(data_to_send);
   state_ = IDLE;
   run();

controllers/hand_grab_sphere_NED_1D_controller/hand_grab_sphere_NED_1D_controller.h

@@ -1,5 +1,5 @@
 #pragma once
-#include "aera_controller.h"
+#include "../AERA_controller_base/aera_controller.h"
 class HandGrabSphereController :
   public AERAController
 {

controllers/hand_grab_sphere_NED_1D_controller/hand_grab_sphere_NED_1D_controller.vcxproj

@@ -156,19 +156,19 @@
     <ClCompile Include="$(WEBOTS_HOME)\src\controller\cpp\TouchSensor.cpp" />
     <ClCompile Include="..\..\submodules\AERA_Protobuf\tcp_connection.cpp" />
     <ClCompile Include="..\..\submodules\AERA_Protobuf\tcp_data_message.pb.cc" />
+    <ClCompile Include="..\AERA_controller_base\aera_controller.cpp" />
+    <ClCompile Include="..\AERA_controller_base\toml_parser.cpp" />
     <ClCompile Include="hand_grab_sphere_NED_1D_controller_main.cpp" />
     <ClCompile Include="hand_grab_sphere_NED_1D_controller.cpp" />
-    <ClCompile Include="aera_controller.cpp" />
-    <ClCompile Include="toml_parser.cpp" />
   </ItemGroup>
   <ItemGroup>
     <ClInclude Include="..\..\submodules\AERA_Protobuf\tcp_connection.h" />
     <ClInclude Include="..\..\submodules\AERA_Protobuf\tcp_data_message.pb.h" />
     <ClInclude Include="..\..\submodules\AERA_Protobuf\utils.h" />
-    <ClInclude Include="aera_controller.h" />
+    <ClInclude Include="..\AERA_controller_base\aera_controller.h" />
+    <ClInclude Include="..\AERA_controller_base\toml.h" />
+    <ClInclude Include="..\AERA_controller_base\toml_parser.h" />
     <ClInclude Include="hand_grab_sphere_NED_1D_controller.h" />
-    <ClInclude Include="toml.h" />
-    <ClInclude Include="toml_parser.h" />
   </ItemGroup>
   <ItemGroup>
     <None Include="..\..\submodules\AERA_Protobuf\README.md" />

controllers/hand_grab_sphere_NED_1D_controller/hand_grab_sphere_NED_1D_controller.vcxproj.filters

@@ -100,7 +100,6 @@
     <ClCompile Include="$(WEBOTS_HOME)\src\controller\cpp\TouchSensor.cpp">
       <Filter>Webots</Filter>
     </ClCompile>
-    <ClCompile Include="aera_controller.cpp" />
     <ClCompile Include="..\..\submodules\AERA_Protobuf\tcp_connection.cpp">
       <Filter>AERA_Protobuf</Filter>
     </ClCompile>
@@ -109,7 +108,12 @@
     </ClCompile>
     <ClCompile Include="hand_grab_sphere_NED_1D_controller_main.cpp" />
     <ClCompile Include="hand_grab_sphere_NED_1D_controller.cpp" />
-    <ClCompile Include="toml_parser.cpp" />
+    <ClCompile Include="..\AERA_controller_base\aera_controller.cpp">
+      <Filter>AERA_controller_base</Filter>
+    </ClCompile>
+    <ClCompile Include="..\AERA_controller_base\toml_parser.cpp">
+      <Filter>AERA_controller_base</Filter>
+    </ClCompile>
   </ItemGroup>
   <ItemGroup>
     <Filter Include="Webots">
@@ -118,9 +122,11 @@
     <Filter Include="AERA_Protobuf">
       <UniqueIdentifier>{5a0c5025-1013-4da5-9116-76d93ba62069}</UniqueIdentifier>
     </Filter>
+    <Filter Include="AERA_controller_base">
+      <UniqueIdentifier>{5405b8cc-fb43-41a7-a013-eec515766060}</UniqueIdentifier>
+    </Filter>
   </ItemGroup>
   <ItemGroup>
-    <ClInclude Include="aera_controller.h" />
     <ClInclude Include="..\..\submodules\AERA_Protobuf\tcp_connection.h">
       <Filter>AERA_Protobuf</Filter>
     </ClInclude>
@@ -131,8 +137,15 @@
       <Filter>AERA_Protobuf</Filter>
     </ClInclude>
     <ClInclude Include="hand_grab_sphere_NED_1D_controller.h" />
-    <ClInclude Include="toml_parser.h" />
-    <ClInclude Include="toml.h" />
+    <ClInclude Include="..\AERA_controller_base\aera_controller.h">
+      <Filter>AERA_controller_base</Filter>
+    </ClInclude>
+    <ClInclude Include="..\AERA_controller_base\toml.h">
+      <Filter>AERA_controller_base</Filter>
+    </ClInclude>
+    <ClInclude Include="..\AERA_controller_base\toml_parser.h">
+      <Filter>AERA_controller_base</Filter>
+    </ClInclude>
   </ItemGroup>
   <ItemGroup>
     <None Include="..\..\submodules\AERA_Protobuf\README.md">