Program Listing for File trajectory.hpp
↰ Return to documentation for file (include/joint_trajectory_controller/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);
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_