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