WIP ProgramStateMessage

Author: fmauch

CMakeLists.txt

@@ -21,8 +21,10 @@ add_library(urcl SHARED
     src/comm/server.cpp
     src/primary/primary_client.cpp
     src/primary/primary_package.cpp
+    src/primary/program_state_message.cpp
     src/primary/robot_message.cpp
     src/primary/robot_state.cpp
+    src/primary/program_state_message/global_variables_update_message.cpp
     src/primary/robot_message/error_code_message.cpp
     src/primary/robot_message/runtime_exception_message.cpp
     src/primary/robot_message/version_message.cpp

examples/primary_pipeline.cpp

@@ -27,7 +27,7 @@
 
 #include <ur_client_library/comm/pipeline.h>
 #include <ur_client_library/comm/producer.h>
-#include <ur_client_library/comm/shell_consumer.h>
+#include <ur_client_library/primary/primary_shell_consumer.h>
 #include <ur_client_library/primary/primary_parser.h>
 
 #ifdef ROS_BUILD
@@ -38,7 +38,7 @@ using namespace urcl;
 
 // In a real-world example it would be better to get those values from command line parameters / a better configuration
 // system such as Boost.Program_options
-const std::string ROBOT_IP = "192.168.56.101";
+const std::string ROBOT_IP = "172.17.0.2";
 
 int main(int argc, char* argv[])
 {
@@ -60,7 +60,7 @@ int main(int argc, char* argv[])
 
   // The shell consumer will print the package contents to the shell
   std::unique_ptr<comm::IConsumer<primary_interface::PrimaryPackage>> consumer;
-  consumer.reset(new comm::ShellConsumer<primary_interface::PrimaryPackage>());
+  consumer.reset(new primary_interface::PrimaryShellConsumer());
 
   // The notifer will be called at some points during connection setup / loss. This isn't fully
   // implemented atm.
@@ -70,6 +70,18 @@ int main(int argc, char* argv[])
   comm::Pipeline<primary_interface::PrimaryPackage> pipeline(prod, consumer.get(), "Pipeline", notifier);
   pipeline.run();
 
+  std::this_thread::sleep_for(std::chrono::seconds(2));
+
+  std::string script_code = "zero_ftsensor()";
+
+  auto program_with_newline = script_code + '\n';
+
+  size_t len = program_with_newline.size();
+  const uint8_t* data = reinterpret_cast<const uint8_t*>(program_with_newline.c_str());
+  size_t written;
+
+  primary_stream.write(data, len, written);
+
   // Package contents will be printed while not being interrupted
   // Note: Packages for which the parsing isn't implemented, will only get their raw bytes printed.
   while (true)

include/ur_client_library/primary/abstract_primary_consumer.h

@@ -36,6 +36,7 @@
 #include "ur_client_library/primary/robot_message/text_message.h"
 #include "ur_client_library/primary/robot_message/version_message.h"
 #include "ur_client_library/primary/robot_state/kinematics_info.h"
+#include "ur_client_library/primary/program_state_message/global_variables_update_message.h"
 
 namespace urcl
 {
@@ -80,6 +81,8 @@ class AbstractPrimaryConsumer : public comm::IConsumer<PrimaryPackage>
   virtual bool consume(TextMessage& pkg) = 0;
   virtual bool consume(VersionMessage& pkg) = 0;
   virtual bool consume(KinematicsInfo& pkg) = 0;
+  virtual bool consume(ProgramStateMessage& pkg) = 0;
+  virtual bool consume(GlobalVariablesUpdateMessage& pkg) = 0;
 
 private:
   /* data */

include/ur_client_library/primary/primary_parser.h

@@ -32,6 +32,8 @@
 #include "ur_client_library/primary/robot_message/runtime_exception_message.h"
 #include "ur_client_library/primary/robot_message/text_message.h"
 #include "ur_client_library/primary/robot_message/version_message.h"
+#include "ur_client_library/primary/program_state_message/global_variables_update_message.h"
+
 
 namespace urcl
 {
@@ -138,6 +140,28 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
         break;
       }
 
+      case RobotPackageType::PROGRAM_STATE_MESSAGE:
+      {
+        uint64_t timestamp;
+        ProgramStateMessageType state_type;
+        LOG_DEBUG("ProgramStateMessage  received");
+
+        bp.parse(timestamp);
+        bp.parse(state_type);
+
+        LOG_DEBUG("ProgramStateMessage of type %d received", static_cast<int>(state_type));
+
+        std::unique_ptr<PrimaryPackage> packet(programStateFromType(state_type, timestamp));
+        if (!packet->parseWith(bp))
+        {
+          LOG_ERROR("Package parsing of type %d failed!", static_cast<int>(state_type));
+          return false;
+        }
+
+        results.push_back(std::move(packet));
+        return true;
+      }
+
       default:
       {
         LOG_DEBUG("Invalid robot package type recieved: %u", static_cast<uint8_t>(type));
@@ -190,6 +214,21 @@ class PrimaryParser : public comm::Parser<PrimaryPackage>
         return new RobotMessage(timestamp, source, type);
     }
   }
+
+  ProgramStateMessage* programStateFromType(ProgramStateMessageType type, uint64_t timestamp)
+  {
+    switch(type)
+    {
+      //case ProgramStateMessageType::GLOBAL_VARIABLES_SETUP:
+        //return new GlobalVariablesSetupMessage(timestamp);
+      case ProgramStateMessageType::GLOBAL_VARIABLES_UPDATE:
+        return new GlobalVariablesUpdateMessage(timestamp);
+      //case ProgramStateMessageType::TYPE_VARIABLES_UPDATE:
+        //return new TypeVariablesUpdateMessage(timestamp);
+      default:
+      return new ProgramStateMessage(timestamp, type);
+    }
+  }
 };
 
 }  // namespace primary_interface

include/ur_client_library/primary/primary_shell_consumer.h

@@ -82,6 +82,16 @@ class PrimaryShellConsumer : public AbstractPrimaryConsumer
     LOG_INFO("%s", msg.toString().c_str());
     return true;
   }
+  virtual bool consume(ProgramStateMessage& msg) override
+  {
+    LOG_INFO("---ProgramStateMessage---%s", msg.toString().c_str());
+    return true;
+  }
+  virtual bool consume(GlobalVariablesUpdateMessage& msg) override
+  {
+    LOG_INFO("---GlobalVariablesUpdateMessage---\n%s", msg.toString().c_str());
+    return true;
+  }
 
 private:
   /* data */

src/primary/primary_client.cpp

@@ -41,19 +41,21 @@ PrimaryClient::PrimaryClient(const std::string& robot_ip, const std::string& cal
   producer_.reset(new comm::URProducer<PrimaryPackage>(*stream_, parser_));
   producer_->setupProducer();
 
-  consumer_.reset(new PrimaryConsumer());
-  std::shared_ptr<CalibrationChecker> calibration_checker(new CalibrationChecker(calibration_checksum));
-  consumer_->setKinematicsInfoHandler(calibration_checker);
+  //consumer_.reset(new PrimaryConsumer());
+  //std::shared_ptr<CalibrationChecker> calibration_checker(new CalibrationChecker(calibration_checksum));
+  //consumer_->setKinematicsInfoHandler(calibration_checker);
 
+  std::unique_ptr<comm::IConsumer<primary_interface::PrimaryPackage>> consumer;
+  consumer.reset(new primary_interface::PrimaryShellConsumer());
   pipeline_.reset(new comm::Pipeline<PrimaryPackage>(*producer_, consumer_.get(), "primary pipeline", notifier_));
   pipeline_->run();
 
-  calibration_checker->isChecked();
-  while (!calibration_checker->isChecked())
-  {
-    std::this_thread::sleep_for(std::chrono::seconds(1));
-  }
-  LOG_DEBUG("Got calibration information from robot.");
+  //calibration_checker->isChecked();
+  //while (!calibration_checker->isChecked())
+  //{
+    //std::this_thread::sleep_for(std::chrono::seconds(1));
+  //}
+  //LOG_DEBUG("Got calibration information from robot.");
 }
 
 bool PrimaryClient::sendScript(const std::string& script_code)