ros_control/RosTrajectoryExecutor (#149)

* ros/Conversions

* Cleaning up CMakeLists.

* CMakeLists cleanup

* Style change to match AIKIDO style guide.

* Checking joint-name matches

* Initial commit for RosTrajectoryExecutor

* Temporarily removing tests in CMakeLists.

* Addressing some of Mike's comments

* Addressing Miks and JS's comments.

* Minor bug fix, adding one test for joint mapping.

* Adding conversion from trajectory to ros trajectory.

* Addressing Mike's comments.

* Line wrapping

* Removing a line.

* Addressing Mike's comments

* Changing method names

* Addressing Mike and JS's comments.

* Changing constructor to templated, removing reorderMap.

* Suppress warning

* Fix Conversions

* Add missing dependencies to package.xml

and use find modules to find actionlib and control_msgs

* Suppress warnings

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 REQUIRED)
+
+# 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 REQUIRED)
+
+# 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/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/perception/eigen_yaml.hpp

@@ -89,12 +89,7 @@ namespace YAML {
 namespace detail {
 
 template <typename MatrixType, bool IsVectorAtCompileTime>
-struct encode_impl {
-  static Node encode(MatrixType const &matrix)
-  {
-    assert(false && "Unknown MatrixType.");
-  }
-};
+struct encode_impl {};
 
 template <typename MatrixType>
 struct encode_impl<MatrixType, true> {

package.xml

@@ -15,6 +15,10 @@
   <depend>python</depend>
   <depend>tinyxml2</depend>
   <depend>yaml-cpp</depend>
+  <depend>actionlib</depend>
+  <depend>control_msgs</depend>
+  <depend>libmicrohttpd</depend>
+  <depend>interactive_markers</depend>
   <doc_depend>doxygen</doc_depend>
   <!-- Workaround to build DART with optional features enabled. -->
   <depend>nlopt</depend>

src/control/ros/CMakeLists.txt

@@ -4,17 +4,35 @@
 find_package(trajectory_msgs QUIET)
 aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs")
 
+# Note: Intentionally use "Module" mode because the config file imports "gtest",
+# "tests", and "run_tests" that conflict with the targets defined by Aikido.
+find_package(actionlib QUIET MODULE)
+aikido_check_package(actionlib "aikido::control::ros" "actionlib")
+
+# Note: Intentionally use "Module" mode because the config file imports "gtest",
+# "tests", and "run_tests" that conflict with the targets defined by Aikido.
+find_package(control_msgs QUIET MODULE)
+aikido_check_package(control_msgs "aikido::control::ros" "control_msgs")
+
+find_package(roscpp QUIET)
+aikido_check_package(roscpp "aikido::control::ros" "roscpp")
+
 #==============================================================================
 # Libraries
 #
 set(sources
+  RosTrajectoryExecutor.cpp
+  RosTrajectoryExecutionException.cpp
   Conversions.cpp
 )
 
 add_library("${PROJECT_NAME}_control_ros" SHARED ${sources})
 target_include_directories("${PROJECT_NAME}_control_ros" SYSTEM
   PUBLIC
     ${trajectory_msgs_INCLUDE_DIRS}
+    ${actionlib_INCLUDE_DIRS}
+    ${control_msgs_INCLUDE_DIRS}
+    ${roscpp_INCLUDE_DIRS}
 )
 
 target_link_libraries("${PROJECT_NAME}_control_ros"
@@ -23,11 +41,13 @@ target_link_libraries("${PROJECT_NAME}_control_ros"
   "${PROJECT_NAME}_trajectory"
   ${DART_LIBRARIES}
   ${trajectory_msgs_LIBRARIES}
+  ${actionlib_LIBRARIES}
+  ${control_msgs_LIBRARIES}
+  ${roscpp_LIBRARIES}
 )
 
 add_component(${PROJECT_NAME} control_ros)
 add_component_targets(${PROJECT_NAME} control_ros "${PROJECT_NAME}_control_ros")
 add_component_dependencies(${PROJECT_NAME} control_ros control statespace trajectory)
 
 coveralls_add_sources(${sources})
-