ARGoS  3
A parallel, multi-engine simulator for swarm robotics
plugins/simulator/entities/box_entity.cpp
Go to the documentation of this file.
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 }