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