ros_control/RosTrajectoryExecutor

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

@@ -0,0 +1,38 @@
+#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>
+#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.
+//              Subspaces must be either 1D RnJoint or SO2Joint.
+/// \param[in] _jointTrajectory ROS JointTrajectory to be converted.
+/// \return Spline trajectory.
+std::unique_ptr<aikido::trajectory::Spline> convertJointTrajectory(
+  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] indexMap Mapping between trajectory's joints and ros joints.
+/// \_param[in] timestep Timestep between two consecutive waypoints.
+const trajectory_msgs::JointTrajectory convertTrajectoryToRosTrajectory(
+  const aikido::trajectory::TrajectoryPtr& trajectory,
+  const std::map<std::string, size_t>& indexMap, 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,95 @@
+#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] indexMap Joint index mapping from statespace to ros trajectory.
+  /// \param[in] connectionTimeout Timeout for server connection.
+  /// \param[in] connectionPollingPeriod Polling period for server connection.
+  RosTrajectoryExecutor(
+    statespace::dart::MetaSkeletonStateSpacePtr space,
+    ::ros::NodeHandle node,
+    const std::string& serverName,
+    double timestep,
+    double goalTimeTolerance,
+    const std::map<std::string, size_t>& jointIndexMap,
+    std::chrono::milliseconds connectionTimeout
+      = std::chrono::milliseconds{1000},
+    std::chrono::milliseconds 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;
+
+  std::map<std::string, size_t> mJointIndexMap;
+
+  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/CMakeLists.txt

@@ -20,3 +20,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,57 @@
+#==============================================================================
+# Dependencies
+#
+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"
+  "${PROJECT_NAME}_control"
+  "${PROJECT_NAME}_statespace"
+  "${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})
+