Add missing directional LED sensor and actuator classes

src/plugins/robots/generic/CMakeLists.txt

@@ -7,12 +7,14 @@ set(ARGOS3_HEADERS_PLUGINS_ROBOTS_GENERIC_CONTROLINTERFACE
   control_interface/ci_battery_sensor.h
   control_interface/ci_camera_sensor.h
   control_interface/ci_camera_sensor_algorithm.h
+  control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_directional_led_detector_algorithm.h
   control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_led_detector_algorithm.h
   control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_tag_detector_algorithm.h
   control_interface/ci_colored_blob_omnidirectional_camera_sensor.h
   control_interface/ci_colored_blob_perspective_camera_sensor.h
   control_interface/ci_differential_steering_actuator.h
   control_interface/ci_differential_steering_sensor.h
+  control_interface/ci_directional_leds_actuator.h
   control_interface/ci_gripper_actuator.h
   control_interface/ci_ground_sensor.h
   control_interface/ci_leds_actuator.h
@@ -65,12 +67,14 @@ set(ARGOS3_SOURCES_PLUGINS_ROBOTS_GENERIC
   control_interface/ci_altitude_sensor.cpp
   control_interface/ci_battery_sensor.cpp
   control_interface/ci_camera_sensor.cpp
+  control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_directional_led_detector_algorithm.cpp
   control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_led_detector_algorithm.cpp
   control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_tag_detector_algorithm.cpp
   control_interface/ci_colored_blob_omnidirectional_camera_sensor.cpp
   control_interface/ci_colored_blob_perspective_camera_sensor.cpp
   control_interface/ci_differential_steering_actuator.cpp
   control_interface/ci_differential_steering_sensor.cpp
+  control_interface/ci_directional_leds_actuator.cpp
   control_interface/ci_gripper_actuator.cpp
   control_interface/ci_ground_sensor.cpp
   control_interface/ci_leds_actuator.cpp

src/plugins/robots/generic/control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_directional_led_detector_algorithm.cpp

@@ -0,0 +1,56 @@
+/**
+ * @file <argos3/plugins/robots/generic/control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_directional_led_detector_algorithm.cpp>
+ *
+ * @author Michael Allwright - <[email protected]>
+ */
+
+#include "ci_camera_sensor_directional_led_detector_algorithm.h"
+
+#ifdef ARGOS_WITH_LUA
+#include <argos3/core/wrappers/lua/lua_utility.h>
+#endif
+
+
+namespace argos {
+
+   /****************************************/
+   /****************************************/
+
+#ifdef ARGOS_WITH_LUA
+   void CCI_CameraSensorDirectionalLEDDetectorAlgorithm::CreateLuaState(lua_State* pt_lua_state) {
+      for(size_t i = 0; i < m_vecReadings.size(); ++i) {
+         CLuaUtility::StartTable(pt_lua_state, i + 1);
+         CLuaUtility::AddToTable(pt_lua_state, "color", m_vecReadings[i].Color);
+         CLuaUtility::AddToTable(pt_lua_state, "center", m_vecReadings[i].Center);
+         CLuaUtility::EndTable(pt_lua_state);
+      }
+   }
+#endif
+
+   /****************************************/
+   /****************************************/
+
+#ifdef ARGOS_WITH_LUA
+   void CCI_CameraSensorDirectionalLEDDetectorAlgorithm::ReadingsToLuaState(lua_State* pt_lua_state) {
+      size_t unLastReadingsNum = lua_rawlen(pt_lua_state, -1);
+      for(size_t i = 0; i < m_vecReadings.size(); ++i) {
+         CLuaUtility::StartTable(pt_lua_state, i + 1);
+         CLuaUtility::AddToTable(pt_lua_state, "color", m_vecReadings[i].Color);
+         CLuaUtility::AddToTable(pt_lua_state, "center", m_vecReadings[i].Center);
+         CLuaUtility::EndTable(pt_lua_state);
+      }
+      if(m_vecReadings.size() < unLastReadingsNum) {
+         /* Remove extra readings from last update by setting them to nil */
+         for(size_t i = m_vecReadings.size() + 1; i <= unLastReadingsNum; ++i) {
+            lua_pushnumber(pt_lua_state, i);
+            lua_pushnil   (pt_lua_state);
+            lua_settable  (pt_lua_state, -3);
+         }
+      }
+   }
+#endif
+
+   /****************************************/
+   /****************************************/
+
+}

src/plugins/robots/generic/control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_directional_led_detector_algorithm.h

@@ -0,0 +1,85 @@
+/**
+ * @file <argos3/plugins/robots/generic/control_interface/ci_camera_sensor_algorithms/ci_camera_sensor_directional_led_detector_algorithm.h>
+ *
+ * @author Michael Allwright - <[email protected]>
+ */
+
+#ifndef CI_CAMERAS_SENSOR_DIRECTIONAL_LED_DETECTOR_ALGORITHM_H
+#define CI_CAMERAS_SENSOR_DIRECTIONAL_LED_DETECTOR_ALGORITHM_H
+
+namespace argos {
+	class CCI_CameraSensorDirectionalLEDDetectorAlgorithm;
+}
+
+#include <argos3/plugins/robots/generic/control_interface/ci_camera_sensor_algorithm.h>
+#include <argos3/core/utility/datatypes/color.h>
+#include <argos3/core/utility/math/vector2.h>
+#include <argos3/core/utility/datatypes/datatypes.h>
+
+#ifdef ARGOS_WITH_LUA
+extern "C" {
+#include <lua.h>
+#include <lualib.h>
+#include <lauxlib.h>
+}
+#endif
+
+
+namespace argos {
+
+   class CCI_CameraSensorDirectionalLEDDetectorAlgorithm : virtual public CCI_CameraSensorAlgorithm {
+
+   public:
+
+      struct SReading {
+         /* Color */
+         CColor Color;
+         /* Coordinates in image */
+         CVector2 Center;
+         /**
+          * Constructor
+          * @param c_color Observation color
+          * @param c_center Image coordinates of the observation
+          */
+         SReading(const CColor& c_color,
+                  const CVector2& c_center) :
+            Color(c_color),
+            Center(c_center) {}
+      };
+
+   public:
+
+      /**
+       * Constructor
+       */
+      CCI_CameraSensorDirectionalLEDDetectorAlgorithm() {}
+
+      /**
+       * Destructor
+       */
+      virtual ~CCI_CameraSensorDirectionalLEDDetectorAlgorithm() {}
+
+      const std::vector<SReading>& GetReadings() const {
+         return m_vecReadings;
+      }
+
+#ifdef ARGOS_WITH_LUA
+      virtual void CreateLuaState(lua_State* pt_lua_state);
+
+      virtual void ReadingsToLuaState(lua_State* pt_lua_state);
+
+      virtual const std::string& GetId() {
+         static std::string strId("directional_led_detector");
+         return strId;
+      }
+#endif
+
+   protected:
+
+      std::vector<SReading> m_vecReadings;
+
+   };
+
+}
+
+#endif

src/plugins/robots/generic/control_interface/ci_directional_leds_actuator.cpp

@@ -0,0 +1,162 @@
+/**
+ * @file <argos3/plugins/robots/generic/control_interface/ci_directional_leds_actuator.cpp>
+ *
+ * @author Michael Allwright <[email protected]>
+ */
+
+#include "ci_directional_leds_actuator.h"
+
+#ifdef ARGOS_WITH_LUA
+#include <argos3/core/wrappers/lua/lua_utility.h>
+#endif
+
+namespace argos {
+
+   /****************************************/
+   /****************************************/
+
+#ifdef ARGOS_WITH_LUA
+   /*
+    * This function expects the stack to have either two or four arguments.
+    * The first argument must always be the index of the LED to set.
+    * Then, in case two arguments are passed, the second argument can be the string
+    * definition of a color. In case of four arguments, the RGB values are expected.
+    */
+   int LuaDirectionalLEDSetSingleColor(lua_State* pt_lua_state) {
+      /* Check parameters */
+      if(lua_gettop(pt_lua_state) != 2 && lua_gettop(pt_lua_state) != 4) {
+         return luaL_error(pt_lua_state, "robot.directional_leds.set_single_color() expects 2 or 4 arguments");
+      }
+      luaL_checktype(pt_lua_state, 1, LUA_TNUMBER);
+      size_t unIdx = lua_tonumber(pt_lua_state, 1);
+      /* Get reference to actuator */
+      CCI_DirectionalLEDsActuator* pcAct = 
+         CLuaUtility::GetDeviceInstance<CCI_DirectionalLEDsActuator>(pt_lua_state, "directional_leds");
+      if(unIdx < 1 || unIdx > pcAct->GetNumLEDs()) {
+         return luaL_error(pt_lua_state, "passed index %d out of bounds [1,%d]", unIdx, pcAct->GetNumLEDs());
+      }
+      /* Create color buffer */
+      CColor cColor;
+      if(lua_gettop(pt_lua_state) == 2) {
+         luaL_checktype(pt_lua_state, 2, LUA_TSTRING);
+         try {
+            cColor.Set(lua_tostring(pt_lua_state, 2));
+         }
+         catch(CARGoSException& ex) {
+            return luaL_error(pt_lua_state, ex.what());
+         }
+      }
+      else {
+         luaL_checktype(pt_lua_state, 2, LUA_TNUMBER);
+         luaL_checktype(pt_lua_state, 3, LUA_TNUMBER);
+         luaL_checktype(pt_lua_state, 4, LUA_TNUMBER);
+         cColor.Set(lua_tonumber(pt_lua_state, 2),
+                    lua_tonumber(pt_lua_state, 3),
+                    lua_tonumber(pt_lua_state, 4));
+      }
+      /* Perform action */
+      pcAct->SetSingleColor(unIdx - 1, cColor);
+      return 0;
+   }
+
+   /*
+    * This function expects the stack to have either one or three arguments.
+    * In case one argument is passed, it must be the string definition of a color.
+    * In case of three arguments, the RGB values are expected.
+    */
+   int LuaDirectionalLEDSetAllColors(lua_State* pt_lua_state) {
+      /* Check parameters */
+      if(lua_gettop(pt_lua_state) != 1 && lua_gettop(pt_lua_state) != 3) {
+         return luaL_error(pt_lua_state, "robot.directional_leds.set_all_colors() expects 1 or 3 arguments");
+      }
+      /* Create color buffer */
+      CColor cColor;
+      if(lua_gettop(pt_lua_state) == 1) {
+         luaL_checktype(pt_lua_state, 1, LUA_TSTRING);
+         try {
+            cColor.Set(lua_tostring(pt_lua_state, 1));
+         }
+         catch(CARGoSException& ex) {
+            return luaL_error(pt_lua_state, ex.what());
+         }
+      }
+      else {
+         luaL_checktype(pt_lua_state, 1, LUA_TNUMBER);
+         luaL_checktype(pt_lua_state, 2, LUA_TNUMBER);
+         luaL_checktype(pt_lua_state, 3, LUA_TNUMBER);
+         cColor.Set(lua_tonumber(pt_lua_state, 1),
+                    lua_tonumber(pt_lua_state, 2),
+                    lua_tonumber(pt_lua_state, 3));
+      }
+      /* Perform action */
+      CLuaUtility::GetDeviceInstance<CCI_DirectionalLEDsActuator>(pt_lua_state, "directional_leds")->
+         SetAllColors(cColor);
+      return 0;
+   }
+#endif
+
+   /****************************************/
+   /****************************************/
+
+   size_t CCI_DirectionalLEDsActuator::GetNumLEDs() const {
+     return m_tSettings.size();
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CCI_DirectionalLEDsActuator::SetSingleColor(UInt32 un_led_number,
+                                         const CColor& c_color) {
+      m_tSettings[un_led_number] = c_color;
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CCI_DirectionalLEDsActuator::SetAllColors(const CColor& c_color) {
+      for(size_t i = 0; i < m_tSettings.size(); ++i) {
+         m_tSettings[i] = c_color;
+      }
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CCI_DirectionalLEDsActuator::SetAllColors(const TSettings& c_colors) {
+      m_tSettings = c_colors;
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CCI_DirectionalLEDsActuator::SetSingleIntensity(UInt32 un_led_number,
+                                             UInt8 un_intensity) {
+      m_tSettings[un_led_number].SetAlpha(un_intensity);
+   }
+
+   /****************************************/
+   /****************************************/
+
+   void CCI_DirectionalLEDsActuator::SetAllIntensities(UInt8 un_intensity) {
+      for(size_t i = 0; i < m_tSettings.size(); ++i) {
+         m_tSettings[i].SetAlpha(un_intensity);
+      }
+   }
+
+   /****************************************/
+   /****************************************/
+
+#ifdef ARGOS_WITH_LUA
+   void CCI_DirectionalLEDsActuator::CreateLuaState(lua_State* pt_lua_state) {
+      CLuaUtility::OpenRobotStateTable(pt_lua_state, "directional_leds");
+      CLuaUtility::AddToTable(pt_lua_state, "_instance", this);
+      CLuaUtility::AddToTable(pt_lua_state, "set_single_color", &LuaDirectionalLEDSetSingleColor);
+      CLuaUtility::AddToTable(pt_lua_state, "set_all_colors", &LuaDirectionalLEDSetAllColors);
+      CLuaUtility::CloseRobotStateTable(pt_lua_state);
+   }
+#endif
+
+   /****************************************/
+   /****************************************/
+
+}