Merge branch 'master' into readme/build_from_source_ros_path

.clang-format

@@ -49,7 +49,7 @@ IncludeCategories:
     Priority:  1
   - Regex:     '^(<|")aikido/' # Aikido headers
     Priority:  3
-  - Regex:     '^(<[a-z]+)'    # dependency headers
+  - Regex:     '^(<[A-z]+)'    # dependency headers
     Priority:  2
   - Regex:     '^".*'          # headers relative to this project
     Priority:  4

CMakeLists.txt

@@ -87,8 +87,12 @@ function(format_add_sources)
 endfunction()
 
 # TODO: Temporarily split header list per subdirectories.
+file(GLOB_RECURSE headers_planner "${CMAKE_SOURCE_DIR}/include/aikido/planner/*.hpp")
 file(GLOB_RECURSE headers_util "${CMAKE_SOURCE_DIR}/include/aikido/util/*.hpp")
+file(GLOB_RECURSE headers_distance "${CMAKE_SOURCE_DIR}/include/aikido/distance/*.hpp")
+format_add_sources(${headers_planner})
 format_add_sources(${headers_util})
+format_add_sources(${headers_distance})
 
 #==============================================================================
 # Helper functions.

cmake/Findactionlib.cmake

@@ -13,7 +13,7 @@ find_package(PkgConfig QUIET REQUIRED)
 
 # Check to see if pkgconfig is installed.
 set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH TRUE)
-pkg_check_modules(PC_actionlib actionlib QUIET REQUIRED)
+pkg_check_modules(PC_actionlib actionlib QUIET)
 
 # Include directories
 find_path(actionlib_INCLUDE_DIRS

cmake/Findcontrol_msgs.cmake

@@ -12,7 +12,7 @@ find_package(PkgConfig QUIET REQUIRED)
 
 # Check to see if pkgconfig is installed.
 set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH TRUE)
-pkg_check_modules(PC_control_msgs control_msgs QUIET REQUIRED)
+pkg_check_modules(PC_control_msgs control_msgs QUIET)
 
 # Include directories
 find_path(control_msgs_INCLUDE_DIRS

include/aikido/control/ros/RosJointStateClient.hpp

@@ -0,0 +1,72 @@
+#ifndef AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_
+#define AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_
+#include <string>
+#include <boost/circular_buffer.hpp>
+#include <dart/dynamics/dynamics.hpp>
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <sensor_msgs/JointState.h>
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+/// Client that listens for JointState messages for each skeleton joint and
+/// provides a method for extracting the most recent position of each joint.
+class RosJointStateClient
+{
+public:
+  /// Constructor.
+  /// \param _skeleton Skeleton to read JointState updates for.
+  /// \param _nodeHandle ROS node.
+  /// \param _topicName Name of topic to subscribe to for JointState updates.
+  /// \param _capacity Number of JointStateRecords that are saved per joint.
+  RosJointStateClient(
+    dart::dynamics::SkeletonPtr _skeleton,
+    ::ros::NodeHandle _nodeHandle,
+    const std::string& _topicName,
+    size_t capacity);
+
+  /// Update mBuffer with any JointState messages that have been received.
+  void spin();
+
+  /// Returns the most recent position of each joint in _metaSkeleton.
+  /// \param _metaSkeleton Skeleton to read DOFs from.
+  /// \return vector of positions for each DOF
+  Eigen::VectorXd getLatestPosition(
+    const dart::dynamics::MetaSkeleton& _metaSkeleton) const;
+
+  // TODO: implement
+  // getPositionAtTime(const MetaSkeleton&, const ros::Time&, bool)
+  // that interpolates position at the specified time, optionally blocking for
+  // new data.
+private:
+  struct JointStateRecord
+  {
+    inline bool isValid() const { return mStamp.isValid(); }
+
+    ::ros::Time mStamp;
+    double mPosition;
+  };
+
+  /// Callback to add a new JointState to mBuffer
+  /// \param _jointState New JointState to add to mBuffer
+  void jointStateCallback(const sensor_msgs::JointState& _jointState);
+
+  mutable std::mutex mMutex;
+
+  dart::dynamics::SkeletonPtr mSkeleton;
+  std::unordered_map<std::string,
+    boost::circular_buffer<JointStateRecord>> mBuffer;
+  size_t mCapacity;
+
+  ::ros::CallbackQueue mCallbackQueue;
+  ::ros::NodeHandle mNodeHandle;
+  ::ros::Subscriber mSubscriber;
+};
+
+} // namespace ros
+} // namespace control
+} // namespace aikido
+
+#endif // ifndef AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_

include/aikido/distance/DistanceMetric.hpp

@@ -3,10 +3,9 @@
 
 #include "../statespace/StateSpace.hpp"
 
-namespace aikido
-{
-namespace distance
-{
+namespace aikido {
+namespace distance {
+
 /// Implements a distance metric defined on a StateSpace
 class DistanceMetric
 {
@@ -29,6 +28,8 @@ class DistanceMetric
 };
 
 using DistanceMetricPtr = std::shared_ptr<DistanceMetric>;
-}
-}
+
+} // namespace distance
+} // namespace aikido
+
 #endif

include/aikido/distance/RnEuclidean.hpp

@@ -1,13 +1,12 @@
 #ifndef AIKIDO_DISTANCE_EUCLIDEANDISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_EUCLIDEANDISTANCEMETRIC_HPP_
 
-#include "DistanceMetric.hpp"
 #include "../statespace/Rn.hpp"
+#include "DistanceMetric.hpp"
+
+namespace aikido {
+namespace distance {
 
-namespace aikido
-{
-namespace distance
-{
 /// Implements a Euclidean distance metric
 template <int N>
 class REuclidean : public DistanceMetric
@@ -23,8 +22,9 @@ class REuclidean : public DistanceMetric
   /// Computes Euclidean distance between two states.
   /// \param _state1 The first state (type Rn::State)
   /// \param _state2 The second state (type Rn::State)
-  double distance(const statespace::StateSpace::State* _state1,
-                  const statespace::StateSpace::State* _state2) const override;
+  double distance(
+      const statespace::StateSpace::State* _state1,
+      const statespace::StateSpace::State* _state2) const override;
 
 private:
   std::shared_ptr<statespace::R<N>> mStateSpace;

include/aikido/distance/SO2Angular.hpp

@@ -1,34 +1,36 @@
 #ifndef AIKIDO_DISTANCE_ANGULARDISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_ANGULARDISTANCEMETRIC_HPP_
 
-#include "DistanceMetric.hpp"
 #include "../statespace/SO2.hpp"
+#include "DistanceMetric.hpp"
+
+namespace aikido {
+namespace distance {
 
-namespace aikido
-{
-namespace distance
-{
 /// Computes the shortest distance between two angles in SO(2)
 class SO2Angular : public DistanceMetric
 {
 public:
   /// Constructor.
   /// \param _space The SO2 this distance metric operates on
-  explicit SO2Angular(
-      std::shared_ptr<statespace::SO2> _space);
+  explicit SO2Angular(std::shared_ptr<statespace::SO2> _space);
 
   // Documentation inherited
   statespace::StateSpacePtr getStateSpace() const override;
 
-  /// Computes shortest distance between two angles. (return value between 0 and pi)
+  /// Computes shortest distance between two angles. (return value between 0 and
+  /// pi)
   /// \param _state1 The first state (type: SO2::State)
   /// \param _state2 The second state (type: SO2::State)
-  double distance(const statespace::StateSpace::State* _state1,
-                  const statespace::StateSpace::State* _state2) const override;
+  double distance(
+      const statespace::StateSpace::State* _state1,
+      const statespace::StateSpace::State* _state2) const override;
 
 private:
   std::shared_ptr<statespace::SO2> mStateSpace;
 };
-}
-}
+
+} // namespace distance
+} // namespace aikido
+
 #endif

include/aikido/distance/SO3Angular.hpp

@@ -1,34 +1,35 @@
 #ifndef AIKIDO_DISTANCE_GEODESICDISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_GEODESICDISTANCEMETRIC_HPP_
 
-#include "DistanceMetric.hpp"
 #include "../statespace/SO3.hpp"
+#include "DistanceMetric.hpp"
+
+namespace aikido {
+namespace distance {
 
-namespace aikido
-{
-namespace distance
-{
 /// Implements a distance metric on SO(3)
 class SO3Angular : public DistanceMetric
 {
 public:
   /// Constructor.
   /// \param _space The SO3 this distance metric operates on
-  explicit SO3Angular(
-      std::shared_ptr<statespace::SO3> _space);
+  explicit SO3Angular(std::shared_ptr<statespace::SO3> _space);
 
   // Documentation inherited
   statespace::StateSpacePtr getStateSpace() const override;
 
   /// Computes distance (in radians) between the two states
   /// \param _state1 The first state (type SO3::State)
   /// \param _state2 The second state (type SO3::State)
-  double distance(const statespace::StateSpace::State* _state1,
-                  const statespace::StateSpace::State* _state2) const override;
+  double distance(
+      const statespace::StateSpace::State* _state1,
+      const statespace::StateSpace::State* _state2) const override;
 
 private:
   std::shared_ptr<statespace::SO3> mStateSpace;
 };
-}
-}
+
+} // namespace distance
+} // namespace aikido
+
 #endif

include/aikido/distance/Weighted.hpp

@@ -1,13 +1,12 @@
 #ifndef AIKIDO_DISTANCE_WEIGHTEDDISTANCEMETRIC_HPP_
 #define AIKIDO_DISTANCE_WEIGHTEDDISTANCEMETRIC_HPP_
 
-#include "DistanceMetric.hpp"
 #include "../statespace/CartesianProduct.hpp"
+#include "DistanceMetric.hpp"
+
+namespace aikido {
+namespace distance {
 
-namespace aikido
-{
-namespace distance
-{
 /// Implements a distance metric on a CartesianProduct. This metric computes
 /// the weighted
 /// sum of distances on the individual components of the statespace.
@@ -18,18 +17,19 @@ class Weighted : public DistanceMetric
   /// \param _space The state space
   /// \param _metrics A vector containing one element for every component of the
   /// CartesianProduct
-  Weighted(std::shared_ptr<statespace::CartesianProduct> _space,
-                         std::vector<DistanceMetricPtr> _metrics);
+  Weighted(
+      std::shared_ptr<statespace::CartesianProduct> _space,
+      std::vector<DistanceMetricPtr> _metrics);
 
   /// Constructor.
   /// \param _space The state space
   /// \param _metrics A vector containing one element for every component of the
   /// CartesianProduct. The first element of every pair in the vector is the
   /// metric and the second is the weight to be applied to the metric. The
   /// weights must all be positive.
-  Weighted(std::shared_ptr<statespace::CartesianProduct> _space,
-                         std::vector<std::pair<DistanceMetricPtr, double>> _metrics);
-
+  Weighted(
+      std::shared_ptr<statespace::CartesianProduct> _space,
+      std::vector<std::pair<DistanceMetricPtr, double>> _metrics);
 
   // Documentation inherited
   statespace::StateSpacePtr getStateSpace() const override;
@@ -38,13 +38,16 @@ class Weighted : public DistanceMetric
   ///  of distances between their matching subcomponents.
   /// \param _state1 The first state (type CartesianProduct::State)
   /// \param _state2 The second state (type CartesianProduct::State)
-  double distance(const statespace::StateSpace::State* _state1,
-                  const statespace::StateSpace::State* _state2) const override;
+  double distance(
+      const statespace::StateSpace::State* _state1,
+      const statespace::StateSpace::State* _state2) const override;
 
 private:
   std::shared_ptr<statespace::CartesianProduct> mStateSpace;
   std::vector<std::pair<DistanceMetricPtr, double>> mMetrics;
 };
-}
-}
+
+} // namespace distance
+} // namespace aikido
+
 #endif

include/aikido/distance/defaults.hpp

@@ -1,21 +1,19 @@
 #ifndef AIKIDO_DISTANCE_DISTANCEMETRICDEFAULTS_HPP_
 #define AIKIDO_DISTANCE_DISTANCEMETRICDEFAULTS_HPP_
 
-#include "DistanceMetric.hpp"
 #include "../statespace/StateSpace.hpp"
+#include "DistanceMetric.hpp"
 
-namespace aikido
-{
-namespace distance
-{
+namespace aikido {
+namespace distance {
 /// Creates a DistanceMetric that is appropriate for the statespace of type
 /// Space
 /// \param _sspace The StateSpace the distance metric will operator on
 template <class Space>
 std::unique_ptr<DistanceMetric> createDistanceMetricFor(
     std::shared_ptr<Space> _sspace);
 
-/// Creates a DistanceMetric that is appropriate for the statespace. 
+/// Creates a DistanceMetric that is appropriate for the statespace.
 /// \param _sspace The StateSpace the distance metric will operator on
 std::unique_ptr<DistanceMetric> createDistanceMetric(
     statespace::StateSpacePtr _sspace);

include/aikido/distance/detail/RnEuclidean-impl.hpp

@@ -4,30 +4,25 @@ namespace aikido {
 namespace distance {
 
 //==============================================================================
-extern template
-class REuclidean<0>;
+extern template class REuclidean<0>;
 
-extern template
-class REuclidean<1>;
+extern template class REuclidean<1>;
 
-extern template
-class REuclidean<2>;
+extern template class REuclidean<2>;
 
-extern template
-class REuclidean<3>;
+extern template class REuclidean<3>;
 
-extern template
-class REuclidean<6>;
+extern template class REuclidean<6>;
 
-extern template
-class REuclidean<Eigen::Dynamic>;
+extern template class REuclidean<Eigen::Dynamic>;
 
 //=============================================================================
 template <int N>
 REuclidean<N>::REuclidean(std::shared_ptr<statespace::R<N>> _space)
-    : mStateSpace(std::move(_space))
+  : mStateSpace(std::move(_space))
 {
-  if (mStateSpace == nullptr) {
+  if (mStateSpace == nullptr)
+  {
     throw std::invalid_argument("Rn is nullptr.");
   }
 }