ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/robots/e-puck/simulator/epuck_entity.cpp
Go to the documentation of this file.
00001 
00007 #include "epuck_entity.h"
00008 
00009 #include <argos3/core/utility/math/matrix/rotationmatrix3.h>
00010 #include <argos3/core/simulator/space/space.h>
00011 #include <argos3/core/simulator/entity/controllable_entity.h>
00012 #include <argos3/core/simulator/entity/embodied_entity.h>
00013 #include <argos3/plugins/simulator/entities/rab_equipped_entity.h>
00014 #include <argos3/plugins/simulator/entities/ground_sensor_equipped_entity.h>
00015 #include <argos3/plugins/simulator/entities/led_equipped_entity.h>
00016 #include <argos3/plugins/simulator/entities/light_sensor_equipped_entity.h>
00017 #include <argos3/plugins/simulator/entities/proximity_sensor_equipped_entity.h>
00018 
00019 namespace argos {
00020 
00021    /****************************************/
00022    /****************************************/
00023 
00024    static const Real BODY_RADIUS                = 0.035f;
00025    static const Real BODY_HEIGHT                = 0.086f;
00026 
00027    static const Real INTERWHEEL_DISTANCE        = 0.053f;
00028    static const Real HALF_INTERWHEEL_DISTANCE   = INTERWHEEL_DISTANCE * 0.5f;
00029    static const Real WHEEL_RADIUS               = 0.0205f;
00030 
00031    static const Real PROXIMITY_SENSOR_RING_ELEVATION       = 0.06f;
00032    static const Real PROXIMITY_SENSOR_RING_RADIUS          = BODY_RADIUS;
00033    static const CRadians PROXIMITY_SENSOR_RING_START_ANGLE = CRadians((ARGOS_PI / 8.0f) * 0.5f);
00034    static const Real PROXIMITY_SENSOR_RING_RANGE           = 0.1f;
00035 
00036    static const CRadians LED_RING_START_ANGLE   = CRadians((ARGOS_PI / 8.0f) * 0.5f);
00037    static const Real LED_RING_RADIUS            = BODY_RADIUS + 0.007;
00038    static const Real LED_RING_ELEVATION         = 0.086f;
00039    static const Real RAB_ELEVATION              = LED_RING_ELEVATION;
00040 
00041    /****************************************/
00042    /****************************************/
00043 
00044    CEPuckEntity::CEPuckEntity() :
00045       CComposableEntity(NULL),
00046       m_pcControllableEntity(NULL),
00047       m_pcEmbodiedEntity(NULL),
00048       m_pcGroundSensorEquippedEntity(NULL),
00049       m_pcLEDEquippedEntity(NULL),
00050       m_pcLightSensorEquippedEntity(NULL),
00051       m_pcProximitySensorEquippedEntity(NULL),
00052       m_pcRABEquippedEntity(NULL),
00053       m_pcWheeledEntity(NULL) {
00054    }
00055 
00056    /****************************************/
00057    /****************************************/
00058 
00059    void CEPuckEntity::Init(TConfigurationNode& t_tree) {
00060       try {
00061          /*
00062           * Init parent
00063           */
00064          CComposableEntity::Init(t_tree);
00065          /*
00066           * Create and init components
00067           */
00068          /* Embodied entity */
00069          m_pcEmbodiedEntity = new CEmbodiedEntity(this);
00070          AddComponent(*m_pcEmbodiedEntity);
00071          m_pcEmbodiedEntity->Init(GetNode(t_tree, "body"));
00072          /* Wheeled entity and wheel positions (left, right) */
00073          m_pcWheeledEntity = new CWheeledEntity(this, "wheels_0", 2);
00074          AddComponent(*m_pcWheeledEntity);
00075          m_pcWheeledEntity->SetWheel(0, CVector3(0.0f,  HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
00076          m_pcWheeledEntity->SetWheel(1, CVector3(0.0f, -HALF_INTERWHEEL_DISTANCE, 0.0f), WHEEL_RADIUS);
00077          /* LED equipped entity, with LEDs [0-11] and beacon [12] */
00078          m_pcLEDEquippedEntity = new CLEDEquippedEntity(this,
00079                                                         "leds_0",
00080                                                         m_pcEmbodiedEntity);
00081          AddComponent(*m_pcLEDEquippedEntity);
00082          m_pcLEDEquippedEntity->AddLEDRing(
00083             CVector3(0.0f, 0.0f, LED_RING_ELEVATION),
00084             LED_RING_RADIUS,
00085             LED_RING_START_ANGLE,
00086             8);
00087          /* Proximity sensor equipped entity */
00088          m_pcProximitySensorEquippedEntity =
00089             new CProximitySensorEquippedEntity(this,
00090                                                "proximity_0");
00091          AddComponent(*m_pcProximitySensorEquippedEntity);
00092          m_pcProximitySensorEquippedEntity->AddSensorRing(
00093             CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
00094             PROXIMITY_SENSOR_RING_RADIUS,
00095             PROXIMITY_SENSOR_RING_START_ANGLE,
00096             PROXIMITY_SENSOR_RING_RANGE,
00097             8);
00098          /* Light sensor equipped entity */
00099          m_pcLightSensorEquippedEntity =
00100             new CLightSensorEquippedEntity(this,
00101                                            "light_0");
00102          AddComponent(*m_pcLightSensorEquippedEntity);
00103          m_pcLightSensorEquippedEntity->AddSensorRing(
00104             CVector3(0.0f, 0.0f, PROXIMITY_SENSOR_RING_ELEVATION),
00105             PROXIMITY_SENSOR_RING_RADIUS,
00106             PROXIMITY_SENSOR_RING_START_ANGLE,
00107             PROXIMITY_SENSOR_RING_RANGE,
00108             8);
00109          /* Ground sensor equipped entity */
00110          m_pcGroundSensorEquippedEntity =
00111             new CGroundSensorEquippedEntity(this,
00112                                             "ground_0");
00113          AddComponent(*m_pcGroundSensorEquippedEntity);
00114          m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f, -0.009f),
00115                                                    CGroundSensorEquippedEntity::TYPE_GRAYSCALE);
00116          m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f,  0.0f),
00117                                                    CGroundSensorEquippedEntity::TYPE_GRAYSCALE);
00118          m_pcGroundSensorEquippedEntity->AddSensor(CVector2(0.03f,  0.009f),
00119                                                    CGroundSensorEquippedEntity::TYPE_GRAYSCALE);
00120          /* RAB equipped entity */
00121          Real fRange = 0.8f;
00122          GetNodeAttributeOrDefault(t_tree, "rab_range", fRange, fRange);
00123          m_pcRABEquippedEntity = new CRABEquippedEntity(this,
00124                                                         "rab_0",
00125                                                         2,
00126                                                         fRange,
00127                                                         *m_pcEmbodiedEntity,
00128                                                         CVector3(0.0f, 0.0f, RAB_ELEVATION));
00129          AddComponent(*m_pcRABEquippedEntity);
00130          /* Controllable entity
00131             It must be the last one, for actuators/sensors to link to composing entities correctly */
00132          m_pcControllableEntity = new CControllableEntity(this);
00133          AddComponent(*m_pcControllableEntity);
00134          m_pcControllableEntity->Init(GetNode(t_tree, "controller"));
00135          /* Update components */
00136          UpdateComponents();
00137       }
00138       catch(CARGoSException& ex) {
00139          THROW_ARGOSEXCEPTION_NESTED("Failed to initialize entity \"" << GetId() << "\".", ex);
00140       }
00141    }
00142 
00143    /****************************************/
00144    /****************************************/
00145 
00146    void CEPuckEntity::Reset() {
00147       /* Reset all components */
00148       CComposableEntity::Reset();
00149       /* Update components */
00150       UpdateComponents();
00151    }
00152 
00153    /****************************************/
00154    /****************************************/
00155 
00156    void CEPuckEntity::Destroy() {
00157       CComposableEntity::Destroy();
00158    }
00159 
00160    /****************************************/
00161    /****************************************/
00162 
00163 #define UPDATE(COMPONENT) if(COMPONENT->IsEnabled()) COMPONENT->Update();
00164 
00165    void CEPuckEntity::UpdateComponents() {
00166       UPDATE(m_pcRABEquippedEntity);
00167       UPDATE(m_pcLEDEquippedEntity);
00168    }
00169 
00170    /****************************************/
00171    /****************************************/
00172    
00173    REGISTER_ENTITY(CEPuckEntity,
00174                    "e-puck",
00175                    "Carlo Pinciroli [ilpincy@gmail.com]",
00176                    "1.0",
00177                    "The e-puck robot.",
00178                    "The e-puck is a open-hardware, extensible robot intended for education. In its\n"
00179                    "simplest form, it is a two-wheeled robot equipped with proximity sensors,\n"
00180                    "ground sensors, light sensors, a microphone, a frontal camera, and a ring of\n"
00181                    "red LEDs. More information is available at http://www.epuck.org\n\n"
00182                    "REQUIRED XML CONFIGURATION\n\n"
00183                    "  <arena ...>\n"
00184                    "    ...\n"
00185                    "    <e-puck id=\"fb0\">\n"
00186                    "      <body position=\"0.4,2.3,0.25\" orientation=\"45,90,0\" />\n"
00187                    "      <controller config=\"mycntrl\" />\n"
00188                    "    </e-puck>\n"
00189                    "    ...\n"
00190                    "  </arena>\n\n"
00191                    "The 'id' attribute is necessary and must be unique among the entities. If two\n"
00192                    "entities share the same id, initialization aborts.\n"
00193                    "The 'body/position' attribute specifies the position of the pucktom point of the\n"
00194                    "e-puck in the arena. When the robot is untranslated and unrotated, the\n"
00195                    "pucktom point is in the origin and it is defined as the middle point between\n"
00196                    "the two wheels on the XY plane and the lowest point of the robot on the Z\n"
00197                    "axis, that is the point where the wheels touch the floor. The attribute values\n"
00198                    "are in the X,Y,Z order.\n"
00199                    "The 'body/orientation' attribute specifies the orientation of the e-puck. All\n"
00200                    "rotations are performed with respect to the pucktom point. The order of the\n"
00201                    "angles is Z,Y,X, which means that the first number corresponds to the rotation\n"
00202                    "around the Z axis, the second around Y and the last around X. This reflects\n"
00203                    "the internal convention used in ARGoS, in which rotations are performed in\n"
00204                    "that order. Angles are expressed in degrees. When the robot is unrotated, it\n"
00205                    "is oriented along the X axis.\n"
00206                    "The 'controller/config' attribute is used to assign a controller to the\n"
00207                    "e-puck. The value of the attribute must be set to the id of a previously\n"
00208                    "defined controller. Controllers are defined in the <controllers> XML subtree.\n\n"
00209                    "OPTIONAL XML CONFIGURATION\n\n"
00210                    "None for the time being.\n",
00211                    "Under development"
00212       );
00213 
00214    /****************************************/
00215    /****************************************/
00216 
00217    REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CEPuckEntity);
00218 
00219    /****************************************/
00220    /****************************************/
00221 
00222 }