Backport to C++<11 to support ctc-naoqi 2.1.4

CMakeLists.txt

@@ -1,7 +1,10 @@
-cmake_minimum_required(VERSION 3.1)
+cmake_minimum_required(VERSION 2.8)
 project(mc_naoqi_dcm)
 set(ROBOT_NAME "pepper" CACHE STRING "Build with this robot (pepper|nao)")
 
+# NOTE: We cannot use C++11 here, nor C++0x or gnu++11 since we need to keep on supporting
+# older version of the ctc toolchain. In particular with boost 1.55 this would fail.
+
 find_package(qibuild)
 
 message(STATUS "Building with ${ROBOT_NAME} robot")

Dockerfile

@@ -13,7 +13,7 @@ RUN mkdir /Workspace && cd /Workspace \
   && qibuild init \
   && qibuild add-config ctc-naoqi-config -t ctc-naoqi --default
 
-COPY . /Workspace/qibuild_ws/mc_naoqi_dcm
+COPY .. /Workspace/qibuild_ws/mc_naoqi_dcm
 RUN cd /Workspace/qibuild_ws/mc_naoqi_dcm \
  && qibuild configure --release -DROBOT_NAME=pepper \
  && qibuild make \

Dockerfile_2.1.4

@@ -0,0 +1,22 @@
+FROM ubuntu:20.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+RUN apt-get update -qq && apt-get install -qq cmake cmake-data wget lsb-release git sudo python3 python3-pip unzip vim
+RUN python3 -m pip install qibuild
+RUN mkdir /Workspace && cd /Workspace \
+  && wget -c https://seafile.lirmm.fr/f/ba804ce9277544f2b6ec/?dl=1 -O ctc-naoqi.zip \
+  && unzip ctc-naoqi.zip -d ctc-naoqi-tmp \
+  && mv ctc-naoqi-tmp/ctc-linux64-atom-2.1.4.13 ctc-naoqi \
+  && qitoolchain create ctc-naoqi ctc-naoqi/toolchain.xml \
+  && mkdir qibuild_ws && cd qibuild_ws \
+  && qibuild init \
+  && qibuild add-config ctc-naoqi-config -t ctc-naoqi --default
+
+COPY . /Workspace/qibuild_ws/mc_naoqi_dcm
+RUN cd /Workspace/qibuild_ws/mc_naoqi_dcm \
+ && qibuild configure --release -DROBOT_NAME=nao \
+ && qibuild make \
+ && mv /Workspace/qibuild_ws/mc_naoqi_dcm/build-ctc-naoqi-config/sdk/lib/naoqi/libmc_naoqi_dcm.so /libmc_naoqi_dcm.so
+ RUN ls /
+ RUN echo "Created library: /mc_naoqi_dcm.so"

README.md

@@ -52,7 +52,7 @@ You may use the provided `Dockerfile` that sets up the cross-compilation environ
 
 ```sh
 cd <mc_naoqi_dcm>
-docker build -t docker-naoqi-dcm .
+docker build -t docker-naoqi-dcm -f Dockerfile
 id=$(docker create docker-naoqi-dcm)
 docker cp $id:/libmc_naoqi_dcm.so /tmp/libmc_naoqi_dcm.so
 docker rm -v $id

docker/Dockerfile

@@ -0,0 +1,22 @@
+FROM ubuntu:22.04
+
+ENV DEBIAN_FRONTEND=noninteractive
+
+RUN apt-get update -qq && apt-get install -qq cmake cmake-data wget lsb-release git sudo python3 python3-pip
+RUN python3 -m pip install qibuild
+RUN mkdir /Workspace && cd /Workspace \
+  && wget -c https://seafile.lirmm.fr/f/5389d0a64d79481ab49e/?dl=1 -O ctc-naoqi.tar.xz \
+  && tar -xf ctc-naoqi.tar.xz \
+  && mv ctc-naoqi-2.5.0 ctc-naoqi \
+  && qitoolchain create ctc-naoqi ctc-naoqi/toolchain.xml \
+  && mkdir qibuild_ws && cd qibuild_ws \
+  && qibuild init \
+  && qibuild add-config ctc-naoqi-config -t ctc-naoqi --default
+
+COPY . /Workspace/qibuild_ws/mc_naoqi_dcm
+RUN cd /Workspace/qibuild_ws/mc_naoqi_dcm \
+ && qibuild configure --release -DROBOT_NAME=pepper \
+ && qibuild make \
+ && mv /Workspace/qibuild_ws/mc_naoqi_dcm/build-ctc-naoqi-config/sdk/lib/naoqi/libmc_naoqi_dcm.so /libmc_naoqi_dcm.so
+RUN ls /
+RUN echo "Created library: /mc_naoqi_dcm.so"

include/RobotModule.h

@@ -1,9 +1,18 @@
 #pragma once
+#include <sstream>
 #include <string>
 #include <vector>
 
 namespace mc_naoqi_dcm
 {
+/** Reimplement std::to_string to support pre-C++11 era */
+template<typename T>
+inline std::string to_string(T num)
+{
+  std::stringstream ss;
+  ss << num;
+  return ss.str();
+}
 
 struct JointGroup
 {
@@ -31,6 +40,8 @@ struct iLedGroup
 
 struct RobotModule
 {
+  RobotModule();
+
   // Robot name (e.g. nao or pepper)
   std::string name;
   // Body joints
@@ -44,8 +55,7 @@ struct RobotModule
   // Memory keys to read all sensors with single call to ALMemoryFastAccess
   std::vector<std::string> readSensorKeys;
   // IMU sensor names
-  std::vector<std::string> imu = {"AccelerometerX", "AccelerometerY", "AccelerometerZ", "GyroscopeX", "GyroscopeY",
-                                  "GyroscopeZ",     "AngleX",         "AngleY",         "AngleZ"};
+  std::vector<std::string> imu;
   // Groups of special robot joints (e.g. wheels)
   std::vector<JointGroup> specialJointGroups;
   // Groups of RGB leds

include/mc_naoqi_dcm.h

@@ -163,7 +163,7 @@ class MCNAOqiDCM : public AL::ALModule
   ProcessSignalConnection fDCMPreProcessConnection;
 
   // Used to check id preprocess is connected
-  bool preProcessConnected = false;
+  bool preProcessConnected;
 
   // Used for fast memory access
   boost::shared_ptr<AL::ALMemoryFastAccess> fMemoryFastAccess;

src/NAORobotModule.cpp

@@ -4,16 +4,37 @@
 
 namespace mc_naoqi_dcm
 {
+
 NAORobotModule::NAORobotModule()
 {
   name = "nao";
 
   // body joints
-  actuators = {"HeadPitch",  "HeadYaw",        "LAnklePitch",    "LAnkleRoll",    "LElbowRoll",
-               "LElbowYaw",  "LHand",          "LHipPitch",      "LHipRoll",      "LHipYawPitch",
-               "LKneePitch", "LShoulderPitch", "LShoulderRoll",  "LWristYaw",     "RAnklePitch",
-               "RAnkleRoll", "RElbowRoll",     "RElbowYaw",      "RHand",         "RHipPitch",
-               "RHipRoll",   "RKneePitch",     "RShoulderPitch", "RShoulderRoll", "RWristYaw"};
+  actuators.push_back("HeadPitch");
+  actuators.push_back("HeadYaw");
+  actuators.push_back("LAnklePitch");
+  actuators.push_back("LAnkleRoll");
+  actuators.push_back("LElbowRoll");
+  actuators.push_back("LElbowYaw");
+  actuators.push_back("LHand");
+  actuators.push_back("LHipPitch");
+  actuators.push_back("LHipRoll");
+  actuators.push_back("LHipYawPitch");
+  actuators.push_back("LKneePitch");
+  actuators.push_back("LShoulderPitch");
+  actuators.push_back("LShoulderRoll");
+  actuators.push_back("LWristYaw");
+  actuators.push_back("RAnklePitch");
+  actuators.push_back("RAnkleRoll");
+  actuators.push_back("RElbowRoll");
+  actuators.push_back("RElbowYaw");
+  actuators.push_back("RHand");
+  actuators.push_back("RHipPitch");
+  actuators.push_back("RHipRoll");
+  actuators.push_back("RKneePitch");
+  actuators.push_back("RShoulderPitch");
+  actuators.push_back("RShoulderRoll");
+  actuators.push_back("RWristYaw");
 
   // generate memory keys for sending commands to the joints (position/stiffness)
   genMemoryKeys("", actuators, "/Position/Actuator/Value", setActuatorKeys);
@@ -25,13 +46,21 @@ NAORobotModule::NAORobotModule()
   genMemoryKeys("", actuators, "/ElectricCurrent/Sensor/Value", readSensorKeys, true, "ElectricCurrent");
   genMemoryKeys("InertialSensor/", imu, "/Sensor/Value", readSensorKeys, true, "");
   // Force Sensitive Resistors of the feet
-  std::vector<std::string> footFSR = {"FrontLeft",   "FrontRight",         "RearLeft",          "RearRight",
-                                      "TotalWeight", "CenterOfPressure/X", "CenterOfPressure/Y"};
+  std::vector<std::string> footFSR;
+  footFSR.push_back("FrontLeft");
+  footFSR.push_back("FrontRight");
+  footFSR.push_back("RearLeft");
+  footFSR.push_back("RearRight");
+  footFSR.push_back("TotalWeight");
+  footFSR.push_back("CenterOfPressure/X");
+  footFSR.push_back("CenterOfPressure/Y");
+
   genMemoryKeys("LFoot/", footFSR, "/Sensor/Value", readSensorKeys, true, "LFoot");
   genMemoryKeys("RFoot/", footFSR, "/Sensor/Value", readSensorKeys, true, "RFoot");
 
   // Feet bumpers (two per each foot)
-  bumpers = {"LFoot", "RFoot"};
+  bumpers.push_back("LFoot");
+  bumpers.push_back("RFoot");
   genMemoryKeys("", bumpers, "/Bumper/Right/Sensor/Value", readSensorKeys, true);
   genMemoryKeys("", bumpers, "/Bumper/Left/Sensor/Value", readSensorKeys, true);
 
@@ -43,8 +72,8 @@ NAORobotModule::NAORobotModule()
   // generate memory keys for changing color of all eyes leds
   for(unsigned i = 0; i < numEyeLeds; i++)
   {
-    eyesLeds.ledNames.push_back("Right/" + std::to_string(i * eyeLedAngleStep) + "Deg");
-    eyesLeds.ledNames.push_back("Left/" + std::to_string(i * eyeLedAngleStep) + "Deg");
+    eyesLeds.ledNames.push_back("Right/" + mc_naoqi_dcm::to_string(i * eyeLedAngleStep) + "Deg");
+    eyesLeds.ledNames.push_back("Left/" + mc_naoqi_dcm::to_string(i * eyeLedAngleStep) + "Deg");
   }
   genMemoryKeys("Face/Led/Red/", eyesLeds.ledNames, "/Actuator/Value", eyesLeds.redLedKeys);
   genMemoryKeys("Face/Led/Green/", eyesLeds.ledNames, "/Actuator/Value", eyesLeds.greenLedKeys);
@@ -58,8 +87,8 @@ NAORobotModule::NAORobotModule()
   // generate memory keys for changing color of all ear leds
   for(unsigned i = 0; i < numEarLeds; i++)
   {
-    earsLeds.ledNames.push_back("Right/" + std::to_string(i * earLedAngleStep) + "Deg");
-    earsLeds.ledNames.push_back("Left/" + std::to_string(i * earLedAngleStep) + "Deg");
+    earsLeds.ledNames.push_back("Right/" + mc_naoqi_dcm::to_string(i * earLedAngleStep) + "Deg");
+    earsLeds.ledNames.push_back("Left/" + mc_naoqi_dcm::to_string(i * earLedAngleStep) + "Deg");
   }
   genMemoryKeys("Face/Led/Red/", earsLeds.ledNames, "/Actuator/Value", earsLeds.redLedKeys);
   genMemoryKeys("Face/Led/Green/", earsLeds.ledNames, "/Actuator/Value", earsLeds.greenLedKeys);

src/PepperRobotModule.cpp

@@ -7,9 +7,23 @@ PepperRobotModule::PepperRobotModule() : RobotModule()
   name = "pepper";
 
   // body joints
-  actuators = {"KneePitch",     "HipPitch",  "HipRoll",    "HeadYaw",   "HeadPitch", "LShoulderPitch",
-               "LShoulderRoll", "LElbowYaw", "LElbowRoll", "LWristYaw", "LHand",     "RShoulderPitch",
-               "RShoulderRoll", "RElbowYaw", "RElbowRoll", "RWristYaw", "RHand"};
+  actuators.push_back("KneePitch");
+  actuators.push_back("HipPitch");
+  actuators.push_back("HipRoll");
+  actuators.push_back("HeadYaw");
+  actuators.push_back("HeadPitch");
+  actuators.push_back("LShoulderPitch");
+  actuators.push_back("LShoulderRoll");
+  actuators.push_back("LElbowYaw");
+  actuators.push_back("LElbowRoll");
+  actuators.push_back("LWristYaw");
+  actuators.push_back("LHand");
+  actuators.push_back("RShoulderPitch");
+  actuators.push_back("RShoulderRoll");
+  actuators.push_back("RElbowYaw");
+  actuators.push_back("RElbowRoll");
+  actuators.push_back("RWristYaw");
+  actuators.push_back("RHand");
 
   // generate memory keys for sending commands to the joints (position/stiffness)
   genMemoryKeys("", actuators, "/Position/Actuator/Value", setActuatorKeys);
@@ -24,43 +38,64 @@ PepperRobotModule::PepperRobotModule() : RobotModule()
   // wheels - special joint groups
   JointGroup wheels;
   wheels.groupName = "wheels";
-  wheels.jointsNames = {"WheelFL", "WheelFR", "WheelB"};
+  wheels.jointsNames.push_back("WheelFL");
+  wheels.jointsNames.push_back("WheelFR");
+  wheels.jointsNames.push_back("WheelB");
   genMemoryKeys("", wheels.jointsNames, "/Speed/Actuator/Value", wheels.setActuatorKeys);
   genMemoryKeys("", wheels.jointsNames, "/Stiffness/Actuator/Value", wheels.setHardnessKeys);
   specialJointGroups.push_back(wheels);
   // add sensors for the special joint group
   genMemoryKeys("", wheels.jointsNames, "/Speed/Sensor/Value", readSensorKeys, true, "Encoder");
 
   // Bumpers
-  bumpers = {"FrontLeft", "FrontRight", "Back"};
+  bumpers.push_back("FrontLeft");
+  bumpers.push_back("FrontRight");
+  bumpers.push_back("Back");
+
   genMemoryKeys("Platform/", bumpers, "/Bumper/Sensor/Value", readSensorKeys, true);
 
   // Tactile sensors
-  tactile = {"Head/Touch/Front", "Head/Touch/Rear", "Head/Touch/Middle", "RHand/Touch/Back", "LHand/Touch/Back"};
+  tactile.push_back("Head/Touch/Front");
+  tactile.push_back("Head/Touch/Rear");
+  tactile.push_back("Head/Touch/Middle");
+  tactile.push_back("RHand/Touch/Back");
+  tactile.push_back("LHand/Touch/Back");
   genMemoryKeys("", tactile, "/Sensor/Value", readSensorKeys, true);
 
   // led groups
   rgbLedGroup eyesCenter;
   eyesCenter.groupName = "eyesCenter";
-  eyesCenter.ledNames = {"Right/45Deg", "Right/225Deg", "Left/270Deg", "Left/90Deg"};
+  eyesCenter.ledNames.push_back("Right/45Deg");
+  eyesCenter.ledNames.push_back("Right/225Deg");
+  eyesCenter.ledNames.push_back("Left/270Deg");
+  eyesCenter.ledNames.push_back("Left/90Deg");
   genMemoryKeys("Face/Led/Red/", eyesCenter.ledNames, "/Actuator/Value", eyesCenter.redLedKeys);
   genMemoryKeys("Face/Led/Green/", eyesCenter.ledNames, "/Actuator/Value", eyesCenter.greenLedKeys);
   genMemoryKeys("Face/Led/Blue/", eyesCenter.ledNames, "/Actuator/Value", eyesCenter.blueLedKeys);
   rgbLedGroups.push_back(eyesCenter);
 
   rgbLedGroup eyesPeripheral;
   eyesPeripheral.groupName = "eyesPeripheral";
-  eyesPeripheral.ledNames = {"Right/0Deg",   "Right/90Deg",  "Right/135Deg", "Right/180Deg",
-                             "Right/270Deg", "Right/315Deg", "Left/0Deg",    "Left/45Deg",
-                             "Left/135Deg",  "Left/180Deg",  "Left/225Deg",  "Left/315Deg"};
+  eyesPeripheral.ledNames.push_back("Right/0Deg");
+  eyesPeripheral.ledNames.push_back("Right/90Deg");
+  eyesPeripheral.ledNames.push_back("Right/135Deg");
+  eyesPeripheral.ledNames.push_back("Right/180Deg");
+  eyesPeripheral.ledNames.push_back("Right/270Deg");
+  eyesPeripheral.ledNames.push_back("Right/315Deg");
+  eyesPeripheral.ledNames.push_back("Left/0Deg");
+  eyesPeripheral.ledNames.push_back("Left/45Deg");
+  eyesPeripheral.ledNames.push_back("Left/135Deg");
+  eyesPeripheral.ledNames.push_back("Left/180Deg");
+  eyesPeripheral.ledNames.push_back("Left/225Deg");
+  eyesPeripheral.ledNames.push_back("Left/315Deg");
   genMemoryKeys("Face/Led/Red/", eyesPeripheral.ledNames, "/Actuator/Value", eyesPeripheral.redLedKeys);
   genMemoryKeys("Face/Led/Green/", eyesPeripheral.ledNames, "/Actuator/Value", eyesPeripheral.greenLedKeys);
   genMemoryKeys("Face/Led/Blue/", eyesPeripheral.ledNames, "/Actuator/Value", eyesPeripheral.blueLedKeys);
   rgbLedGroups.push_back(eyesPeripheral);
 
   rgbLedGroup shoulderLeds;
   shoulderLeds.groupName = "shoulderLeds";
-  shoulderLeds.ledNames = {""};
+  shoulderLeds.ledNames.push_back("");
   genMemoryKeys("ChestBoard/Led/Red", shoulderLeds.ledNames, "/Actuator/Value", shoulderLeds.redLedKeys);
   genMemoryKeys("ChestBoard/Led/Green", shoulderLeds.ledNames, "/Actuator/Value", shoulderLeds.greenLedKeys);
   genMemoryKeys("ChestBoard/Led/Blue", shoulderLeds.ledNames, "/Actuator/Value", shoulderLeds.blueLedKeys);
@@ -73,8 +108,8 @@ PepperRobotModule::PepperRobotModule() : RobotModule()
   // generate memory keys for changing color of all ear leds
   for(unsigned i = 0; i < numEarLeds; i++)
   {
-    earsLeds.ledNames.push_back("Right/" + std::to_string(i * earLedAngleStep) + "Deg");
-    earsLeds.ledNames.push_back("Left/" + std::to_string(i * earLedAngleStep) + "Deg");
+    earsLeds.ledNames.push_back("Right/" + mc_naoqi_dcm::to_string(i * earLedAngleStep) + "Deg");
+    earsLeds.ledNames.push_back("Left/" + mc_naoqi_dcm::to_string(i * earLedAngleStep) + "Deg");
   }
   genMemoryKeys("Ears/Led/", earsLeds.ledNames, "/Actuator/Value", earsLeds.intensityLedKeys);
   iLedGroups.push_back(earsLeds);

src/RobotModule.cpp

@@ -3,6 +3,19 @@
 namespace mc_naoqi_dcm
 {
 
+RobotModule::RobotModule()
+{
+  imu.push_back("AccelerometerX");
+  imu.push_back("AccelerometerY");
+  imu.push_back("AccelerometerZ");
+  imu.push_back("GyroscopeX");
+  imu.push_back("GyroscopeY");
+  imu.push_back("GyroscopeZ");
+  imu.push_back("AngleX");
+  imu.push_back("AngleY");
+  imu.push_back("AngleZ");
+}
+
 // Function to create various memory key groups
 void RobotModule::genMemoryKeys(std::string prefix,
                                 std::vector<std::string> & devices,

src/mc_naoqi_dcm.cpp

@@ -32,7 +32,8 @@
 namespace mc_naoqi_dcm
 {
 MCNAOqiDCM::MCNAOqiDCM(boost::shared_ptr<AL::ALBroker> broker, const std::string & name)
-: AL::ALModule(broker, name), fMemoryFastAccess(boost::shared_ptr<AL::ALMemoryFastAccess>(new AL::ALMemoryFastAccess()))
+: AL::ALModule(broker, name),
+  fMemoryFastAccess(boost::shared_ptr<AL::ALMemoryFastAccess>(new AL::ALMemoryFastAccess())), preProcessConnected(false)
 {
   setModuleDescription("Module to communicate with mc_rtc_naoqi interface for whole-body control via mc_rtc framework");
 
@@ -363,7 +364,10 @@ void MCNAOqiDCM::createLedAliases()
     createAliasPrepareCommand(gName, leds.greenLedKeys, greenLedCommands, "Merge");
     createAliasPrepareCommand(bName, leds.blueLedKeys, blueLedCommands, "Merge");
     // map led group name to led commands
-    ledCmdMap[leds.groupName] = {redLedCommands, greenLedCommands, blueLedCommands};
+    ledCmdMap[leds.groupName].clear();
+    ledCmdMap[leds.groupName].push_back(redLedCommands);
+    ledCmdMap[leds.groupName].push_back(greenLedCommands);
+    ledCmdMap[leds.groupName].push_back(blueLedCommands);
   }
 
   // Single channel led groups
@@ -373,7 +377,8 @@ void MCNAOqiDCM::createLedAliases()
     AL::ALValue intensityLedCommands;
     createAliasPrepareCommand(leds.groupName, leds.intensityLedKeys, intensityLedCommands, "Merge");
     // map led group name to led commands
-    ledCmdMap[leds.groupName] = {intensityLedCommands};
+    ledCmdMap[leds.groupName].clear();
+    ledCmdMap[leds.groupName].push_back(intensityLedCommands);
   }
 }