diff --git a/tesseract_common/CMakeLists.txt b/tesseract_common/CMakeLists.txt
index 40a0b59cf29..3a0d0c29b0a 100644
--- a/tesseract_common/CMakeLists.txt
+++ b/tesseract_common/CMakeLists.txt
@@ -60,6 +60,8 @@ add_library(
   src/shader_param.cpp
   src/shader_params.cpp
   src/shader_type.cpp
+  src/tool_path.cpp
+  src/tool_path_segment.cpp
   src/types.cpp)
 target_link_libraries(
   ${PROJECT_NAME}
@@ -85,8 +87,7 @@ target_code_coverage(
   ENABLE ${TESSERACT_ENABLE_CODE_COVERAGE})
 target_include_directories(${PROJECT_NAME} PUBLIC "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"
                                                   "$<INSTALL_INTERFACE:include>")
-target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC
-    ${FreeImage_INCLUDE_DIRS})
+target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${FreeImage_INCLUDE_DIRS})
 
 configure_package(NAMESPACE tesseract TARGETS ${PROJECT_NAME})
 
diff --git a/tesseract_common/include/tesseract_common/tool_path.h b/tesseract_common/include/tesseract_common/tool_path.h
new file mode 100644
index 00000000000..2daa1520c8c
--- /dev/null
+++ b/tesseract_common/include/tesseract_common/tool_path.h
@@ -0,0 +1,241 @@
+/**
+ * @file tool_path.h
+ * @brief Common Tesseract Tool Path
+ *
+ * @author Levi Armstrong
+ * @date Nov 16, 2022
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2022, Levi Armstrong
+ *
+ * @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_COMMON_TOOL_PATH_H
+#define TESSERACT_COMMON_TOOL_PATH_H
+
+#include <string>
+#include <tesseract_common/tool_path_segment.h>
+
+namespace tesseract_common
+{
+class ToolPath
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+  ToolPath(std::string description = "");
+  virtual ~ToolPath() = default;
+
+  /** @brief Get the uuid */
+  boost::uuids::uuid getUUID() const;
+
+  /** @brief Regenerate uuid */
+  void regenerateUUID();
+
+  /**
+   * @brief Get the parent uuid
+   * @details This can be null
+   */
+  const boost::uuids::uuid& getParentUUID() const;
+
+  /**
+   * @brief Set the parent uuid
+   * @details This can be used in cases were a segment is split during a filter process and want tracability.
+   * @param uuid
+   */
+  void setParentUUID(const boost::uuids::uuid& uuid);
+
+  /** @brief Set the tool path description */
+  void setDescription(const std::string& desc);
+
+  /** @brief Get the tool path description */
+  const std::string& getDescription() const;
+
+  /**
+   * @brief Set a namespace for the tool path
+   * @details This can be multiple namespace by separating by '::' for example 'main::left::place'
+   * which will have it show up in the widget as a teared item in the tree view.
+   * @param ns A namespace the tool path is associated with
+   */
+  void setNamespace(std::string ns);
+
+  /**
+   * @brief Get the namespace for the tool path
+   * @return The namespace
+   */
+  const std::string& getNamespace() const;
+
+  bool operator==(const ToolPath& rhs) const;
+  bool operator!=(const ToolPath& rhs) const;
+
+  ///////////////////////////
+  // C++ container support //
+  ///////////////////////////
+  /** pointer */
+  using pointer = typename AlignedVector<ToolPathSegment>::pointer;
+  /** const_pointer */
+  using const_pointer = typename AlignedVector<ToolPathSegment>::const_pointer;
+  /** reference */
+  using reference = typename AlignedVector<ToolPathSegment>::reference;
+  /** const_reference */
+  using const_reference = typename AlignedVector<ToolPathSegment>::const_reference;
+  /** size_type */
+  using size_type = typename AlignedVector<ToolPathSegment>::size_type;
+  /** difference_type */
+  using difference_type = typename AlignedVector<ToolPathSegment>::difference_type;
+  /** iterator */
+  using iterator = typename AlignedVector<ToolPathSegment>::iterator;
+  /** const_iterator */
+  using const_iterator = typename AlignedVector<ToolPathSegment>::const_iterator;
+  /** reverse_iterator */
+  using reverse_iterator = typename AlignedVector<ToolPathSegment>::reverse_iterator;
+  /** const_reverse_iterator */
+  using const_reverse_iterator = typename AlignedVector<ToolPathSegment>::const_reverse_iterator;
+
+  ///////////////
+  // Iterators //
+  ///////////////
+  /** @brief returns an iterator to the beginning */
+  iterator begin();
+  /** @brief returns an iterator to the beginning */
+  const_iterator begin() const;
+  /** @brief returns an iterator to the end */
+  iterator end();
+  /** @brief returns an iterator to the end */
+  const_iterator end() const;
+  /** @brief returns a reverse iterator to the beginning */
+  reverse_iterator rbegin();
+  /** @brief returns a reverse iterator to the beginning */
+  const_reverse_iterator rbegin() const;
+  /** @brief returns a reverse iterator to the end */
+  reverse_iterator rend();
+  /** @brief returns a reverse iterator to the end */
+  const_reverse_iterator rend() const;
+  /** @brief returns an iterator to the beginning */
+  const_iterator cbegin() const;
+  /** @brief returns an iterator to the end */
+  const_iterator cend() const;
+  /** @brief returns a reverse iterator to the beginning */
+  const_reverse_iterator crbegin() const;
+  /** @brief returns a reverse iterator to the end */
+  const_reverse_iterator crend() const;
+
+  //////////////
+  // Capacity //
+  //////////////
+  /** @brief checks whether the container is empty */
+  bool empty() const;
+  /** @brief returns the number of elements */
+  size_type size() const;
+  /** @brief returns the maximum possible number of elements */
+  size_type max_size() const;
+  /** @brief reserve number of elements */
+  void reserve(size_type n);
+  /** @brief returns the number of elements that can be held in currently allocated storage */
+  size_type capacity() const;
+  /** @brief reduces memory usage by freeing unused memory  */
+  void shrink_to_fit();
+
+  ////////////////////
+  // Element Access //
+  ////////////////////
+  /** @brief access the first element */
+  reference front();
+  /** @brief access the first element */
+  const_reference front() const;
+  /** @brief access the last element */
+  reference back();
+  /** @brief access the last element */
+  const_reference back() const;
+  /** @brief access specified element with bounds checking */
+  reference at(size_type n);
+  /** @brief access specified element with bounds checking */
+  const_reference at(size_type n) const;
+  /** @brief direct access to the underlying array  */
+  pointer data();
+  /** @brief direct access to the underlying array  */
+  const_pointer data() const;
+  /** @brief access specified element */
+  reference operator[](size_type pos);
+  /** @brief access specified element */
+  const_reference operator[](size_type pos) const;
+
+  ///////////////
+  // Modifiers //
+  ///////////////
+  /** @brief clears the contents */
+  void clear();
+
+  /** @brief inserts element */
+  iterator insert(const_iterator p, const ToolPathSegment& x);
+  iterator insert(const_iterator p, ToolPathSegment&& x);
+  iterator insert(const_iterator p, std::initializer_list<ToolPathSegment> l);
+  template <class InputIt>
+  void insert(const_iterator pos, InputIt first, InputIt last)
+  {
+    container_.insert(pos, first, last);
+  }
+
+  /** @brief constructs element in-place */
+  template <class... Args>
+  iterator emplace(const_iterator pos, Args&&... args);
+
+  /** @brief erases element */
+  iterator erase(const_iterator p);
+  iterator erase(const_iterator first, const_iterator last);
+
+  /** Append element to container */
+  void push_back(const ToolPathSegment& x);
+  void push_back(const ToolPathSegment&& x);
+
+  /** @brief constructs an element in-place at the end  */
+  template <typename... Args>
+#if __cplusplus > 201402L
+  reference emplace_back(Args&&... args);
+#else
+  void emplace_back(Args&&... args);
+#endif
+
+  /** @brief removes the last element */
+  void pop_back();
+
+  /** @brief swaps the contents  */
+  void swap(AlignedVector<ToolPathSegment>& other);
+
+protected:
+  /** @brief The uuid */
+  boost::uuids::uuid uuid_;
+
+  /** @brief The parent uuid */
+  boost::uuids::uuid parent_uuid_;
+
+  /** @brief The description */
+  std::string description_;
+
+  /** @brief The container */
+  AlignedVector<ToolPathSegment> container_;
+
+  /** @brief The namespace associated with the tool path */
+  std::string ns_{ "general" };
+
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version);  // NOLINT
+};
+
+}  // namespace tesseract_common
+
+#endif  // TESSERACT_COMMON_TOOL_PATH_H
diff --git a/tesseract_common/include/tesseract_common/tool_path_segment.h b/tesseract_common/include/tesseract_common/tool_path_segment.h
new file mode 100644
index 00000000000..d48f95390db
--- /dev/null
+++ b/tesseract_common/include/tesseract_common/tool_path_segment.h
@@ -0,0 +1,223 @@
+/**
+ * @file tool_path_segment.h
+ * @brief Common Tesseract Tool Path Segment
+ *
+ * @author Levi Armstrong
+ * @date Nov 16, 2022
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2022, Levi Armstrong
+ *
+ * @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_COMMON_TOOL_PATH_SEGMENT_H
+#define TESSERACT_COMMON_TOOL_PATH_SEGMENT_H
+
+#include <string>
+#include <boost/uuid/uuid.hpp>
+#include <tesseract_common/types.h>
+
+namespace tesseract_common
+{
+class ToolPathSegment
+{
+public:
+  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+  ToolPathSegment(std::string description = "");
+  virtual ~ToolPathSegment() = default;
+
+  /** @brief Get the uuid */
+  boost::uuids::uuid getUUID() const;
+
+  /** @brief Regenerate uuid */
+  void regenerateUUID();
+
+  /**
+   * @brief Get the parent uuid
+   * @details This can be null
+   */
+  const boost::uuids::uuid& getParentUUID() const;
+
+  /**
+   * @brief Set the parent uuid
+   * @details This can be used in cases were a segment is split during a filter process and want tracability.
+   * @param uuid
+   */
+  void setParentUUID(const boost::uuids::uuid& uuid);
+
+  /** @brief Set the segment description */
+  void setDescription(const std::string& desc);
+
+  /** @brief Get the segment description */
+  const std::string& getDescription() const;
+
+  bool operator==(const ToolPathSegment& rhs) const;
+  bool operator!=(const ToolPathSegment& rhs) const;
+
+  ///////////////////////////
+  // C++ container support //
+  ///////////////////////////
+  /** pointer */
+  using pointer = typename VectorIsometry3d::pointer;
+  /** const_pointer */
+  using const_pointer = typename VectorIsometry3d::const_pointer;
+  /** reference */
+  using reference = typename VectorIsometry3d::reference;
+  /** const_reference */
+  using const_reference = typename VectorIsometry3d::const_reference;
+  /** size_type */
+  using size_type = typename VectorIsometry3d::size_type;
+  /** difference_type */
+  using difference_type = typename VectorIsometry3d::difference_type;
+  /** iterator */
+  using iterator = typename VectorIsometry3d::iterator;
+  /** const_iterator */
+  using const_iterator = typename VectorIsometry3d::const_iterator;
+  /** reverse_iterator */
+  using reverse_iterator = typename VectorIsometry3d::reverse_iterator;
+  /** const_reverse_iterator */
+  using const_reverse_iterator = typename VectorIsometry3d::const_reverse_iterator;
+
+  ///////////////
+  // Iterators //
+  ///////////////
+  /** @brief returns an iterator to the beginning */
+  iterator begin();
+  /** @brief returns an iterator to the beginning */
+  const_iterator begin() const;
+  /** @brief returns an iterator to the end */
+  iterator end();
+  /** @brief returns an iterator to the end */
+  const_iterator end() const;
+  /** @brief returns a reverse iterator to the beginning */
+  reverse_iterator rbegin();
+  /** @brief returns a reverse iterator to the beginning */
+  const_reverse_iterator rbegin() const;
+  /** @brief returns a reverse iterator to the end */
+  reverse_iterator rend();
+  /** @brief returns a reverse iterator to the end */
+  const_reverse_iterator rend() const;
+  /** @brief returns an iterator to the beginning */
+  const_iterator cbegin() const;
+  /** @brief returns an iterator to the end */
+  const_iterator cend() const;
+  /** @brief returns a reverse iterator to the beginning */
+  const_reverse_iterator crbegin() const;
+  /** @brief returns a reverse iterator to the end */
+  const_reverse_iterator crend() const;
+
+  //////////////
+  // Capacity //
+  //////////////
+  /** @brief checks whether the container is empty */
+  bool empty() const;
+  /** @brief returns the number of elements */
+  size_type size() const;
+  /** @brief returns the maximum possible number of elements */
+  size_type max_size() const;
+  /** @brief reserve number of elements */
+  void reserve(size_type n);
+  /** @brief returns the number of elements that can be held in currently allocated storage */
+  size_type capacity() const;
+  /** @brief reduces memory usage by freeing unused memory  */
+  void shrink_to_fit();
+
+  ////////////////////
+  // Element Access //
+  ////////////////////
+  /** @brief access the first element */
+  reference front();
+  /** @brief access the first element */
+  const_reference front() const;
+  /** @brief access the last element */
+  reference back();
+  /** @brief access the last element */
+  const_reference back() const;
+  /** @brief access specified element with bounds checking */
+  reference at(size_type n);
+  /** @brief access specified element with bounds checking */
+  const_reference at(size_type n) const;
+  /** @brief direct access to the underlying array  */
+  pointer data();
+  /** @brief direct access to the underlying array  */
+  const_pointer data() const;
+  /** @brief access specified element */
+  reference operator[](size_type pos);
+  /** @brief access specified element */
+  const_reference operator[](size_type pos) const;
+
+  ///////////////
+  // Modifiers //
+  ///////////////
+  /** @brief clears the contents */
+  void clear();
+
+  /** @brief inserts element */
+  iterator insert(const_iterator p, const Eigen::Isometry3d& x);
+  iterator insert(const_iterator p, Eigen::Isometry3d&& x);
+  iterator insert(const_iterator p, std::initializer_list<Eigen::Isometry3d> l);
+  template <class InputIt>
+  void insert(const_iterator pos, InputIt first, InputIt last)
+  {
+    container_.insert(pos, first, last);
+  }
+
+  /** @brief constructs element in-place */
+  template <class... Args>
+  iterator emplace(const_iterator pos, Args&&... args);
+
+  /** @brief erases element */
+  iterator erase(const_iterator p);
+  iterator erase(const_iterator first, const_iterator last);
+
+  /** Append element to container */
+  void push_back(const Eigen::Isometry3d& x);
+  void push_back(const Eigen::Isometry3d&& x);
+
+  /** @brief constructs an element in-place at the end  */
+  template <typename... Args>
+#if __cplusplus > 201402L
+  reference emplace_back(Args&&... args);
+#else
+  void emplace_back(Args&&... args);
+#endif
+
+  /** @brief removes the last element */
+  void pop_back();
+
+  /** @brief swaps the contents  */
+  void swap(VectorIsometry3d& other);
+
+protected:
+  /** @brief The uuid */
+  boost::uuids::uuid uuid_;
+
+  /** @brief The parent uuid */
+  boost::uuids::uuid parent_uuid_;
+
+  /** @brief The description */
+  std::string description_;
+
+  /** @brief The container */
+  VectorIsometry3d container_;
+
+  friend class boost::serialization::access;
+  template <class Archive>
+  void serialize(Archive& ar, const unsigned int version);  // NOLINT
+};
+
+}  // namespace tesseract_common
+#endif  // TESSERACT_COMMON_TOOL_PATH_SEGMENT_H
diff --git a/tesseract_common/src/tool_path.cpp b/tesseract_common/src/tool_path.cpp
new file mode 100644
index 00000000000..93a6953df13
--- /dev/null
+++ b/tesseract_common/src/tool_path.cpp
@@ -0,0 +1,169 @@
+/**
+ * @file tool_path.cpp
+ * @brief Common Tesseract Tool Path
+ *
+ * @author Levi Armstrong
+ * @date Nov 16, 2022
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2022, Levi Armstrong
+ *
+ * @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 <boost/serialization/access.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/uuid/uuid_io.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_serialize.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_common/tool_path.h>
+
+namespace tesseract_common
+{
+ToolPath::ToolPath(std::string description)
+  : uuid_(boost::uuids::random_generator()()), description_(std::move(description))
+{
+}
+
+boost::uuids::uuid ToolPath::getUUID() const { return uuid_; }
+
+void ToolPath::regenerateUUID() { uuid_ = boost::uuids::random_generator()(); }
+
+const boost::uuids::uuid& ToolPath::getParentUUID() const { return parent_uuid_; }
+
+void ToolPath::setParentUUID(const boost::uuids::uuid& uuid) { parent_uuid_ = uuid; }
+
+void ToolPath::setDescription(const std::string& desc) { description_ = desc; }
+const std::string& ToolPath::getDescription() const { return description_; }
+
+bool ToolPath::operator==(const ToolPath& rhs) const
+{
+  bool equal = true;
+  equal &= (uuid_ == rhs.uuid_);                // NOLINT
+  equal &= (parent_uuid_ == rhs.parent_uuid_);  // NOLINT
+  equal &= (description_ == rhs.description_);  // NOLINT
+  equal &= (container_ == rhs.container_);
+  return equal;
+}
+
+void ToolPath::setNamespace(std::string ns) { ns_ = std::move(ns); }
+
+const std::string& ToolPath::getNamespace() const { return ns_; }
+
+bool ToolPath::operator!=(const ToolPath& rhs) const { return !operator==(rhs); }
+
+template <class Archive>
+void ToolPath::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_NVP(uuid_);
+  ar& BOOST_SERIALIZATION_NVP(parent_uuid_);
+  ar& BOOST_SERIALIZATION_NVP(description_);
+  ar& BOOST_SERIALIZATION_NVP(container_);
+}
+
+///////////////////////////
+// C++ container support //
+///////////////////////////
+
+///////////////
+// Iterators //
+///////////////
+ToolPath::iterator ToolPath::begin() { return container_.begin(); }
+ToolPath::const_iterator ToolPath::begin() const { return container_.begin(); }
+ToolPath::iterator ToolPath::end() { return container_.end(); }
+ToolPath::const_iterator ToolPath::end() const { return container_.end(); }
+ToolPath::reverse_iterator ToolPath::rbegin() { return container_.rbegin(); }
+ToolPath::const_reverse_iterator ToolPath::rbegin() const { return container_.rbegin(); }
+ToolPath::reverse_iterator ToolPath::rend() { return container_.rend(); }
+ToolPath::const_reverse_iterator ToolPath::rend() const { return container_.rend(); }
+ToolPath::const_iterator ToolPath::cbegin() const { return container_.cbegin(); }
+ToolPath::const_iterator ToolPath::cend() const { return container_.cend(); }
+ToolPath::const_reverse_iterator ToolPath::crbegin() const { return container_.crbegin(); }
+ToolPath::const_reverse_iterator ToolPath::crend() const { return container_.crend(); }
+
+//////////////
+// Capacity //
+//////////////
+bool ToolPath::empty() const { return container_.empty(); }
+ToolPath::size_type ToolPath::size() const { return container_.size(); }
+ToolPath::size_type ToolPath::max_size() const { return container_.max_size(); }
+void ToolPath::reserve(size_type n) { container_.reserve(n); }
+ToolPath::size_type ToolPath::capacity() const { return container_.capacity(); }
+void ToolPath::shrink_to_fit() { container_.shrink_to_fit(); }
+
+////////////////////
+// Element Access //
+////////////////////
+ToolPath::reference ToolPath::front() { return container_.front(); }
+ToolPath::const_reference ToolPath::front() const { return container_.front(); }
+ToolPath::reference ToolPath::back() { return container_.back(); }
+ToolPath::const_reference ToolPath::back() const { return container_.back(); }
+ToolPath::reference ToolPath::at(size_type n) { return container_.at(n); }
+ToolPath::const_reference ToolPath::at(size_type n) const { return container_.at(n); }
+ToolPath::pointer ToolPath::data() { return container_.data(); }
+ToolPath::const_pointer ToolPath::data() const { return container_.data(); }
+ToolPath::reference ToolPath::operator[](size_type pos) { return container_[pos]; }
+ToolPath::const_reference ToolPath::operator[](size_type pos) const { return container_[pos]; }
+
+///////////////
+// Modifiers //
+///////////////
+void ToolPath::clear() { container_.clear(); }
+
+ToolPath::iterator ToolPath::insert(const_iterator p, const ToolPathSegment& x) { return container_.insert(p, x); }
+ToolPath::iterator ToolPath::insert(const_iterator p, ToolPathSegment&& x) { return container_.insert(p, x); }
+ToolPath::iterator ToolPath::insert(const_iterator p, std::initializer_list<ToolPathSegment> l)
+{
+  return container_.insert(p, l);
+}
+
+template <class... Args>
+ToolPath::iterator ToolPath::emplace(const_iterator pos, Args&&... args)
+{
+  return container_.emplace(pos, std::forward<Args>(args)...);
+}
+
+ToolPath::iterator ToolPath::erase(const_iterator p) { return container_.erase(p); }
+ToolPath::iterator ToolPath::erase(const_iterator first, const_iterator last) { return container_.erase(first, last); }
+void ToolPath::push_back(const ToolPathSegment& x) { container_.push_back(x); }
+void ToolPath::push_back(const ToolPathSegment&& x) { container_.push_back(x); }
+
+template <typename... Args>
+#if __cplusplus > 201402L
+ToolPath::reference ToolPath::emplace_back(Args&&... args)
+{
+  return container_.emplace_back(std::forward<Args>(args)...);
+}
+#else
+void ToolPath::emplace_back(Args&&... args)
+{
+  container_.emplace_back(std::forward<Args>(args)...);
+}
+#endif
+
+void ToolPath::pop_back() { container_.pop_back(); }
+void ToolPath::swap(AlignedVector<ToolPathSegment>& other) { container_.swap(other); }
+
+}  // namespace tesseract_common
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ToolPath)
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ToolPath)
diff --git a/tesseract_common/src/tool_path_segment.cpp b/tesseract_common/src/tool_path_segment.cpp
new file mode 100644
index 00000000000..1a2023ebf46
--- /dev/null
+++ b/tesseract_common/src/tool_path_segment.cpp
@@ -0,0 +1,185 @@
+/**
+ * @file tool_path_segment.cpp
+ * @brief Common Tesseract Tool Path Segment
+ *
+ * @author Levi Armstrong
+ * @date Nov 16, 2022
+ * @version TODO
+ * @bug No known bugs
+ *
+ * @copyright Copyright (c) 2022, Levi Armstrong
+ *
+ * @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 <boost/serialization/access.hpp>
+#include <boost/serialization/nvp.hpp>
+#include <boost/serialization/vector.hpp>
+#include <boost/uuid/uuid_io.hpp>
+#include <boost/uuid/uuid_generators.hpp>
+#include <boost/uuid/uuid_serialize.hpp>
+TESSERACT_COMMON_IGNORE_WARNINGS_POP
+
+#include <tesseract_common/tool_path_segment.h>
+#include <tesseract_common/eigen_serialization.h>
+
+namespace tesseract_common
+{
+ToolPathSegment::ToolPathSegment(std::string description)
+  : uuid_(boost::uuids::random_generator()()), description_(std::move(description))
+{
+}
+
+boost::uuids::uuid ToolPathSegment::getUUID() const { return uuid_; }
+
+void ToolPathSegment::regenerateUUID() { uuid_ = boost::uuids::random_generator()(); }
+
+const boost::uuids::uuid& ToolPathSegment::getParentUUID() const { return parent_uuid_; }
+
+void ToolPathSegment::setParentUUID(const boost::uuids::uuid& uuid) { parent_uuid_ = uuid; }
+
+void ToolPathSegment::setDescription(const std::string& desc) { description_ = desc; }
+const std::string& ToolPathSegment::getDescription() const { return description_; }
+
+bool ToolPathSegment::operator==(const ToolPathSegment& rhs) const
+{
+  bool equal = true;
+  equal &= (uuid_ == rhs.uuid_);                // NOLINT
+  equal &= (parent_uuid_ == rhs.parent_uuid_);  // NOLINT
+  equal &= (description_ == rhs.description_);  // NOLINT
+  equal &= (container_.size() == rhs.container_.size());
+  if (equal)
+  {
+    for (std::size_t i = 0; i < container_.size(); ++i)
+    {
+      equal &= (container_[i].isApprox(rhs.container_[i], std::numeric_limits<float>::epsilon()));
+
+      if (!equal)
+        break;
+    }
+  }
+  return equal;
+}
+
+bool ToolPathSegment::operator!=(const ToolPathSegment& rhs) const { return !operator==(rhs); }
+
+template <class Archive>
+void ToolPathSegment::serialize(Archive& ar, const unsigned int /*version*/)
+{
+  ar& BOOST_SERIALIZATION_NVP(uuid_);
+  ar& BOOST_SERIALIZATION_NVP(parent_uuid_);
+  ar& BOOST_SERIALIZATION_NVP(description_);
+  ar& BOOST_SERIALIZATION_NVP(container_);
+}
+
+///////////////////////////
+// C++ container support //
+///////////////////////////
+
+///////////////
+// Iterators //
+///////////////
+ToolPathSegment::iterator ToolPathSegment::begin() { return container_.begin(); }
+ToolPathSegment::const_iterator ToolPathSegment::begin() const { return container_.begin(); }
+ToolPathSegment::iterator ToolPathSegment::end() { return container_.end(); }
+ToolPathSegment::const_iterator ToolPathSegment::end() const { return container_.end(); }
+ToolPathSegment::reverse_iterator ToolPathSegment::rbegin() { return container_.rbegin(); }
+ToolPathSegment::const_reverse_iterator ToolPathSegment::rbegin() const { return container_.rbegin(); }
+ToolPathSegment::reverse_iterator ToolPathSegment::rend() { return container_.rend(); }
+ToolPathSegment::const_reverse_iterator ToolPathSegment::rend() const { return container_.rend(); }
+ToolPathSegment::const_iterator ToolPathSegment::cbegin() const { return container_.cbegin(); }
+ToolPathSegment::const_iterator ToolPathSegment::cend() const { return container_.cend(); }
+ToolPathSegment::const_reverse_iterator ToolPathSegment::crbegin() const { return container_.crbegin(); }
+ToolPathSegment::const_reverse_iterator ToolPathSegment::crend() const { return container_.crend(); }
+
+//////////////
+// Capacity //
+//////////////
+bool ToolPathSegment::empty() const { return container_.empty(); }
+ToolPathSegment::size_type ToolPathSegment::size() const { return container_.size(); }
+ToolPathSegment::size_type ToolPathSegment::max_size() const { return container_.max_size(); }
+void ToolPathSegment::reserve(size_type n) { container_.reserve(n); }
+ToolPathSegment::size_type ToolPathSegment::capacity() const { return container_.capacity(); }
+void ToolPathSegment::shrink_to_fit() { container_.shrink_to_fit(); }
+
+////////////////////
+// Element Access //
+////////////////////
+ToolPathSegment::reference ToolPathSegment::front() { return container_.front(); }
+ToolPathSegment::const_reference ToolPathSegment::front() const { return container_.front(); }
+ToolPathSegment::reference ToolPathSegment::back() { return container_.back(); }
+ToolPathSegment::const_reference ToolPathSegment::back() const { return container_.back(); }
+ToolPathSegment::reference ToolPathSegment::at(size_type n) { return container_.at(n); }
+ToolPathSegment::const_reference ToolPathSegment::at(size_type n) const { return container_.at(n); }
+ToolPathSegment::pointer ToolPathSegment::data() { return container_.data(); }
+ToolPathSegment::const_pointer ToolPathSegment::data() const { return container_.data(); }
+ToolPathSegment::reference ToolPathSegment::operator[](size_type pos) { return container_[pos]; }
+ToolPathSegment::const_reference ToolPathSegment::operator[](size_type pos) const { return container_[pos]; }
+
+///////////////
+// Modifiers //
+///////////////
+void ToolPathSegment::clear() { container_.clear(); }
+
+ToolPathSegment::iterator ToolPathSegment::insert(const_iterator p, const Eigen::Isometry3d& x)
+{
+  return container_.insert(p, x);
+}
+ToolPathSegment::iterator ToolPathSegment::insert(const_iterator p, Eigen::Isometry3d&& x)
+{
+  return container_.insert(p, x);
+}
+ToolPathSegment::iterator ToolPathSegment::insert(const_iterator p, std::initializer_list<Eigen::Isometry3d> l)
+{
+  return container_.insert(p, l);
+}
+
+template <class... Args>
+ToolPathSegment::iterator ToolPathSegment::emplace(const_iterator pos, Args&&... args)
+{
+  return container_.emplace(pos, std::forward<Args>(args)...);
+}
+
+ToolPathSegment::iterator ToolPathSegment::erase(const_iterator p) { return container_.erase(p); }
+ToolPathSegment::iterator ToolPathSegment::erase(const_iterator first, const_iterator last)
+{
+  return container_.erase(first, last);
+}
+void ToolPathSegment::push_back(const Eigen::Isometry3d& x) { container_.push_back(x); }
+void ToolPathSegment::push_back(const Eigen::Isometry3d&& x) { container_.push_back(x); }
+
+template <typename... Args>
+#if __cplusplus > 201402L
+ToolPathSegment::reference ToolPathSegment::emplace_back(Args&&... args)
+{
+  return container_.emplace_back(std::forward<Args>(args)...);
+}
+#else
+void ToolPathSegment::emplace_back(Args&&... args)
+{
+  container_.emplace_back(std::forward<Args>(args)...);
+}
+#endif
+
+void ToolPathSegment::pop_back() { container_.pop_back(); }
+void ToolPathSegment::swap(VectorIsometry3d& other) { container_.swap(other); }
+
+}  // namespace tesseract_common
+
+#include <tesseract_common/serialization.h>
+TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ToolPathSegment)
+BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ToolPathSegment)