Windows fixes with passing unit tests (#751)

* Fix bug in OFKTStateSolver::moveLinkHelper

* Use binary ifstream ond ofstream in serialization.h

* Add c++17 flag to windows_noetic_build.yml

* Fix SceneGraph move constructor, restore modified unit tests

.github/workflows/windows_noetic_build.yml

@@ -37,18 +37,26 @@ jobs:
 
         rosdep install -q --from-paths . --ignore-src -y
 
+        set CXXFLAGS=%CXXFLAGS% /std:c++17
+
         catkin_make_isolated --install --force-cmake --only-pkg-with-deps tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_state_solver tesseract_support tesseract_urdf tesseract_visualization --cmake-args -DCMAKE_BUILD_TYPE=Release -DINSTALL_OMPL=OFF -DINSTALL_FCL=ON -DINSTALL_BULLET=ON -DBUILD_IPOPT=OFF -DBUILD_SNOPT=OFF
+        if %errorlevel% neq 0 exit /b %errorlevel%
         call "D:\a\tesseract\tesseract\install_isolated\setup.bat"
-        catkin_make_isolated --install --force-cmake --pkg tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_state_solver tesseract_support tesseract_urdf tesseract_visualization --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DINSTALL_OMPL=OFF -DINSTALL_FCL=ON -DINSTALL_BULLET=ON -DBUILD_IPOPT=OFF -DBUILD_SNOPT=OFF
+        catkin_make_isolated --install --force-cmake --pkg tesseract_collision tesseract_common tesseract_environment tesseract_geometry tesseract_kinematics tesseract_scene_graph tesseract_state_solver tesseract_support tesseract_urdf tesseract_srdf tesseract_visualization --cmake-args -DCMAKE_BUILD_TYPE=Release -DTESSERACT_ENABLE_TESTING=ON -DINSTALL_OMPL=OFF -DINSTALL_FCL=ON -DINSTALL_BULLET=ON -DBUILD_IPOPT=OFF -DBUILD_SNOPT=OFF
+        if %errorlevel% neq 0 exit /b %errorlevel%
         
-        install_isolated\setup.bat
-        cmd /c "cd build_isolated\tesseract_collision\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_common\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_environment\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_geometry\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_kinematics\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_scene_graph\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_srdf\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_state_solver\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_urdf\install\test && ctest -C Release"
-        cmd /c "cd build_isolated\tesseract_visualization\install\test && ctest -C Release"
+        set test_error=0
+
+        call install_isolated\setup.bat
+        cmd /c "cd build_isolated\tesseract_common\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_collision\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_environment\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_geometry\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_kinematics\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_scene_graph\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_srdf\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_state_solver\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_urdf\install\test && ctest -C Release --output-on-failure" || set test_error=1
+        cmd /c "cd build_isolated\tesseract_visualization\install\test && ctest -C Release --output-on-failure" || set test_error=1
+
+        exit /b %test_error%

tesseract_common/include/tesseract_common/serialization.h

@@ -131,7 +131,7 @@ struct Serialization
                                   const std::string& file_path,
                                   const std::string& name = "")
   {
-    std::ofstream os(file_path);
+    std::ofstream os(file_path, std::ios_base::binary);
     {  // Must be scoped because all data is not written until the oost::archive::xml_oarchive goes out of scope
       boost::archive::binary_oarchive oa(os);
       // Boost uses the same function for serialization and deserialization so it requires a non-const reference
@@ -182,7 +182,7 @@ struct Serialization
     SerializableType archive_type;
 
     {  // Must be scoped because all data is not written until the oost::archive::xml_oarchive goes out of scope
-      std::ifstream ifs(file_path);
+      std::ifstream ifs(file_path, std::ios_base::binary);
       assert(ifs.good());
       boost::archive::binary_iarchive ia(ifs);
       ia >> BOOST_SERIALIZATION_NVP(archive_type);

tesseract_common/test/resource_locator_unit.cpp

@@ -60,15 +60,15 @@ TEST(ResourceLocatorUnit, SimpleResourceLocatorUnit)  // NOLINT
   EXPECT_TRUE(resource != nullptr);
   EXPECT_TRUE(resource->isFile());
   EXPECT_EQ(resource->getUrl(), "package://tesseract_common/package.xml");
-  EXPECT_EQ(resource->getFilePath(), (package_path / "package.xml").string());
+  EXPECT_EQ(tesseract_common::fs::path(resource->getFilePath()), (package_path / "package.xml"));
   EXPECT_FALSE(resource->getResourceContents().empty());
   EXPECT_TRUE(resource->getResourceContentStream() != nullptr);
 
   Resource::Ptr sub_resource = resource->locateResource("colcon.pkg");
   EXPECT_TRUE(sub_resource != nullptr);
   EXPECT_TRUE(sub_resource->isFile());
   EXPECT_EQ(sub_resource->getUrl(), "package://tesseract_common/colcon.pkg");
-  EXPECT_EQ(sub_resource->getFilePath(), (package_path / "colcon.pkg").string());
+  EXPECT_EQ(tesseract_common::fs::path(sub_resource->getFilePath()), (package_path / "colcon.pkg"));
   EXPECT_FALSE(sub_resource->getResourceContents().empty());
   EXPECT_TRUE(sub_resource->getResourceContentStream() != nullptr);
 
@@ -107,7 +107,7 @@ TEST(ResourceLocatorUnit, ByteResourceUnit)  // NOLINT
   EXPECT_TRUE(sub_resource != nullptr);
   EXPECT_TRUE(sub_resource->isFile());
   EXPECT_EQ(sub_resource->getUrl(), "package://tesseract_common/colcon.pkg");
-  EXPECT_EQ(sub_resource->getFilePath(), (package_path / "colcon.pkg").string());
+  EXPECT_EQ(tesseract_common::fs::path(sub_resource->getFilePath()), (package_path / "colcon.pkg"));
   EXPECT_FALSE(sub_resource->getResourceContents().empty());
   EXPECT_TRUE(sub_resource->getResourceContentStream() != nullptr);
 

tesseract_common/test/tesseract_common_serialization_unit.cpp

@@ -207,14 +207,14 @@ TEST(TesseractCommonSerializeUnit, VectorXd)  // NOLINT
   {  // Serialize empty object
     Eigen::VectorXd ev;
     {
-      std::ofstream os("/tmp/eigen_vector_xd_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_vector_xd_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(ev);
     }
 
     Eigen::VectorXd nev;
     {
-      std::ifstream ifs("/tmp/eigen_vector_xd_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_vector_xd_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 
@@ -229,14 +229,14 @@ TEST(TesseractCommonSerializeUnit, VectorXd)  // NOLINT
     Eigen::VectorXd ev = Eigen::VectorXd::Random(6);
 
     {
-      std::ofstream os("/tmp/eigen_vector_xd_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_vector_xd_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(ev);
     }
 
     Eigen::VectorXd nev = Eigen::VectorXd::Random(6);
     {
-      std::ifstream ifs("/tmp/eigen_vector_xd_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_vector_xd_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 
@@ -253,14 +253,14 @@ TEST(TesseractCommonSerializeUnit, VectorXd)  // NOLINT
     Eigen::VectorXd ev = Eigen::VectorXd::Random(6);
 
     {
-      std::ofstream os("/tmp/eigen_vector_xd_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_vector_xd_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(ev);
     }
 
     Eigen::VectorXd nev = Eigen::VectorXd::Random(3);
     {
-      std::ifstream ifs("/tmp/eigen_vector_xd_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_vector_xd_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 
@@ -277,14 +277,14 @@ TEST(TesseractCommonSerializeUnit, VectorXd)  // NOLINT
     Eigen::VectorXd ev = Eigen::VectorXd::Random(6);
 
     {
-      std::ofstream os("/tmp/eigen_vector_xd_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_vector_xd_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(ev);
     }
 
     Eigen::VectorXd nev;
     {
-      std::ifstream ifs("/tmp/eigen_vector_xd_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_vector_xd_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 
@@ -302,14 +302,14 @@ TEST(TesseractCommonSerializeUnit, MatrixX2d)  // NOLINT
     Eigen::MatrixX2d em;
 
     {
-      std::ofstream os("/tmp/eigen_matrix_x2d_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_matrix_x2d_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(em);
     }
 
     Eigen::MatrixX2d nem;
     {
-      std::ifstream ifs("/tmp/eigen_matrix_x2d_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_matrix_x2d_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 
@@ -326,14 +326,14 @@ TEST(TesseractCommonSerializeUnit, MatrixX2d)  // NOLINT
     Eigen::MatrixX2d em = Eigen::MatrixX2d::Random(4, 2);
 
     {
-      std::ofstream os("/tmp/eigen_matrix_x2d_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_matrix_x2d_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(em);
     }
 
     Eigen::MatrixX2d nem = Eigen::MatrixX2d::Random(4, 2);
     {
-      std::ifstream ifs("/tmp/eigen_matrix_x2d_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_matrix_x2d_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 
@@ -350,14 +350,14 @@ TEST(TesseractCommonSerializeUnit, MatrixX2d)  // NOLINT
     Eigen::MatrixX2d em = Eigen::MatrixX2d::Random(4, 2);
 
     {
-      std::ofstream os("/tmp/eigen_matrix_x2d_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_matrix_x2d_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(em);
     }
 
     Eigen::MatrixX2d nem = Eigen::MatrixX2d::Random(2, 2);
     {
-      std::ifstream ifs("/tmp/eigen_matrix_x2d_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_matrix_x2d_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 
@@ -374,14 +374,14 @@ TEST(TesseractCommonSerializeUnit, MatrixX2d)  // NOLINT
     Eigen::MatrixX2d em = Eigen::MatrixX2d::Random(4, 2);
 
     {
-      std::ofstream os("/tmp/eigen_matrix_x2d_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_matrix_x2d_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(em);
     }
 
     Eigen::MatrixX2d nem;
     {
-      std::ifstream ifs("/tmp/eigen_matrix_x2d_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_matrix_x2d_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 
@@ -402,14 +402,14 @@ TEST(TesseractCommonSerializeUnit, Isometry3d)  // NOLINT
     pose.translation() = Eigen::Vector3d::Random();
 
     {
-      std::ofstream os("/tmp/eigen_isometry3d_boost.xml");
+      std::ofstream os(tesseract_common::getTempPath() + "eigen_isometry3d_boost.xml");
       boost::archive::xml_oarchive oa(os);
       oa << BOOST_SERIALIZATION_NVP(pose);
     }
 
     Eigen::Isometry3d npose;
     {
-      std::ifstream ifs("/tmp/eigen_isometry3d_boost.xml");
+      std::ifstream ifs(tesseract_common::getTempPath() + "eigen_isometry3d_boost.xml");
       assert(ifs.good());
       boost::archive::xml_iarchive ia(ifs);
 

tesseract_common/test/tesseract_common_unit.cpp

@@ -341,14 +341,14 @@ TEST(TesseractCommonUnit, anyUnit)  // NOLINT
   EXPECT_TRUE(&any_type_const_ref1 == &any_type_const_ref2);
 
   {
-    std::ofstream os("/tmp/any_type_boost.xml");
+    std::ofstream os(tesseract_common::getTempPath() + "any_type_boost.xml");
     boost::archive::xml_oarchive oa(os);
     oa << BOOST_SERIALIZATION_NVP(any_type);
   }
 
   tesseract_common::Any nany_type;
   {
-    std::ifstream ifs("/tmp/any_type_boost.xml");
+    std::ifstream ifs(tesseract_common::getTempPath() + "any_type_boost.xml");
     assert(ifs.good());
     boost::archive::xml_iarchive ia(ifs);
 
@@ -471,7 +471,7 @@ TEST(TesseractCommonUnit, isIdenticalArrayUnit)  // NOLINT
   }
   {
     // Clang-tidy catches unitialized arrays anyway, but check it just in case the caller isn't running clang-tidy
-    std::array<int, 4> v1 = { 1, 2, 3, 4 };
+    std::array<int, 4> v1 = { 1, 2, 3, 6 };
     std::array<int, 4> v2;  // NOLINT
     bool equal = tesseract_common::isIdenticalArray<int, 4>(v1, v2);
     EXPECT_FALSE(equal);

tesseract_scene_graph/include/tesseract_scene_graph/graph.h

@@ -130,7 +130,8 @@ struct ShortestPath
 
 class SceneGraph
 #ifndef SWIG
-  : public Graph
+  : public Graph,
+    public boost::noncopyable
 #endif  // SWIG
 {
 public:
@@ -148,8 +149,8 @@ class SceneGraph
   SceneGraph(const SceneGraph& other) = delete;
   SceneGraph& operator=(const SceneGraph& other) = delete;
 
-  SceneGraph(SceneGraph&& other) = default;
-  SceneGraph& operator=(SceneGraph&& other) = default;
+  SceneGraph(SceneGraph&& other) noexcept;
+  SceneGraph& operator=(SceneGraph&& other) noexcept;
 
   /**
    * @brief Clone the scene graph

tesseract_scene_graph/src/graph.cpp

@@ -71,6 +71,63 @@ SceneGraph::SceneGraph(const std::string& name) : acm_(std::make_shared<tesserac
   boost::set_property(static_cast<Graph&>(*this), boost::graph_name, name);
 }
 
+SceneGraph::SceneGraph(SceneGraph&& other) noexcept
+  : Graph(std::forward<Graph>(other))
+  , link_map_(std::move(other.link_map_))
+  , joint_map_(std::move(other.joint_map_))
+  , acm_(std::move(other.acm_))
+{
+  {
+    link_map_.clear();
+    joint_map_.clear();
+    Graph::vertex_iterator i, iend;
+    for (boost::tie(i, iend) = boost::vertices(*this); i != iend; ++i)
+    {
+      Link::Ptr link = boost::get(boost::vertex_link, *this)[*i];
+      link_map_[link->getName()] = std::make_pair(link, *i);
+    }
+  }
+  {
+    Graph::edge_iterator i, iend;
+    for (boost::tie(i, iend) = boost::edges(*this); i != iend; ++i)
+    {
+      Joint::Ptr joint = boost::get(boost::edge_joint, *this)[*i];
+      joint_map_[joint->getName()] = std::make_pair(joint, *i);
+    }
+  }
+}
+
+SceneGraph& SceneGraph::operator=(SceneGraph&& other) noexcept
+{
+  {
+    Graph::operator=(std::forward<Graph>(other));
+
+    link_map_ = std::move(other.link_map_);
+    joint_map_ = std::move(other.joint_map_);
+    acm_ = std::move(other.acm_);
+
+    // rebuild link_map_ and joint_map_
+    link_map_.clear();
+    joint_map_.clear();
+    Graph::vertex_iterator i, iend;
+    for (boost::tie(i, iend) = boost::vertices(*this); i != iend; ++i)
+    {
+      Link::Ptr link = boost::get(boost::vertex_link, *this)[*i];
+      link_map_[link->getName()] = std::make_pair(link, *i);
+    }
+  }
+  {
+    Graph::edge_iterator i, iend;
+    for (boost::tie(i, iend) = boost::edges(*this); i != iend; ++i)
+    {
+      Joint::Ptr joint = boost::get(boost::edge_joint, *this)[*i];
+      joint_map_[joint->getName()] = std::make_pair(joint, *i);
+    }
+  }
+
+  return *this;
+}
+
 SceneGraph::UPtr SceneGraph::clone() const
 {
   auto cloned_graph = std::make_unique<SceneGraph>();
@@ -403,7 +460,10 @@ bool SceneGraph::removeJoint(const std::string& name, bool recursive)
   else
   {
     if (getInboundJoints(found->second.first->child_link_name).size() == 1)
-      removeLink(found->second.first->child_link_name, true);
+    {
+      std::string child_link_name = found->second.first->child_link_name;
+      removeLink(child_link_name, true);
+    }
   }
 
   return true;

tesseract_srdf/test/tesseract_srdf_unit.cpp

@@ -509,14 +509,31 @@ TEST(TesseractSRDFUnit, LoadSRDFFailureCasesUnit)  // NOLINT
     EXPECT_ANY_THROW(srdf.initFile(*g, "/tmp/file_does_not_exist.srdf", locator));  // NOLINT
   }
 }
+
+class TempResourceLocator : public tesseract_common::ResourceLocator
+{
+public:
+  std::shared_ptr<tesseract_common::Resource> locateResource(const std::string& url) const override final
+  {
+    tesseract_common::fs::path mod_url(url);
+    if (!mod_url.is_absolute())
+    {
+      mod_url = tesseract_common::fs::path(tesseract_common::getTempPath() + url);
+    }
+
+    return std::make_shared<tesseract_common::SimpleLocatedResource>(
+        url, mod_url.string(), std::make_shared<TempResourceLocator>(*this));
+  }
+};
+
 TEST(TesseractSRDFUnit, LoadSRDFSaveUnit)  // NOLINT
 {
   using namespace tesseract_scene_graph;
   using namespace tesseract_srdf;
   using namespace tesseract_common;
 
   SceneGraph::Ptr g = getABBSceneGraph(ABBConfig::ROBOT_ON_RAIL);
-  TesseractSupportResourceLocator locator;
+  TempResourceLocator locator;
 
   std::string xml_string =
       R"(<robot name="abb_irb2400" version="1.0.0">