camera_sensor_led_detector_algorithm.cpp
Go to the documentation of this file.
1 
8 
9 #include <argos3/core/simulator/simulator.h>
10 #include <argos3/core/utility/math/matrix/squarematrix.h>
11 #include <argos3/core/utility/math/matrix/transformationmatrix3.h>
12 
13 #include <argos3/plugins/simulator/media/led_medium.h>
14 #include <argos3/plugins/simulator/entities/led_entity.h>
15 
16 namespace argos {
17 
18  /****************************************/
19  /****************************************/
20 
22  m_bShowRays(false),
23  m_pcLEDIndex(nullptr) {}
24 
25  /****************************************/
26  /****************************************/
27 
29  try {
30  /* Parent class init */
32  /* Show rays? */
33  GetNodeAttributeOrDefault(t_tree, "show_rays", m_bShowRays, m_bShowRays);
34  /* Get LED medium from id specified in the XML */
35  std::string strMedium;
36  GetNodeAttribute(t_tree, "medium", strMedium);
37  m_pcLEDIndex = &(CSimulator::GetInstance().GetMedium<CLEDMedium>(strMedium).GetIndex());
38  }
39  catch(CARGoSException& ex) {
40  THROW_ARGOSEXCEPTION_NESTED("Error initializing the LED detector algorithm", ex);
41  }
42  }
43 
44  /****************************************/
45  /****************************************/
46 
48  const std::array<CPlane, 6>& arr_frustum_planes,
49  const CTransformationMatrix3& c_world_to_camera_transform,
50  const CVector3& c_camera_location,
51  const CVector3& c_bounding_box_position,
52  const CVector3& c_bounding_box_half_extents) {
53  /* Define a class for the update operation */
54  CUpdateOperation cUpdateOperation(c_projection_matrix, arr_frustum_planes,
55  c_world_to_camera_transform, c_camera_location,
56  *this);
57  /* Clear out readings from last update */
58  m_vecReadings.clear();
59  /* Clear out checked rays from last update */
60  m_vecCheckedRays.clear();
61  /* Run the operation */
62  m_pcLEDIndex->ForEntitiesInBoxRange(c_bounding_box_position,
63  c_bounding_box_half_extents,
64  cUpdateOperation);
65  }
66 
67  /****************************************/
68  /****************************************/
69 
71  "led_detector",
72  "Michael Allwright [allsey87@gmail.com]",
73  "1.0",
74  "This algorithm detects nearby LEDs seen by the camera and\n"
75  "returns the X and Y coordinates on the sensor",
76  "This algorithm detects nearby LEDs seen by the camera and\n"
77  "returns the X and Y coordinates on the sensor",
78  "Under development");
79 }
void GetNodeAttribute(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer)
Returns the value of a node&#39;s attribute.
A 3D vector class.
Definition: vector3.h:29
#define THROW_ARGOSEXCEPTION_NESTED(message, nested)
This macro throws an ARGoS exception with the passed message and nesting the passed exception...
void GetNodeAttributeOrDefault(TConfigurationNode &t_node, const std::string &str_attribute, T &t_buffer, const T &t_default)
Returns the value of a node&#39;s attribute, or the passed default value.
virtual void Update(const CSquareMatrix< 3 > &c_projection_matrix, const std::array< CPlane, 6 > &arr_frustum_planes, const CTransformationMatrix3 &c_world_to_camera_transform, const CVector3 &c_camera_location, const CVector3 &c_bounding_box_position, const CVector3 &c_bounding_box_half_extents)
REGISTER_CAMERA_SENSOR_ALGORITHM(CCameraSensorDirectionalLEDDetectorAlgorithm, "directional_led_detector", "Michael Allwright [allsey87@gmail.com]", "1.0", "This algorithm detects nearby LEDs seen by the camera and\ "returns the X and Y coordinates on the sensor", "This algorithm detects nearby LEDs seen by the camera and\" "returns the X and Y coordinates on the sensor", "Under development")
The exception that wraps all errors in ARGoS.
virtual void Init(TConfigurationNode &t_node)
Initializes the resource.
ticpp::Element TConfigurationNode
The ARGoS configuration XML node.
static CSimulator & GetInstance()
Returns the instance to the CSimulator class.
Definition: simulator.cpp:78
std::vector< std::pair< bool, CRay3 > > m_vecCheckedRays
virtual void Init(TConfigurationNode &t_tree)
Initializes the resource.
This class provides the most general interface to a camera.
T & GetMedium(const std::string &str_id)
Returns a reference to a medium.
Definition: simulator.h:129
The namespace containing all the ARGoS related code.
Definition: ci_actuator.h:12