add ground truth equipped entity

src/core/simulator/entity/controllable_entity.cpp

@@ -179,7 +179,7 @@ namespace argos {
          for(itSens = itSens.begin(&tSensors);
              itSens != itSens.end();
              ++itSens) {
-            /* itSens->Value() is the name of the current actuator */
+            /* itSens->Value() is the name of the current sensor */
             GetNodeAttribute(*itSens, "implementation", strImpl);
             CSimulatedSensor* pcSens = CFactory<CSimulatedSensor>::New(itSens->Value() + " (" + strImpl + ")");
             CCI_Sensor* pcCISens = dynamic_cast<CCI_Sensor*>(pcSens);
@@ -207,7 +207,7 @@ namespace argos {
       m_vecIntersectionPoints.clear();
       for(std::map<std::string, CSimulatedSensor*>::iterator it = m_mapSensors.begin();
           it != m_mapSensors.end(); ++it) {
-         it->second->Update();
+          it->second->Update();
       }
    }
 

src/plugins/simulator/entities/CMakeLists.txt

@@ -10,6 +10,7 @@ set(ARGOS3_HEADERS_PLUGINS_SIMULATOR_ENTITIES
   directional_led_equipped_entity.h
   gripper_equipped_entity.h
   ground_sensor_equipped_entity.h
+  ground_truth_equipped_entity.h
   led_entity.h
   led_equipped_entity.h
   light_entity.h
@@ -41,6 +42,7 @@ set(ARGOS3_SOURCES_PLUGINS_SIMULATOR_ENTITIES
   directional_led_equipped_entity.cpp
   gripper_equipped_entity.cpp
   ground_sensor_equipped_entity.cpp
+  ground_truth_equipped_entity.cpp
   led_entity.cpp
   led_equipped_entity.cpp
   light_entity.cpp

src/plugins/simulator/entities/ground_truth_equipped_entity.cpp

@@ -0,0 +1,122 @@
+
+/**
+ * @file <argos3/plugins/simulator/entities/ground_sensor_equipped_entity.cpp>
+ *
+ * @author Carlo Pinciroli <[email protected]>
+ * @author Xinyi Joffre <[email protected]>
+ */
+#include "ground_truth_equipped_entity.h"
+#include <argos3/core/simulator/space/space.h>
+#include <argos3/core/simulator/entity/composable_entity.h>
+
+namespace argos {
+
+   /****************************************/
+   /****************************************/
+
+   CGroundTruthEquippedEntity::CGroundTruthEquippedEntity(CComposableEntity* pc_parent) :
+      CEntity(pc_parent) {
+      Disable();
+   }
+
+   /****************************************/
+   /****************************************/
+
+   CGroundTruthEquippedEntity::CGroundTruthEquippedEntity(CComposableEntity* pc_parent,
+                                                                  const std::string& str_id) :
+      CEntity(pc_parent, str_id) {
+      Disable();
+   }
+
+   /****************************************/
+   /****************************************/
+
+   CGroundTruthEquippedEntity::~CGroundTruthEquippedEntity() {
+      while(! m_tSensors.empty()) {
+         delete m_tSensors.back();
+         m_tSensors.pop_back();
+      }
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CGroundTruthEquippedEntity::Init(TConfigurationNode& t_tree) {
+      try {
+         /*
+          * Parse basic entity stuff
+          */
+         CEntity::Init(t_tree);
+         /*
+          * Parse ground truth sensors
+          */
+         /* Not adding any sensor is a fatal error */
+         if(t_tree.NoChildren()) {
+            THROW_ARGOSEXCEPTION("No sensors defined");
+         }
+         /* Go through children */
+         TConfigurationNodeIterator it;
+         for(it = it.begin(&t_tree); it != it.end(); ++it) {
+            std::string strAnchorId;
+            GetNodeAttribute(*it, "anchor", strAnchorId);
+            /*
+             * NOTE: here we get a reference to the embodied entity
+             * This line works under the assumption that:
+             * 1. the GroundTruthEquippedEntity has a parent;
+             * 2. the parent has a child whose id is "body"
+             * 3. the "body" is an embodied entity
+             * If any of the above is false, this line will bomb out.
+             */
+            CEmbodiedEntity& cBody = GetParent().GetComponent<CEmbodiedEntity>("body");
+            if(it->Value() == "sensor") {
+               CVector2 cOffset;
+               GetNodeAttribute(*it, "offset", cOffset);
+               AddSensor(cOffset, cBody.GetAnchor(strAnchorId));
+            }
+            else {
+               THROW_ARGOSEXCEPTION("Unrecognized tag \"" << it->Value() << "\"");
+            }
+         }
+      }
+      catch(CARGoSException& ex) {
+         THROW_ARGOSEXCEPTION_NESTED("Initialization error in ground sensor equipped entity", ex);
+      }
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CGroundTruthEquippedEntity::Enable() {
+      CEntity::Enable();
+      for(size_t i = 0; i < m_tSensors.size(); ++i) {
+         m_tSensors[i]->Anchor.Enable();
+      }
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CGroundTruthEquippedEntity::Disable() {
+      CEntity::Disable();
+      for(size_t i = 0; i < m_tSensors.size(); ++i) {
+         m_tSensors[i]->Anchor.Disable();
+      }
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CGroundTruthEquippedEntity::AddSensor(const CVector2& c_offset,
+                                               SAnchor& s_anchor) {
+      m_tSensors.push_back(new SSensor(c_offset, s_anchor));
+   }
+
+   /****************************************/
+   /****************************************/
+
+   REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CGroundTruthEquippedEntity);
+
+   /****************************************/
+   /****************************************/
+
+}

src/plugins/simulator/entities/ground_truth_equipped_entity.h

@@ -0,0 +1,83 @@
+/**
+ * @file <argos3/plugins/simulator/entities/ground_sensor_equipped_entity.h>
+ *
+ * @author Carlo Pinciroli - <[email protected]>
+ * @author Xinyi Joffre - <[email protected]>
+ */
+
+#ifndef GROUND_TRUTH_EQUIPPED_ENTITY_H
+#define GROUND_TRUTH_EQUIPPED_ENTITY_H
+
+namespace argos {
+   class CGroundTruthEquippedEntity;
+}
+
+#include <argos3/core/utility/math/vector3.h>
+#include <argos3/core/simulator/entity/entity.h>
+#include <argos3/core/simulator/entity/embodied_entity.h>
+#include <map>
+
+namespace argos {
+
+   class CGroundTruthEquippedEntity : public CEntity {
+
+   public:
+
+      ENABLE_VTABLE();
+
+      struct SSensor {
+         typedef std::vector<SSensor*> TList;
+
+         CVector2 Offset;
+         SAnchor& Anchor;
+
+         SSensor(const CVector2& c_position,
+                 SAnchor& s_anchor) :
+            Offset(c_position),
+            Anchor(s_anchor) {}
+      };
+
+   public:
+
+      CGroundTruthEquippedEntity(CComposableEntity* pc_parent);
+
+      CGroundTruthEquippedEntity(CComposableEntity* pc_parent,
+                                  const std::string& str_id);
+
+      virtual ~CGroundTruthEquippedEntity();
+
+      virtual void Init(TConfigurationNode& t_tree);
+
+      virtual std::string GetTypeDescription() const {
+         return "ground_truth";
+      }
+
+      virtual void Enable();
+
+      virtual void Disable();
+
+      inline size_t GetNumSensors() const {
+         return m_tSensors.size();
+      }
+
+      inline SSensor& GetSensor(size_t un_idx) {
+         return *m_tSensors[un_idx];
+      }
+
+      inline SSensor::TList& GetSensors() {
+         return m_tSensors;
+      }
+
+      void AddSensor(const CVector2& c_offset,
+                     SAnchor& s_anchor);
+
+   protected:
+
+      /** The list of sensors */
+      SSensor::TList m_tSensors;
+
+   };
+
+}
+
+#endif

src/plugins/simulator/physics_engines/dynamics2d/CMakeLists.txt

@@ -42,6 +42,7 @@ set(ARGOS3_HEADERS_PLUGINS_SIMULATOR_PHYSICS_ENGINES_DYNAMICS2D
   dynamics2d_engine.h
   dynamics2d_gripping.h
   dynamics2d_single_body_object_model.h
+  dynamics2d_multi_body_object_model.h
   dynamics2d_stretchable_object_model.h
   dynamics2d_velocity_control.h)