Permalink
Browse files

Threaded Leds into Man. Fixed led names. Started work on color manipu…

…lation
  • Loading branch information...
1 parent 1065fe6 commit 69703061a6fae2ba58675fa5b515439f52a5f617 Johannes Strom committed Jun 20, 2009
Showing with 123 additions and 53 deletions.
  1. +7 −2 Man.cpp
  2. +4 −1 Man.h
  3. +3 −2 TMan.cpp
  4. +2 −1 TMan.h
  5. +3 −2 TTMan.cpp
  6. +2 −1 TTMan.h
  7. +11 −0 corpus/Lights.cpp
  8. +2 −8 corpus/Lights.h
  9. +49 −7 corpus/NaoLights.cpp
  10. +4 −2 corpus/NaoLights.h
  11. +2 −1 corpus/cmake.man.corpus/CMakeLists.txt
  12. +12 −12 include/ALLedNames.h
  13. +11 −10 include/ALNames.h
  14. +6 −3 manmodule.cpp
  15. +4 −1 nao_soccer_player.cpp
  16. +1 −0 vision/Profiler.h
View
9 Man.cpp
@@ -47,11 +47,13 @@ Man::Man (shared_ptr<Sensors> _sensors,
shared_ptr<Transcriber> _transcriber,
shared_ptr<ImageTranscriber> _imageTranscriber,
shared_ptr<MotionEnactor> _enactor,
- shared_ptr<Synchro> synchro)
+ shared_ptr<Synchro> synchro,
+ shared_ptr<Lights> _lights)
: sensors(_sensors),
transcriber(_transcriber),
imageTranscriber(_imageTranscriber),
- enactor(_enactor)
+ enactor(_enactor),
+ lights(_lights)
{
// initialize system helper modules
profiler = shared_ptr<Profiler>(new Profiler(&micro_time));
@@ -174,6 +176,9 @@ Man::processFrame ()
#ifdef USE_NOGGIN
noggin->runStep();
#endif
+ PROF_ENTER(profiler.get(), P_LIGHTS);
+ lights->sendLights();
+ PROF_EXIT(profiler.get(), P_LIGHTS);
PROF_EXIT(profiler.get(), P_FINAL);
PROF_NFRAME(profiler.get());
View
5 Man.h
@@ -33,6 +33,7 @@
#include "ImageSubscriber.h"
#include "ImageTranscriber.h"
#include "Transcriber.h"
+#include "Lights.h"
#include "Common.h"
#include "Profiler.h"
#include "Sensors.h"
@@ -61,7 +62,8 @@ class Man : public ImageSubscriber
boost::shared_ptr<Transcriber> _transcriber,
boost::shared_ptr<ImageTranscriber> _imageTranscriber,
boost::shared_ptr<MotionEnactor> _enactor,
- boost::shared_ptr<Synchro> synchro);
+ boost::shared_ptr<Synchro> synchro,
+ boost::shared_ptr<Lights> _lights);
// destructor
virtual ~Man();
@@ -115,6 +117,7 @@ class Man : public ImageSubscriber
#ifdef USE_NOGGIN
boost::shared_ptr<Noggin> noggin;
#endif// USE_NOGGIN
+ boost::shared_ptr<Lights> lights;
};
View
5 TMan.cpp
@@ -6,8 +6,9 @@ TMan::TMan(boost::shared_ptr<Sensors> _sensors,
boost::shared_ptr<Transcriber> _transcriber,
boost::shared_ptr<ThreadedImageTranscriber> _imageTranscriber,
boost::shared_ptr<MotionEnactor> _enactor,
- boost::shared_ptr<Synchro> synchro)
- :Man(_sensors,_transcriber,_imageTranscriber,_enactor,synchro),
+ boost::shared_ptr<Synchro> synchro,
+ boost::shared_ptr<Lights> _lights)
+ :Man(_sensors,_transcriber,_imageTranscriber,_enactor,synchro,_lights),
threadedImageTranscriber(_imageTranscriber)
{
View
3 TMan.h
@@ -18,7 +18,8 @@ class TMan : public Man {
boost::shared_ptr<Transcriber> _transcriber,
boost::shared_ptr<ThreadedImageTranscriber> _imageTranscriber,
boost::shared_ptr<MotionEnactor> _enactor,
- boost::shared_ptr<Synchro> synchro);
+ boost::shared_ptr<Synchro> synchro,
+ boost::shared_ptr<Lights> _lights);
virtual ~TMan();
View
5 TTMan.cpp
@@ -8,8 +8,9 @@ TTMan::TTMan(shared_ptr<Sensors> _sensors,
shared_ptr<Transcriber> _transcriber,
shared_ptr<ThreadedImageTranscriber> _imageTranscriber,
shared_ptr<ThreadedMotionEnactor> _enactor,
- shared_ptr<Synchro> synchro)
- :Man(_sensors,_transcriber,_imageTranscriber,_enactor,synchro),
+ shared_ptr<Synchro> synchro,
+ shared_ptr<Lights> _lights)
+ :Man(_sensors,_transcriber,_imageTranscriber,_enactor,synchro,_lights),
threadedImageTranscriber(_imageTranscriber),
threadedEnactor(_enactor){}
View
3 TTMan.h
@@ -16,7 +16,8 @@ class TTMan : public Man {
boost::shared_ptr<Transcriber> _transcriber,
boost::shared_ptr<ThreadedImageTranscriber> _imageTranscriber,
boost::shared_ptr<ThreadedMotionEnactor> _enactor,
- boost::shared_ptr<Synchro> synchro);
+ boost::shared_ptr<Synchro> synchro,
+ boost::shared_ptr<Lights> _lights);
~TTMan();
void startSubThreads();
View
11 corpus/Lights.cpp
@@ -0,0 +1,11 @@
+#include "Lights.h"
+const std::string Lights::LED_NAMES[Lights::NUM_LED_NAMES] = {
+ "LeftEar","RightEar",
+ "LeftEye","RightEye",
+ "Chest",
+ "LeftFoot", "RightFoot"};
+
+
+Lights::Lights(){};
+Lights::~Lights(){};
+
View
10 corpus/Lights.h
@@ -15,8 +15,8 @@
*/
class Lights{
public:
- Lights(){};
- virtual ~Lights(){};
+ Lights();
+ virtual ~Lights();
public:
virtual void setRGB(std::string led_id, int rdbHex) = 0;
@@ -29,10 +29,4 @@ class Lights{
static const float OFF = 0.0f;
};
-
-const std::string Lights::LED_NAMES[Lights::NUM_LED_NAMES] = {
- "LeftEar","RightEar",
- "LeftEye","RightEye",
- "Chest",
- "LeftFoot", "RightFoot"};
#endif
View
56 corpus/NaoLights.cpp
@@ -1,6 +1,9 @@
#include "NaoLights.h"
#include "ALLedNames.h"
+#define LEDS_ENABLED
+#define DEBUG_NAOLIGHTS_INIT
+
NaoLights::NaoLights(AL::ALPtr<AL::ALBroker> broker)
:Lights()
{
@@ -9,6 +12,9 @@ NaoLights::NaoLights(AL::ALPtr<AL::ALBroker> broker)
} catch(AL::ALError &e) {
cout << "Failed to initialize proxy to DCM" << endl;
}
+
+ initDCMAliases();
+ initDCMCommands();
}
@@ -22,12 +28,11 @@ void NaoLights::setRGB(std::string led_id, int rgbHex){
void NaoLights::sendLights(){
//Left Eye
- for(unsigned int i = 0; i < ALNames::NUM_ONE_EYE_LEDS; i++){
- leftFaceLedCommand[5][i][0] = OFF;
- }
-
- leftFaceLedCommand[4][0] = dcmProxy->getTime(20);
-
+ updateLightCommand(leftFaceLedCommand,0,ALNames::NUM_FACE_LEDS);
+ // for(unsigned int i = 0; i < ALNames::NUM_ONE_EYE_LEDS; i++){
+ // leftFaceLedCommand[5][i][0] = OFF;
+ // }
+ // leftFaceLedCommand[4][0] = dcmProxy->getTime(20);
#ifdef LEDS_ENABLED
try {
dcmProxy->setAlias(leftFaceLedCommand);
@@ -37,26 +42,56 @@ void NaoLights::sendLights(){
#endif
}
+void NaoLights::updateLightCommand(ALValue & command, const int rgbHex,
+ const unsigned int numRGBLeds){
+ unsigned int ledIndex = 0;
+ for(unsigned int c = 0; c < ALNames::NUM_LED_COLORS; c++){
+ for(unsigned int led = 0; led < numRGBLeds; led++){
+ const float color =
+ getColor(static_cast<ALNames::LedColor>(c),rgbHex);
+ command[5][ledIndex][0] = color;
+ ledIndex++;
+ }
+ }
+ command[4][0] = dcmProxy->getTime(20);
+}
+
+/*
+ * Returns a float between 0.0 and 1.0 corresponding to the 'c' channel
+ * of the hex value
+ */
+const float NaoLights::getColor(const ALNames::LedColor c, const int rgbHex){
+ return OFF;
+}
+
/**
* Creates the appropriate aliases with the DCM
*/
void NaoLights::initDCMAliases(){
+#ifdef DEBUG_NAOLIGHTS_INIT
+ std::cout << " NaoLights::initDCMAliases() start" << std::endl;
+#endif
ALValue leftFaceLightsAlias;
leftFaceLightsAlias.arraySetSize(3);
leftFaceLightsAlias[0] = string("LeftFaceLeds");
leftFaceLightsAlias[1].arraySetSize(ALNames::NUM_FACE_LEDS*
ALNames::NUM_LED_COLORS);
int faceIndex = 0;
//Left Eye
- for(unsigned int c = 0; c < ALNames::NUM_LED_ORIENTATIONS; c++){
+ for(unsigned int c = 0; c < ALNames::NUM_LED_COLORS; c++){
for(unsigned int n = 0; n < ALNames::NUM_FACE_LEDS; n++){
leftFaceLightsAlias[1][faceIndex] =
ALNames::faceL[ALNames::LEFT_LED][c][n];
+ cout << ALNames::faceL[ALNames::LEFT_LED][c][n] <<endl;
+
faceIndex++;
}
}
dcmProxy->createAlias(leftFaceLightsAlias);
+#ifdef DEBUG_NAOLIGHTS_INIT
+ std::cout << " NaoLights::initDCMAliases() end" << std::endl;
+#endif
}
/**
@@ -65,6 +100,9 @@ void NaoLights::initDCMAliases(){
*
*/
void NaoLights::initDCMCommands(){
+#ifdef DEBUG_NAOLIGHTS_INIT
+ std::cout << " NaoLights::initDCMCommands() start" << std::endl;
+#endif
//Left Eye
leftFaceLedCommand.arraySetSize(6);
leftFaceLedCommand[0] = string("LeftFaceLeds");
@@ -77,5 +115,9 @@ void NaoLights::initDCMCommands(){
leftFaceLedCommand[5][i].arraySetSize(1);
leftFaceLedCommand[5][i][0] = OFF;
}
+
+#ifdef DEBUG_NAOLIGHTS_INIT
+ std::cout << " NaoLights::initDCMCommands() end" << std::endl;
+#endif
}
View
6 corpus/NaoLights.h
@@ -3,7 +3,7 @@
#include "Lights.h"
#include "dcmproxy.h"
-
+#include "ALLedNames.h"
/**
* This class implements LED capability on the Nao robot using the DCM in Naoqi
* @author Johannes Strom
@@ -22,7 +22,9 @@ class NaoLights : public Lights{
private:
void initDCMAliases();
void initDCMCommands();
-
+ const float getColor(const ALNames::LedColor c, const int rgbHex);
+ void updateLightCommand(ALValue &command, const int rgbHex,
+ const unsigned int numRGBLeds);
private:
AL::ALPtr<AL::DCMProxy> dcmProxy;
AL::ALValue leftFaceLedCommand;
View
3 corpus/cmake.man.corpus/CMakeLists.txt
@@ -135,7 +135,8 @@ SET( SYNCHRO_SRCS ${CORPUS_INCLUDE_DIR}/synchro
SET( ROBOT_CONNECT_SRCS ${CORPUS_INCLUDE_DIR}/AccEKF
${CORPUS_INCLUDE_DIR}/RoboGuardian
${CORPUS_INCLUDE_DIR}/ClickableButton
- ${CORPUS_INCLUDE_DIR}/PyRoboGuardian)
+ ${CORPUS_INCLUDE_DIR}/PyRoboGuardian
+ ${CORPUS_INCLUDE_DIR}/Lights)
IF(WEBOTS_BACKEND)
LIST( APPEND ROBOT_CONNECT_SRCS ${CORPUS_INCLUDE_DIR}/WBEnactor
View
24 include/ALLedNames.h
@@ -4,11 +4,11 @@
#include "ALNames.h"
namespace ALNames {
- static const string FaceLed ="/Face/Led";
- static const string EarLed ="/Ear/Led";
- static const string LFootLed ="/LFoot/Led";
- static const string RFootLed ="/RFoot/Led";
- static const string ChestLed ="/ChestBoard/Led";
+ static const string FaceLed ="Face/Led";
+ static const string EarLed ="Ear/Led";
+ static const string LFootLed ="LFoot/Led";
+ static const string RFootLed ="RFoot/Led";
+ static const string ChestLed ="ChestBoard/Led";
static const string Green ="/Green";
static const string Red ="/Red";
@@ -17,7 +17,7 @@ namespace ALNames {
static const string Left ="/Left";
static const string Right ="/Right";
- enum LedColors{
+ enum LedColor{
RED_LED = 0,
GREEN_LED,
BLUE_LED
@@ -37,7 +37,7 @@ namespace ALNames {
{
/* Red*/
{ValuePreFix + FaceLed + Red + Left + "/0Deg"+LedPostFix,
- ValuePreFix + FaceLed + Red + Left + "/35Deg"+LedPostFix,
+ ValuePreFix + FaceLed + Red + Left + "/45Deg"+LedPostFix,
ValuePreFix + FaceLed + Red + Left + "/90Deg"+LedPostFix,
ValuePreFix + FaceLed + Red + Left + "/135Deg"+LedPostFix,
ValuePreFix + FaceLed + Red + Left + "/180Deg"+LedPostFix,
@@ -46,7 +46,7 @@ namespace ALNames {
ValuePreFix + FaceLed + Red + Left + "/315Deg"+LedPostFix},
/* Green*/
{ValuePreFix + FaceLed + Green + Left + "/0Deg"+LedPostFix,
- ValuePreFix + FaceLed + Green + Left + "/35Deg"+LedPostFix,
+ ValuePreFix + FaceLed + Green + Left + "/45Deg"+LedPostFix,
ValuePreFix + FaceLed + Green + Left + "/90Deg"+LedPostFix,
ValuePreFix + FaceLed + Green + Left + "/135Deg"+LedPostFix,
ValuePreFix + FaceLed + Green + Left + "/180Deg"+LedPostFix,
@@ -55,7 +55,7 @@ namespace ALNames {
ValuePreFix + FaceLed + Green + Left + "/315Deg"+LedPostFix},
/* Blue*/
{ValuePreFix + FaceLed + Blue + Left + "/0Deg"+LedPostFix,
- ValuePreFix + FaceLed + Blue + Left + "/35Deg"+LedPostFix,
+ ValuePreFix + FaceLed + Blue + Left + "/45Deg"+LedPostFix,
ValuePreFix + FaceLed + Blue + Left + "/90Deg"+LedPostFix,
ValuePreFix + FaceLed + Blue + Left + "/135Deg"+LedPostFix,
ValuePreFix + FaceLed + Blue + Left + "/180Deg"+LedPostFix,
@@ -67,7 +67,7 @@ namespace ALNames {
{
/* Red*/
{ValuePreFix + FaceLed + Red + Right + "/0Deg"+LedPostFix,
- ValuePreFix + FaceLed + Red + Right + "/35Deg"+LedPostFix,
+ ValuePreFix + FaceLed + Red + Right + "/45Deg"+LedPostFix,
ValuePreFix + FaceLed + Red + Right + "/90Deg"+LedPostFix,
ValuePreFix + FaceLed + Red + Right + "/135Deg"+LedPostFix,
ValuePreFix + FaceLed + Red + Right + "/180Deg"+LedPostFix,
@@ -76,7 +76,7 @@ namespace ALNames {
ValuePreFix + FaceLed + Red + Right + "/315Deg"+LedPostFix},
/* Green*/
{ValuePreFix + FaceLed + Green + Right + "/0Deg"+LedPostFix,
- ValuePreFix + FaceLed + Green + Right + "/35Deg"+LedPostFix,
+ ValuePreFix + FaceLed + Green + Right + "/45Deg"+LedPostFix,
ValuePreFix + FaceLed + Green + Right + "/90Deg"+LedPostFix,
ValuePreFix + FaceLed + Green + Right + "/135Deg"+LedPostFix,
ValuePreFix + FaceLed + Green + Right + "/180Deg"+LedPostFix,
@@ -85,7 +85,7 @@ namespace ALNames {
ValuePreFix + FaceLed + Green + Right + "/315Deg"+LedPostFix},
/* Blue*/
{ValuePreFix + FaceLed + Blue + Right + "/0Deg"+LedPostFix,
- ValuePreFix + FaceLed + Blue + Right + "/35Deg"+LedPostFix,
+ ValuePreFix + FaceLed + Blue + Right + "/45Deg"+LedPostFix,
ValuePreFix + FaceLed + Blue + Right + "/90Deg"+LedPostFix,
ValuePreFix + FaceLed + Blue + Right + "/135Deg"+LedPostFix,
ValuePreFix + FaceLed + Blue + Right + "/180Deg"+LedPostFix,
View
21 include/ALNames.h
@@ -1,19 +1,20 @@
#ifndef _ALNames_DEFINED
#define _ALNames_DEFINED
+#include <string>
#include "Kinematics.h"
namespace ALNames {
- static const string PositionPostFix = "/Position/Actuator/Value";
- static const string HardnessPostFix = "/Hardness/Actuator/Value";
- static const string LedPostFix = "/Actuator/Value";
- static const string ValuePostFix = "/Position/Sensor/Value";
- static const string ValuePreFix = "Device/SubDeviceList/";
- static const string TempValuePostFix= "/Temperature/Sensor/Value";
+ static const std::string PositionPostFix = "/Position/Actuator/Value";
+ static const std::string HardnessPostFix = "/Hardness/Actuator/Value";
+ static const std::string LedPostFix = "/Actuator/Value";
+ static const std::string ValuePostFix = "/Position/Sensor/Value";
+ static const std::string ValuePreFix = "Device/SubDeviceList/";
+ static const std::string TempValuePostFix= "/Temperature/Sensor/Value";
- static const string jointsP[Kinematics::NUM_JOINTS] = {
+ static const std::string jointsP[Kinematics::NUM_JOINTS] = {
"HeadYaw" + PositionPostFix,
"HeadPitch" + PositionPostFix,
"LShoulderPitch" + PositionPostFix,
@@ -38,7 +39,7 @@ namespace ALNames {
"RElbowYaw" + PositionPostFix,
"RElbowRoll" + PositionPostFix
};
- static const string jointsH[Kinematics::NUM_JOINTS] = {
+ static const std::string jointsH[Kinematics::NUM_JOINTS] = {
"HeadYaw" + HardnessPostFix,
"HeadPitch" + HardnessPostFix,
"LShoulderPitch" + HardnessPostFix,
@@ -63,7 +64,7 @@ namespace ALNames {
"RElbowYaw" + HardnessPostFix,
"RElbowRoll" + HardnessPostFix
};
- static const string jointsV[Kinematics::NUM_JOINTS] = {
+ static const std::string jointsV[Kinematics::NUM_JOINTS] = {
ValuePreFix + "HeadYaw" + ValuePostFix,
ValuePreFix + "HeadPitch" + ValuePostFix,
ValuePreFix + "LShoulderPitch" + ValuePostFix,
@@ -88,7 +89,7 @@ namespace ALNames {
ValuePreFix + "RElbowYaw" + ValuePostFix,
ValuePreFix + "RElbowRoll" + ValuePostFix
};
- static const string jointsT[Kinematics::NUM_JOINTS] = {
+ static const std::string jointsT[Kinematics::NUM_JOINTS] = {
ValuePreFix + "HeadYaw" + TempValuePostFix,
ValuePreFix + "HeadPitch" + TempValuePostFix,
ValuePreFix + "LShoulderPitch" + TempValuePostFix,
View
9 manmodule.cpp
@@ -54,6 +54,7 @@ typedef TTMan ALMan;
#include "_ledsmodule.h"
+#include "NaoLights.h"
#include "almodule.h"
#include "alsentinelproxy.h"
@@ -71,6 +72,7 @@ static shared_ptr<Synchro> synchro;
static shared_ptr<ALTranscriber> transcriber;
static shared_ptr<ALImageTranscriber> imageTranscriber;
static shared_ptr<EnactorT> enactor;
+static shared_ptr<Lights> lights;
void ALCreateMan( ALPtr<ALBroker> broker){
try{
@@ -96,15 +98,16 @@ void ALCreateMan( ALPtr<ALBroker> broker){
enactor = shared_ptr<EnactorT>(new EnactorT(sensors,synchro,
transcriber,broker));
#endif
+ lights = shared_ptr<Lights>(new NaoLights(broker));
-
- setLedsProxy(AL::ALPtr<AL::ALLedsProxy>(new AL::ALLedsProxy(broker)));
+ //setLedsProxy(AL::ALPtr<AL::ALLedsProxy>(new AL::ALLedsProxy(broker)));
man = boost::shared_ptr<ALMan> (new ALMan(sensors,
transcriber,
imageTranscriber,
enactor,
- synchro));
+ synchro,
+ lights));
man->startSubThreads();
}
View
5 nao_soccer_player.cpp
@@ -28,6 +28,7 @@ static shared_ptr<Synchro> synchro;
static shared_ptr<WBTranscriber> transcriber;
static shared_ptr<WBImageTranscriber> imageTranscriber;
static shared_ptr<WBEnactor> enactor;
+static shared_ptr<Lights> lights;
void WBCreateMan(){
@@ -41,12 +42,14 @@ void WBCreateMan(){
enactor = shared_ptr<WBEnactor>(new WBEnactor(sensors,
transcriber));
+ lights = shared_ptr<Lights>(new WBLights());
man = boost::shared_ptr<WBMan> (new WBMan(sensors,
transcriber,
imageTranscriber,
enactor,
- synchro));
+ synchro,
+ lights));
man->startSubThreads();
}
View
1 vision/Profiler.h
@@ -40,6 +40,7 @@ enum ProfiledComponent {
P_LOGGING,
P_AIBOCONNECT,
P_TOOLCONNECT,
+ P_LIGHTS,
P_FINAL,
};
static const int NUM_PCOMPONENTS = P_FINAL + 1;

0 comments on commit 6970306

Please sign in to comment.