cartesian_trajectory_controller.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

#include <cartesian_trajectory_controller/control_policies.h>
#include <cartesian_trajectory_interpolation/cartesian_trajectory.h>
#include <cartesian_trajectory_interpolation/cartesian_state.h>
#include <cartesian_control_msgs/FollowCartesianTrajectoryAction.h>
#include <cartesian_control_msgs/CartesianTolerance.h>
#include <atomic>
#include <mutex>
#include <actionlib/server/simple_action_server.h>
#include <speed_scaling_interface/speed_scaling_interface.h>

namespace cartesian_trajectory_controller
{
template <class HWInterface>
class CartesianTrajectoryController : public ros_controllers_cartesian::ControlPolicy<HWInterface>
{
public:
  CartesianTrajectoryController() : ros_controllers_cartesian::ControlPolicy<HWInterface>(){};

  virtual ~CartesianTrajectoryController(){};

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

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

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

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

  void executeCB(const cartesian_control_msgs::FollowCartesianTrajectoryGoalConstPtr& goal);

  void preemptCB();

protected:
  using ControlPolicy = ros_controllers_cartesian::ControlPolicy<HWInterface>;

  struct TrajectoryDuration
  {
    TrajectoryDuration() : now(0.0), end(0.0)
    {
    }

    ros::Duration end;  
    ros::Duration now;  
  };

  void timesUp();

  void monitorExecution(const ros_controllers_cartesian::CartesianState& error);

  bool withinTolerances(const ros_controllers_cartesian::CartesianState& error,
                        const cartesian_control_msgs::CartesianTolerance& tolerance);

private:
  std::unique_ptr<scaled_controllers::SpeedScalingHandle> speed_scaling_;
  std::unique_ptr<actionlib::SimpleActionServer<cartesian_control_msgs::FollowCartesianTrajectoryAction> >
      action_server_;
  std::atomic<bool> done_;
  std::mutex lock_;
  ros_controllers_cartesian::CartesianTrajectory trajectory_;
  TrajectoryDuration trajectory_duration_;
  cartesian_control_msgs::CartesianTolerance path_tolerances_;
  cartesian_control_msgs::CartesianTolerance goal_tolerances_;
};

}  // namespace cartesian_trajectory_controller

#include <cartesian_trajectory_controller/cartesian_trajectory_controller.hpp>