trajectory.hpp

// Copyright 2017 Open Source Robotics Foundation, Inc.
//
// 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.
 
#ifndef JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
#define JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_
 
#include <memory>
#include <vector>
 
#include "joint_trajectory_controller/interpolation_methods.hpp"
#include "joint_trajectory_controller/visibility_control.h"
#include "rclcpp/time.hpp"
#include "trajectory_msgs/msg/joint_trajectory.hpp"
#include "trajectory_msgs/msg/joint_trajectory_point.hpp"
namespace joint_trajectory_controller
{
using TrajectoryPointIter = std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::iterator;
using TrajectoryPointConstIter =
  std::vector<trajectory_msgs::msg::JointTrajectoryPoint>::const_iterator;
 
class Trajectory
{
public:
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  Trajectory();
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  explicit Trajectory(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  explicit Trajectory(
    const rclcpp::Time & current_time,
    const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
    std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  void set_point_before_trajectory_msg(
    const rclcpp::Time & current_time,
    const trajectory_msgs::msg::JointTrajectoryPoint & current_point,
    const std::vector<bool> & joints_angle_wraparound = std::vector<bool>());
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  void update(std::shared_ptr<trajectory_msgs::msg::JointTrajectory> joint_trajectory);
 
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  bool sample(
    const rclcpp::Time & sample_time,
    const interpolation_methods::InterpolationMethod interpolation_method,
    trajectory_msgs::msg::JointTrajectoryPoint & output_state,
    TrajectoryPointConstIter & start_segment_itr, TrajectoryPointConstIter & end_segment_itr,
    const bool search_monotonically_increasing = true);
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  void interpolate_between_points(
    const rclcpp::Time & time_a, const trajectory_msgs::msg::JointTrajectoryPoint & state_a,
    const rclcpp::Time & time_b, const trajectory_msgs::msg::JointTrajectoryPoint & state_b,
    const rclcpp::Time & sample_time, trajectory_msgs::msg::JointTrajectoryPoint & output);
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  TrajectoryPointConstIter begin() const;
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  TrajectoryPointConstIter end() const;
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  rclcpp::Time time_from_start() const;
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  bool has_trajectory_msg() const;
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  bool has_nontrivial_msg() const;
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> get_trajectory_msg() const
  {
    return trajectory_msg_;
  }
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  bool is_sampled_already() const { return sampled_already_; }
 
 
  JOINT_TRAJECTORY_CONTROLLER_PUBLIC
  size_t last_sample_index() const { return last_sample_idx_; }
 
private:
  void deduce_from_derivatives(
    trajectory_msgs::msg::JointTrajectoryPoint & first_state,
    trajectory_msgs::msg::JointTrajectoryPoint & second_state, const size_t dim,
    const double delta_t);
 
  std::shared_ptr<trajectory_msgs::msg::JointTrajectory> trajectory_msg_;
  rclcpp::Time trajectory_start_time_;
 
  rclcpp::Time time_before_traj_msg_;
  trajectory_msgs::msg::JointTrajectoryPoint state_before_traj_msg_;
 
  bool sampled_already_ = false;
  size_t last_sample_idx_ = 0;
};
 
template <class T>
inline std::vector<size_t> mapping(const T & t1, const T & t2)
{
  // t1 must be a subset of t2
  if (t1.size() > t2.size())
  {
    return std::vector<size_t>();
  }
 
  std::vector<size_t> mapping_vector(t1.size());  // Return value
  for (auto t1_it = t1.begin(); t1_it != t1.end(); ++t1_it)
  {
    auto t2_it = std::find(t2.begin(), t2.end(), *t1_it);
    if (t2.end() == t2_it)
    {
      return std::vector<size_t>();
    }
    else
    {
      const size_t t1_dist = static_cast<size_t>(std::distance(t1.begin(), t1_it));
      const size_t t2_dist = static_cast<size_t>(std::distance(t2.begin(), t2_it));
      mapping_vector[t1_dist] = t2_dist;
    }
  }
  return mapping_vector;
}
 
void wraparound_joint(
  std::vector<double> & current_position, const std::vector<double> next_position,
  const std::vector<bool> & joints_angle_wraparound);
 
}  // namespace joint_trajectory_controller
 
#endif  // JOINT_TRAJECTORY_CONTROLLER__TRAJECTORY_HPP_