Use replacement dictionary for replacing variables

Author: urfeex

include/ur_client_library/control/script_reader.h

@@ -50,24 +50,18 @@ class ScriptReader
     RobotType robot_type;
   };
 
-  ScriptReader() = delete;
+  using DataDict = std::unordered_map<std::string, std::variant<std::string, double, int>>;
 
-  /*!
-   * \brief Creates a ScriptReader object with the robot info
-   */
-  explicit ScriptReader(const RobotInfo& robot_info) : robot_info_(robot_info)
-  {
-  }
+  ScriptReader() = default;
 
-  std::string readScriptFile(const std::string& filename);
+  std::string readScriptFile(const std::string& filename, const DataDict& data = DataDict());
 
 private:
   std::filesystem::path script_path_;
-  RobotInfo robot_info_;
-  std::unordered_map<std::string, std::variant<std::string, float, int>> replacement_data_;
 
-  void replaceIncludes(std::string& script_code);
   std::string readFileContent(const std::string& filename);
+  void replaceIncludes(std::string& script_code);
+  void replaceVariables(std::string& script_code, const DataDict& data);
 };
 }  // namespace control
 }  // namespace urcl

src/control/script_reader.cpp

@@ -33,17 +33,19 @@
 
 #include <fstream>
 #include <regex>
+#include "ur_client_library/log.h"
 
 namespace urcl
 {
 namespace control
 {
-std::string ScriptReader::readScriptFile(const std::string& filename)
+std::string ScriptReader::readScriptFile(const std::string& filename, const DataDict& data)
 {
   script_path_ = filename;
   std::string script_code = readFileContent(filename);
 
   replaceIncludes(script_code);
+  replaceVariables(script_code, data);
 
   return script_code;
 }
@@ -71,28 +73,57 @@ std::string ScriptReader::readFileContent(const std::string& filename)
 
 void ScriptReader::replaceIncludes(std::string& script)
 {
-  std::string line;
   std::regex include_pattern(R"(\{\%\s*include\s*['|"]([^'"]+)['|"]\s*\%\})");
 
-  std::stringstream input_stream(script);
-  std::stringstream output_stream;
+  std::smatch match;
 
-  while (std::getline(input_stream, line))
+  // Replace all include patterns in the line
+  while (std::regex_search(script, match, include_pattern))
   {
-    std::smatch match;
-    std::string processed_line = line;
+    std::filesystem::path file_path(match[1]);
+    std::string file_content = readFileContent(script_path_.parent_path() / file_path.string());
+    script.replace(match.position(0), match.length(0), file_content);
+  }
+}
 
-    // Replace all include patterns in the line
-    while (std::regex_search(processed_line, match, include_pattern))
+void ScriptReader::replaceVariables(std::string& script_code, const DataDict& data)
+{
+  std::regex pattern(R"(\{\{\s*([\w-]+)\s*\}\})");
+  std::smatch match;
+  while (std::regex_search(script_code, match, pattern))
+  {
+    std::string key = match[1];
+    URCL_LOG_INFO("Found replacement pattern %s", match[0].str().c_str());
+    if (data.find(key) == data.end())
     {
-      std::filesystem::path file_path(match[1]);
-      std::string file_content = readFileContent(script_path_.parent_path() / file_path.string());
-      processed_line.replace(match.position(0), match.length(0), file_content);
+      std::stringstream ss;
+      ss << "Variable '" << key << "' not found in data.";
+      URCL_LOG_ERROR(ss.str().c_str());
+      throw UrException(ss.str().c_str());
     }
+    std::string replaced_value;
 
-    output_stream << processed_line << '\n';
+    if (std::holds_alternative<std::string>(data.at(key)))
+    {
+      replaced_value = std::get<std::string>(data.at(key));
+    }
+    else if (std::holds_alternative<double>(data.at(key)))
+    {
+      replaced_value = std::to_string(std::get<double>(data.at(key)));
+    }
+    else if (std::holds_alternative<int>(data.at(key)))
+    {
+      replaced_value = std::to_string(std::get<int>(data.at(key)));
+    }
+    else
+    {
+      std::stringstream ss;
+      ss << "Unsupported type for variable '" << key << "'.";
+      URCL_LOG_ERROR(ss.str().c_str());
+      throw UrException(ss.str().c_str());
+    }
+    script_code.replace(match.position(0), match.length(0), replaced_value);
   }
-  script = output_stream.str();
 }
 
 }  // namespace control

src/ur/ur_driver.cpp

@@ -32,6 +32,7 @@
 //----------------------------------------------------------------------
 
 #include "ur_client_library/ur/ur_driver.h"
+#include "ur_client_library/control/script_reader.h"
 #include "ur_client_library/exceptions.h"
 #include "ur_client_library/helpers.h"
 #include "ur_client_library/primary/primary_parser.h"
@@ -42,16 +43,16 @@
 
 namespace urcl
 {
-static const std::string BEGIN_REPLACE("{{BEGIN_REPLACE}}");
-static const std::string JOINT_STATE_REPLACE("{{JOINT_STATE_REPLACE}}");
-static const std::string TIME_REPLACE("{{TIME_REPLACE}}");
-static const std::string SERVO_J_REPLACE("{{SERVO_J_REPLACE}}");
-static const std::string SERVER_IP_REPLACE("{{SERVER_IP_REPLACE}}");
-static const std::string SERVER_PORT_REPLACE("{{SERVER_PORT_REPLACE}}");
-static const std::string TRAJECTORY_PORT_REPLACE("{{TRAJECTORY_SERVER_PORT_REPLACE}}");
-static const std::string SCRIPT_COMMAND_PORT_REPLACE("{{SCRIPT_COMMAND_SERVER_PORT_REPLACE}}");
-static const std::string FORCE_MODE_SET_DAMPING_REPLACE("{{FORCE_MODE_SET_DAMPING_REPLACE}}");
-static const std::string FORCE_MODE_SET_GAIN_SCALING_REPLACE("{{FORCE_MODE_SET_GAIN_SCALING_REPLACE}}");
+static const std::string BEGIN_REPLACE("BEGIN_REPLACE");
+static const std::string JOINT_STATE_REPLACE("JOINT_STATE_REPLACE");
+static const std::string TIME_REPLACE("TIME_REPLACE");
+static const std::string SERVO_J_REPLACE("SERVO_J_REPLACE");
+static const std::string SERVER_IP_REPLACE("SERVER_IP_REPLACE");
+static const std::string SERVER_PORT_REPLACE("SERVER_PORT_REPLACE");
+static const std::string TRAJECTORY_PORT_REPLACE("TRAJECTORY_SERVER_PORT_REPLACE");
+static const std::string SCRIPT_COMMAND_PORT_REPLACE("SCRIPT_COMMAND_SERVER_PORT_REPLACE");
+static const std::string FORCE_MODE_SET_DAMPING_REPLACE("FORCE_MODE_SET_DAMPING_REPLACE");
+static const std::string FORCE_MODE_SET_GAIN_SCALING_REPLACE("FORCE_MODE_SET_GAIN_SCALING_REPLACE");
 
 UrDriver::~UrDriver()
 {
@@ -87,52 +88,21 @@ void UrDriver::init(const UrDriverConfiguration& config)
   // Figure out the ip automatically if the user didn't provide it
   std::string local_ip = config.reverse_ip.empty() ? rtde_client_->getIP() : config.reverse_ip;
 
-  startPrimaryClientCommunication();
-  waitFor([this]() { return primary_client_->getConfigurationData() != nullptr; }, std::chrono::milliseconds(500));
-  control::ScriptReader::RobotInfo robot_info;
-  robot_info.robot_type = primary_client_->getRobotType();
-  script_reader_.reset(new control::ScriptReader(robot_info));
+  trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port));
+  script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port));
 
-  std::string prog = readScriptFile(config.script_file);
-  while (prog.find(JOINT_STATE_REPLACE) != std::string::npos)
-  {
-    prog.replace(prog.find(JOINT_STATE_REPLACE), JOINT_STATE_REPLACE.length(),
-                 std::to_string(control::ReverseInterface::MULT_JOINTSTATE));
-  }
-  while (prog.find(TIME_REPLACE) != std::string::npos)
-  {
-    prog.replace(prog.find(TIME_REPLACE), TIME_REPLACE.length(),
-                 std::to_string(control::TrajectoryPointInterface::MULT_TIME));
-  }
+  startPrimaryClientCommunication();
 
+  control::ScriptReader::DataDict data;
+  data[JOINT_STATE_REPLACE] = std::to_string(control::ReverseInterface::MULT_JOINTSTATE);
+  data[TIME_REPLACE] = std::to_string(control::TrajectoryPointInterface::MULT_TIME);
   std::ostringstream out;
   out << "lookahead_time=" << servoj_lookahead_time_ << ", gain=" << servoj_gain_;
-  while (prog.find(SERVO_J_REPLACE) != std::string::npos)
-  {
-    prog.replace(prog.find(SERVO_J_REPLACE), SERVO_J_REPLACE.length(), out.str());
-  }
-
-  while (prog.find(SERVER_IP_REPLACE) != std::string::npos)
-  {
-    prog.replace(prog.find(SERVER_IP_REPLACE), SERVER_IP_REPLACE.length(), local_ip);
-  }
-
-  while (prog.find(SERVER_PORT_REPLACE) != std::string::npos)
-  {
-    prog.replace(prog.find(SERVER_PORT_REPLACE), SERVER_PORT_REPLACE.length(), std::to_string(config.reverse_port));
-  }
-
-  while (prog.find(TRAJECTORY_PORT_REPLACE) != std::string::npos)
-  {
-    prog.replace(prog.find(TRAJECTORY_PORT_REPLACE), TRAJECTORY_PORT_REPLACE.length(),
-                 std::to_string(config.trajectory_port));
-  }
-
-  while (prog.find(SCRIPT_COMMAND_PORT_REPLACE) != std::string::npos)
-  {
-    prog.replace(prog.find(SCRIPT_COMMAND_PORT_REPLACE), SCRIPT_COMMAND_PORT_REPLACE.length(),
-                 std::to_string(config.script_command_port));
-  }
+  data[SERVO_J_REPLACE] = out.str();
+  data[SERVER_IP_REPLACE] = local_ip;
+  data[SERVER_PORT_REPLACE] = std::to_string(config.reverse_port);
+  data[TRAJECTORY_PORT_REPLACE] = std::to_string(config.trajectory_port);
+  data[SCRIPT_COMMAND_PORT_REPLACE] = std::to_string(config.script_command_port);
 
   robot_version_ = rtde_client_->getVersion();
 
@@ -153,10 +123,9 @@ void UrDriver::init(const UrDriverConfiguration& config)
                   << config.tool_comm_setup->getStopBits() << ", " << config.tool_comm_setup->getRxIdleChars() << ", "
                   << config.tool_comm_setup->getTxIdleChars() << ")";
   }
-  prog.replace(prog.find(BEGIN_REPLACE), BEGIN_REPLACE.length(), begin_replace.str());
-
-  trajectory_interface_.reset(new control::TrajectoryPointInterface(config.trajectory_port));
-  script_command_interface_.reset(new control::ScriptCommandInterface(config.script_command_port));
+  data[BEGIN_REPLACE] = begin_replace.str();
+  script_reader_.reset(new control::ScriptReader());
+  std::string prog = script_reader_->readScriptFile(config.script_file, data);
 
   if (in_headless_mode_)
   {

tests/test_script_reader.cpp

@@ -48,8 +48,6 @@ class ScriptReaderTest : public ::testing::Test
 
   std::stringstream simple_script_;
 
-  ScriptReader::RobotInfo robot_info_;
-
   void SetUp() override
   {
     invalid_script_path_ = "test_resources/non_existent_script.urscript";
@@ -66,49 +64,43 @@ class ScriptReaderTest : public ::testing::Test
 
     valid_script_path_ = existing_script_file;
 
-    simple_script_ << "movej([0,0,0,0,0,0])\n";
+    simple_script_ << "movej([0,0,0,0,0,0])";
 
     // Create test resources
     std::ofstream valid_script(valid_script_path_);
     valid_script << simple_script_.str();
     valid_script.close();
-
-    std::ofstream empty_script(empty_script_path_);
-    empty_script.close();
-
-    robot_info_.robot_type = urcl::RobotType::UR3;
   }
 
   void TearDown() override
   {
     std::remove(valid_script_path_.c_str());
-    std::remove(empty_script_path_.c_str());
   }
 };
 
 TEST_F(ScriptReaderTest, ReadValidScript)
 {
-  ScriptReader reader(robot_info_);
+  ScriptReader reader;
   std::string content = reader.readScriptFile(valid_script_path_);
   EXPECT_EQ(content, simple_script_.str());
 }
 
 TEST_F(ScriptReaderTest, ReadEmptyScript)
 {
-  ScriptReader reader(robot_info_);
+  ScriptReader reader;
   std::string content = reader.readScriptFile(empty_script_path_);
   EXPECT_EQ(content, "");
 }
 
 TEST_F(ScriptReaderTest, ReadNonExistentScript)
 {
-  ScriptReader reader(robot_info_);
+  ScriptReader reader;
   EXPECT_THROW(reader.readScriptFile(invalid_script_path_), std::runtime_error);
 }
 
 TEST_F(ScriptReaderTest, ReplaceIncludes)
 {
-  ScriptReader reader(robot_info_);
+  ScriptReader reader;
 
   char existing_script_file[] = "main_script.XXXXXX";
   std::ignore = mkstemp(existing_script_file);
@@ -135,8 +127,34 @@ TEST_F(ScriptReaderTest, ReplaceIncludes)
   ofs_included.close();
 
   std::string processed_script = reader.readScriptFile(existing_script_file);
-  EXPECT_EQ(processed_script, "movej([1,2,3,4,5,6])\n");
+  EXPECT_EQ(processed_script, "movej([1,2,3,4,5,6])");
 
   std::remove(existing_script_file);
   std::remove(existing_include_file);
 }
+
+TEST_F(ScriptReaderTest, ReplaceVariables)
+{
+  ScriptReader reader;
+  ScriptReader::DataDict data;
+  data["VAR1"] = "value1";
+  data["VAR2"] = 42;
+  data["VAR3"] = 6.28;
+
+  char existing_script_file[] = "main_script.XXXXXX";
+  std::ignore = mkstemp(existing_script_file);
+  std::ofstream ofs(existing_script_file);
+  if (ofs.bad())
+  {
+    std::cout << "Failed to create temporary files" << std::endl;
+    GTEST_FAIL();
+  }
+  ofs << "movej([{{VAR1}}, {{VAR2}}, {{VAR3}}, 0, 0, 0])" << std::endl;
+  ofs << "This is just a line without any replacement" << std::endl;
+  ofs.close();
+
+  std::string script = reader.readScriptFile(existing_script_file, data);
+
+  // By default std::to_string will convert double to 6 decimal places
+  EXPECT_EQ(script, "movej([value1, 42, 6.280000, 0, 0, 0])\nThis is just a line without any replacement\n");
+}