Merge pull request #44 from dfki-ric/channels

Channel support for data types

.gitattributes

@@ -0,0 +1 @@
+COMMIT_ID ident

.gitignore

@@ -1,3 +1,6 @@
 build/
 *.sw?
 *.pb.*
+/src/GitVersion.hpp
+/src/LibraryVersion.hpp
+/src/ProtocolVersion.hpp

COMMIT_ID

@@ -0,0 +1 @@
+$Id$

VERSION

@@ -1,3 +1,4 @@
+5.0.0 - Channel support
 4.0.0 - added CLI, api change fpr simpleactions
 3.0.0 - consistend use of a header type for metadata in telemetry
 2.0.0

doc/README.md

@@ -87,6 +87,16 @@ When time synchronization of the command is important, a "ComplexAction" can be
 
 When the robot should be usable for generic user interfaces (that use the RobotController library), the initControllableFrames() function should be used to report the availabe commands. 
 
+## Channels
+
+In some scenarios, multiple data has to use the same set function of the ControlledRobot class. In this case, you can set the frame name in the header to differenciate the data, but the messages are sharing the same receive buffer in the RobotController class. If the data fraquency on the robot side differs a lot, you might loose the data with the lower frequency, when it is overwritten by high freqency data in the receive buffer before it was read.
+
+For this scenario there is a optional "channel" paramater in each set function. In order to use it you need to call `addChannel(const TelemetryMessageType& type, const std::string name = "")` on the ControlledRobot first in order to create the sendbuffer, it returns the channel number to use. The name parameter can be used to identify the channel purpose/origin of data.
+
+The RobtoController can use `requestChannelsDefinition(ChannelsDefinition *channels)` to receive all channels used by the robot. Receive buffers for the channels are automatically created on first receive of the channel message, so even when `requestChannelsDefinition()` is not called, no data is lost.
+
+As an alternative to channels, you can also add a 2nd instance of ControlledRobot, which might beneficial if you mix high frequency small data with low frequency big data of different types, because the high frequency data is interrupted while the big message is transferred (this scenario should only apply when rrc is used as internal interface instead of remote controlling a robot).
+
 
 ## Defining Actions
 

examples/ControlledRobotMain.cpp

@@ -91,20 +91,6 @@ int main(int argc, char** argv)
     robot_remote_control::ComplexActions complexActions;
     robot.initComplexActions(complexActions);
 
-    robot_remote_control::SimpleSensors sensors;
-    robot_remote_control::SimpleSensor* sens;
-    sens = sensors.add_sensors();
-    sens->set_name("temperature");
-    sens->set_id(1);
-    sens->mutable_size()->set_x(1);  // only single value
-
-    sens = sensors.add_sensors();
-    sens->set_name("velocity");
-    sens->set_id(2);
-    sens->mutable_size()->set_x(3);  // 3 value vector
-
-    robot.initSimpleSensors(sensors);
-
     // init done, now fake a robot
 
     // robot state
@@ -114,11 +100,13 @@ int main(int argc, char** argv)
     robot_remote_control::Pose currentpose, targetpose;
 
     robot_remote_control::SimpleSensor temperature, velocity;
-    temperature.set_id(1);
     // init value
     temperature.add_value(42);
 
-    velocity.set_id(2);
+    // send velocity in a "channel" (seperate buffers in the RobotController)
+    // channels are numbered uint8_t, default channel is 0, first channel added is 1, etc.)
+    int velocity_channel_no = robot.addChannel(robot_remote_control::SIMPLE_SENSOR, "velocity");
+
     // init value
     velocity.add_value(0);
     velocity.add_value(0);
@@ -158,14 +146,14 @@ int main(int argc, char** argv)
     robot_remote_control::Poses posescommand;
 
 
-
-
     // fill inital robot state
     currentpose.mutable_position();
     currentpose.mutable_orientation()->set_w(1);
     robot.setCurrentPose(currentpose);
 
-
+    // define an extra channel to send two poses (only needed when you need extra receive buffers on the RobotController)
+    // otherwise use the header.frame field instead of headers to differentiate poses
+    uint8_t pose2channelno = robot.addChannel(robot_remote_control::CURRENT_POSE, "2nd pose via channel");
 
     // for requests to work, you need a valid connection:
     // only works when heartbeats are set up
@@ -176,7 +164,7 @@ int main(int argc, char** argv)
 
     robot.addCommandReceivedCallback(robot_remote_control::TARGET_POSE_COMMAND, []() {
         // WARNING: this callback run in the reveive thread, you should not use this to access data, only to notify other threads
-        //printf("Pose Command Callback\n");
+        // printf("Pose Command Callback\n");
     });
 
 
@@ -189,11 +177,10 @@ int main(int argc, char** argv)
     permreq.set_requestuid("testuid2");
     std::shared_future<bool> perm2 = robot.requestPermission(permreq);
 
-    //perm1.wait();
-    //printf("result of permission request 1: %s\n", perm1.get() ? "true" : "false");
+    // perm1.wait();
+    // printf("result of permission request 1: %s\n", perm1.get() ? "true" : "false");
 
     while (true) {
-
         // if (perm2.valid()) {
         //     printf("result of permission request 2: %s\n", perm2.get() ? "true" : "false");
         // }
@@ -262,17 +249,17 @@ int main(int argc, char** argv)
         // fake some joint movement
         robot.setJointState(jointsstate);
 
+        robot.setCurrentTransforms(transforms);
+
         temperature.set_value(0, 42);
         robot.setSimpleSensor(temperature);
 
-        robot.setCurrentTransforms(transforms);
-
         velocity.set_value(0, 0.1);
         velocity.set_value(1, 0.1);
         velocity.set_value(2, 0.1);
-        robot.setSimpleSensor(velocity);
+        robot.setSimpleSensor(velocity, velocity_channel_no);  // 2nd param is the channelno.
+
 
-
         robot_remote_control::Twist twistState;
         twistState.mutable_linear()->set_x(1);
         twistState.mutable_linear()->set_y(2);

examples/RobotControllerMain.cpp

@@ -82,7 +82,7 @@ int main(int argc, char** argv) {
 
 
         robot_remote_control::SimpleSensor sens;
-        controller.getSimpleSensor(1, &sens);
+        controller.getSimpleSensor(&sens);
 
         // read only one element (no matter how much available)
         if (controller.getCurrentTransforms(&transforms)) {

src/CLI/CMakeLists.txt

@@ -21,7 +21,7 @@ if (CURSES_HAVE_NCURSES_H)
              EXPORT robot_remote_control-targets
              RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}
     )
-endif(CURSES_FOUND)
+endif(CURSES_HAVE_NCURSES_H)
 
 add_executable(robot_controller RobotControllerCLI.cpp ConsoleCommands.cpp)
 target_link_libraries(robot_controller

src/CLI/ConsoleCommands.cpp

@@ -202,10 +202,14 @@ bool ConsoleCommands::runCommand(std::vector<std::string> &line) {
             while (params.size() < iter->second.params.size()) {
                 std::string question = iter->second.params[params.size()].hint + "[" + iter->second.params[params.size()].defaultvalues.front().param +"]:";
                 std::string param;
-                std::cout << "missing paramater: " << question;
-                std::getline(std::cin, param);
-                if (param == "") {
+                if (iter->second.params[params.size()].defaultvalues.front().autoapply) {
                     param = iter->second.params[params.size()].defaultvalues.front().param;
+                } else {
+                    std::cout << "missing paramater: " << question;
+                    std::getline(std::cin, param);
+                    if (param == "") {
+                        param = iter->second.params[params.size()].defaultvalues.front().param;
+                    }
                 }
                 params.push_back(param);
             }

src/CLI/ConsoleCommands.hpp

@@ -16,13 +16,14 @@ class ConsoleCommands {
     virtual ~ConsoleCommands();
 
     struct DefaultParam {
-        DefaultParam(const std::string& param, const std::string &only_if_before = ""):param(param), only_if_before(only_if_before){};
+        explicit DefaultParam(const std::string& param, const std::string &only_if_before = "", bool autoapply = false):param(param), only_if_before(only_if_before), autoapply(autoapply) {}
         std::string param;
         std::string only_if_before;
+        bool autoapply;
     };
 
     struct ParamDef {
-        ParamDef(const std::string& hint, const std::string& defaultvalue):hint(hint), defaultvalues({DefaultParam(defaultvalue, "")}) {}
+        ParamDef(const std::string& hint, const std::string& defaultvalue, bool autoapply = false):hint(hint), defaultvalues({DefaultParam(defaultvalue, "", autoapply)}) {}
         std::string hint;
         std::vector<DefaultParam> defaultvalues;
     };

src/CLI/RobotControllerCLI.cpp

@@ -12,7 +12,7 @@ using robot_remote_control::TransportZmq;
 #define STRING(A) #A
 
 // rrc_type defined outside of lamda to print latest values if no new ones are received
-#define DEFINE_WATCH_COMMAND(TYPE, FUNCTION, DOC) \
+#define DEFINE_WATCH_COMMAND(TYPE, FUNCTION, CHANNEL, DOC) \
     robot_remote_control::TYPE rrc_type_watch_##FUNCTION; \
     console.registerCommand("watch_" STRING(FUNCTION), DOC, [&](const std::vector<std::string> &params) -> bool { \
             if (controller.FUNCTION(&rrc_type_watch_##FUNCTION)) { \
@@ -26,15 +26,24 @@ using robot_remote_control::TransportZmq;
 #define DEFINE_PRINT_COMMAND(TYPE, FUNCTION, DOC) \
     robot_remote_control::TYPE rrc_type_##FUNCTION; \
     console.registerCommand(#FUNCTION, DOC, [&](const std::vector<std::string> &params) -> bool { \
-        if (controller.FUNCTION(&rrc_type_##FUNCTION, true)) { \
+        uint8_t channel = 0; \
+        if (params.size()) { \
+            channel = std::atoi(params.front().c_str()); \
+        } \
+        if (controller.FUNCTION(&rrc_type_##FUNCTION, true, channel)) { \
             rrc_type_##FUNCTION.PrintDebugString(); \
             return true; \
         } else { \
             printf("no new data received \n"); \
             return false; \
         } \
     }); \
-    DEFINE_WATCH_COMMAND(TYPE, FUNCTION, "continuously "#DOC", press Enter to stop")
+    { \
+        std::vector<ConsoleCommands::ParamDef> params; \
+        params.push_back(ConsoleCommands::ParamDef("channel", "0", true)); \
+        console.registerParamsForCommand(#FUNCTION, params); \
+    } \
+    DEFINE_WATCH_COMMAND(TYPE, FUNCTION, channel, "continuously "#DOC", press Enter to stop")
 
 #define DEFINE_REQUEST_COMMAND(TYPE, FUNCTION, DOC) \
     robot_remote_control::TYPE rrc_type_##FUNCTION; \
@@ -85,9 +94,6 @@ int main(int argc, char** argv) {
     ConsoleCommands console;
     std::vector<ConsoleCommands::ParamDef> params;
 
-    // set Heartbeat to one second
-    controller.setHeartBeatDuration(1);
-
     bool run = true;
 
     console.registerCommand("help", "display this help" , [&](const std::vector<std::string> &params){
@@ -138,13 +144,16 @@ int main(int argc, char** argv) {
     DEFINE_PRINT_COMMAND(IMU, getCurrentIMUState, "print current IMU");
     DEFINE_PRINT_COMMAND(Odometry, getOdometry, "print current Odometry");
     DEFINE_PRINT_COMMAND(Transforms, getCurrentTransforms, "print current Transforms");
+    DEFINE_PRINT_COMMAND(SimpleSensor, getSimpleSensor, "print simple sensor content");
+
+
 
 
     DEFINE_REQUEST_COMMAND(ControllableFrames, requestControllableFrames, "print ControllableFrames set by the robot");
     DEFINE_REQUEST_COMMAND(RobotName, requestRobotName, "print Robot name");
     DEFINE_REQUEST_COMMAND(VideoStreams, requestVideoStreams, "print video Stream ulrs");
     DEFINE_REQUEST_COMMAND(RobotState, requestRobotState, "print current Robot state");
-
+    DEFINE_REQUEST_COMMAND(ChannelsDefinition, requestChannelsDefinition, "get channels defined by the robot")
 
     /**
      * @brief specialized getRobotState (no optional onlyNewest flag)
@@ -156,12 +165,12 @@ int main(int argc, char** argv) {
             received = true;
             rrc_type_requestRobotState.PrintDebugString();
         }
-        if (!received){
+        if (!received) {
             printf("no new data received \n");
         }
         return received;
     });
-    DEFINE_WATCH_COMMAND(RobotState, getRobotState, "continuously print current Robot state, press Enter to stop")
+    DEFINE_WATCH_COMMAND(RobotState, getRobotState, 0, "continuously print current Robot state, press Enter to stop")
 
 
     /**
@@ -296,7 +305,6 @@ int main(int argc, char** argv) {
             } else if (params[0] == "effort") {
                 cmd.add_effort(std::stof(params[2]));
             }
-
         } catch (const std::invalid_argument &e) {
             std::cout << "second value must be a number, was '" << params[1] <<"' " << std::endl;
             std::cout << e.what() << std::endl;
@@ -387,23 +395,22 @@ int main(int argc, char** argv) {
     };
 
     console.registerCommand("requestSimpleActions", "request simple actions and add them to autocomplete", requestSimpleActions);
-    
+
     robot_remote_control::Map map;
     console.registerCommand("requestMap", "get a map", [&](const std::vector<std::string> &params) {
-       int id = std::atoi(params[0].c_str());
-        if (controller.requestMap(&map,id)) {
+        int id = std::atoi(params[0].c_str());
+        if (controller.requestMap(&map, id)) {
             printf("%s\n", map.ShortDebugString().c_str());
-            //map.PrintDebugString();
             return true;
-        } else { 
+        } else {
             printf("no new data received \n");
-            return false; 
-        } 
+            return false;
+        }
     });
     params.push_back(ConsoleCommands::ParamDef("value (int)", "1"));
     console.registerParamsForCommand("requestMap", params);
     params.clear();
-   
+
 
 
     /**
@@ -500,43 +507,47 @@ int main(int argc, char** argv) {
     console.registerParamsForCommand("requestRobotModel", params);
     params.clear();
 
-    /**
-     * Simeplesensors
-     */
-    console.registerCommand("getSimpleSensor", "get simple sensor", [&](const std::vector<std::string> &params){
-        robot_remote_control::SimpleSensor simplesensor;
-        // simplesensores have a buffer of size 1
-        if (controller.getSimpleSensor(std::stoi(params[0]), &simplesensor)) {
-            if (simplesensor.value().size() > 25) {
-                // delete data (so it is not printed)
-                simplesensor.mutable_value()->Clear();
-            }
-            simplesensor.PrintDebugString();
-            return true;
-        } else {
-            printf("no files received\n");
-            return false;
-        }
+
+    console.registerCommand("requestProtocolVersion", "print the protocol sha256sum",  [&](const std::vector<std::string> &params) {
+        std::cout << "remote : " << controller.requestProtocolVersion() << std::endl;
+        std::cout << "local  : " << controller.protocolVersion() << std::endl;
+        return true;
     });
-    params.push_back(ConsoleCommands::ParamDef("id (int)", ""));
-    console.registerParamsForCommand("getSimpleSensor", params);
-    params.clear();
 
+    console.registerCommand("requestLibraryVersion", "print the library version of the remote",  [&](const std::vector<std::string> &params) {
+        std::cout << "remote : " << controller.requestLibraryVersion() << std::endl;
+        std::cout << "local  : " << controller.libraryVersion() << std::endl;
+        return true;
+    });
+
+    console.registerCommand("requestGitVersion", "print the git version of the remote",  [&](const std::vector<std::string> &params) {
+        std::cout << "remote : " << controller.requestGitVersion() << std::endl;
+        std::cout << "local  : " << controller.gitVersion() << std::endl;
+        return true;
+    });
 
     /**
      * Main loop
      */
-    controller.waitForConnection();
 
+    // set Heartbeat to one second, needed tp detect the connection
+    controller.setHeartBeatDuration(1);
+
+    printf("waiting for connection\n");
+    controller.waitForConnection();
     printf("connected\n");
 
+
     // call request lamda (output off)
     requestControllableJoints(std::vector<std::string>());
     printRequestControllableJoints = true;
 
     requestSimpleActions(std::vector<std::string>());
     printRequestSimpleActions = true;
 
+    controller.checkProtocolVersion();
+    controller.checkLibraryVersion();
+
     while (run)  {
         SUCCESS = console.readline("rrc@" + ip + " $ ", EXIT_ON_FAILURE);
         if (!SUCCESS) {