Add RosPositionCommandExecutor

include/aikido/control/ros/Conversions.hpp

@@ -3,6 +3,7 @@
 
 #include <memory>
 #include <trajectory_msgs/JointTrajectory.h>
+#include <sensor_msgs/JointState.h>
 #include <aikido/statespace/dart/MetaSkeletonStateSpace.hpp>
 #include <aikido/trajectory/Spline.hpp>
 
@@ -28,6 +29,14 @@ std::unique_ptr<aikido::trajectory::Spline> toSplineJointTrajectory(
 trajectory_msgs::JointTrajectory toRosJointTrajectory(
   const aikido::trajectory::TrajectoryPtr& trajectory, double timestep);
 
+/// Converts Eigen VectorXd and joint names to JointState
+/// \param[in] goalPositions The required positions for the fingers
+/// \param[in] jointNames The corresponding names of the joints
+/// \return The JointState message object
+sensor_msgs::JointState toJointState(
+  const Eigen::VectorXd& goalPositions, const std::vector<std::string> jointNames);
+
+
 } // namespace ros
 } // namespace control
 } // namespace aikido

include/aikido/control/ros/RosBarrettHandPositionCommandExecutor.hpp

@@ -0,0 +1,81 @@
+// Assume it has 4 positions
+#ifndef AIKIDO_CONTROL_ROS_ROSBARRETTHANDPOSITIONCOMMANDEXECUTOR_HPP_
+#define AIKIDO_CONTROL_ROS_ROSBARRETTHANDPOSITIONCOMMANDEXECUTOR_HPP_
+#include <chrono>
+#include <future>
+#include <mutex>
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <sensor_msgs/JointState.h>
+#include <pr_control_msgs/SetPositionAction.h>
+#include <actionlib/client/action_client.h>
+#include <Eigen/Dense>
+
+namespace aikido{
+namespace control {
+namespace ros {
+
+/// This class sends commands for the hand position to the ROS server
+class RosBarrettHandPositionCommandExecutor
+{
+public:
+  /// Constructor
+  /// \param[in] node ROS node handle for action client.
+  /// \param[in] serverName Name of the server to send traejctory to.
+  /// \param[in] connectionTimeout Timeout for server connection.
+  /// \param[in] connectionPollingPeriod Polling period for server connection.
+  template <typename DurationA, typename DurationB>
+  RosBarrettHandPositionCommandExecutor(
+    ::ros::NodeHandle node,
+    const std::string& serverName,
+    const DurationA& connectionTimeout = std::chrono::milliseconds{1000},
+    const DurationB& connectionPollingPeriod = std::chrono::milliseconds{20}
+  );
+
+  ~RosBarrettHandPositionCommandExecutor();
+
+  /// Sends positions to ROS server for execution.
+  /// \param[in] goalPositions Vector of 3 finger positions and 1 spread position
+  /// \param[in] jointNames Vector of 4 joint names for fingers
+  std::future<void> execute(Eigen::VectorXd& goalPositions,std::vector<std::string>& jointNames);
+
+  /// Sends positions to ROS server for execution.
+  /// \param[in] goalPositions Vector of 3 finger positions and 1 spread position
+  /// \param[in] jointNames Vector of 4 joint names for fingers
+  /// \param[in] startTime Start time for the trajectory.
+  std::future<void> execute(
+    Eigen::VectorXd& goalPositions, 
+    std::vector<std::string>& jointNames,
+    const ::ros::Time& startTime);
+
+  void step();
+
+private:
+  using HandPositionActionClient
+    = actionlib::ActionClient<pr_control_msgs::SetPositionAction>;
+  using GoalHandle = HandPositionActionClient::GoalHandle;
+
+  bool waitForServer();
+
+  void transitionCallback(GoalHandle handle);
+
+  ::ros::NodeHandle mNode;
+  ::ros::CallbackQueue mCallbackQueue;
+  HandPositionActionClient mClient;
+  HandPositionActionClient::GoalHandle mGoalHandle;
+
+  std::chrono::milliseconds mConnectionTimeout;
+  std::chrono::milliseconds mConnectionPollingPeriod;
+
+  bool mInProgress;
+  std::promise<void> mPromise;
+  Eigen::VectorXd mGoalPositions;
+
+  std::mutex mMutex;
+};
+
+} // namespace ros
+} // namespace control
+} // namespace aikido
+
+#endif // ifndef AIKIDO_CONTROL_ROS_ROSBARRETTHANDPOSITIONCOMMANDEXECUTOR_HPP_

src/control/ros/CMakeLists.txt

@@ -34,6 +34,7 @@ set(sources
   RosTrajectoryExecutionException.cpp
   Conversions.cpp
   RosJointStateClient.cpp
+  RosBarrettHandPositionCommandExecutor.cpp
 )
 
 add_library("${PROJECT_NAME}_control_ros" SHARED ${sources})

src/control/ros/Conversions.cpp

@@ -429,6 +429,36 @@ trajectory_msgs::JointTrajectory toRosJointTrajectory(
   return jointTrajectory;
 }
 
+//=============================================================================
+sensor_msgs::JointState toJointState(
+  const Eigen::VectorXd& goalPositions, const std::vector<std::string> jointNames)
+{
+  if (goalPositions.size() != jointNames.size())
+  {
+    std::stringstream message;
+    message << "The size of goalPositions must be the same as jointNames!" ;
+    throw std::invalid_argument(message.str());
+  }
+
+  sensor_msgs::JointState jointState;
+
+  const auto numJoints = jointNames.size();
+
+  jointState.name.reserve(numJoints);
+  jointState.position.reserve(numJoints);
+
+  for (size_t i = 0; i < numJoints ; ++i)
+  {
+    jointState.name.emplace_back(jointNames[i]);
+    jointState.position.emplace_back(goalPositions[i]);
+  }
+
+  return jointState;
+
+}
+
+
+
 } // namespace ros
 } // namespace control
 } // namespace aikido

src/control/ros/RosBarrettHandPositionCommandExecutor.cpp

@@ -0,0 +1,181 @@
+#include <aikido/control/ros/RosBarrettHandPositionCommandExecutor.hpp>
+#include <aikido/control/ros/Conversions.hpp>
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+using std::chrono::milliseconds;
+
+//=============================================================================
+template <typename DurationA, typename DurationB>
+RosBarrettHandPositionCommandExecutor::RosBarrettHandPositionCommandExecutor(
+      ::ros::NodeHandle node,
+      const std::string& serverName,
+      const DurationA& connectionTimeout,
+      const DurationB& connectionPollingPeriod)
+  : mNode(std::move(node))
+  , mCallbackQueue{}
+  , mClient{mNode, serverName, &mCallbackQueue}
+  , mConnectionTimeout{
+    std::chrono::duration_cast<milliseconds>(connectionTimeout)}
+  , mConnectionPollingPeriod{
+    std::chrono::duration_cast<milliseconds>(connectionPollingPeriod)}
+  , mInProgress{false}
+{
+}
+
+//=============================================================================
+RosBarrettHandPositionCommandExecutor::~RosBarrettHandPositionCommandExecutor()
+{
+  // Do nothing.
+  // TODO: Should we wait for the current trajectory to finish executing?
+}
+
+//=============================================================================
+std::future<void> RosBarrettHandPositionCommandExecutor::execute(
+  Eigen::VectorXd& goalPositions,
+  std::vector<std::string>& jointNames)
+{
+  static const ::ros::Time invalidTime;
+  return execute(goalPositions, jointNames, invalidTime);
+}
+
+//=============================================================================
+std::future<void> RosBarrettHandPositionCommandExecutor::execute(
+  Eigen::VectorXd& goalPositions, 
+  std::vector<std::string>& jointNames,
+  const ::ros::Time& startTime)
+{
+  if (goalPositions.size() != 4)
+  {
+    std::stringstream message;
+    message << "GoalPositions must have 4 elements, but ["
+      << goalPositions.size() << "] given.";
+    throw std::invalid_argument(message.str());
+  }
+
+  pr_control_msgs::SetPositionGoal goal;
+
+  // Convert goal positions and joint names to jointstate
+  goal.command = toJointState(goalPositions,jointNames);
+
+  goal.command.header.stamp = startTime;
+
+  if (!waitForServer())
+    throw std::runtime_error("Unable to connect to action server.");
+
+  {
+    std::lock_guard<std::mutex> lock(mMutex);
+    DART_UNUSED(lock); // Suppress unused variable warning
+
+    if (mInProgress)
+      throw std::runtime_error("Another trajectory is in progress.");
+
+    mPromise = std::promise<void>();
+    mInProgress = true;
+    mGoalHandle = mClient.sendGoal(goal, 
+      boost::bind(&RosBarrettHandPositionCommandExecutor::transitionCallback, this, _1));
+
+    return mPromise.get_future();
+  }
+
+}
+
+//=============================================================================
+// TODO : This is directly reused from RosTrajectoryExecutor.cpp
+// Anyway to reuse this? (assuming we want this here)
+bool RosBarrettHandPositionCommandExecutor::waitForServer()
+{
+  using Clock = std::chrono::steady_clock;
+
+  const auto startTime = Clock::now();
+  const auto endTime = startTime + mConnectionTimeout;
+  auto currentTime = startTime + mConnectionPollingPeriod;
+
+  while (currentTime < endTime)
+  {
+    mCallbackQueue.callAvailable();
+
+    // TODO: Is this thread safe?
+    if (mClient.isServerConnected())
+      return true;
+
+    currentTime += mConnectionPollingPeriod;
+    std::this_thread::sleep_until(currentTime);
+  }
+
+  return false;
+}
+
+//=============================================================================
+void RosBarrettHandPositionCommandExecutor::transitionCallback(GoalHandle handle)
+{
+
+  using actionlib::TerminalState;
+  using Result = pr_control_msgs::SetPositionResult;
+
+  if (handle.getCommState() == actionlib::CommState::DONE)
+  {
+    std::stringstream message;
+    bool isSuccessful = true;
+
+    const auto terminalState = handle.getTerminalState();
+    if (terminalState != TerminalState::SUCCEEDED)
+    {
+      message << "Action " << terminalState.toString();
+
+      const auto terminalMessage = terminalState.getText();
+      if (!terminalMessage.empty())
+        message << " (" << terminalMessage << ")";
+
+      mPromise.set_exception(std::make_exception_ptr(
+          std::runtime_error(message.str())));
+
+      isSuccessful = false;
+    }
+    else{
+      message << "Execution failed.";
+    }
+
+    const auto result = handle.getResult();
+    if (result && result->success == false)
+    {
+      if (!result->message.empty())
+        message << " (" << result->message << ")";
+
+      mPromise.set_exception(std::make_exception_ptr(
+          std::runtime_error(message.str())));
+
+      isSuccessful = false;
+    }
+
+    if (isSuccessful)
+      mPromise.set_value();
+
+    mInProgress = false;
+
+  }
+}
+
+//=============================================================================
+// TODO : This is also basically copied from RosTrajectoryExecutor
+void RosBarrettHandPositionCommandExecutor::step()
+{
+  std::lock_guard<std::mutex> lock(mMutex);
+  DART_UNUSED(lock); // Suppress unused variable warning.
+
+  mCallbackQueue.callAvailable();
+
+  if (!::ros::ok() && mInProgress)
+  {
+    mPromise.set_exception(
+      std::make_exception_ptr(std::runtime_error("Detected ROS shutdown.")));
+    mInProgress = false;
+  }
+}
+
+
+} // namespace ros
+} // namespace control
+} // namespace aikido