Add RosPositionCommandExecutor (#173)

* Renaming, adding an abstract BarretHandPositionCommandExecutor class

* Adding -override

* Exposing step

* Renaiming PositionCommandExecutor

* Changing header files

* Changing names and step function.

* Update simulation speed constants and matrix->vector methods.

* Add collisionDetector argument. Make collideWith an optional argument.

* Don't reset mCollideWith every time.

* Simplify logic of optional collision detector and collision groups.

Duplicate changes in BarrettHandKinematicSimulationPositionCommandExecutor.

* Add ShapeFrames of original collision group to new collision group.

* Adding RosBarrettHandPositionCommandExecutor to CMakeLists of aikido/control/ros

* Adding conversion from goal positions and joint names to sensor_msgs/JointState

* Ros executor for hand commands

* Make collisionDetector, collideWith optional.

* Instantiate FingerPositionCommandExecutors and FingerSpreadCommandExecutor in HandPositionCommandExecutor.

Adapted from https://github.com/personalrobotics/libherb/pull/20.

* Clean up some documentation.

* Remove parameter from step.

* Set mCollisionDetector when setting mCollideWith.

* Make FingerCommandExecutors implement PositionCommandExecutor.

* Swap argument order in HandPositionExecutor.

* Partial test fixes.

Make test_BarrettFinger* compile (although actual execution seems to fail).
Make part of test_BarrettHand compile, although introducing `robot` as a
parameter seems like it will make testing this difficult.

* Adding time-reset when a new execute command is given.

* Update expected values in test to make them pass.

Decrease expected proximal value in BarrettFingerKinematicSimulationPositionCommandExecutorTest.execute_proximalStopsAtCollsionDistalContinuesUntilCollision

* Renamed RosBarrettHandPositionCommandExecutor to RosPositionCommandExecutor; moved jointNames to constructor, made it inherit from PositionCommandExecutor

* Changed methods to match header file changes, addressed Mike's other comments:

* Edited CMakeLists of control/ros to reflect new filename

* Edited the toJointState method

* Created waitForActionServer helper function

* Deleted renamed files

* Adding detail/util-impl in the necessary convention

* Taking joint names by non-const value

* Addressed rest of Mike
's nits

* Better docstring for class

* Fixed incorrect currentTime

* Taking joint names by non-const value in constructor

* Renaming toJointState to positionsToJointState

* Fixing signed with unsigned check

* Resolve compilation issues:

- Use find module for pr_control_msgs
- Fix unmatched function signature in Conversions
- Fix function termination without return value
- Fix unordered initialization list

* Remove debug code

* Add pr_control_msgs to package.xml

* Clean up ros/CMakeLists.txt

cmake/Findpr_control_msgs.cmake

@@ -0,0 +1,33 @@
+# Find pr_control_msgs
+#
+# This sets the following variables:
+# pr_control_msgs_FOUND
+# pr_control_msgs_INCLUDE_DIRS
+# pr_control_msgs_VERSION
+
+# Note: This find module is necessary because the config file imports "gtest",
+# "tests", and "run_tests" that conflict with the targets defined by Aikido.
+
+find_package(PkgConfig QUIET REQUIRED)
+
+# Check to see if pkgconfig is installed.
+set(PKG_CONFIG_USE_CMAKE_PREFIX_PATH TRUE)
+pkg_check_modules(PC_pr_control_msgs pr_control_msgs QUIET)
+
+# Include directories
+find_path(pr_control_msgs_INCLUDE_DIRS
+  NAMES pr_control_msgs/TriggerResult.h
+  HINTS ${PC_pr_control_msgs_LIBRARY_DIRS}
+)
+
+# Version
+set(pr_control_msgs_VERSION ${PC_pr_control_msgs_VERSION})
+
+# Set (NAME)_FOUND if all the variables and the version are satisfied.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(pr_control_msgs
+    FOUND_VAR pr_control_msgs_FOUND
+    FAIL_MESSAGE  DEFAULT_MSG
+    REQUIRED_VARS pr_control_msgs_INCLUDE_DIRS
+    VERSION_VAR   pr_control_msgs_VERSION
+)

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 positionsToJointState(
+  const Eigen::VectorXd& goalPositions, const std::vector<std::string>& jointNames);
+
+
 } // namespace ros
 } // namespace control
 } // namespace aikido

include/aikido/control/ros/RosPositionCommandExecutor.hpp

@@ -0,0 +1,76 @@
+#ifndef AIKIDO_CONTROL_ROS_ROSPOSITIONCOMMANDEXECUTOR_HPP_
+#define AIKIDO_CONTROL_ROS_ROSPOSITIONCOMMANDEXECUTOR_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 <aikido/control/PositionCommandExecutor.hpp>
+#include <Eigen/Dense>
+
+namespace aikido{
+namespace control {
+namespace ros {
+
+/// This class uses actionlib to command an action of the type
+/// pr_control_msgs/SetPosition. It specifies a set of target
+/// positions and sends it to the ROS server for execution 
+class RosPositionCommandExecutor : public aikido::control::PositionCommandExecutor
+{
+public:
+  /// Constructor
+  /// \param[in] node ROS node handle for action client.
+  /// \param[in] serverName Name of the server to send trajectory to.
+  /// \param[in] The names of the joints to set position targets for
+  /// \param[in] connectionTimeout Timeout for server connection.
+  /// \param[in] connectionPollingPeriod Polling period for server connection.
+  RosPositionCommandExecutor(
+    ::ros::NodeHandle node,
+    const std::string& serverName,
+    std::vector<std::string> jointNames,
+    std::chrono::milliseconds connectionTimeout = std::chrono::milliseconds{1000},
+    std::chrono::milliseconds connectionPollingPeriod = std::chrono::milliseconds{20}
+  );
+
+  virtual ~RosPositionCommandExecutor();
+
+  /// Sends positions to ROS server for execution.
+  /// \param[in] goalPositions Vector of target positions for each joint
+  std::future<void> execute(const Eigen::VectorXd& goalPositions) override;
+
+
+  void step() override;
+
+private:
+  using RosPositionActionClient
+    = actionlib::ActionClient<pr_control_msgs::SetPositionAction>;
+  using GoalHandle = RosPositionActionClient::GoalHandle;
+
+  bool waitForServer();
+
+  void transitionCallback(GoalHandle handle);
+
+  ::ros::NodeHandle mNode;
+  ::ros::CallbackQueue mCallbackQueue;
+  RosPositionActionClient mClient;
+  RosPositionActionClient::GoalHandle mGoalHandle;
+
+  std::chrono::milliseconds mConnectionTimeout;
+  std::chrono::milliseconds mConnectionPollingPeriod;
+
+  bool mInProgress;
+  std::promise<void> mPromise;
+  Eigen::VectorXd mGoalPositions;
+  std::vector<std::string> mJointNames;
+
+  std::mutex mMutex;
+};
+
+} // namespace ros
+} // namespace control
+} // namespace aikido
+
+#endif // ifndef AIKIDO_CONTROL_ROS_ROSPOSITIONCOMMANDEXECUTOR_HPP_

include/aikido/control/ros/detail/util-impl.hpp

@@ -0,0 +1,42 @@
+#include <chrono>
+#include <mutex>
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <actionlib/client/action_client.h>
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+template <class ActionSpec, class TimeoutDuration, class PeriodDuration>
+bool waitForActionServer(
+  actionlib::ActionClient<ActionSpec>& actionClient,
+  ::ros::CallbackQueue& callbackQueue,
+  TimeoutDuration timeoutDuration = std::chrono::milliseconds{ 1000 },
+  PeriodDuration periodDuration = std::chrono::milliseconds{ 10 }
+)
+{
+  using Clock = std::chrono::steady_clock;
+
+  const auto startTime = Clock::now();
+  const auto endTime = startTime + timeoutDuration;
+  auto currentTime = startTime;
+
+  while(currentTime < endTime)
+  {
+    callbackQueue.callAvailable();
+
+    // TODO : Is this thread safe?
+    if (actionClient.isServerConnected())
+      return true;
+
+    currentTime += periodDuration;
+    std::this_thread::sleep_until(currentTime);
+  }
+
+  return false;
+}
+
+} // namespace ros
+} // namespace control
+} // namespace aikido

include/aikido/control/ros/util.hpp

@@ -0,0 +1,26 @@
+#ifndef AIKIDO_CONTROL_ROS_UTIL_HPP_
+#define AIKIDO_CONTROL_ROS_UTIL_HPP_
+#include <chrono>
+#include <ros/ros.h>
+#include <ros/callback_queue.h>
+#include <actionlib/client/action_client.h>
+
+namespace aikido {
+namespace control {
+namespace ros {
+
+template <class ActionSpec, class TimeoutDuration, class PeriodDuration>
+bool waitForActionServer(
+  actionlib::ActionClient<ActionSpec>& actionClient,
+  ::ros::CallbackQueue& callbackQueue,
+  TimeoutDuration timeoutDuration = std::chrono::milliseconds{ 1000 },
+  PeriodDuration periodDuration = std::chrono::milliseconds{ 10 } );
+
+
+} // namespace ros
+} // namespace control
+} // namespace aikido
+
+#include "detail/util-impl.hpp"
+
+#endif // AIKIDO_CONTROL_ROS_UTIL_HPP_

package.xml

@@ -19,6 +19,7 @@
   <depend>control_msgs</depend>
   <depend>libmicrohttpd</depend>
   <depend>interactive_markers</depend>
+  <depend>pr_control_msgs</depend>
   <doc_depend>doxygen</doc_depend>
   <!-- Workaround to build DART with optional features enabled. -->
   <depend>nlopt</depend>

src/control/ros/CMakeLists.txt

@@ -1,12 +1,6 @@
 #==============================================================================
 # 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")
 
@@ -23,8 +17,10 @@ aikido_check_package(actionlib "aikido::control::ros" "actionlib")
 find_package(control_msgs QUIET MODULE)
 aikido_check_package(control_msgs "aikido::control::ros" "control_msgs")
 
-find_package(roscpp QUIET)
-aikido_check_package(roscpp "aikido::control::ros" "roscpp")
+# Note: Intentionally use "Module" mode because the config file imports "gtest",
+# "tests", and "run_tests" that conflict with the targets defined by Aikido.
+find_package(pr_control_msgs QUIET MODULE)
+aikido_check_package(pr_control_msgs "aikido::control::ros" "pr_control_msgs")
 
 #==============================================================================
 # Libraries
@@ -34,6 +30,7 @@ set(sources
   RosTrajectoryExecutionException.cpp
   Conversions.cpp
   RosJointStateClient.cpp
+  RosPositionCommandExecutor.cpp
 )
 
 add_library("${PROJECT_NAME}_control_ros" SHARED ${sources})
@@ -46,6 +43,7 @@ target_include_directories("${PROJECT_NAME}_control_ros" SYSTEM
     ${actionlib_INCLUDE_DIRS}
     ${control_msgs_INCLUDE_DIRS}
     ${roscpp_INCLUDE_DIRS}
+    ${pr_control_msgs_INCLUDE_DIRS}
 )
 
 target_link_libraries("${PROJECT_NAME}_control_ros"

src/control/ros/Conversions.cpp

@@ -429,6 +429,29 @@ trajectory_msgs::JointTrajectory toRosJointTrajectory(
   return jointTrajectory;
 }
 
+//=============================================================================
+sensor_msgs::JointState positionsToJointState(
+  const Eigen::VectorXd& goalPositions, const std::vector<std::string>& jointNames)
+{
+  if ((size_t)goalPositions.size() != jointNames.size())
+  {
+    std::stringstream message;
+    message << "The size of goalPositions ("<<goalPositions.size()<<
+      ") must be the same as jointNames ("<<jointNames.size()<<")!" ;
+    throw std::invalid_argument(message.str());
+  }
+
+  sensor_msgs::JointState jointState;
+
+  jointState.name = std::move(jointNames);
+  jointState.position.assign(goalPositions.data(), goalPositions.data() + goalPositions.size());
+
+  return jointState;
+
+}
+
+
+
 } // namespace ros
 } // namespace control
 } // namespace aikido