trajectory_interface.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 <functional>
#include <cartesian_control_msgs/FollowCartesianTrajectoryGoal.h>
#include <cartesian_control_msgs/FollowCartesianTrajectoryResult.h>
#include <cartesian_control_msgs/FollowCartesianTrajectoryFeedback.h>
#include <control_msgs/FollowJointTrajectoryGoal.h>
#include <control_msgs/FollowJointTrajectoryFeedback.h>
#include <vector>
#include <hardware_interface/hardware_interface.h>
#include <ros/ros.h>

namespace hardware_interface
{
enum class ExecutionState
{
  SUCCESS = 0,
  PREEMPTED = -1,
  ABORTED = -2,
};

using JointTrajectory = control_msgs::FollowJointTrajectoryGoal;

using CartesianTrajectory = cartesian_control_msgs::FollowCartesianTrajectoryGoal;

using JointTrajectoryFeedback = control_msgs::FollowJointTrajectoryFeedback;

using CartesianTrajectoryFeedback = cartesian_control_msgs::FollowCartesianTrajectoryFeedback;

template <class TrajectoryType, class FeedbackType>
class TrajectoryInterface : public hardware_interface::HardwareInterface
{
public:
  void registerGoalCallback(std::function<void(const TrajectoryType&)> f)
  {
    goal_callback_ = f;
  }

  void registerCancelCallback(std::function<void()> f)
  {
    cancel_callback_ = f;
  }

  void registerDoneCallback(std::function<void(const ExecutionState&)> f)
  {
    done_callback_ = f;
  }

  bool setGoal(TrajectoryType goal);

  void setCancel()
  {
    if (cancel_callback_ != nullptr)
      cancel_callback_();
  };

  void setDone(const ExecutionState& state)
  {
    if (done_callback_ != nullptr)
      done_callback_(state);
  }

  void setFeedback(FeedbackType feedback)
  {
    feedback_ = feedback;
  }

  FeedbackType getFeedback() const
  {
    return feedback_;
  }

  std::vector<std::string> getJointNames() const
  {
    return joint_names_;
  }

  void setResources(std::vector<std::string> resources)
  {
    for (const std::string& joint : resources)
    {
      hardware_interface::HardwareInterface::claim(joint);
    }
    joint_names_ = resources;
  }

private:
  std::function<void(const TrajectoryType&)> goal_callback_;
  std::function<void()> cancel_callback_;
  std::function<void(const ExecutionState&)> done_callback_;
  FeedbackType feedback_;
  std::vector<std::string> joint_names_;
};

// Full spezialization for JointTrajectory
template <>
inline bool TrajectoryInterface<JointTrajectory, JointTrajectoryFeedback>::setGoal(JointTrajectory goal)
{
  control_msgs::FollowJointTrajectoryGoal tmp = goal;

  // Respect joint order by computing the map between msg indices to expected indices.
  // If msg is {A, C, D, B} and expected is {A, B, C, D}, the associated mapping vector is {0, 2, 3, 1}
  auto msg = goal.trajectory.joint_names;
  auto expected = joint_names_;
  std::vector<size_t> msg_joints(msg.size());
  if (msg.size() != expected.size())
  {
    // msg must contain all joint names.
    ROS_WARN("Not forwarding trajectory. It contains wrong number of joints");
    return false;
  }
  for (auto msg_it = msg.begin(); msg_it != msg.end(); ++msg_it)
  {
    auto expected_it = std::find(expected.begin(), expected.end(), *msg_it);
    if (expected.end() == expected_it)
    {
      ROS_WARN_STREAM("Not forwarding trajectory. It contains at least one unexpected joint name: " << *msg_it);
      return false;
    }
    else
    {
      const size_t msg_dist = std::distance(msg.begin(), msg_it);
      const size_t expected_dist = std::distance(expected.begin(), expected_it);
      msg_joints[msg_dist] = expected_dist;
    }
  }

  // Reorder the joint names and data fields in all trajectory points
  tmp.trajectory.joint_names = expected;
  tmp.trajectory.points.clear();

  for (auto point : goal.trajectory.points)
  {
    trajectory_msgs::JointTrajectoryPoint p{ point };  // init for equal data size

    for (size_t i = 0; i < expected.size(); ++i)
    {
      auto jnt_id = msg_joints[i];

      if (point.positions.size() == expected.size())
        p.positions[jnt_id] = point.positions[i];
      if (point.velocities.size() == expected.size())
        p.velocities[jnt_id] = point.velocities[i];
      if (point.accelerations.size() == expected.size())
        p.accelerations[jnt_id] = point.accelerations[i];
      if (point.effort.size() == expected.size())
        p.effort[jnt_id] = point.effort[i];
    }
    tmp.trajectory.points.push_back(p);
  }

  if (goal_callback_ != nullptr)
  {
    goal_callback_(tmp);
    return true;
  }
  return false;
}

// Full spezialization for CartesianTrajectory
template <>
inline bool TrajectoryInterface<CartesianTrajectory, CartesianTrajectoryFeedback>::setGoal(CartesianTrajectory goal)
{
  if (goal_callback_ != nullptr)
  {
    goal_callback_(goal);
    return true;
  }
  return false;
}

using JointTrajectoryInterface = TrajectoryInterface<JointTrajectory, JointTrajectoryFeedback>;

using CartesianTrajectoryInterface = TrajectoryInterface<CartesianTrajectory, CartesianTrajectoryFeedback>;

}  // namespace hardware_interface