RosTrajectoryExecutor.hpp

Go to the documentation of this file.

#ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_
#define AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_
 
#include <chrono>
#include <future>
#include <mutex>
 
#include <actionlib/client/action_client.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <ros/callback_queue.h>
#include <ros/ros.h>
 
#include "aikido/control/TrajectoryExecutor.hpp"
#include "aikido/trajectory/Trajectory.hpp"
 
namespace aikido {
namespace control {
namespace ros {
 
class RosTrajectoryExecutor : public aikido::control::TrajectoryExecutor
{
public:
  RosTrajectoryExecutor(
      const ::ros::NodeHandle& node,
      const std::string& serverName,
      double waypointTimestep,
      double goalTimeTolerance,
      const ::dart::dynamics::MetaSkeletonPtr metaskeleton,
      const std::chrono::milliseconds& connectionTimeout
      = std::chrono::milliseconds{1000},
      const std::chrono::milliseconds& connectionPollingPeriod
      = std::chrono::milliseconds{20});
 
  virtual ~RosTrajectoryExecutor();
 
  // Documentation inherited.
  void validate(const trajectory::Trajectory* traj) override;
 
  std::future<void> execute(
      const trajectory::ConstTrajectoryPtr& traj) override;
 
  std::future<void> execute(
      const trajectory::ConstTrajectoryPtr& traj, const ::ros::Time& startTime);
 
  void step(const std::chrono::system_clock::time_point& timepoint) override;
 
  void cancel() override;
 
private:
  using TrajectoryActionClient
      = actionlib::ActionClient<control_msgs::FollowJointTrajectoryAction>;
  using GoalHandle = TrajectoryActionClient::GoalHandle;
 
  void transitionCallback(GoalHandle handle);
 
  const ::dart::dynamics::MetaSkeletonPtr mMetaSkeleton;
 
  ::ros::NodeHandle mNode;
  ::ros::CallbackQueue mCallbackQueue;
  TrajectoryActionClient mClient;
  TrajectoryActionClient::GoalHandle mGoalHandle;
 
  double mWaypointTimestep;
  double mGoalTimeTolerance;
 
  std::chrono::milliseconds mConnectionTimeout;
  std::chrono::milliseconds mConnectionPollingPeriod;
 
  bool mInProgress;
  std::unique_ptr<std::promise<void>> mPromise;
 
  mutable std::mutex mMutex;
};
 
} // namespace ros
} // namespace control
} // namespace aikido
 
#endif // ifndef AIKIDO_CONTROL_ROS_ROSTRAJECTORYEXECUTOR_HPP_