pass_through_controllers.h

Go to the documentation of this file.

// -- BEGIN LICENSE BLOCK ----------------------------------------------
// Copyright 2020 FZI Forschungszentrum Informatik
// Created on behalf of Universal Robots A/S
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
// -- END LICENSE BLOCK ------------------------------------------------

//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------

#pragma once

// ROS control
#include <speed_scaling_interface/speed_scaling_interface.h>
#include <controller_interface/multi_interface_controller.h>
#include <hardware_interface/robot_hw.h>
#include <pass_through_controllers/trajectory_interface.h>

// Joint-based
#include <control_msgs/FollowJointTrajectoryGoal.h>
#include <control_msgs/JointTolerance.h>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include <control_msgs/FollowJointTrajectoryAction.h>

// Cartesian
#include <cartesian_control_msgs/CartesianTolerance.h>
#include <cartesian_control_msgs/CartesianTrajectoryPoint.h>
#include <cartesian_control_msgs/FollowCartesianTrajectoryGoal.h>
#include <cartesian_control_msgs/FollowCartesianTrajectoryAction.h>

// Other
#include <actionlib/server/simple_action_server.h>
#include <vector>
#include <memory>
#include <atomic>

namespace trajectory_controllers
{
struct JointBase
{
  using Tolerance = std::vector<control_msgs::JointTolerance>;
  using TrajectoryPoint = trajectory_msgs::JointTrajectoryPoint;
  using TrajectoryFeedback = hardware_interface::JointTrajectoryFeedback;
  using FollowTrajectoryAction = control_msgs::FollowJointTrajectoryAction;
  using FollowTrajectoryResult = control_msgs::FollowJointTrajectoryResult;
  using GoalConstPtr = control_msgs::FollowJointTrajectoryGoalConstPtr;
};

struct CartesianBase
{
  using Tolerance = cartesian_control_msgs::CartesianTolerance;
  using TrajectoryPoint = cartesian_control_msgs::CartesianTrajectoryPoint;
  using TrajectoryFeedback = hardware_interface::CartesianTrajectoryFeedback;
  using FollowTrajectoryAction = cartesian_control_msgs::FollowCartesianTrajectoryAction;
  using FollowTrajectoryResult = cartesian_control_msgs::FollowCartesianTrajectoryResult;
  using GoalConstPtr = cartesian_control_msgs::FollowCartesianTrajectoryGoalConstPtr;
};

template <class TrajectoryInterface>
class PassThroughController
  : public controller_interface::MultiInterfaceController<TrajectoryInterface,
                                                          scaled_controllers::SpeedScalingInterface>,
    public std::conditional<std::is_same<TrajectoryInterface, hardware_interface::JointTrajectoryInterface>::value,
                            JointBase, CartesianBase>::type
{
public:
  PassThroughController()
    : controller_interface::MultiInterfaceController<TrajectoryInterface,
                                                     scaled_controllers::SpeedScalingInterface>(
          true)  // Make speed scaling optional
  {
  }

  // Alias for full qualifications of inherited types.
  // This enables a compact definition of member functions for both joint-based
  // and Cartesian-based PassThroughControllers.
  using Base =
      typename std::conditional<std::is_same<TrajectoryInterface, hardware_interface::JointTrajectoryInterface>::value,
                                JointBase, CartesianBase>::type;

  bool init(hardware_interface::RobotHW* hw, ros::NodeHandle& root_nh, ros::NodeHandle& controller_nh);

  void starting(const ros::Time& time);

  void stopping(const ros::Time& time);

  void update(const ros::Time& time, const ros::Duration& period);

  void executeCB(const typename Base::GoalConstPtr& goal);

  void preemptCB();

private:
  struct ActionDuration
  {
    ActionDuration() : target(0.0), current(0.0)
    {
    }

    ros::Duration target;   
    ros::Duration current;  
  };

  void monitorExecution(const typename Base::TrajectoryFeedback& feedback);

  bool withinTolerances(const typename Base::TrajectoryPoint& error, const typename Base::Tolerance& tolerances);

  bool isValid(const typename Base::GoalConstPtr& goal);

  void doneCB(const hardware_interface::ExecutionState& state);

  std::atomic<bool> done_;
  ActionDuration action_duration_;
  std::unique_ptr<scaled_controllers::SpeedScalingHandle> speed_scaling_;
  std::vector<std::string> joint_names_;
  typename Base::Tolerance path_tolerances_;
  typename Base::Tolerance goal_tolerances_;
  TrajectoryInterface* trajectory_interface_;  
  std::unique_ptr<actionlib::SimpleActionServer<typename Base::FollowTrajectoryAction> > action_server_;
};

}  // namespace trajectory_controllers

#include <pass_through_controllers/pass_through_controllers.hpp>