Program Listing for File always_insert_never_prune_policy.hpp

↰ Return to documentation for file (include/moveit/trajectory_cache/cache_insert_policies/always_insert_never_prune_policy.hpp)

// Copyright 2024 Intrinsic Innovation LLC.
//
// 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.

#pragma once

#include <memory>

#include <warehouse_ros/message_collection.h>
#include <moveit/move_group_interface/move_group_interface.hpp>
#include <moveit_msgs/msg/motion_plan_request.hpp>
#include <moveit_msgs/msg/robot_trajectory.hpp>
#include <moveit_msgs/srv/get_cartesian_path.hpp>

#include <moveit/trajectory_cache/cache_insert_policies/cache_insert_policy_interface.hpp>
#include <moveit/trajectory_cache/features/features_interface.hpp>

namespace moveit_ros
{
namespace trajectory_cache
{

// =================================================================================================
// AlwaysInsertNeverPrunePolicy.
// =================================================================================================
// moveit_msgs::msg::MotionPlanRequest <=> moveit::planning_interface::MoveGroupInterface::Plan

class AlwaysInsertNeverPrunePolicy final
  : public CacheInsertPolicyInterface<moveit_msgs::msg::MotionPlanRequest,
                                      moveit::planning_interface::MoveGroupInterface::Plan,
                                      moveit_msgs::msg::RobotTrajectory>
{
public:
  static std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::msg::MotionPlanRequest>>>
  getSupportedFeatures(double start_tolerance, double goal_tolerance);

  AlwaysInsertNeverPrunePolicy();

  std::string getName() const override;

  moveit::core::MoveItErrorCode
  checkCacheInsertInputs(const moveit::planning_interface::MoveGroupInterface& move_group,
                         const warehouse_ros::MessageCollection<moveit_msgs::msg::RobotTrajectory>& coll,
                         const moveit_msgs::msg::MotionPlanRequest& key,
                         const moveit::planning_interface::MoveGroupInterface::Plan& value) override;

  std::vector<warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr>
  fetchMatchingEntries(const moveit::planning_interface::MoveGroupInterface& move_group,
                       const warehouse_ros::MessageCollection<moveit_msgs::msg::RobotTrajectory>& coll,
                       const moveit_msgs::msg::MotionPlanRequest& key,
                       const moveit::planning_interface::MoveGroupInterface::Plan& value,
                       double exact_match_precision) override;

  bool shouldPruneMatchingEntry(
      const moveit::planning_interface::MoveGroupInterface& move_group, const moveit_msgs::msg::MotionPlanRequest& key,
      const moveit::planning_interface::MoveGroupInterface::Plan& value,
      const warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr& matching_entry,
      std::string* reason = nullptr) override;

  bool shouldInsert(const moveit::planning_interface::MoveGroupInterface& move_group,
                    const moveit_msgs::msg::MotionPlanRequest& key,
                    const moveit::planning_interface::MoveGroupInterface::Plan& value,
                    std::string* reason = nullptr) override;

  moveit::core::MoveItErrorCode
  appendInsertMetadata(warehouse_ros::Metadata& metadata,
                       const moveit::planning_interface::MoveGroupInterface& move_group,
                       const moveit_msgs::msg::MotionPlanRequest& key,
                       const moveit::planning_interface::MoveGroupInterface::Plan& value) override;

  void reset() override;

private:
  const std::string name_;
  std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::msg::MotionPlanRequest>>> exact_matching_supported_features_;
};

// =================================================================================================
// CartesianAlwaysInsertNeverPrunePolicy.
// =================================================================================================
// moveit_msgs::srv::GetCartesianPath::Request <=> moveit_msgs::srv::GetCartesianPath::Response

class CartesianAlwaysInsertNeverPrunePolicy final
  : public CacheInsertPolicyInterface<moveit_msgs::srv::GetCartesianPath::Request,
                                      moveit_msgs::srv::GetCartesianPath::Response, moveit_msgs::msg::RobotTrajectory>
{
public:
  static std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::srv::GetCartesianPath::Request>>>
  getSupportedFeatures(double start_tolerance, double goal_tolerance, double min_fraction);

  CartesianAlwaysInsertNeverPrunePolicy();

  std::string getName() const override;

  moveit::core::MoveItErrorCode
  checkCacheInsertInputs(const moveit::planning_interface::MoveGroupInterface& move_group,
                         const warehouse_ros::MessageCollection<moveit_msgs::msg::RobotTrajectory>& coll,
                         const moveit_msgs::srv::GetCartesianPath::Request& key,
                         const moveit_msgs::srv::GetCartesianPath::Response& value) override;

  std::vector<warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr>
  fetchMatchingEntries(const moveit::planning_interface::MoveGroupInterface& move_group,
                       const warehouse_ros::MessageCollection<moveit_msgs::msg::RobotTrajectory>& coll,
                       const moveit_msgs::srv::GetCartesianPath::Request& key,
                       const moveit_msgs::srv::GetCartesianPath::Response& value,
                       double exact_match_precision) override;

  bool shouldPruneMatchingEntry(
      const moveit::planning_interface::MoveGroupInterface& move_group,
      const moveit_msgs::srv::GetCartesianPath::Request& key, const moveit_msgs::srv::GetCartesianPath::Response& value,
      const warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr& matching_entry,
      std::string* reason = nullptr) override;

  bool shouldInsert(const moveit::planning_interface::MoveGroupInterface& move_group,
                    const moveit_msgs::srv::GetCartesianPath::Request& key,
                    const moveit_msgs::srv::GetCartesianPath::Response& value, std::string* reason = nullptr) override;

  moveit::core::MoveItErrorCode appendInsertMetadata(warehouse_ros::Metadata& metadata,
                                                     const moveit::planning_interface::MoveGroupInterface& move_group,
                                                     const moveit_msgs::srv::GetCartesianPath::Request& key,
                                                     const moveit_msgs::srv::GetCartesianPath::Response& value) override;

  void reset() override;

private:
  const std::string name_;
  std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::srv::GetCartesianPath::Request>>>
      exact_matching_supported_features_;
};

}  // namespace trajectory_cache
}  // namespace moveit_ros