diff --git a/rmf_traffic/CMakeLists.txt b/rmf_traffic/CMakeLists.txt
index d5516187..f06361f0 100644
--- a/rmf_traffic/CMakeLists.txt
+++ b/rmf_traffic/CMakeLists.txt
@@ -22,17 +22,15 @@ include(GNUInstallDirs)
find_package(fcl 0.6 QUIET)
if(fcl_FOUND)
- set(FCL_LIBRARIES fcl)
- set(using_new_fcl true)
- message(STATUS "Using FCL version: ${FCL_VERSION}")
+ message(STATUS "Using installed FCL version: ${FCL_VERSION}")
else()
- include(FindPkgConfig)
- pkg_check_modules(PC_FCL REQUIRED fcl)
- pkg_check_modules(PC_CCD REQUIRED ccd)
- set(FCL_LIBRARIES ${PC_FCL_LIBRARIES} ${PC_CCD_LIBRARIES})
- set(using_new_fcl)
- message(STATUS "Using FCL version: ${PC_FCL_VERSION}")
+ message(STATUS "Using in-source FCL")
+ add_subdirectory(thirdparty/fcl)
endif()
+# TODO(MXG): Remove everything related to maintaining compatibility with 0.5,
+# since we are now requiring FCL-0.6+
+set(using_new_fcl true)
+set(FCL_LIBRARIES fcl)
find_package(rmf_utils REQUIRED)
find_package(Eigen3 REQUIRED)
diff --git a/rmf_traffic/package.xml b/rmf_traffic/package.xml
index 569cbd4f..1322ae02 100644
--- a/rmf_traffic/package.xml
+++ b/rmf_traffic/package.xml
@@ -12,10 +12,12 @@
rmf_utils
rmf_utils
- libccd-dev
- libfcl-dev
-
-
+ libccd-dev
+
+
ament_cmake_catch2
rmf_cmake_uncrustify
diff --git a/rmf_traffic/thirdparty/fcl/.clang-format b/rmf_traffic/thirdparty/fcl/.clang-format
new file mode 100644
index 00000000..5e53779d
--- /dev/null
+++ b/rmf_traffic/thirdparty/fcl/.clang-format
@@ -0,0 +1,40 @@
+# -*- mode: yaml -*-
+# vi: set ft=yaml :
+
+# Copyright (c) 2018, Toyota Research Institute, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# * Neither the name of the copyright holder nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Author: Jamie Snape, Kitware, Inc.
+
+---
+BasedOnStyle: Google
+---
+Language: Cpp
+DerivePointerAlignment: false
+PointerAlignment: Left
diff --git a/rmf_traffic/thirdparty/fcl/.clang-tidy b/rmf_traffic/thirdparty/fcl/.clang-tidy
new file mode 100644
index 00000000..1fe616b0
--- /dev/null
+++ b/rmf_traffic/thirdparty/fcl/.clang-tidy
@@ -0,0 +1,36 @@
+# -*- mode: yaml -*-
+# vi: set ft=yaml :
+
+# Copyright (c) 2018, Toyota Research Institute, Inc.
+# All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice, this
+# list of conditions and the following disclaimer.
+#
+# * Redistributions in binary form must reproduce the above copyright notice,
+# this list of conditions and the following disclaimer in the documentation
+# and/or other materials provided with the distribution.
+#
+# * Neither the name of the copyright holder nor the names of its
+# contributors may be used to endorse or promote products derived from
+# this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+# Author: Jamie Snape, Kitware, Inc.
+
+---
+Checks: 'clang-analyzer-*,clang-diagnostic-*,cppcoreguidelines-*,google-*,modernize-*,performance-*,readability-*'
diff --git a/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/joint.h b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/joint.h
new file mode 100644
index 00000000..3d92d2a1
--- /dev/null
+++ b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/joint.h
@@ -0,0 +1,375 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2016, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_JOINT_H
+#define FCL_ARTICULATED_MODEL_JOINT_H
+
+#include "fcl/common/data_types.h"
+
+#include
+#include
+#include
+#include
+#include
+
+namespace fcl
+{
+
+class JointConfig;
+class Link;
+
+enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER};
+
+/// @brief Base Joint
+template
+class Joint
+{
+public:
+
+ Joint(const std::shared_ptr & link_parent, const std::shared_ptr & link_child,
+ const Transform3& transform_to_parent,
+ const std::string& name);
+
+ Joint(const std::string& name);
+
+ virtual ~Joint() {}
+
+ const std::string& getName() const;
+ void setName(const std::string& name);
+
+ virtual Transform3 getLocalTransform() const = 0;
+
+ virtual std::size_t getNumDofs() const = 0;
+
+ std::shared_ptr getJointConfig() const;
+ void setJointConfig(const std::shared_ptr& joint_cfg);
+
+ std::shared_ptr getParentLink() const;
+ std::shared_ptr getChildLink() const;
+
+ void setParentLink(const std::shared_ptr & link);
+ void setChildLink(const std::shared_ptr & link);
+
+ JointType getJointType() const;
+
+ const Transform3& getTransformToParent() const;
+ void setTransformToParent(const Transform3& t);
+
+protected:
+
+ /// links to parent and child are only for connection, so weak_ptr to avoid cyclic dependency
+ std::weak_ptr link_parent_, link_child_;
+
+ JointType type_;
+
+ std::string name_;
+
+ std::shared_ptr joint_cfg_;
+
+ Transform3 transform_to_parent_;
+
+public:
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+};
+
+template
+class PrismaticJoint : public Joint
+{
+public:
+ PrismaticJoint(const std::shared_ptr & link_parent, const std::shared_ptr & link_child,
+ const Transform3& transform_to_parent,
+ const std::string& name,
+ const Vector3& axis);
+
+ virtual ~PrismaticJoint() {}
+
+ Transform3 getLocalTransform() const;
+
+ std::size_t getNumDofs() const;
+
+ const Vector3& getAxis() const;
+
+protected:
+ Vector3 axis_;
+};
+
+template
+class RevoluteJoint : public Joint
+{
+public:
+ RevoluteJoint(const std::shared_ptr & link_parent, const std::shared_ptr & link_child,
+ const Transform3& transform_to_parent,
+ const std::string& name,
+ const Vector3& axis);
+
+ virtual ~RevoluteJoint() {}
+
+ Transform3 getLocalTransform() const;
+
+ std::size_t getNumDofs() const;
+
+ const Vector3& getAxis() const;
+
+protected:
+ Vector3 axis_;
+};
+
+template
+class BallEulerJoint : public Joint
+{
+public:
+ BallEulerJoint(const std::shared_ptr & link_parent, const std::shared_ptr & link_child,
+ const Transform3& transform_to_parent,
+ const std::string& name);
+
+ virtual ~BallEulerJoint() {}
+
+ std::size_t getNumDofs() const;
+
+ Transform3 getLocalTransform() const;
+};
+
+//============================================================================//
+// //
+// Implementations //
+// //
+//============================================================================//
+
+//==============================================================================
+template
+Joint::Joint(const std::shared_ptr & link_parent, const std::shared_ptr & link_child,
+ const Transform3& transform_to_parent,
+ const std::string& name) :
+ link_parent_(link_parent), link_child_(link_child),
+ name_(name),
+ transform_to_parent_(transform_to_parent)
+{}
+
+//==============================================================================
+template
+Joint::Joint(const std::string& name) :
+ name_(name)
+{
+}
+
+//==============================================================================
+template
+const std::string& Joint::getName() const
+{
+ return name_;
+}
+
+//==============================================================================
+template
+void Joint::setName(const std::string& name)
+{
+ name_ = name;
+}
+
+//==============================================================================
+template
+std::shared_ptr Joint::getJointConfig() const
+{
+ return joint_cfg_;
+}
+
+//==============================================================================
+template
+void Joint::setJointConfig(const std::shared_ptr& joint_cfg)
+{
+ joint_cfg_ = joint_cfg;
+}
+
+//==============================================================================
+template
+std::shared_ptr Joint::getParentLink() const
+{
+ return link_parent_.lock();
+}
+
+//==============================================================================
+template
+std::shared_ptr Joint::getChildLink() const
+{
+ return link_child_.lock();
+}
+
+//==============================================================================
+template
+void Joint::setParentLink(const std::shared_ptr & link)
+{
+ link_parent_ = link;
+}
+
+//==============================================================================
+template
+void Joint::setChildLink(const std::shared_ptr & link)
+{
+ link_child_ = link;
+}
+
+//==============================================================================
+template
+JointType Joint::getJointType() const
+{
+ return type_;
+}
+
+//==============================================================================
+template
+const Transform3& Joint::getTransformToParent() const
+{
+ return transform_to_parent_;
+}
+
+//==============================================================================
+template
+void Joint::setTransformToParent(const Transform3& t)
+{
+ transform_to_parent_ = t;
+}
+
+//==============================================================================
+template
+PrismaticJoint::PrismaticJoint(const std::shared_ptr & link_parent, const std::shared_ptr & link_child,
+ const Transform3& transform_to_parent,
+ const std::string& name,
+ const Vector3& axis) :
+ Joint(link_parent, link_child, transform_to_parent, name),
+ axis_(axis)
+{
+ type_ = JT_PRISMATIC;
+}
+
+//==============================================================================
+template
+const Vector3& PrismaticJoint::getAxis() const
+{
+ return axis_;
+}
+
+//==============================================================================
+template
+std::size_t PrismaticJoint::getNumDofs() const
+{
+ return 1;
+}
+
+//==============================================================================
+template
+Transform3 PrismaticJoint::getLocalTransform() const
+{
+ const Quaternion quat(transform_to_parent_.linear());
+ const Vector3& transl = transform_to_parent_.translation();
+
+ Transform3 tf = Transform3::Identity();
+ tf.linear() = quat.toRotationMatrix();
+ tf.translation() = quat * (axis_ * (*joint_cfg_)[0]) + transl;
+
+ return tf;
+}
+
+//==============================================================================
+template
+RevoluteJoint::RevoluteJoint(const std::shared_ptr & link_parent, const std::shared_ptr & link_child,
+ const Transform3& transform_to_parent,
+ const std::string& name,
+ const Vector3& axis) :
+ Joint(link_parent, link_child, transform_to_parent, name),
+ axis_(axis)
+{
+ type_ = JT_REVOLUTE;
+}
+
+//==============================================================================
+template
+const Vector3& RevoluteJoint::getAxis() const
+{
+ return axis_;
+}
+
+//==============================================================================
+template
+std::size_t RevoluteJoint::getNumDofs() const
+{
+ return 1;
+}
+
+//==============================================================================
+template
+Transform3 RevoluteJoint::getLocalTransform() const
+{
+ Transform3 tf = Transform3::Identity();
+ tf.linear() = transform_to_parent_.linear() * AngleAxis((*joint_cfg_)[0], axis_);
+ tf.translation() = transform_to_parent_.translation();
+
+ return tf;
+}
+
+//==============================================================================
+template
+BallEulerJoint::BallEulerJoint(const std::shared_ptr & link_parent, const std::shared_ptr & link_child,
+ const Transform3& transform_to_parent,
+ const std::string& name) :
+ Joint(link_parent, link_child, transform_to_parent, name)
+{}
+
+//==============================================================================
+template
+std::size_t BallEulerJoint::getNumDofs() const
+{
+ return 3;
+}
+
+//==============================================================================
+template
+Transform3 BallEulerJoint::getLocalTransform() const
+{
+ Matrix3 rot(
+ AngleAxis((*joint_cfg_)[0], Eigen::Vector3::UnitX())
+ * AngleAxis((*joint_cfg_)[1], Eigen::Vector3::UnitY())
+ * AngleAxis((*joint_cfg_)[2], Eigen::Vector3::UnitZ()));
+
+ Transform3 tf = Transform3::Identity();
+ tf.linear() = rot;
+
+ return transform_to_parent_ * tf;
+}
+
+} // namespace fcl
+
+#endif
diff --git a/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/joint_config.h b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/joint_config.h
new file mode 100644
index 00000000..b711de21
--- /dev/null
+++ b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/joint_config.h
@@ -0,0 +1,189 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2016, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
+#define FCL_ARTICULATED_MODEL_JOINT_CONFIG_H
+
+#include "fcl/common/data_types.h"
+#include
+#include
+
+namespace fcl
+{
+
+template
+class Joint;
+
+template
+class JointConfig
+{
+public:
+ JointConfig();
+
+ JointConfig(const JointConfig& joint_cfg);
+
+ JointConfig(const std::shared_ptr& joint,
+ S default_value = 0,
+ S default_value_min = 0,
+ S default_value_max = 0);
+
+ std::size_t getDim() const;
+
+ inline S operator [] (std::size_t i) const
+ {
+ return values_[i];
+ }
+
+ inline S& operator [] (std::size_t i)
+ {
+ return values_[i];
+ }
+
+ S getValue(std::size_t i) const;
+
+ S& getValue(std::size_t i);
+
+ S getLimitMin(std::size_t i) const;
+
+ S& getLimitMin(std::size_t i);
+
+ S getLimitMax(std::size_t i) const;
+
+ S& getLimitMax(std::size_t i);
+
+ std::shared_ptr getJoint() const;
+
+private:
+ std::weak_ptr joint_;
+
+ std::vector values_;
+ std::vector limits_min_;
+ std::vector limits_max_;
+};
+
+//============================================================================//
+// //
+// Implementations //
+// //
+//============================================================================//
+
+//==============================================================================
+template
+JointConfig::JointConfig() {}
+
+//==============================================================================
+template
+JointConfig::JointConfig(const JointConfig& joint_cfg) :
+ joint_(joint_cfg.joint_),
+ values_(joint_cfg.values_),
+ limits_min_(joint_cfg.limits_min_),
+ limits_max_(joint_cfg.limits_max_)
+{
+}
+
+//==============================================================================
+template
+JointConfig::JointConfig(const std::shared_ptr& joint,
+ S default_value,
+ S default_value_min,
+ S default_value_max) :
+ joint_(joint)
+{
+ values_.resize(joint->getNumDofs(), default_value);
+ limits_min_.resize(joint->getNumDofs(), default_value_min);
+ limits_max_.resize(joint->getNumDofs(), default_value_max);
+}
+
+//==============================================================================
+template
+std::size_t JointConfig::getDim() const
+{
+ return values_.size();
+}
+
+//==============================================================================
+template
+S JointConfig::getValue(std::size_t i) const
+{
+ return values_[i];
+}
+
+//==============================================================================
+template
+S& JointConfig::getValue(std::size_t i)
+{
+ return values_[i];
+}
+
+//==============================================================================
+template
+S JointConfig::getLimitMin(std::size_t i) const
+{
+ return limits_min_[i];
+}
+
+//==============================================================================
+template
+S& JointConfig::getLimitMin(std::size_t i)
+{
+ return limits_min_[i];
+}
+
+//==============================================================================
+template
+S JointConfig::getLimitMax(std::size_t i) const
+{
+ return limits_max_[i];
+}
+
+//==============================================================================
+template
+S& JointConfig::getLimitMax(std::size_t i)
+{
+ return limits_max_[i];
+}
+
+//==============================================================================
+template
+std::shared_ptr JointConfig::getJoint() const
+{
+ return joint_.lock();
+}
+
+} // namespace fcl
+
+#endif
diff --git a/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/link.h b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/link.h
new file mode 100644
index 00000000..a36d5766
--- /dev/null
+++ b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/link.h
@@ -0,0 +1,145 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2016, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_LINK_H
+#define FCL_ARTICULATED_MODEL_LINK_H
+
+#include "fcl/common/data_types.h"
+#include "fcl/object/collision_object.h"
+
+#include
+#include
+
+namespace fcl
+{
+
+template
+class Joint;
+
+template
+class Link
+{
+public:
+ Link(const std::string& name);
+
+ const std::string& getName() const;
+
+ void setName(const std::string& name);
+
+ void addChildJoint(const std::shared_ptr& joint);
+
+ void setParentJoint(const std::shared_ptr& joint);
+
+ void addObject(const std::shared_ptr>& object);
+
+ std::size_t getNumChildJoints() const;
+
+ std::size_t getNumObjects() const;
+
+protected:
+ std::string name_;
+
+ std::vector> > objects_;
+
+ std::vector > children_joints_;
+
+ std::shared_ptr parent_joint_;
+};
+
+//============================================================================//
+// //
+// Implementations //
+// //
+//============================================================================//
+
+//==============================================================================
+template
+Link::Link(const std::string& name) : name_(name)
+{}
+
+//==============================================================================
+template
+const std::string& Link::getName() const
+{
+ return name_;
+}
+
+//==============================================================================
+template
+void Link::setName(const std::string& name)
+{
+ name_ = name;
+}
+
+//==============================================================================
+template
+void Link::addChildJoint(const std::shared_ptr& joint)
+{
+ children_joints_.push_back(joint);
+}
+
+//==============================================================================
+template
+void Link::setParentJoint(const std::shared_ptr& joint)
+{
+ parent_joint_ = joint;
+}
+
+//==============================================================================
+template
+void Link::addObject(const std::shared_ptr>& object)
+{
+ objects_.push_back(object);
+}
+
+//==============================================================================
+template
+std::size_t Link::getNumChildJoints() const
+{
+ return children_joints_.size();
+}
+
+//==============================================================================
+template
+std::size_t Link::getNumObjects() const
+{
+ return objects_.size();
+}
+
+} // namespace fcl
+
+#endif
diff --git a/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/model.h b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/model.h
new file mode 100644
index 00000000..ecffbda0
--- /dev/null
+++ b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/model.h
@@ -0,0 +1,237 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2016, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_MODEL_H
+#define FCL_ARTICULATED_MODEL_MODEL_H
+
+#include "fcl/articulated_model/joint.h"
+#include "fcl/articulated_model/link.h"
+
+#include "fcl/common/data_types.h"
+#include
+
+#include
+#include
+
+namespace fcl
+{
+
+class ModelParseError : public std::runtime_error
+{
+public:
+ ModelParseError(const std::string& error_msg) : std::runtime_error(error_msg) {}
+};
+
+template
+class Model
+{
+public:
+ Model() {}
+
+ virtual ~Model() {}
+
+ const std::string& getName() const;
+
+ void addLink(const std::shared_ptr & link);
+
+ void addJoint(const std::shared_ptr& joint);
+
+ void initRoot(const std::map& link_parent_tree);
+
+ void initTree(std::map& link_parent_tree);
+
+ std::size_t getNumDofs() const;
+
+ std::size_t getNumLinks() const;
+
+ std::size_t getNumJoints() const;
+
+ std::shared_ptr getRoot() const;
+ std::shared_ptr getLink(const std::string& name) const;
+ std::shared_ptr getJoint(const std::string& name) const;
+
+ std::vector > getLinks() const;
+ std::vector > getJoints() const;
+protected:
+ std::shared_ptr root_link_;
+ std::map > links_;
+ std::map > joints_;
+
+ std::string name_;
+
+};
+
+//==============================================================================
+template
+std::shared_ptr Model::getRoot() const
+{
+ return root_link_;
+}
+
+//==============================================================================
+template
+std::shared_ptr Model::getLink(const std::string& name) const
+{
+ std::shared_ptr ptr;
+ std::map >::const_iterator it = links_.find(name);
+ if(it == links_.end())
+ ptr.reset();
+ else
+ ptr = it->second;
+ return ptr;
+}
+
+//==============================================================================
+template
+std::shared_ptr Model::getJoint(const std::string& name) const
+{
+ std::shared_ptr ptr;
+ std::map >::const_iterator it = joints_.find(name);
+ if(it == joints_.end())
+ ptr.reset();
+ else
+ ptr = it->second;
+ return ptr;
+}
+
+//==============================================================================
+template
+const std::string& Model::getName() const
+{
+ return name_;
+}
+
+//==============================================================================
+template
+std::vector > Model::getLinks() const
+{
+ std::vector > links;
+ for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it)
+ {
+ links.push_back(it->second);
+ }
+
+ return links;
+}
+
+//==============================================================================
+template
+std::size_t Model::getNumLinks() const
+{
+ return links_.size();
+}
+
+//==============================================================================
+template
+std::size_t Model::getNumJoints() const
+{
+ return joints_.size();
+}
+
+//==============================================================================
+template
+std::size_t Model::getNumDofs() const
+{
+ std::size_t dof = 0;
+ for(std::map >::const_iterator it = joints_.begin(); it != joints_.end(); ++it)
+ {
+ dof += it->second->getNumDofs();
+ }
+
+ return dof;
+}
+
+//==============================================================================
+template
+void Model::addLink(const std::shared_ptr & link)
+{
+ links_[link->getName()] = link;
+}
+
+//==============================================================================
+template
+void Model::addJoint(const std::shared_ptr& joint)
+{
+ joints_[joint->getName()] = joint;
+}
+
+//==============================================================================
+template
+void Model::initRoot(const std::map& link_parent_tree)
+{
+ root_link_.reset();
+
+ /// find the links that have no parent in the tree
+ for(std::map >::const_iterator it = links_.begin(); it != links_.end(); ++it)
+ {
+ std::map::const_iterator parent = link_parent_tree.find(it->first);
+ if(parent == link_parent_tree.end())
+ {
+ if(!root_link_)
+ {
+ root_link_ = getLink(it->first);
+ }
+ else
+ {
+ throw ModelParseError("Two root links found: [" + root_link_->getName() + "] and [" + it->first + "]");
+ }
+ }
+ }
+
+ if(!root_link_)
+ throw ModelParseError("No root link found.");
+}
+
+//==============================================================================
+template
+void Model::initTree(std::map& link_parent_tree)
+{
+ for(std::map >::iterator it = joints_.begin(); it != joints_.end(); ++it)
+ {
+ std::string parent_link_name = it->second->getParentLink()->getName();
+ std::string child_link_name = it->second->getChildLink()->getName();
+
+ it->second->getParentLink()->addChildJoint(it->second);
+ it->second->getChildLink()->setParentJoint(it->second);
+
+ link_parent_tree[child_link_name] = parent_link_name;
+ }
+}
+
+} // namespace fcl
+
+#endif
diff --git a/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/model_config.h b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/model_config.h
new file mode 100644
index 00000000..60f07b47
--- /dev/null
+++ b/rmf_traffic/thirdparty/fcl/.deprecated/articulated_model/model_config.h
@@ -0,0 +1,137 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2016, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Dalibor Matura, Jia Pan */
+
+#ifndef FCL_ARTICULATED_MODEL_MODEL_CONFIG_H
+#define FCL_ARTICULATED_MODEL_MODEL_CONFIG_H
+
+#include
+#include
+#include
+#include "fcl/common/data_types.h"
+#include "fcl/articulated_model/joint_config.h"
+
+namespace fcl
+{
+
+template
+class ModelConfig
+{
+public:
+ ModelConfig();
+
+ ModelConfig(const ModelConfig& model_cfg);
+
+ ModelConfig(std::map > joints_map);
+
+ JointConfig getJointConfigByJointName(const std::string& joint_name) const;
+ JointConfig& getJointConfigByJointName(const std::string& joint_name);
+
+ JointConfig getJointConfigByJoint(std::shared_ptr joint) const;
+ JointConfig& getJointConfigByJoint(std::shared_ptr joint);
+
+ std::map getJointCfgsMap() const
+ { return joint_cfgs_map_; }
+
+private:
+ std::map joint_cfgs_map_;
+};
+
+//============================================================================//
+// //
+// Implementations //
+// //
+//============================================================================//
+
+//==============================================================================
+template
+ModelConfig::ModelConfig()
+{
+ // Do nothing
+}
+
+//==============================================================================
+template
+ModelConfig::ModelConfig(const ModelConfig& model_cfg) :
+ joint_cfgs_map_(model_cfg.joint_cfgs_map_)
+{}
+
+//==============================================================================
+template
+ModelConfig::ModelConfig(std::map > joints_map)
+{
+ std::map >::iterator it;
+ for(it = joints_map.begin(); it != joints_map.end(); ++it)
+ joint_cfgs_map_[it->first] = JointConfig(it->second);
+}
+
+//==============================================================================
+template
+JointConfig ModelConfig::getJointConfigByJointName(const std::string& joint_name) const
+{
+ std::map::const_iterator it = joint_cfgs_map_.find(joint_name);
+ assert(it != joint_cfgs_map_.end());
+
+ return it->second;
+}
+
+//==============================================================================
+template
+JointConfig& ModelConfig::getJointConfigByJointName(const std::string& joint_name)
+{
+ std::map::iterator it = joint_cfgs_map_.find(joint_name);
+ assert(it != joint_cfgs_map_.end());
+
+ return it->second;
+}
+
+//==============================================================================
+template
+JointConfig ModelConfig::getJointConfigByJoint(std::shared_ptr joint) const
+{
+ return getJointConfigByJointName(joint->getName());
+}
+
+//==============================================================================
+template
+JointConfig& ModelConfig::getJointConfigByJoint(std::shared_ptr joint)
+{
+ return getJointConfigByJointName(joint->getName());
+}
+
+} // namespace fcl
+
+#endif
diff --git a/rmf_traffic/thirdparty/fcl/.deprecated/learning/classifier.h b/rmf_traffic/thirdparty/fcl/.deprecated/learning/classifier.h
new file mode 100644
index 00000000..06ef87c4
--- /dev/null
+++ b/rmf_traffic/thirdparty/fcl/.deprecated/learning/classifier.h
@@ -0,0 +1,260 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2013-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2016, Open Source Robotics Foundation
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_LEARNING_CLASSIFIER_H
+#define FCL_LEARNING_CLASSIFIER_H
+
+#include
+
+#include "fcl/common/data_types.h"
+
+namespace fcl
+{
+
+template
+struct Item
+{
+ VectorN q;
+ bool label;
+ S w;
+
+ Item(const VectorN& q_, bool label_, S w_ = 1);
+
+ Item();
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(S, N)
+};
+
+template
+struct Scaler
+{
+ VectorN v_min, v_max;
+ Scaler();
+
+ Scaler(const VectorN& v_min_, const VectorN& v_max_);
+
+ VectorN scale(const VectorN& v) const;
+
+ VectorN unscale(const VectorN& v) const;
+
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW_IF_VECTORIZABLE_FIXED_SIZE(S, N)
+};
+
+template
+struct PredictResult
+{
+ bool label;
+ S prob;
+
+ PredictResult();
+ PredictResult(bool label_, S prob_ = 1);
+};
+
+template
+class SVMClassifier
+{
+public:
+
+ ~SVMClassifier();
+
+ virtual PredictResult predict(const VectorN& q) const = 0;
+ virtual std::vector