trajectory_cache.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 <chrono>
#include <cstdint>
#include <memory>
#include <vector>
 
#include <rclcpp/rclcpp.hpp>
 
#include <warehouse_ros/message_collection.h>
#include <warehouse_ros/database_connection.h>
 
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
 
// moveit modules
#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
{

class TrajectoryCache
{
public:

  static std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::msg::MotionPlanRequest>>>
  getDefaultFeatures(double start_tolerance, double goal_tolerance);

  static std::unique_ptr<CacheInsertPolicyInterface<moveit_msgs::msg::MotionPlanRequest,
                                                    moveit::planning_interface::MoveGroupInterface::Plan,
                                                    moveit_msgs::msg::RobotTrajectory>>
  getDefaultCacheInsertPolicy();

  static std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::srv::GetCartesianPath::Request>>>
  getDefaultCartesianFeatures(double start_tolerance, double goal_tolerance, double min_fraction);

  static std::unique_ptr<
      CacheInsertPolicyInterface<moveit_msgs::srv::GetCartesianPath::Request,
                                 moveit_msgs::srv::GetCartesianPath::Response, moveit_msgs::msg::RobotTrajectory>>
  getDefaultCartesianCacheInsertPolicy();

  static std::string getDefaultSortFeature();



  explicit TrajectoryCache(const rclcpp::Node::SharedPtr& node);

  struct Options
  {
    std::string db_path = ":memory:";
    uint32_t db_port = 0;
 
    double exact_match_precision = 1e-6;
    size_t num_additional_trajectories_to_preserve_when_pruning_worse = 1;
  };

  bool init(const Options& options);



  unsigned countTrajectories(const std::string& cache_namespace);

  unsigned countCartesianTrajectories(const std::string& cache_namespace);

  std::string getDbPath() const;

  uint32_t getDbPort() const;

  double getExactMatchPrecision() const;

  void setExactMatchPrecision(double exact_match_precision);

  size_t getNumAdditionalTrajectoriesToPreserveWhenPruningWorse() const;

  void setNumAdditionalTrajectoriesToPreserveWhenPruningWorse(
      size_t num_additional_trajectories_to_preserve_when_pruning_worse);



  std::vector<warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr>
  fetchAllMatchingTrajectories(
      const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
      const moveit_msgs::msg::MotionPlanRequest& plan_request,
      const std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::msg::MotionPlanRequest>>>& features,
      const std::string& sort_by, bool ascending = true, bool metadata_only = false) const;

  warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr fetchBestMatchingTrajectory(
      const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
      const moveit_msgs::msg::MotionPlanRequest& plan_request,
      const std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::msg::MotionPlanRequest>>>& features,
      const std::string& sort_by, bool ascending = true, bool metadata_only = false) const;

  bool insertTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group,
                        const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request,
                        const moveit::planning_interface::MoveGroupInterface::Plan& plan,
                        CacheInsertPolicyInterface<moveit_msgs::msg::MotionPlanRequest,
                                                   moveit::planning_interface::MoveGroupInterface::Plan,
                                                   moveit_msgs::msg::RobotTrajectory>& cache_insert_policy,
                        bool prune_worse_trajectories = true,
                        const std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::msg::MotionPlanRequest>>>&
                            additional_features = {});



  std::vector<warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr>
  fetchAllMatchingCartesianTrajectories(
      const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
      const moveit_msgs::srv::GetCartesianPath::Request& plan_request,
      const std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::srv::GetCartesianPath::Request>>>& features,
      const std::string& sort_by, bool ascending = true, bool metadata_only = false) const;

  warehouse_ros::MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr fetchBestMatchingCartesianTrajectory(
      const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
      const moveit_msgs::srv::GetCartesianPath::Request& plan_request,
      const std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::srv::GetCartesianPath::Request>>>& features,
      const std::string& sort_by, bool ascending = true, bool metadata_only = false) const;

  bool insertCartesianTrajectory(
      const moveit::planning_interface::MoveGroupInterface& move_group, const std::string& cache_namespace,
      const moveit_msgs::srv::GetCartesianPath::Request& plan_request,
      const moveit_msgs::srv::GetCartesianPath::Response& plan,
      CacheInsertPolicyInterface<moveit_msgs::srv::GetCartesianPath::Request,
                                 moveit_msgs::srv::GetCartesianPath::Response, moveit_msgs::msg::RobotTrajectory>&
          cache_insert_policy,
      bool prune_worse_trajectories = true,
      const std::vector<std::unique_ptr<FeaturesInterface<moveit_msgs::srv::GetCartesianPath::Request>>>&
          additional_features = {});

 
private:
  rclcpp::Node::SharedPtr node_;
  rclcpp::Logger logger_;
  warehouse_ros::DatabaseConnection::Ptr db_;
 
  Options options_;
};
 
}  // namespace trajectory_cache
}  // namespace moveit_ros