From ee5325fa0cdff937d523c6a9b90f69e4cf9adabe Mon Sep 17 00:00:00 2001 From: Levi Armstrong Date: Sat, 19 Nov 2022 17:57:51 -0500 Subject: [PATCH] Add ToolPath and ToolPathSegment class --- .github/workflows/windows_2019.yml | 2 +- tesseract_common/CMakeLists.txt | 2 + .../include/tesseract_common/tool_path.h | 254 ++++++++++++++++++ .../tesseract_common/tool_path_segment.h | 238 ++++++++++++++++ tesseract_common/src/tool_path.cpp | 175 ++++++++++++ tesseract_common/src/tool_path_segment.cpp | 193 +++++++++++++ .../tesseract_common_serialization_unit.cpp | 34 +++ .../test/tesseract_common_unit.cpp | 115 ++++++++ 8 files changed, 1012 insertions(+), 1 deletion(-) create mode 100644 tesseract_common/include/tesseract_common/tool_path.h create mode 100644 tesseract_common/include/tesseract_common/tool_path_segment.h create mode 100644 tesseract_common/src/tool_path.cpp create mode 100644 tesseract_common/src/tool_path_segment.cpp diff --git a/.github/workflows/windows_2019.yml b/.github/workflows/windows_2019.yml index 4171eee9dfd..4de24ae31fd 100644 --- a/.github/workflows/windows_2019.yml +++ b/.github/workflows/windows_2019.yml @@ -49,7 +49,7 @@ jobs: set CMAKE_PREFIX_PATH=%GITHUB_WORKSPACE%\vcpkg\installed\x64-windows-release vcs import --input "${{ github.workspace }}/workspace/src/tesseract/dependencies.rosinstall" src/ if %ERRORLEVEL% GEQ 1 exit 1 - colcon build --event-handlers console_direct+ --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=x64-windows-release -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON + colcon build --event-handlers console_direct+ --cmake-args -G "Ninja" -DVCPKG_TARGET_TRIPLET=x64-windows-release -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DBOOST_UUID_FORCE_AUTO_LINK=ON if %ERRORLEVEL% GEQ 1 exit 1 - name: run-test diff --git a/tesseract_common/CMakeLists.txt b/tesseract_common/CMakeLists.txt index 3f20ca3717e..2528176839a 100644 --- a/tesseract_common/CMakeLists.txt +++ b/tesseract_common/CMakeLists.txt @@ -53,6 +53,8 @@ add_library( src/eigen_serialization.cpp src/utils.cpp src/resource_locator.cpp + src/tool_path.cpp + src/tool_path_segment.cpp src/types.cpp) target_link_libraries( ${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..c2e8073f9eb --- /dev/null +++ b/tesseract_common/include/tesseract_common/tool_path.h @@ -0,0 +1,254 @@ +/** + * @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 +#include +#include +#include + +namespace tesseract_common +{ +class ToolPath +{ +public: + // LCOV_EXCL_START + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // LCOV_EXCL_STOP + + ToolPath(std::string description = ""); + ToolPath(boost::uuids::uuid uuid, 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; + + // LCOV_EXCL_START + + /////////////////////////// + // C++ container support // + /////////////////////////// + /** pointer */ + using pointer = typename AlignedVector::pointer; + /** const_pointer */ + using const_pointer = typename AlignedVector::const_pointer; + /** reference */ + using reference = typename AlignedVector::reference; + /** const_reference */ + using const_reference = typename AlignedVector::const_reference; + /** size_type */ + using size_type = typename AlignedVector::size_type; + /** difference_type */ + using difference_type = typename AlignedVector::difference_type; + /** iterator */ + using iterator = typename AlignedVector::iterator; + /** const_iterator */ + using const_iterator = typename AlignedVector::const_iterator; + /** reverse_iterator */ + using reverse_iterator = typename AlignedVector::reverse_iterator; + /** const_reverse_iterator */ + using const_reverse_iterator = typename AlignedVector::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 l); + template + void insert(const_iterator pos, InputIt first, InputIt last) + { + container_.insert(pos, first, last); + } + + /** @brief constructs element in-place */ + template + 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 +#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& other); + // LCOV_EXCL_STOP + +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 container_; + + /** @brief The namespace associated with the tool path */ + std::string ns_{ "general" }; + + friend struct tesseract_common::Serialization; + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +} // namespace tesseract_common + +#include +#include +BOOST_CLASS_EXPORT_KEY2(tesseract_common::ToolPath, "ToolPath") + +#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..0effb24bd41 --- /dev/null +++ b/tesseract_common/include/tesseract_common/tool_path_segment.h @@ -0,0 +1,238 @@ +/** + * @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 +#include +#include +#include + +namespace tesseract_common +{ +class ToolPathSegment +{ +public: + // LCOV_EXCL_START + EIGEN_MAKE_ALIGNED_OPERATOR_NEW + // LCOV_EXCL_STOP + + ToolPathSegment(std::string description = ""); + ToolPathSegment(boost::uuids::uuid uuid, 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; + + // LCOV_EXCL_START + + /////////////////////////// + // 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 l); + template + void insert(const_iterator pos, InputIt first, InputIt last) + { + container_.insert(pos, first, last); + } + + /** @brief constructs element in-place */ + template + 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 +#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); + + // LCOV_EXCL_STOP + +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 struct tesseract_common::Serialization; + friend class boost::serialization::access; + template + void serialize(Archive& ar, const unsigned int version); // NOLINT +}; + +} // namespace tesseract_common + +#include +#include +BOOST_CLASS_EXPORT_KEY2(tesseract_common::ToolPathSegment, "ToolPathSegment") + +#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..26ab24b1ab6 --- /dev/null +++ b/tesseract_common/src/tool_path.cpp @@ -0,0 +1,175 @@ +/** + * @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_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include + +namespace tesseract_common +{ +ToolPath::ToolPath(std::string description) + : uuid_(boost::uuids::random_generator()()), description_(std::move(description)) +{ +} + +ToolPath::ToolPath(boost::uuids::uuid uuid, std::string description) : uuid_(uuid), 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 +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_); +} + +// LCOV_EXCL_START + +/////////////////////////// +// 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 l) +{ + return container_.insert(p, l); +} + +template +ToolPath::iterator ToolPath::emplace(const_iterator pos, Args&&... args) +{ + return container_.emplace(pos, std::forward(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 +#if __cplusplus > 201402L +ToolPath::reference ToolPath::emplace_back(Args&&... args) +{ + return container_.emplace_back(std::forward(args)...); +} +#else +void ToolPath::emplace_back(Args&&... args) +{ + container_.emplace_back(std::forward(args)...); +} +#endif + +void ToolPath::pop_back() { container_.pop_back(); } +void ToolPath::swap(AlignedVector& other) { container_.swap(other); } +// LCOV_EXCL_STOP +} // namespace tesseract_common + +#include +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..1db392bf6c9 --- /dev/null +++ b/tesseract_common/src/tool_path_segment.cpp @@ -0,0 +1,193 @@ +/** + * @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_IGNORE_WARNINGS_PUSH +#include +#include +#include +#include +#include +#include +TESSERACT_COMMON_IGNORE_WARNINGS_POP + +#include +#include + +namespace tesseract_common +{ +ToolPathSegment::ToolPathSegment(std::string description) + : uuid_(boost::uuids::random_generator()()), description_(std::move(description)) +{ +} + +ToolPathSegment::ToolPathSegment(boost::uuids::uuid uuid, std::string description) + : uuid_(uuid), 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::epsilon())); + + if (!equal) + break; + } + } + return equal; +} + +bool ToolPathSegment::operator!=(const ToolPathSegment& rhs) const { return !operator==(rhs); } + +template +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_); +} + +// LCOV_EXCL_START + +/////////////////////////// +// 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 l) +{ + return container_.insert(p, l); +} + +template +ToolPathSegment::iterator ToolPathSegment::emplace(const_iterator pos, Args&&... args) +{ + return container_.emplace(pos, std::forward(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 +#if __cplusplus > 201402L +ToolPathSegment::reference ToolPathSegment::emplace_back(Args&&... args) +{ + return container_.emplace_back(std::forward(args)...); +} +#else +void ToolPathSegment::emplace_back(Args&&... args) +{ + container_.emplace_back(std::forward(args)...); +} +#endif + +void ToolPathSegment::pop_back() { container_.pop_back(); } +void ToolPathSegment::swap(VectorIsometry3d& other) { container_.swap(other); } + +// LCOV_EXCL_STOP +} // namespace tesseract_common + +#include +TESSERACT_SERIALIZE_ARCHIVES_INSTANTIATE(tesseract_common::ToolPathSegment) +BOOST_CLASS_EXPORT_IMPLEMENT(tesseract_common::ToolPathSegment) diff --git a/tesseract_common/test/tesseract_common_serialization_unit.cpp b/tesseract_common/test/tesseract_common_serialization_unit.cpp index 5737f187a12..fed6bda2487 100644 --- a/tesseract_common/test/tesseract_common_serialization_unit.cpp +++ b/tesseract_common/test/tesseract_common_serialization_unit.cpp @@ -27,6 +27,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -39,6 +41,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include using namespace tesseract_common; @@ -526,6 +530,36 @@ TEST(TesseractCommonSerializeUnit, ExtensionBinaryMacro) // NOLINT EXPECT_EQ(default_ext, "trsb"); } +TEST(TesseractCommonSerializeUnit, ToolPath) // NOLINT +{ + tesseract_common::ToolPath tool_path("Description"); + tool_path.setNamespace("Namespace"); + tesseract_common::ToolPathSegment segment; + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + + tool_path.push_back(segment); + tool_path.push_back(segment); + tool_path.push_back(segment); + + tesseract_common::testSerialization(tool_path, "ToolPath"); +} + +TEST(TesseractCommonSerializeUnit, ToolPathSegment) // NOLINT +{ + tesseract_common::ToolPathSegment segment; + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + + tesseract_common::testSerialization(segment, "ToolPathSegment"); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/tesseract_common/test/tesseract_common_unit.cpp b/tesseract_common/test/tesseract_common_unit.cpp index 40f4a0b7088..922f84a14b0 100644 --- a/tesseract_common/test/tesseract_common_unit.cpp +++ b/tesseract_common/test/tesseract_common_unit.cpp @@ -4,6 +4,7 @@ TESSERACT_COMMON_IGNORE_WARNINGS_PUSH #include #include #include +#include TESSERACT_COMMON_IGNORE_WARNINGS_POP #include @@ -15,6 +16,8 @@ TESSERACT_COMMON_IGNORE_WARNINGS_POP #include #include #include +#include +#include TEST(TesseractCommonUnit, isNumeric) // NOLINT { @@ -2110,6 +2113,118 @@ TEST(TesseractCommonUnit, concat) // NOLINT EXPECT_TRUE(c.tail(3).isApprox(b)); } +/** @brief Tests ToolPath */ +TEST(TesseractCommonUnit, ToolPath) // NOLINT +{ + { // Default + tesseract_common::ToolPath tool_path; + EXPECT_FALSE(tool_path.getUUID().is_nil()); + EXPECT_TRUE(tool_path.getParentUUID().is_nil()); + EXPECT_TRUE(tool_path.getDescription().empty()); + EXPECT_EQ(tool_path.getNamespace(), "general"); + + boost::uuids::uuid uuid = tool_path.getUUID(); + tool_path.regenerateUUID(); + EXPECT_NE(uuid, tool_path.getUUID()); + + tool_path.setParentUUID(uuid); + EXPECT_EQ(uuid, tool_path.getParentUUID()); + + tool_path.setDescription("Description"); + EXPECT_EQ(tool_path.getDescription(), "Description"); + + tool_path.setNamespace("Namespace"); + EXPECT_EQ(tool_path.getNamespace(), "Namespace"); + } + + { // Constructor 1 + tesseract_common::ToolPath tool_path("Tool path description"); + EXPECT_FALSE(tool_path.getUUID().is_nil()); + EXPECT_TRUE(tool_path.getParentUUID().is_nil()); + EXPECT_EQ(tool_path.getDescription(), "Tool path description"); + } + + { // Constructor 2 + auto uuid = boost::uuids::random_generator()(); + tesseract_common::ToolPath tool_path(uuid, "Tool path description"); + EXPECT_EQ(uuid, tool_path.getUUID()); + EXPECT_TRUE(tool_path.getParentUUID().is_nil()); + EXPECT_EQ(tool_path.getDescription(), "Tool path description"); + } + + { // Equal + tesseract_common::ToolPath tool_path("Description"); + tool_path.setNamespace("Namespace"); + tesseract_common::ToolPathSegment segment; + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + EXPECT_EQ(segment.size(), 5); + + tool_path.push_back(segment); + tool_path.push_back(segment); + tool_path.push_back(segment); + EXPECT_EQ(tool_path.size(), 3); + + tesseract_common::ToolPath tool_path_copy{ tool_path }; + EXPECT_EQ(tool_path, tool_path_copy); + } +} + +/** @brief Tests ToolPath */ +TEST(TesseractCommonUnit, ToolPathSegment) // NOLINT +{ + { // Default + tesseract_common::ToolPathSegment segment; + EXPECT_FALSE(segment.getUUID().is_nil()); + EXPECT_TRUE(segment.getParentUUID().is_nil()); + EXPECT_TRUE(segment.getDescription().empty()); + + boost::uuids::uuid uuid = segment.getUUID(); + segment.regenerateUUID(); + EXPECT_NE(uuid, segment.getUUID()); + + segment.setParentUUID(uuid); + EXPECT_EQ(uuid, segment.getParentUUID()); + + segment.setDescription("Description"); + EXPECT_EQ(segment.getDescription(), "Description"); + } + + { // Constructor 1 + tesseract_common::ToolPathSegment segment("Description"); + EXPECT_FALSE(segment.getUUID().is_nil()); + EXPECT_TRUE(segment.getParentUUID().is_nil()); + EXPECT_EQ(segment.getDescription(), "Description"); + } + + { // Constructor 2 + auto uuid = boost::uuids::random_generator()(); + tesseract_common::ToolPathSegment segment(uuid, "Description"); + EXPECT_EQ(uuid, segment.getUUID()); + EXPECT_TRUE(segment.getParentUUID().is_nil()); + EXPECT_EQ(segment.getDescription(), "Description"); + } + + { // Equal + tesseract_common::ToolPathSegment segment("Description"); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + segment.push_back(Eigen::Isometry3d::Identity()); + EXPECT_EQ(segment.size(), 5); + + tesseract_common::ToolPathSegment segment_copy{ segment }; + EXPECT_EQ(segment, segment_copy); + + segment_copy.front().translation() = Eigen::Vector3d(1, 2, 3); + EXPECT_NE(segment, segment_copy); + } +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv);