ARGoS
3
A parallel, multi-engine simulator for swarm robotics
|
00001 00007 #include "cylinder_entity.h" 00008 #include <argos3/core/utility/math/matrix/rotationmatrix3.h> 00009 #include <argos3/core/simulator/space/space.h> 00010 #include <argos3/core/simulator/simulator.h> 00011 #include <argos3/plugins/simulator/media/led_medium.h> 00012 00013 namespace argos { 00014 00015 /****************************************/ 00016 /****************************************/ 00017 00018 CCylinderEntity::CCylinderEntity() : 00019 CComposableEntity(NULL), 00020 m_pcEmbodiedEntity(NULL), 00021 m_pcLEDEquippedEntity(NULL), 00022 m_fMass(1.0f), 00023 m_pcLEDMedium(NULL) { 00024 } 00025 00026 /****************************************/ 00027 /****************************************/ 00028 00029 CCylinderEntity::CCylinderEntity(const std::string& str_id, 00030 const CVector3& c_position, 00031 const CQuaternion& c_orientation, 00032 bool b_movable, 00033 Real f_radius, 00034 Real f_height, 00035 Real f_mass) : 00036 CComposableEntity(NULL, str_id), 00037 m_pcEmbodiedEntity( 00038 new CEmbodiedEntity(this, 00039 "body_0", 00040 c_position, 00041 c_orientation, 00042 b_movable)), 00043 m_pcLEDEquippedEntity( 00044 new CLEDEquippedEntity(this, 00045 "leds_0", 00046 m_pcEmbodiedEntity)), 00047 m_fRadius(f_radius), 00048 m_fHeight(f_height), 00049 m_fMass(f_mass) { 00050 AddComponent(*m_pcEmbodiedEntity); 00051 AddComponent(*m_pcLEDEquippedEntity); 00052 } 00053 00054 /****************************************/ 00055 /****************************************/ 00056 00057 void CCylinderEntity::Init(TConfigurationNode& t_tree) { 00058 try { 00059 /* Init parent */ 00060 CComposableEntity::Init(t_tree); 00061 /* Parse XML to get the radius */ 00062 GetNodeAttribute(t_tree, "radius", m_fRadius); 00063 /* Parse XML to get the height */ 00064 GetNodeAttribute(t_tree, "height", m_fHeight); 00065 /* Parse XML to get the movable attribute */ 00066 bool bMovable; 00067 GetNodeAttribute(t_tree, "movable", bMovable); 00068 if(bMovable) { 00069 /* Parse XML to get the mass */ 00070 GetNodeAttribute(t_tree, "mass", m_fMass); 00071 } 00072 else { 00073 m_fMass = 0.0f; 00074 } 00075 /* Create embodied entity using parsed data */ 00076 m_pcEmbodiedEntity = new CEmbodiedEntity(this); 00077 AddComponent(*m_pcEmbodiedEntity); 00078 m_pcEmbodiedEntity->Init(GetNode(t_tree, "body")); 00079 m_pcEmbodiedEntity->SetMovable(bMovable); 00080 /* Init LED equipped entity component */ 00081 m_pcLEDEquippedEntity = new CLEDEquippedEntity(this, 00082 m_pcEmbodiedEntity); 00083 AddComponent(*m_pcLEDEquippedEntity); 00084 if(NodeExists(t_tree, "leds")) { 00085 /* Create LED equipped entity 00086 * NOTE: the LEDs are not added to the medium yet 00087 */ 00088 m_pcLEDEquippedEntity->Init(GetNode(t_tree, "leds")); 00089 /* Add the LEDs to the medium */ 00090 std::string strMedium; 00091 GetNodeAttribute(GetNode(t_tree, "leds"), "medium", strMedium); 00092 m_pcLEDMedium = &CSimulator::GetInstance().GetMedium<CLEDMedium>(strMedium); 00093 m_pcLEDEquippedEntity->AddToMedium(*m_pcLEDMedium); 00094 } 00095 else { 00096 /* No LEDs added, no need to update this entity */ 00097 m_pcLEDEquippedEntity->Disable(); 00098 m_pcLEDEquippedEntity->SetCanBeEnabledIfDisabled(false); 00099 } 00100 UpdateComponents(); 00101 } 00102 catch(CARGoSException& ex) { 00103 THROW_ARGOSEXCEPTION_NESTED("Failed to initialize the cylinder entity.", ex); 00104 } 00105 } 00106 00107 /****************************************/ 00108 /****************************************/ 00109 00110 void CCylinderEntity::Reset() { 00111 /* Reset all components */ 00112 m_pcEmbodiedEntity->Reset(); 00113 m_pcLEDEquippedEntity->Reset(); 00114 /* Update components */ 00115 UpdateComponents(); 00116 } 00117 00118 /****************************************/ 00119 /****************************************/ 00120 00121 REGISTER_ENTITY(CCylinderEntity, 00122 "cylinder", 00123 "Carlo Pinciroli [ilpincy@gmail.com]", 00124 "1.0", 00125 "A stretchable cylinder.", 00126 "The cylinder entity can be used to model obstacles or cylinder-shaped\n" 00127 "grippable objects. The cylinder has a red LED on the center of one\n" 00128 "of the circular surfaces, that allows perception using the cameras.\n" 00129 "The cylinder can be movable or not. A movable object can be pushed\n" 00130 "and gripped. An unmovable object is pretty much like a column.\n\n" 00131 "REQUIRED XML CONFIGURATION\n\n" 00132 "To declare an unmovable object (i.e., a column) you need the following:\n\n" 00133 " <arena ...>\n" 00134 " ...\n" 00135 " <cylinder id=\"cyl1\" radius=\"0.8\" height=\"0.5\" movable=\"false\">\n" 00136 " <body position=\"0.4,2.3,0\" orientation=\"45,0,0\" />\n" 00137 " </cylinder>\n" 00138 " ...\n" 00139 " </arena>\n\n" 00140 "To declare a movable object you need the following:\n\n" 00141 " <arena ...>\n" 00142 " ...\n" 00143 " <cylinder id=\"cyl1\" radius=\"0.8\" height=\"0.5\"\n" 00144 " movable=\"true\" mass=\"2.5\">\n" 00145 " <body position=\"0.4,2.3,0\" orientation=\"45,0,0\" />\n" 00146 " </cylinder>\n" 00147 " ...\n" 00148 " </arena>\n\n" 00149 "The 'id' attribute is necessary and must be unique among the entities. If two\n" 00150 "entities share the same id, initialization aborts.\n" 00151 "The 'radius' and 'height' attributes specify the size of the cylinder. When\n" 00152 "you add a cylinder, imagine it initially unrotated and centered in the origin.\n" 00153 "The base of the cylinder, then, is parallel to the XY plane and its height\n" 00154 "goes with the Z axis.\n" 00155 "The 'movable' attribute specifies whether or not the object is movable. When\n" 00156 "set to 'false', the object is unmovable: if another object pushes against it,\n" 00157 "the cylinder won't move. When the attribute is set to 'true', the cylinder is\n" 00158 "movable upon pushing or gripping. When an object is movable, the 'mass'\n" 00159 "attribute is required.\n" 00160 "The 'mass' attribute quantifies the mass of the cylinder in kg.\n" 00161 "The 'body/position' attribute specifies the position of the base of the\n" 00162 "cylinder in the arena. The three values are in the X,Y,Z order.\n" 00163 "The 'body/orientation' attribute specifies the orientation of the cylinder. All\n" 00164 "rotations are performed with respect to the center of mass. The order of the\n" 00165 "angles is Z,Y,X, which means that the first number corresponds to the rotation\n" 00166 "around the Z axis, the second around Y and the last around X. This reflects\n" 00167 "the internal convention used in ARGoS, in which rotations are performed in\n" 00168 "that order. Angles are expressed in degrees.\n\n" 00169 "OPTIONAL XML CONFIGURATION\n\n" 00170 "It is possible to add any number of colored LEDs to the cylinder. In this way,\n" 00171 "the cylinder is visible with a robot camera. The position and color of the\n" 00172 "LEDs is specified with the following syntax:\n\n" 00173 " <arena ...>\n" 00174 " ...\n" 00175 " <cylinder id=\"cyl1\" radius=\"0.8\" height=\"0.5\"\n" 00176 " movable=\"true\" mass=\"2.5\">\n" 00177 " <body position=\"0.4,2.3,0\" orientation=\"45,0,0\" />\n" 00178 " <leds>\n" 00179 " <led position=\" 0.15, 0.15,0.15\" color=\"white\" />\n" 00180 " <led position=\"-0.15, 0.15,0\" color=\"red\" />\n" 00181 " <led position=\" 0.15, 0.15,0\" color=\"blue\" />\n" 00182 " <led position=\" 0.15,-0.15,0\" color=\"green\" />\n" 00183 " </leds>\n" 00184 " </cylinder>\n" 00185 " ...\n" 00186 " </arena>\n\n" 00187 "In the example, four LEDs are added around the cylinder. The LEDs have\n" 00188 "different colors and are located around the cylinder.\n", 00189 "Usable" 00190 ); 00191 00192 /****************************************/ 00193 /****************************************/ 00194 00195 REGISTER_STANDARD_SPACE_OPERATIONS_ON_COMPOSABLE(CCylinderEntity); 00196 00197 /****************************************/ 00198 /****************************************/ 00199 00200 }