ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
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 }