Add Set Digital Instruction

Author: Levi-Armstrong

tesseract_command_language/CMakeLists.txt

@@ -51,6 +51,7 @@ add_library(
   src/poly/waypoint_poly.cpp
   src/move_instruction.cpp
   src/set_analog_instruction.cpp
+  src/set_digital_instruction.cpp
   src/set_tool_instruction.cpp
   src/timer_instruction.cpp
   src/wait_instruction.cpp

tesseract_command_language/include/tesseract_command_language/fwd.h

@@ -32,6 +32,7 @@ enum class CompositeInstructionOrder : std::uint8_t;
 class CompositeInstruction;
 class MoveInstruction;
 class SetAnalogInstruction;
+class SetDigitalInstruction;
 class SetToolInstruction;
 enum class TimerInstructionType : std::uint8_t;
 class TimerInstruction;

tesseract_command_language/include/tesseract_command_language/instruction_type.h

@@ -32,6 +32,8 @@ class InstructionPoly;
 
 bool isSetAnalogInstruction(const InstructionPoly& instruction);
 
+bool isSetDigitalInstruction(const InstructionPoly& instruction);
+
 bool isSetToolInstruction(const InstructionPoly& instruction);
 
 bool isTimerInstruction(const InstructionPoly& instruction);

tesseract_command_language/include/tesseract_command_language/set_digital_instruction.h

@@ -0,0 +1,138 @@
+/**
+ * @file set_digital_instruction.h
+ * @brief Set Digital Instruction
+ *
+ * @author Levi Armstrong
+ * @date March 23, 2025
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2025, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef TESSERACT_COMMAND_LANGUAGE_SET_DIGITAL_INSTRUCTION_H
+#define TESSERACT_COMMAND_LANGUAGE_SET_DIGITAL_INSTRUCTION_H
+
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <string>
+#include <boost/uuid/uuid.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_command_language/poly/instruction_poly.h>
+
+namespace tesseract_planning
+{
+class SetDigitalInstruction final : public InstructionInterface
+{
+public:
+  SetDigitalInstruction() = default;  // Required for boost serialization do not use
+  SetDigitalInstruction(std::string key, int index, bool value);
+
+  // Instruction
+
+  /**
+   * @brief Get the UUID
+   * @return The UUID
+   */
+  const boost::uuids::uuid& getUUID() const override final;
+  /**
+   * @brief Set the UUID
+   * @param uuid The UUID
+   */
+  void setUUID(const boost::uuids::uuid& uuid) override final;
+  /**
+   * @brief Regenerate the UUID
+   */
+  void regenerateUUID() override final;
+
+  /**
+   * @brief Get the parent UUID
+   * @return The parent UUID
+   */
+  const boost::uuids::uuid& getParentUUID() const override final;
+  /**
+   * @brief Set the parent UUID
+   * @param uuid The parent UUID
+   */
+  void setParentUUID(const boost::uuids::uuid& uuid) override final;
+
+  /**
+   * @brief Get the description
+   * @return The description
+   */
+  const std::string& getDescription() const override final;
+  /**
+   * @brief Set the description
+   * @param description The description
+   */
+  void setDescription(const std::string& description) override final;
+
+  /**
+   * @brief Output the contents to std::cout
+   * @param prefix The prefix to add to each variable
+   */
+  void print(const std::string& prefix = "") const override final;  // NOLINT
+
+  /**
+   * @brief Make a deep copy of the object
+   * @return A deep copy
+   */
+  std::unique_ptr<InstructionInterface> clone() const override final;
+
+  // SetDigitalInstruction
+
+  /** @brief Get the digital key */
+  std::string getKey() const;
+
+  /** @brief Get the digital index */
+  int getIndex() const;
+
+  /** @brief Get the digital value */
+  bool getValue() const;
+
+private:
+  /** @brief The instructions UUID */
+  boost::uuids::uuid uuid_{};
+  /** @brief The parent UUID if created from createChild */
+  boost::uuids::uuid parent_uuid_{};
+  /** @brief The description of the instruction */
+  std::string description_{ "Tesseract Set Digital Instruction" };
+  /** @brief The key is used to identify which type of digital to set */
+  std::string key_;
+  /** @brief The digital index to set */
+  int index_{ 0 };
+  /** @brief The digital value */
+  bool value_{ false };
+
+  /**
+   * @brief Check if two objects are equal
+   * @param other The other object to compare with
+   * @return True if equal, otherwise false
+   */
+  bool equals(const InstructionInterface& other) const override final;
+
+  friend class boost::serialization::access;
+  friend struct tesseract_common::Serialization;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version);  // NOLINT
+};
+}  // namespace tesseract_planning
+
+BOOST_CLASS_EXPORT_KEY(tesseract_planning::SetDigitalInstruction)
+BOOST_CLASS_TRACKING(tesseract_planning::SetDigitalInstruction, boost::serialization::track_never)
+
+#endif  // TESSERACT_COMMAND_LANGUAGE_SET_DIGITAL_INSTRUCTION_H

tesseract_command_language/src/instruction_type.cpp

@@ -25,6 +25,7 @@
  */
 
 #include <tesseract_command_language/set_analog_instruction.h>
+#include <tesseract_command_language/set_digital_instruction.h>
 #include <tesseract_command_language/set_tool_instruction.h>
 #include <tesseract_command_language/timer_instruction.h>
 #include <tesseract_command_language/wait_instruction.h>
@@ -36,6 +37,11 @@ bool isSetAnalogInstruction(const InstructionPoly& instruction)
   return (instruction.getType() == std::type_index(typeid(SetAnalogInstruction)));
 }
 
+bool isSetDigitalInstruction(const InstructionPoly& instruction)
+{
+  return (instruction.getType() == std::type_index(typeid(SetDigitalInstruction)));
+}
+
 bool isSetToolInstruction(const InstructionPoly& instruction)
 {
   return (instruction.getType() == std::type_index(typeid(SetToolInstruction)));

tesseract_command_language/src/set_digital_instruction.cpp

@@ -0,0 +1,108 @@
+/**
+ * @file set_digital_instruction.cpp
+ * @brief Set Digital Instruction
+ *
+ * @author Levi Armstrong
+ * @date March 23, 2025
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2025, Southwest Research Institute
+ *
+ * @par License
+ * Software License Agreement (Apache License)
+ * @par
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ * http://www.apache.org/licenses/LICENSE-2.0
+ * @par
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <tesseract_common/macros.h>
+TESSERACT_COMMON_IGNORE_WARNINGS_PUSH
+#include <iostream>
+#include <boost/serialization/base_object.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_io.hpp>
+#include <boost/uuid/uuid_serialize.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_command_language/set_digital_instruction.h>
+#include <tesseract_common/utils.h>
+
+namespace tesseract_planning
+{
+SetDigitalInstruction::SetDigitalInstruction(std::string key, int index, bool value)
+  : uuid_(boost::uuids::random_generator()()), key_(std::move(key)), index_(index), value_(value)
+{
+}
+
+const boost::uuids::uuid& SetDigitalInstruction::getUUID() const { return uuid_; }
+void SetDigitalInstruction::setUUID(const boost::uuids::uuid& uuid)
+{
+  if (uuid.is_nil())
+    throw std::runtime_error("SetDigitalInstruction, tried to set uuid to null!");
+
+  uuid_ = uuid;
+}
+void SetDigitalInstruction::regenerateUUID() { uuid_ = boost::uuids::random_generator()(); }
+
+const boost::uuids::uuid& SetDigitalInstruction::getParentUUID() const { return parent_uuid_; }
+void SetDigitalInstruction::setParentUUID(const boost::uuids::uuid& uuid) { parent_uuid_ = uuid; }
+
+const std::string& SetDigitalInstruction::getDescription() const { return description_; }
+
+void SetDigitalInstruction::setDescription(const std::string& description) { description_ = description; }
+
+void SetDigitalInstruction::print(const std::string& prefix) const  // NOLINT
+{
+  std::cout << prefix + "Set Analog Instruction, Key: " << key_ << ", Index: " << index_ << ", Value: " << value_;
+  std::cout << ", Description: " << getDescription() << "\n";
+}
+
+std::unique_ptr<InstructionInterface> SetDigitalInstruction::clone() const
+{
+  return std::make_unique<SetDigitalInstruction>(*this);
+}
+
+std::string SetDigitalInstruction::getKey() const { return key_; }
+
+int SetDigitalInstruction::getIndex() const { return index_; }
+
+bool SetDigitalInstruction::getValue() const { return value_; }
+
+bool SetDigitalInstruction::equals(const InstructionInterface& other) const
+{
+  const auto* rhs = dynamic_cast<const SetDigitalInstruction*>(&other);
+  if (rhs == nullptr)
+    return false;
+
+  bool equal = true;
+  equal &= (key_ == rhs->key_);
+  equal &= (index_ == rhs->index_);
+  equal &= (value_ == rhs->value_);
+  return equal;
+}
+
+template <class Archive>
+void SetDigitalInstruction::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_BASE_OBJECT_NVP(InstructionInterface);
+  ar& boost::serialization::make_nvp("uuid", uuid_);
+  ar& boost::serialization::make_nvp("parent_uuid", parent_uuid_);
+  ar& boost::serialization::make_nvp("description", description_);
+  ar& boost::serialization::make_nvp("key", key_);
+  ar& boost::serialization::make_nvp("index", index_);
+  ar& boost::serialization::make_nvp("value", value_);
+}
+
+}  // namespace tesseract_planning
+
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_planning::SetDigitalInstruction)
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_planning::SetDigitalInstruction)

tesseract_command_language/test/command_language_unit.cpp

@@ -40,6 +40,7 @@
 #include <tesseract_command_language/state_waypoint.h>
 #include <tesseract_command_language/move_instruction.h>
 #include <tesseract_command_language/set_analog_instruction.h>
+#include <tesseract_command_language/set_digital_instruction.h>
 #include <tesseract_command_language/set_tool_instruction.h>
 #include <tesseract_command_language/timer_instruction.h>
 #include <tesseract_command_language/wait_instruction.h>
@@ -216,6 +217,59 @@ TEST(TesseractCommandLanguageUnit, SetAnalogInstructionTests)  // NOLINT
   }
 }
 
+TEST(TesseractCommandLanguageUnit, SetDigitalInstructionTests)  // NOLINT
+{
+  using T = SetDigitalInstruction;
+  test_suite::runInstructionInterfaceTest(InstructionPoly(T()));
+
+  {
+    const std::string key{ "key" };
+    const int index{ 5 };
+    const bool value{ true };
+    T instr(key, index, value);
+    EXPECT_EQ(instr.getKey(), key);
+    EXPECT_EQ(instr.getIndex(), index);
+    EXPECT_EQ(instr.getValue(), value);
+
+    // Copy
+    InstructionPoly poly{ instr };
+    InstructionPoly copy(poly);
+    EXPECT_EQ(copy.as<T>().getKey(), key);
+    EXPECT_EQ(copy.as<T>().getIndex(), index);
+    EXPECT_EQ(copy.as<T>().getValue(), value);
+    EXPECT_TRUE(isSetDigitalInstruction(poly));
+    EXPECT_TRUE(isSetDigitalInstruction(copy));
+    test_suite::runInstructionSerializationTest(poly);
+    test_suite::runInstructionSerializationTest(copy);
+
+    // Equal
+    EXPECT_TRUE(poly == copy);
+    EXPECT_TRUE(copy == poly);
+    EXPECT_FALSE(copy != poly);
+
+    {  // Not Equal
+      {
+        InstructionPoly ne_poly{ T("ne", index, value) };
+        EXPECT_FALSE(poly == ne_poly);
+        EXPECT_FALSE(ne_poly == poly);
+        EXPECT_TRUE(ne_poly != poly);
+      }
+      {
+        InstructionPoly ne_poly{ T(key, 1, value) };
+        EXPECT_FALSE(poly == ne_poly);
+        EXPECT_FALSE(ne_poly == poly);
+        EXPECT_TRUE(ne_poly != poly);
+      }
+      {
+        InstructionPoly ne_poly{ T(key, index, false) };
+        EXPECT_FALSE(poly == ne_poly);
+        EXPECT_FALSE(ne_poly == poly);
+        EXPECT_TRUE(ne_poly != poly);
+      }
+    }
+  }
+}
+
 TEST(TesseractCommandLanguageUnit, SetToolInstructionTests)  // NOLINT
 {
   using T = SetToolInstruction;