ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/entities/led_equipped_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "led_equipped_entity.h"
00008 #include <argos3/core/simulator/simulator.h>
00009 #include <argos3/core/simulator/space/space.h>
00010 #include <argos3/plugins/simulator/media/led_medium.h>
00011 
00012 namespace argos {
00013 
00014    /****************************************/
00015    /****************************************/
00016 
00017    CLEDEquippedEntity::CLEDEquippedEntity(CComposableEntity* pc_parent,
00018                                           CPositionalEntity* pc_reference) :
00019       CComposableEntity(pc_parent),
00020       m_pcReferenceEntity(pc_reference) {
00021    }
00022 
00023    /****************************************/
00024    /****************************************/
00025 
00026    CLEDEquippedEntity::CLEDEquippedEntity(CComposableEntity* pc_parent,
00027                                           const std::string& str_id,
00028                                           CPositionalEntity* pc_reference) :
00029       CComposableEntity(pc_parent, str_id),
00030       m_pcReferenceEntity(pc_reference) {
00031    }
00032 
00033    /****************************************/
00034    /****************************************/
00035 
00036    void CLEDEquippedEntity::Init(TConfigurationNode& t_tree) {
00037       try {
00038          /* Init parent */
00039          CComposableEntity::Init(t_tree);
00040          /* Go through the led entries */
00041          CVector3 cPosition;
00042          CColor cColor;
00043          TConfigurationNodeIterator itLED("led");
00044          for(itLED = itLED.begin(&t_tree);
00045              itLED != itLED.end();
00046              ++itLED) {
00047             /* Initialise the LED using the XML */
00048             CLEDEntity* pcLED = new CLEDEntity(this);
00049             pcLED->Init(*itLED);
00050             /* Add the LED to this container */
00051             m_vecLEDOffsetPositions.push_back(pcLED->GetPosition());
00052             m_tLEDs.push_back(pcLED);
00053             AddComponent(*pcLED);
00054          }
00055          UpdateComponents();
00056       }
00057       catch(CARGoSException& ex) {
00058          THROW_ARGOSEXCEPTION_NESTED("Failed to initialize LED equipped entity \"" << GetId() << "\".", ex);
00059       }
00060    }
00061 
00062    /****************************************/
00063    /****************************************/
00064 
00065    void CLEDEquippedEntity::Reset() {
00066       for(CLEDEntity::TList::iterator it = m_tLEDs.begin();
00067           it != m_tLEDs.end();
00068           ++it) {
00069          (*it)->Reset();
00070       }
00071    }
00072 
00073    /****************************************/
00074    /****************************************/
00075 
00076    void CLEDEquippedEntity::AddLED(const CVector3& c_position,
00077                                    const CColor& c_color) {
00078       CLEDEntity* pcLED =
00079          new CLEDEntity(
00080             this,
00081             std::string("led_") + ToString(m_tLEDs.size()),
00082             c_position,
00083             c_color);
00084       m_tLEDs.push_back(pcLED);
00085       AddComponent(*pcLED);
00086    }
00087 
00088    /****************************************/
00089    /****************************************/
00090 
00091    void CLEDEquippedEntity::AddLEDRing(const CVector3& c_center,
00092                                        Real f_radius,
00093                                        const CRadians& c_start_angle,
00094                                        UInt32 un_num_leds,
00095                                        const CColor& c_color) {
00096       CRadians cLEDSpacing = CRadians::TWO_PI / un_num_leds;
00097       CRadians cAngle;
00098       CVector3 cPos;
00099       for(UInt32 i = 0; i < un_num_leds; ++i) {
00100          cAngle = c_start_angle + i * cLEDSpacing;
00101          cAngle.SignedNormalize();
00102          cPos.Set(f_radius, 0.0f, 0.0f);
00103          cPos.RotateZ(cAngle);
00104          cPos += c_center;
00105          AddLED(cPos, c_color);
00106       }
00107    }
00108 
00109    /****************************************/
00110    /****************************************/
00111 
00112    CLEDEntity& CLEDEquippedEntity::GetLED(UInt32 un_index) {
00113       ARGOS_ASSERT(un_index < m_tLEDs.size(),
00114                    "CLEDEquippedEntity::GetLED(), id=\"" <<
00115                    GetId() <<
00116                    "\": index out of bounds: un_index = " <<
00117                    un_index <<
00118                    ", m_tLEDs.size() = " <<
00119                    m_tLEDs.size());
00120       return *m_tLEDs[un_index];
00121    }
00122 
00123    /****************************************/
00124    /****************************************/
00125 
00126    void CLEDEquippedEntity::SetLEDPosition(UInt32 un_index,
00127                                            const CVector3& c_position) {
00128       ARGOS_ASSERT(un_index < m_tLEDs.size(),
00129                    "CLEDEquippedEntity::SetLEDPosition(), id=\"" <<
00130                    GetId() <<
00131                    "\": index out of bounds: un_index = " <<
00132                    un_index <<
00133                    ", m_tLEDs.size() = " <<
00134                    m_tLEDs.size());
00135       m_tLEDs[un_index]->SetPosition(c_position);
00136    }
00137 
00138    /****************************************/
00139    /****************************************/
00140 
00141    void CLEDEquippedEntity::SetLEDColor(UInt32 un_index,
00142                                         const CColor& c_color) {
00143       ARGOS_ASSERT(un_index < m_tLEDs.size(),
00144                    "CLEDEquippedEntity::SetLEDColor(), id=\"" <<
00145                    GetId() <<
00146                    "\": index out of bounds: un_index = " <<
00147                    un_index <<
00148                    ", m_tLEDs.size() = " <<
00149                    m_tLEDs.size());
00150       m_tLEDs[un_index]->SetColor(c_color);
00151    }
00152 
00153    /****************************************/
00154    /****************************************/
00155 
00156    void CLEDEquippedEntity::SetAllLEDsColors(const CColor& c_color) {
00157       for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
00158          m_tLEDs[i]->SetColor(c_color);
00159       }
00160    }
00161 
00162    /****************************************/
00163    /****************************************/
00164 
00165    void CLEDEquippedEntity::SetAllLEDsColors(const std::vector<CColor>& vec_colors) {
00166       if(vec_colors.size() == m_tLEDs.size()) {
00167          for(UInt32 i = 0; i < vec_colors.size(); ++i) {
00168             m_tLEDs[i]->SetColor(vec_colors[i]);
00169          }
00170       }
00171       else {
00172          THROW_ARGOSEXCEPTION(
00173             "CLEDEquippedEntity::SetAllLEDsColors(), id=\"" <<
00174             GetId() <<
00175             "\": number of LEDs (" <<
00176             m_tLEDs.size() <<
00177             ") is lower than the passed color vector size (" <<
00178             vec_colors.size() <<
00179             ")");
00180       }
00181    }
00182 
00183    /****************************************/
00184    /****************************************/
00185 
00186    void CLEDEquippedEntity::UpdateComponents() {
00187       if(HasReferenceEntity()) {
00188          /* Set LED position wrt reference */
00189          CVector3 cLEDPosition;
00190          for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
00191             cLEDPosition = m_vecLEDOffsetPositions[i];
00192             cLEDPosition.Rotate(m_pcReferenceEntity->GetOrientation());
00193             cLEDPosition += m_pcReferenceEntity->GetPosition();
00194             SetLEDPosition(i, cLEDPosition);
00195          }
00196       }
00197    }
00198 
00199    /****************************************/
00200    /****************************************/
00201 
00202    void CLEDEquippedEntity::AddToMedium(CLEDMedium& c_medium) {
00203       for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
00204          m_tLEDs[i]->AddToMedium(c_medium);
00205       }
00206    }
00207 
00208    /****************************************/
00209    /****************************************/
00210 
00211    void CLEDEquippedEntity::RemoveFromMedium(CLEDMedium& c_medium) {
00212       for(UInt32 i = 0; i < m_tLEDs.size(); ++i) {
00213          m_tLEDs[i]->RemoveFromMedium(c_medium);
00214       }
00215    }
00216 
00217    /****************************************/
00218    /****************************************/
00219 
00220    REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CLEDEquippedEntity);
00221 
00222    /****************************************/
00223    /****************************************/
00224 
00225 }