ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/entities/ground_sensor_equipped_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "ground_sensor_equipped_entity.h"
00008 #include <argos3/core/simulator/space/space.h>
00009 
00010 namespace argos {
00011 
00012    /****************************************/
00013    /****************************************/
00014 
00015    CGroundSensorEquippedEntity::CGroundSensorEquippedEntity(CComposableEntity* pc_parent) :
00016       CEntity(pc_parent) {
00017       SetCanBeEnabledIfDisabled(false);
00018    }
00019    
00020    /****************************************/
00021    /****************************************/
00022 
00023    CGroundSensorEquippedEntity::CGroundSensorEquippedEntity(CComposableEntity* pc_parent,
00024                                                                   const std::string& str_id) :
00025       CEntity(pc_parent, str_id) {
00026       SetCanBeEnabledIfDisabled(false);
00027    }
00028 
00029    /****************************************/
00030    /****************************************/
00031 
00032    CGroundSensorEquippedEntity::~CGroundSensorEquippedEntity() {
00033       while(! m_tSensors.empty()) {
00034          delete m_tSensors.back();
00035          m_tSensors.pop_back();
00036       }
00037    }
00038 
00039    /****************************************/
00040    /****************************************/
00041 
00042    void CGroundSensorEquippedEntity::Init(TConfigurationNode& t_tree) {
00043       try {
00044          /*
00045           * Parse basic entity stuff
00046           */
00047          CEntity::Init(t_tree);
00048          /*
00049           * Parse ground sensors
00050           */
00051          /* Not adding any sensor is a fatal error */
00052          if(t_tree.NoChildren()) {
00053             THROW_ARGOSEXCEPTION("No sensors defined");
00054          }
00055          /* Go through children */
00056          TConfigurationNodeIterator it;
00057          for(it = it.begin(&t_tree); it != it.end(); ++it) {
00058             if(it->Value() == "sensor") {
00059                CVector2 cOffset;
00060                GetNodeAttribute(*it, "offset", cOffset);
00061                std::string strType;
00062                GetNodeAttribute(*it, "type", strType);
00063                AddSensor(cOffset, ParseType(strType));
00064             }
00065             else if(it->Value() == "ring") {
00066                CVector2 cRingCenter;
00067                GetNodeAttributeOrDefault(t_tree, "center", cRingCenter, cRingCenter);
00068                Real fRadius;
00069                GetNodeAttribute(t_tree, "radius", fRadius);
00070                CDegrees cRingStartAngleDegrees;
00071                GetNodeAttributeOrDefault(t_tree, "start_angle", cRingStartAngleDegrees, cRingStartAngleDegrees);
00072                CRadians cRingStartAngleRadians = ToRadians(cRingStartAngleDegrees);
00073                std::string strType;
00074                GetNodeAttribute(*it, "type", strType);
00075                ESensorType eType = ParseType(strType);
00076                UInt32 unNumSensors;
00077                GetNodeAttribute(t_tree, "num_sensors", unNumSensors);
00078                AddSensorRing(cRingCenter,
00079                              fRadius,
00080                              cRingStartAngleRadians,
00081                              eType,
00082                              unNumSensors);
00083             }
00084             else {
00085                THROW_ARGOSEXCEPTION("Unrecognized tag \"" << it->Value() << "\"");
00086             }
00087          }
00088       }
00089       catch(CARGoSException& ex) {
00090          THROW_ARGOSEXCEPTION_NESTED("Initialization error in ground sensor equipped entity", ex);
00091       }
00092    }
00093 
00094    /****************************************/
00095    /****************************************/
00096 
00097    void CGroundSensorEquippedEntity::AddSensor(const CVector2& c_offset,
00098                                                ESensorType e_type) {
00099       m_tSensors.push_back(new SSensor(c_offset, e_type));
00100    }
00101 
00102    /****************************************/
00103    /****************************************/
00104    
00105    void CGroundSensorEquippedEntity::AddSensorRing(const CVector2& c_center,
00106                                                       Real f_radius,
00107                                                       const CRadians& c_start_angle,
00108                                                       ESensorType e_type,
00109                                                       UInt32 un_num_sensors) {
00110       CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
00111       CRadians cAngle;
00112       CVector2 cOffset;
00113       for(UInt32 i = 0; i < un_num_sensors; ++i) {
00114          cAngle = c_start_angle + i * cSensorSpacing;
00115          cAngle.SignedNormalize();
00116          cOffset.Set(f_radius, 0.0f);
00117          cOffset.Rotate(cAngle);
00118          cOffset += c_center;
00119          AddSensor(cOffset, e_type);
00120       }
00121    }
00122 
00123    /****************************************/
00124    /****************************************/
00125 
00126    CGroundSensorEquippedEntity::ESensorType CGroundSensorEquippedEntity::ParseType(const std::string& str_type) const {
00127       if(str_type == "bw")    return TYPE_BLACK_WHITE;
00128       if(str_type == "gray") return TYPE_GRAYSCALE;
00129       THROW_ARGOSEXCEPTION("Unrecognized ground sensor type \"" << str_type << "\"");
00130    }
00131 
00132    /****************************************/
00133    /****************************************/
00134 
00135    REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CGroundSensorEquippedEntity);
00136    
00137    /****************************************/
00138    /****************************************/
00139 
00140 }