ros_control/RosTrajectoryExecutor

include/aikido/control/ros/RosTrajectoryExecutor.hpp

@@ -0,0 +1,79 @@
+#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 <dart/dart.hpp>
+#include <aikido/control/TrajectoryExecutor.hpp>
+#include <aikido/trajectory/Trajectory.hpp>
+
+// actionlib and DART both #define this macro.
+#undef DEPRECATED
+#include <actionlib/client/action_client.h>
+#undef DEPRECATED
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor
+{
+public:
+  RosTrajectoryExecutor(
+    ::dart::dynamics::MetaSkeletonPtr skeleton, 
+    ::ros::NodeHandle node,
+    const std::string& serverName,
+    double timestep,
+    double goalTimeTolerance,
+    std::chrono::milliseconds connectionTimeout
+      = std::chrono::milliseconds{1000},
+    std::chrono::milliseconds connectionPollingRate
+      = std::chrono::milliseconds{20}
+  );
+
+  virtual ~RosTrajectoryExecutor();
+
+  std::future<void> execute(trajectory::TrajectoryPtr _traj) override;
+
+  std::future<void> execute(
+    trajectory::TrajectoryPtr _traj, const ::ros::Time& _startTime);
+
+  /// Simulates mTraj. To be executed on a separate thread.
+  void spin();
+
+private:
+  using TrajectoryActionClient
+    = actionlib::ActionClient<control_msgs::FollowJointTrajectoryAction>;
+  using GoalHandle = TrajectoryActionClient::GoalHandle;
+
+  bool waitForServer();
+
+  void transitionCallback(GoalHandle _handle);
+
+  ::ros::NodeHandle mNode;
+  ::ros::CallbackQueue mCallbackQueue;
+  TrajectoryActionClient mClient;
+  TrajectoryActionClient::GoalHandle mGoalHandle;
+
+  ::dart::dynamics::MetaSkeletonPtr mSkeleton;
+  double mTimestep;
+  double mGoalTimeTolerance;
+
+  std::chrono::milliseconds mConnectionTimeout;
+  std::chrono::milliseconds mConnectionPollingRate;
+
+  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/CMakeLists.txt

@@ -17,3 +17,5 @@ add_component_targets(${PROJECT_NAME} control "${PROJECT_NAME}_control")
 add_component_dependencies(${PROJECT_NAME} control statespace trajectory)
 
 coveralls_add_sources(${sources})
+
+add_subdirectory("ros") # Dependencies: control, statespace, trajectory

src/control/ros/CMakeLists.txt

@@ -0,0 +1,41 @@
+#==============================================================================
+# Dependencies
+#
+find_package(actionlib QUIET)
+aikido_check_package(actionlib "aikido::control::ros" "actionlib")
+
+find_package(control_msgs QUIET)
+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
+)
+
+add_library("${PROJECT_NAME}_control_ros" SHARED ${sources})
+target_include_directories("${PROJECT_NAME}_control_ros" SYSTEM
+  PUBLIC
+    ${actionlib_INCLUDE_DIRS}
+    ${control_msgs_INCLUDE_DIRS}
+    ${roscpp_INCLUDE_DIRS}
+)
+target_link_libraries("${PROJECT_NAME}_control_ros"
+  "${PROJECT_NAME}_control"
+  "${PROJECT_NAME}_statespace"
+  "${PROJECT_NAME}_trajectory"
+  ${DART_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})