ros_control/RosJointStateClient (#150)

* Initial commit for RosJointStateClient

* Add some documentation.

* Address Mike's comments.

include/aikido/control/ros/RosJointStateClient.hpp

@@ -0,0 +1,72 @@
+#ifndef AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_
+#define AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_
+#include <string>
+#include <boost/circular_buffer.hpp>
+#include <dart/dynamics/dynamics.hpp>
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <sensor_msgs/JointState.h>
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+/// Client that listens for JointState messages for each skeleton joint and
+/// provides a method for extracting the most recent position of each joint.
+class RosJointStateClient
+{
+public:
+  /// Constructor.
+  /// \param _skeleton Skeleton to read JointState updates for.
+  /// \param _nodeHandle ROS node.
+  /// \param _topicName Name of topic to subscribe to for JointState updates.
+  /// \param _capacity Number of JointStateRecords that are saved per joint.
+  RosJointStateClient(
+    dart::dynamics::SkeletonPtr _skeleton,
+    ::ros::NodeHandle _nodeHandle,
+    const std::string& _topicName,
+    size_t capacity);
+
+  /// Update mBuffer with any JointState messages that have been received.
+  void spin();
+
+  /// Returns the most recent position of each joint in _metaSkeleton.
+  /// \param _metaSkeleton Skeleton to read DOFs from.
+  /// \return vector of positions for each DOF
+  Eigen::VectorXd getLatestPosition(
+    const dart::dynamics::MetaSkeleton& _metaSkeleton) const;
+
+  // TODO: implement
+  // getPositionAtTime(const MetaSkeleton&, const ros::Time&, bool)
+  // that interpolates position at the specified time, optionally blocking for
+  // new data.
+private:
+  struct JointStateRecord
+  {
+    inline bool isValid() const { return mStamp.isValid(); }
+
+    ::ros::Time mStamp;
+    double mPosition;
+  };
+
+  /// Callback to add a new JointState to mBuffer
+  /// \param _jointState New JointState to add to mBuffer
+  void jointStateCallback(const sensor_msgs::JointState& _jointState);
+
+  mutable std::mutex mMutex;
+
+  dart::dynamics::SkeletonPtr mSkeleton;
+  std::unordered_map<std::string,
+    boost::circular_buffer<JointStateRecord>> mBuffer;
+  size_t mCapacity;
+
+  ::ros::CallbackQueue mCallbackQueue;
+  ::ros::NodeHandle mNodeHandle;
+  ::ros::Subscriber mSubscriber;
+};
+
+} // namespace ros
+} // namespace control
+} // namespace aikido
+
+#endif // ifndef AIKIDO_CONTROL_ROS_ROSJOINTSTATECLIENT_HPP_

src/control/ros/CMakeLists.txt

@@ -1,6 +1,15 @@
 #==============================================================================
 # 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")
+
 find_package(trajectory_msgs QUIET)
 aikido_check_package(trajectory_msgs "aikido::control::ros" "trajectory_msgs")
 
@@ -24,11 +33,15 @@ set(sources
   RosTrajectoryExecutor.cpp
   RosTrajectoryExecutionException.cpp
   Conversions.cpp
+  RosJointStateClient.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}
     ${trajectory_msgs_INCLUDE_DIRS}
     ${actionlib_INCLUDE_DIRS}
     ${control_msgs_INCLUDE_DIRS}
@@ -40,6 +53,9 @@ target_link_libraries("${PROJECT_NAME}_control_ros"
   "${PROJECT_NAME}_statespace"
   "${PROJECT_NAME}_trajectory"
   ${DART_LIBRARIES}
+  ${actionlib_LIBRARIES}
+  ${control_msgs_LIBRARIES}
+  ${roscpp_LIBRARIES}
   ${trajectory_msgs_LIBRARIES}
   ${actionlib_LIBRARIES}
   ${control_msgs_LIBRARIES}

src/control/ros/RosJointStateClient.cpp

@@ -0,0 +1,116 @@
+#include <aikido/control/ros/RosJointStateClient.hpp>
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+//=============================================================================
+RosJointStateClient::RosJointStateClient(
+      dart::dynamics::SkeletonPtr _skeleton,
+      ::ros::NodeHandle _nodeHandle,
+      const std::string& _topicName,
+      size_t capacity)
+  : mSkeleton{std::move(_skeleton)}
+  , mBuffer{}
+  , mCapacity{capacity}
+  , mCallbackQueue{} // Must be after mNodeHandle for order of destruction.
+  , mNodeHandle{std::move(_nodeHandle)}
+{
+  if (!mSkeleton)
+    throw std::invalid_argument("Skeleton is null.");
+
+  if (capacity < 1)
+    throw std::invalid_argument("Capacity must be positive.");
+
+  mNodeHandle.setCallbackQueue(&mCallbackQueue);
+  mSubscriber = mNodeHandle.subscribe(_topicName, 1,
+    &RosJointStateClient::jointStateCallback, this);
+}
+
+//=============================================================================
+void RosJointStateClient::spin()
+{
+  std::lock_guard<std::mutex> skeletonLock{mSkeleton->getMutex()};
+  std::lock_guard<std::mutex> bufferLock{mMutex};
+
+  mCallbackQueue.callAvailable();
+}
+
+//=============================================================================
+Eigen::VectorXd RosJointStateClient::getLatestPosition(
+  const dart::dynamics::MetaSkeleton& _metaSkeleton) const
+{
+  std::lock_guard<std::mutex> bufferLock{mMutex};
+  Eigen::VectorXd position(_metaSkeleton.getNumDofs());
+
+  for (size_t idof = 0; idof < _metaSkeleton.getNumDofs(); ++idof)
+  {
+    const auto dof = _metaSkeleton.getDof(idof);
+    const auto it = mBuffer.find(dof->getName());
+    if (it == std::end(mBuffer))
+    {
+      std::stringstream msg;
+      msg << "No data is available for '" << dof->getName() << "'.";
+      throw std::runtime_error(msg.str());
+    }
+
+    const auto& buffer = it->second;
+    if (buffer.empty())
+    {
+      std::stringstream msg;
+      msg << "Data for '" << dof->getName() << "' is invalid.";
+      throw std::runtime_error(msg.str());
+    }
+
+    const auto& record = buffer.back();
+    position[idof] = record.mPosition;
+  }
+
+  return position;
+}
+
+//=============================================================================
+void RosJointStateClient::jointStateCallback(
+  const sensor_msgs::JointState& _jointState)
+{
+  // This method assumes that mSkeleton->getMutex() and mMutex are locked.
+
+  if (_jointState.position.size() != _jointState.name.size())
+  {
+    ROS_WARN_STREAM("Incorrect number of positions: expected "
+      << _jointState.name.size() << ", got "
+      << _jointState.position.size() << ".");
+    return;
+  }
+  // TODO: Also check for velocities.
+
+  for (size_t i = 0; i < _jointState.name.size(); ++i)
+  {
+    const auto result = mBuffer.emplace(_jointState.name[i],
+      boost::circular_buffer<JointStateRecord>{mCapacity});
+    auto& buffer = result.first->second;
+
+    if (_jointState.header.stamp < buffer.back().mStamp)
+    {
+      // Ignore out of order JointState message.
+      ROS_WARN_STREAM("Ignoring out of order message: received timestamp of "
+                      << _jointState.header.stamp << " is before previously "
+                      << "received timestamp of " << buffer.back().mStamp);
+
+      continue;
+    }
+
+    JointStateRecord record;
+    record.mStamp = _jointState.header.stamp;
+    record.mPosition = _jointState.position[i];
+
+    // TODO: check that the data in buffer is sequential, i.e. that we did not
+    // receive JointState messages out of order. It's probably safe to ignore
+    // the message (and print a warning!) if this occurs.
+    buffer.push_back(record);
+  }
+}
+
+} // namespace ros
+} // namespace control
+} // namespace aikido