Merge branch 'master' into SimBarrettHandCommandExecutor

.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.

README.md

@@ -36,6 +36,15 @@ $ make
 $ sudo make install
 ```
 
+Aikido includes several optional components that depend on [ROS]. While we
+suggest building Aikido in a Catkin workspace (see below) to enable the ROS
+components, it is also possible to build those components in a standalone
+build. To do so, source the `setup.bash` file in your Catkin workspace before
+running the above commands, e.g.:
+```shell
+$ . /path/to/my/workspace/setup.bash
+```
+
 #### Build from source (Catkin)
 It is also possible to build AIKIDO as a [third-party package][REP-136] inside a
 [Catkin workspace][Catkin Workspaces]. To do so, clone AIKIDO into your Catkin

cmake/Findactionlib.cmake

@@ -0,0 +1,40 @@
+# Find actionlib
+#
+# This sets the following variables:
+# actionlib_FOUND
+# actionlib_INCLUDE_DIRS
+# actionlib_LIBRARIES
+# actionlib_VERSION
+
+# Note: This find module is necessary because the config file imports "gtest",
+# "tests", and "run_tests" that conflict with the targets defined by Aikido.
+
+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)
+
+# Include directories
+find_path(actionlib_INCLUDE_DIRS
+  NAMES actionlib/action_definition.h
+  HINTS ${PC_actionlib_LIBRARY_DIRS}
+)
+
+# Libraries
+find_library(actionlib_LIBRARIES
+  actionlib
+  HINTS ${PC_actionlib_LIBRARY_DIRS}
+)
+
+# Version
+set(actionlib_VERSION ${PC_actionlib_VERSION})
+
+# Set (NAME)_FOUND if all the variables and the version are satisfied.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(actionlib
+    FOUND_VAR actionlib_FOUND
+    FAIL_MESSAGE  DEFAULT_MSG
+    REQUIRED_VARS actionlib_INCLUDE_DIRS actionlib_LIBRARIES
+    VERSION_VAR   actionlib_VERSION
+)

cmake/Findcontrol_msgs.cmake

@@ -0,0 +1,33 @@
+# Find control_msgs
+#
+# This sets the following variables:
+# control_msgs_FOUND
+# control_msgs_INCLUDE_DIRS
+# control_msgs_VERSION
+
+# Note: This find module is necessary because the config file imports "gtest",
+# "tests", and "run_tests" that conflict with the targets defined by Aikido.
+
+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)
+
+# Include directories
+find_path(control_msgs_INCLUDE_DIRS
+  NAMES control_msgs/JointTolerance.h
+  HINTS ${PC_control_msgs_LIBRARY_DIRS}
+)
+
+# Version
+set(control_msgs_VERSION ${PC_control_msgs_VERSION})
+
+# Set (NAME)_FOUND if all the variables and the version are satisfied.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(control_msgs
+    FOUND_VAR control_msgs_FOUND
+    FAIL_MESSAGE  DEFAULT_MSG
+    REQUIRED_VARS control_msgs_INCLUDE_DIRS
+    VERSION_VAR   control_msgs_VERSION
+)

include/aikido/control/TrajectoryExecutor.hpp

@@ -1,7 +1,7 @@
 #ifndef AIKIDO_CONTROL_TRAJECTORYEXECUTOR_HPP_
 #define AIKIDO_CONTROL_TRAJECTORYEXECUTOR_HPP_
 #include "TrajectoryResult.hpp"
-#include "../trajectory/Trajectory.hpp"
+#include <aikido/trajectory/Trajectory.hpp>
 #include <future>
 
 namespace aikido {

include/aikido/control/ros/Conversions.hpp

@@ -1,5 +1,6 @@
 #ifndef AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_
 #define AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_
+
 #include <memory>
 #include <trajectory_msgs/JointTrajectory.h>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
@@ -11,18 +12,24 @@ namespace ros {
 
 /// Converts a ROS JointTrajectory into an aikido's Spline trajectory.
 /// This method only handles single-DOF joints.
-/// \param[in] _space MetaSkeletonStateSpace for Spline trajectory.
-//              Subspaces must be either 1D RnJoint or SO2Joint.
-/// \param[in] _jointTrajectory ROS JointTrajectory to be converted.
+/// \param[in] space MetaSkeletonStateSpace for Spline trajectory.
+//              Subspaces must be either R1Joint or SO2Joint.
+/// \param[in] jointTrajectory ROS JointTrajectory to be converted.
 /// \return Spline trajectory.
-std::unique_ptr<aikido::trajectory::Spline> convertJointTrajectory(
+std::unique_ptr<aikido::trajectory::Spline> toSplineJointTrajectory(
   const std::shared_ptr<
     aikido::statespace::dart::MetaSkeletonStateSpace>& space,
   const trajectory_msgs::JointTrajectory& jointTrajectory);
 
+/// Converts Aikido Trajectory to ROS JointTrajectory.
+/// Supports only 1D RnJoints and SO2Joints.
+/// \param[in] trajectory Aikido trajectory to be converted.
+/// \param[in] timestep Timestep between two consecutive waypoints.
+trajectory_msgs::JointTrajectory toRosJointTrajectory(
+  const aikido::trajectory::TrajectoryPtr& trajectory, double timestep);
+
 } // namespace ros
 } // namespace control
 } // namespace aikido
 
 #endif // ifndef AIKIDO_CONTROL_ROS_CONVERSIONS_HPP_
-

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/control/ros/RosTrajectoryExecutionException.hpp

@@ -0,0 +1,33 @@
+#ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTIONEXCEPTION_HPP_
+#define AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTIONEXCEPTION_HPP_
+#include <exception>
+#include <actionlib/client/action_client.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+/// This class wraps various exceptions that may arise during trajectory
+/// execution over ROS.
+class RosTrajectoryExecutionException: public std::runtime_error
+{
+public:
+
+  RosTrajectoryExecutionException(
+    const std::string& what,
+    actionlib::TerminalState terminalState);
+
+  RosTrajectoryExecutionException(
+    const std::string& what,
+    int result);
+
+  virtual ~RosTrajectoryExecutionException() = default;
+
+};
+
+} // ros
+} // control
+} // aikido
+
+#endif

include/aikido/control/ros/RosTrajectoryExecutor.hpp

@@ -0,0 +1,90 @@
+#ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_
+#define AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_
+#include <chrono>
+#include <future>
+#include <mutex>
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <control_msgs/FollowJointTrajectoryAction.h>
+#include <aikido/control/TrajectoryExecutor.hpp>
+#include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
+#include <aikido/trajectory/Trajectory.hpp>
+#include <actionlib/client/action_client.h>
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+/// This class sends trajectory commands to ROS server.
+class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor
+{
+public:
+  /// Constructor.
+  /// \param[in] space Space in which trajectries operate.
+  /// \param[in] node ROS node handle for action client.
+  /// \param[in] serverName Name of the server to send traejctory to.
+  /// \param[in] timestep Step size for interpolating trajectories.
+  /// \param[in] goalTimeTolerance
+  /// \param[in] connectionTimeout Timeout for server connection.
+  /// \param[in] connectionPollingPeriod Polling period for server connection.
+  template <typename DurationA, typename DurationB>
+  RosTrajectoryExecutor(
+    statespace::dart::MetaSkeletonStateSpacePtr space,
+    ::ros::NodeHandle node,
+    const std::string& serverName,
+    double timestep,
+    double goalTimeTolerance,
+    const DurationA& connectionTimeout = std::chrono::milliseconds{1000},
+    const DurationB& connectionPollingPeriod = std::chrono::milliseconds{20}
+  );
+
+  virtual ~RosTrajectoryExecutor();
+
+  /// Sends trajectory to ROS server for execution.
+  /// \param[in] traj Trajecotry to be executed.
+  std::future<void> execute(trajectory::TrajectoryPtr traj) override;
+
+  /// Sends trajectory to ROS server for execution.
+  /// \param[in] traj Trajectrory to be executed.
+  /// \param[in] startTime Start time for the trajectory.
+  std::future<void> execute(
+    trajectory::TrajectoryPtr traj, const ::ros::Time& startTime);
+
+  /// To be executed on a separate thread.
+  /// Regularly checks for the completion of a sent trajectory.
+  void spin();
+
+private:
+  using TrajectoryActionClient
+    = actionlib::ActionClient<control_msgs::FollowJointTrajectoryAction>;
+  using GoalHandle = TrajectoryActionClient::GoalHandle;
+
+  bool waitForServer();
+
+  void transitionCallback(GoalHandle handle);
+
+  statespace::dart::MetaSkeletonStateSpacePtr mSpace;
+  ::ros::NodeHandle mNode;
+  ::ros::CallbackQueue mCallbackQueue;
+  TrajectoryActionClient mClient;
+  TrajectoryActionClient::GoalHandle mGoalHandle;
+
+  double mTimestep;
+  double mGoalTimeTolerance;
+
+  std::chrono::milliseconds mConnectionTimeout;
+  std::chrono::milliseconds mConnectionPollingPeriod;
+
+  bool mInProgress;
+  std::promise<void> mPromise;
+  trajectory::TrajectoryPtr mTrajectory;
+
+  std::mutex mMutex;
+};
+
+} // namespace ros
+} // namespace control
+} // namespace aikido
+
+#endif // ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_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