Merge 6ad1d9c into 4fb24a5

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

@@ -4,25 +4,32 @@
 #include <trajectory_msgs/JointTrajectory.h>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
 #include <aikido/trajectory/Spline.hpp>
+#include <map>
 
 namespace aikido {
 namespace control {
 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.
+/// \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] 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_
+

src/control/ros/CMakeLists.txt

@@ -4,17 +4,38 @@
 find_package(trajectory_msgs QUIET)
 aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs")
 
+# TODO: Workaround because find_package(actionlib) imports "tests" and "run_tests"
+# targets that conflict with the targets defined by Aikido.
+find_path(actionlib_INCLUDE_DIRS actionlib PATHS "/opt/ros/indigo/include")
+find_library(actionlib_LIBRARIES actionlib PATHS "/opt/ros/indigo/lib")
+evaluate_condition(actionlib_FOUND actionlib_INCLUDE_DIRS AND actionlib_LIBRARIES)
+#find_package(actionlib QUIET)
+aikido_check_package(actionlib "aikido::control::ros" "actionlib")
+
+find_path(control_msgs_INCLUDE_DIRS control_msgs PATHS "/opt/ros/indigo/include")
+#find_library(control_msgs_LIBRARIES control_msgs PATHS "/opt/ros/indigo/lib")
+evaluate_condition(control_msgs_FOUND control_msgs_INCLUDE_DIRS)
+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,6 +44,9 @@ 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)