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 <memory>
#include <optional>
 
#include <rclcpp/rclcpp.hpp>
 
#include <warehouse_ros/message_collection.h>
#include <warehouse_ros/database_connection.h>
 
// TF2
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>
 
// ROS2 Messages
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
 
// moveit modules
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/srv/get_cartesian_path.hpp>
 
namespace moveit_ros
{
namespace trajectory_cache
{
 
class TrajectoryCache
{
public:
  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_deleting_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 getNumAdditionalTrajectoriesToPreserveWhenDeletingWorse() const;
 
  void setNumAdditionalTrajectoriesToPreserveWhenDeletingWorse(
      size_t num_additional_trajectories_to_preserve_when_deleting_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, double start_tolerance,
                               double goal_tolerance, bool metadata_only = false,
                               const std::string& sort_by = "execution_time_s", bool ascending = true) 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, double start_tolerance, double goal_tolerance,
      bool metadata_only = false, const std::string& sort_by = "execution_time_s", bool ascending = true) const;
 
  bool insertTrajectory(const moveit::planning_interface::MoveGroupInterface& move_group,
                        const std::string& cache_namespace, const moveit_msgs::msg::MotionPlanRequest& plan_request,
                        const moveit_msgs::msg::RobotTrajectory& trajectory, double execution_time_s,
                        double planning_time_s, bool prune_worse_trajectories = true);
 
  moveit_msgs::srv::GetCartesianPath::Request
  constructGetCartesianPathRequest(moveit::planning_interface::MoveGroupInterface& move_group,
                                   const std::vector<geometry_msgs::msg::Pose>& waypoints, double max_step,
                                   double jump_threshold, bool avoid_collisions = true);
 
  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,
                                        double min_fraction, double start_tolerance, double goal_tolerance,
                                        bool metadata_only = false, const std::string& sort_by = "execution_time_s",
                                        bool ascending = true) 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, double min_fraction, double start_tolerance,
      double goal_tolerance, bool metadata_only = false, const std::string& sort_by = "execution_time_s",
      bool ascending = true) 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::msg::RobotTrajectory& trajectory, double execution_time_s,
                                 double planning_time_s, double fraction, bool prune_worse_trajectories = true);
 
private:
  bool extractAndAppendTrajectoryStartToQuery(warehouse_ros::Query& query,
                                              const moveit::planning_interface::MoveGroupInterface& move_group,
                                              const moveit_msgs::msg::MotionPlanRequest& plan_request,
                                              double match_tolerance) const;
 
  bool extractAndAppendTrajectoryGoalToQuery(warehouse_ros::Query& query,
                                             const moveit::planning_interface::MoveGroupInterface& move_group,
                                             const moveit_msgs::msg::MotionPlanRequest& plan_request,
                                             double match_tolerance) const;
 
  bool extractAndAppendTrajectoryStartToMetadata(warehouse_ros::Metadata& metadata,
                                                 const moveit::planning_interface::MoveGroupInterface& move_group,
                                                 const moveit_msgs::msg::MotionPlanRequest& plan_request) const;
 
  bool extractAndAppendTrajectoryGoalToMetadata(warehouse_ros::Metadata& metadata,
                                                const moveit::planning_interface::MoveGroupInterface& move_group,
                                                const moveit_msgs::msg::MotionPlanRequest& plan_request) const;
 
  bool extractAndAppendCartesianTrajectoryStartToQuery(warehouse_ros::Query& query,
                                                       const moveit::planning_interface::MoveGroupInterface& move_group,
                                                       const moveit_msgs::srv::GetCartesianPath::Request& plan_request,
                                                       double match_tolerance) const;
 
  bool extractAndAppendCartesianTrajectoryGoalToQuery(warehouse_ros::Query& query,
                                                      const moveit::planning_interface::MoveGroupInterface& move_group,
                                                      const moveit_msgs::srv::GetCartesianPath::Request& plan_request,
                                                      double match_tolerance) const;
 
  bool extractAndAppendCartesianTrajectoryStartToMetadata(
      warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group,
      const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const;
 
  bool extractAndAppendCartesianTrajectoryGoalToMetadata(
      warehouse_ros::Metadata& metadata, const moveit::planning_interface::MoveGroupInterface& move_group,
      const moveit_msgs::srv::GetCartesianPath::Request& plan_request) const;
 
  rclcpp::Node::SharedPtr node_;
  rclcpp::Logger logger_;
  warehouse_ros::DatabaseConnection::Ptr db_;
 
  Options options_;
 
  std::unique_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
};
 
}  // namespace trajectory_cache
}  // namespace moveit_ros