best_seen_execution_time_policy.hpp

Go to the documentation of this file.

// 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
{
 
// =================================================================================================
// BestSeenExecutionTimePolicy.
// =================================================================================================
// moveit_msgs::msg::MotionPlanRequest <=> moveit::planning_interface::MoveGroupInterface::Plan
 
class BestSeenExecutionTimePolicy 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);
 
  BestSeenExecutionTimePolicy();
 
  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_;
 
  double best_seen_execution_time_;
};
 
// =================================================================================================
// CartesianBestSeenExecutionTimePolicy.
// =================================================================================================
// moveit_msgs::srv::GetCartesianPath::Request <=> moveit_msgs::srv::GetCartesianPath::Response
 
class CartesianBestSeenExecutionTimePolicy 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);
 
  CartesianBestSeenExecutionTimePolicy();
 
  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_;
 
  double best_seen_execution_time_;
};
 
}  // namespace trajectory_cache
}  // namespace moveit_ros