ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/entities/light_sensor_equipped_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "light_sensor_equipped_entity.h"
00008 #include <argos3/core/simulator/space/space.h>
00009 
00010 namespace argos {
00011 
00012    /****************************************/
00013    /****************************************/
00014 
00015    CLightSensorEquippedEntity::CLightSensorEquippedEntity(CComposableEntity* pc_parent) :
00016       CEntity(pc_parent) {
00017       SetCanBeEnabledIfDisabled(false);
00018    }
00019    
00020    /****************************************/
00021    /****************************************/
00022 
00023    CLightSensorEquippedEntity::CLightSensorEquippedEntity(CComposableEntity* pc_parent,
00024                                                                   const std::string& str_id) :
00025       CEntity(pc_parent, str_id) {
00026       SetCanBeEnabledIfDisabled(false);
00027    }
00028 
00029    /****************************************/
00030    /****************************************/
00031 
00032    CLightSensorEquippedEntity::~CLightSensorEquippedEntity() {
00033       while(! m_tSensors.empty()) {
00034          delete m_tSensors.back();
00035          m_tSensors.pop_back();
00036       }
00037    }
00038 
00039    /****************************************/
00040    /****************************************/
00041 
00042    void CLightSensorEquippedEntity::Init(TConfigurationNode& t_tree) {
00043       try {
00044          /*
00045           * Parse basic entity stuff
00046           */
00047          CEntity::Init(t_tree);
00048          /*
00049           * Parse light 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                CVector3 cPos, cDir;
00060                Real fRange;
00061                GetNodeAttribute(*it, "position", cPos);
00062                GetNodeAttribute(*it, "direction", cDir);
00063                GetNodeAttribute(*it, "range", fRange);
00064                AddSensor(cPos, cDir, fRange);
00065             }
00066             else if(it->Value() == "ring") {
00067                CVector3 cRingCenter;
00068                GetNodeAttributeOrDefault(t_tree, "center", cRingCenter, cRingCenter);
00069                Real fRadius;
00070                GetNodeAttribute(t_tree, "radius", fRadius);
00071                CDegrees cRingStartAngleDegrees;
00072                GetNodeAttributeOrDefault(t_tree, "start_angle", cRingStartAngleDegrees, cRingStartAngleDegrees);
00073                CRadians cRingStartAngleRadians = ToRadians(cRingStartAngleDegrees);
00074                Real fRange;
00075                GetNodeAttribute(t_tree, "range", fRange);
00076                UInt32 unNumSensors;
00077                GetNodeAttribute(t_tree, "num_sensors", unNumSensors);
00078                AddSensorRing(cRingCenter,
00079                              fRadius,
00080                              cRingStartAngleRadians,
00081                              fRange,
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 light sensor equipped entity", ex);
00091       }
00092    }
00093 
00094    /****************************************/
00095    /****************************************/
00096 
00097    void CLightSensorEquippedEntity::AddSensor(const CVector3& c_position,
00098                                                   const CVector3& c_direction,
00099                                                   Real f_range) {
00100       m_tSensors.push_back(new SSensor(c_position, c_direction, f_range));
00101    }
00102 
00103    /****************************************/
00104    /****************************************/
00105    
00106    void CLightSensorEquippedEntity::AddSensorRing(const CVector3& c_center,
00107                                                       Real f_radius,
00108                                                       const CRadians& c_start_angle,
00109                                                       Real f_range,
00110                                                       UInt32 un_num_sensors) {
00111       CRadians cSensorSpacing = CRadians::TWO_PI / un_num_sensors;
00112       CRadians cAngle;
00113       CVector3 cPos, cDir;
00114       for(UInt32 i = 0; i < un_num_sensors; ++i) {
00115          cAngle = c_start_angle + i * cSensorSpacing;
00116          cAngle.SignedNormalize();
00117          cPos.Set(f_radius, 0.0f, 0.0f);
00118          cPos.RotateZ(cAngle);
00119          cPos += c_center;
00120          cDir.Set(f_range, 0.0f, 0.0f);
00121          cDir.RotateZ(cAngle);
00122          AddSensor(cPos, cDir, f_range);
00123       }
00124    }
00125 
00126    /****************************************/
00127    /****************************************/
00128 
00129    REGISTER_STANDARD_SPACE_OPERATIONS_ON_ENTITY(CLightSensorEquippedEntity);
00130    
00131    /****************************************/
00132    /****************************************/
00133 
00134 }