From 79b7f95660180d6554d1c6f153ada5a4ea339595 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Wed, 31 Jul 2024 23:18:58 -0700 Subject: [PATCH] Add MotionPlanRequest features Signed-off-by: methylDragon --- moveit_ros/trajectory_cache/CMakeLists.txt | 15 + .../features/motion_plan_request_features.hpp | 268 ++++++++++ .../features/motion_plan_request_features.cpp | 498 ++++++++++++++++++ 3 files changed, 781 insertions(+) create mode 100644 moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/motion_plan_request_features.hpp create mode 100644 moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 02707be298..452e535d7c 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -45,6 +45,19 @@ target_include_directories( ament_target_dependencies(moveit_ros_trajectory_cache_utils_lib ${TRAJECTORY_CACHE_DEPENDENCIES}) +# Features library +add_library(moveit_ros_trajectory_features_lib + src/features/motion_plan_request_features.cpp) +generate_export_header(moveit_ros_trajectory_features_lib) +target_link_libraries(moveit_ros_trajectory_features_lib + moveit_ros_trajectory_cache_utils_lib) +target_include_directories( + moveit_ros_trajectory_features_lib + PUBLIC $ + $) +ament_target_dependencies(moveit_ros_trajectory_features_lib + ${TRAJECTORY_CACHE_DEPENDENCIES}) + # Trajectory cache library add_library(moveit_ros_trajectory_cache_lib src/trajectory_cache.cpp) generate_export_header(moveit_ros_trajectory_cache_lib) @@ -57,6 +70,8 @@ target_include_directories( ament_target_dependencies(moveit_ros_trajectory_cache_lib ${TRAJECTORY_CACHE_DEPENDENCIES}) +# ============================================================================== + install( TARGETS ${TRAJECTORY_CACHE_LIBRARIES} EXPORT moveit_ros_trajectory_cacheTargets diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/motion_plan_request_features.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/motion_plan_request_features.hpp new file mode 100644 index 0000000000..fa05d87d36 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/motion_plan_request_features.hpp @@ -0,0 +1,268 @@ +// 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. + +/** @file + * @brief moveit_msgs::msg::MotionPlanRequest features to key the trajectory cache on. + * @see FeaturesInterface + * + * @author methylDragon + */ + +#pragma once + +#include +#include + +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// "Start" features. =============================================================================== + +/** @class WorkspaceFeatures + * @brief Extracts group name and details of the `workspace_parameters` field in the plan request. + * + * A cache hit with this feature is valid if the requested planning constraints has a workspace that + * completely subsumes a cached entry's workspace. + * + * For example: (We ignore z for simplicity) + * If workspace is defined by the extrema (x_min, y_min, x_max, y_max), + * + * Potential valid match if other constraints fulfilled: + * Request: (-1, -1, 1, 1) + * Plan in cache: (-0.5, -0.5, 0.5, 0.5) + * + * No match, since this plan might cause the end effector to go out of bounds.: + * Request: (-1, -1, 1, 1) + * Plan in cache: (-2, -0.5, 0.5, 0.5) + */ +class WorkspaceFeatures : public FeaturesInterface +{ +public: + WorkspaceFeatures(); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + const std::string name_; +}; + +/** @class StartStateJointStateFeatures + * @brief Extracts details of the joint state from the `start_state` field in the plan request. + * + * The start state will always be re-interpreted into explicit joint state positions. + * + * WARNING: + * MultiDOF joints and attached collision objects are not supported. + */ +class StartStateJointStateFeatures : public FeaturesInterface +{ +public: + StartStateJointStateFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + rclcpp::Logger getLogger() const; + + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +// "Goal" features. ================================================================================ + +/** @class MaxSpeedAndAccelerationFeatures + * @brief Extracts max velocity and acceleration scaling, and cartesian speed limits from the plan request. + */ +class MaxSpeedAndAccelerationFeatures : public FeaturesInterface +{ +public: + MaxSpeedAndAccelerationFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +/** @class GoalConstraintsFeatures + * @brief Extracts features fom the `goal_constraints` field in the plan request. + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class GoalConstraintsFeatures : public FeaturesInterface +{ +public: + GoalConstraintsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + rclcpp::Logger getLogger() const; + + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +/** @class PathConstraintsFeatures + * @brief Extracts features fom the `path_constraints` field in the plan request. + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class PathConstraintsFeatures : public FeaturesInterface +{ +public: + PathConstraintsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + rclcpp::Logger getLogger() const; + + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +/** @class TrajectoryConstraintsFeatures + * @brief Extracts features fom the `trajectory_constraints` field in the plan request. + * + * @see appendConstraintsAsFetchQueryWithTolerance + * @see appendConstraintsAsInsertMetadata + */ +class TrajectoryConstraintsFeatures : public FeaturesInterface +{ +public: + TrajectoryConstraintsFeatures(double match_tolerance); + + std::string getName() const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsFuzzyFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsExactFetchQuery(warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, + double exact_match_precision) const override; + + moveit::core::MoveItErrorCode + appendFeaturesAsInsertMetadata(warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const override; + +private: + rclcpp::Logger getLogger() const; + + moveit::core::MoveItErrorCode appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const; + + const std::string name_; + const double match_tolerance_; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp b/moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp new file mode 100644 index 0000000000..8d77175730 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp @@ -0,0 +1,498 @@ +// 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. + +/** @file + * @brief Implementation of moveit_msgs::msg::MotionPlanRequest features to key the trajectory cache on. + * @see FeaturesInterface + * + * @author methylDragon + */ + +#include +#include + +#include +#include +#include + +#include +#include + +namespace moveit_ros +{ +namespace trajectory_cache +{ + +// "Start" features. =============================================================================== + +// WorkspaceFeatures. + +WorkspaceFeatures::WorkspaceFeatures() : name_("WorkspaceFeatures") +{ +} + +std::string WorkspaceFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode WorkspaceFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsExactFetchQuery(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode WorkspaceFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double /*exact_match_precision*/) const +{ + query.append(name_ + ".group_name", source.group_name); + query.append(name_ + ".workspace_parameters.header.frame_id", + getWorkspaceFrameId(move_group, source.workspace_parameters)); + query.appendGTE(name_ + ".workspace_parameters.min_corner.x", source.workspace_parameters.min_corner.x); + query.appendGTE(name_ + ".workspace_parameters.min_corner.y", source.workspace_parameters.min_corner.y); + query.appendGTE(name_ + ".workspace_parameters.min_corner.z", source.workspace_parameters.min_corner.z); + query.appendLTE(name_ + ".workspace_parameters.max_corner.x", source.workspace_parameters.max_corner.x); + query.appendLTE(name_ + ".workspace_parameters.max_corner.y", source.workspace_parameters.max_corner.y); + query.appendLTE(name_ + ".workspace_parameters.max_corner.z", source.workspace_parameters.max_corner.z); + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +moveit::core::MoveItErrorCode WorkspaceFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + metadata.append(name_ + ".group_name", source.group_name); + metadata.append(name_ + ".workspace_parameters.header.frame_id", + getWorkspaceFrameId(move_group, source.workspace_parameters)); + metadata.append(name_ + ".workspace_parameters.min_corner.x", source.workspace_parameters.min_corner.x); + metadata.append(name_ + ".workspace_parameters.min_corner.y", source.workspace_parameters.min_corner.y); + metadata.append(name_ + ".workspace_parameters.min_corner.z", source.workspace_parameters.min_corner.z); + metadata.append(name_ + ".workspace_parameters.max_corner.x", source.workspace_parameters.max_corner.x); + metadata.append(name_ + ".workspace_parameters.max_corner.y", source.workspace_parameters.max_corner.y); + metadata.append(name_ + ".workspace_parameters.max_corner.z", source.workspace_parameters.max_corner.z); + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// StartStateJointStateFeatures. + +StartStateJointStateFeatures::StartStateJointStateFeatures(double match_tolerance) + : name_("StartStateJointStateFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string StartStateJointStateFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode StartStateJointStateFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +moveit::core::MoveItErrorCode StartStateJointStateFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode StartStateJointStateFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + // Make ignored members explicit + if (!source.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(getLogger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!source.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(getLogger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // Only accounts for joint_state position. + if (source.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // This issue should go away once the class is used within the move group's + // Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(getLogger(), "Skipping insert metadata append: Could not get robot state."); + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the metadata is not + // supported. + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA, + "Could not get robot state."); + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + metadata.append(name_ + "start_state.joint_state.name_" + std::to_string(i), + current_state_msg.joint_state.name.at(i)); + metadata.append(name_ + "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i)); + } + } + else + { + for (size_t i = 0; i < source.start_state.joint_state.name.size(); i++) + { + metadata.append(name_ + "start_state.joint_state.name_" + std::to_string(i), + source.start_state.joint_state.name.at(i)); + metadata.append(name_ + "start_state.joint_state.position_" + std::to_string(i), + source.start_state.joint_state.position.at(i)); + } + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +rclcpp::Logger StartStateJointStateFeatures::getLogger() const +{ + return moveit::getLogger("moveit.ros.trajectory_cache.features.StartStateJointStateFeatures"); +} + +moveit::core::MoveItErrorCode StartStateJointStateFeatures::appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const +{ + // Make ignored members explicit + if (!source.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN(getLogger(), "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!source.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN(getLogger(), "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // Only accounts for joint_state position. + if (source.start_state.is_diff) + { + // If plan request states that the start_state is_diff, then we need to get + // the current state from the move_group. + + // NOTE: methyldragon - + // I think if is_diff is on, the joint states will not be populated in all + // of our motion plan requests? If this isn't the case we might need to + // apply the joint states as offsets as well. + // + // TODO: Since MoveIt also potentially does another getCurrentState() call + // when planning, there is a chance that the current state in the cache + // differs from the state used in MoveIt's plan. + // + // This issue should go away once the class is used within the move group's + // Plan call. + moveit::core::RobotStatePtr current_state = move_group.getCurrentState(); + if (!current_state) + { + RCLCPP_WARN(getLogger(), "Skipping fetch query append: Could not get robot state."); + // NOTE: methyldragon - + // Ideally we would restore the original state here and undo our changes, however copy of the query is not + // supported. + return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::UNABLE_TO_AQUIRE_SENSOR_DATA, + "Could not get robot state."); + } + + moveit_msgs::msg::RobotState current_state_msg; + robotStateToRobotStateMsg(*current_state, current_state_msg); + + for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) + { + query.append(name_ + "start_state.joint_state.name_" + std::to_string(i), + current_state_msg.joint_state.name.at(i)); + queryAppendCenterWithTolerance(query, name_ + "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i), match_tolerance); + } + } + else + { + for (size_t i = 0; i < source.start_state.joint_state.name.size(); i++) + { + query.append(name_ + "start_state.joint_state.name_" + std::to_string(i), + source.start_state.joint_state.name.at(i)); + queryAppendCenterWithTolerance(query, name_ + "start_state.joint_state.position_" + std::to_string(i), + source.start_state.joint_state.position.at(i), match_tolerance); + } + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// "Goal" features. ================================================================================ + +// MaxSpeedAndAccelerationFeatures. + +MaxSpeedAndAccelerationFeatures::MaxSpeedAndAccelerationFeatures(double match_tolerance) + : name_("MaxSpeedAndAccelerationFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string MaxSpeedAndAccelerationFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode MaxSpeedAndAccelerationFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +moveit::core::MoveItErrorCode MaxSpeedAndAccelerationFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode MaxSpeedAndAccelerationFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const +{ + metadata.append(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + metadata.append(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + metadata.append(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +moveit::core::MoveItErrorCode MaxSpeedAndAccelerationFeatures::appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/, double match_tolerance) const +{ + queryAppendCenterWithTolerance(query, name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor, + match_tolerance); + queryAppendCenterWithTolerance(query, name_ + ".max_acceleration_scaling_factor", + source.max_acceleration_scaling_factor, match_tolerance); + queryAppendCenterWithTolerance(query, name_ + ".max_cartesian_speed", source.max_cartesian_speed, match_tolerance); + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +// GoalConstraintsFeatures. + +GoalConstraintsFeatures::GoalConstraintsFeatures(double match_tolerance) + : name_("GoalConstraintsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string GoalConstraintsFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode GoalConstraintsFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +moveit::core::MoveItErrorCode GoalConstraintsFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode GoalConstraintsFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + + moveit::core::MoveItErrorCode ret = appendConstraintsAsInsertMetadata(metadata, source.goal_constraints, move_group, + workspace_id, name_ + ".goal_constraints"); + + if (ret) + { + RCLCPP_WARN(getLogger(), "Error while appending insert metadata: %s", ret.message.c_str()); + } + return ret; +}; + +rclcpp::Logger GoalConstraintsFeatures::getLogger() const +{ + return moveit::getLogger("moveit.ros.trajectory_cache.features.GoalConstraintsFeatures"); +} + +moveit::core::MoveItErrorCode GoalConstraintsFeatures::appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + + moveit::core::MoveItErrorCode ret = appendConstraintsAsFetchQueryWithTolerance( + query, source.goal_constraints, move_group, match_tolerance, workspace_id, name_ + ".goal_constraints"); + + if (ret) + { + RCLCPP_WARN(getLogger(), "Error while appending insert metadata: %s", ret.message.c_str()); + } + return ret; +}; + +// PathConstraintsFeatures. + +PathConstraintsFeatures::PathConstraintsFeatures(double match_tolerance) + : name_("PathConstraintsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string PathConstraintsFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode PathConstraintsFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +moveit::core::MoveItErrorCode PathConstraintsFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode PathConstraintsFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + + moveit::core::MoveItErrorCode ret = appendConstraintsAsInsertMetadata( + metadata, { source.path_constraints }, move_group, workspace_id, name_ + ".path_constraints"); + + if (ret) + { + RCLCPP_WARN(getLogger(), "Error while appending insert metadata: %s", ret.message.c_str()); + } + return ret; +}; + +rclcpp::Logger PathConstraintsFeatures::getLogger() const +{ + return moveit::getLogger("moveit.ros.trajectory_cache.features.PathConstraintsFeatures"); +} + +moveit::core::MoveItErrorCode PathConstraintsFeatures::appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + + moveit::core::MoveItErrorCode ret = appendConstraintsAsFetchQueryWithTolerance( + query, { source.path_constraints }, move_group, match_tolerance, workspace_id, name_ + ".path_constraints"); + + if (ret) + { + RCLCPP_WARN(getLogger(), "Error while appending insert metadata: %s", ret.message.c_str()); + } + return ret; +}; + +// TrajectoryConstraintsFeatures. + +TrajectoryConstraintsFeatures::TrajectoryConstraintsFeatures(double match_tolerance) + : name_("TrajectoryConstraintsFeatures"), match_tolerance_(match_tolerance) +{ +} + +std::string TrajectoryConstraintsFeatures::getName() const +{ + return name_; +} + +moveit::core::MoveItErrorCode TrajectoryConstraintsFeatures::appendFeaturesAsFuzzyFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, match_tolerance_ + exact_match_precision); +}; + +moveit::core::MoveItErrorCode TrajectoryConstraintsFeatures::appendFeaturesAsExactFetchQuery( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double exact_match_precision) const +{ + return appendFeaturesAsFetchQueryWithTolerance(query, source, move_group, exact_match_precision); +}; + +moveit::core::MoveItErrorCode TrajectoryConstraintsFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + + moveit::core::MoveItErrorCode ret = + appendConstraintsAsInsertMetadata(metadata, source.trajectory_constraints.constraints, move_group, workspace_id, + name_ + ".trajectory_constraints.constraints"); + + if (ret) + { + RCLCPP_WARN(getLogger(), "Error while appending insert metadata: %s", ret.message.c_str()); + } + return ret; +}; + +rclcpp::Logger TrajectoryConstraintsFeatures::getLogger() const +{ + return moveit::getLogger("moveit.ros.trajectory_cache.features.TrajectoryConstraintsFeatures"); +} + +moveit::core::MoveItErrorCode TrajectoryConstraintsFeatures::appendFeaturesAsFetchQueryWithTolerance( + warehouse_ros::Query& query, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance) const +{ + std::string workspace_id = getWorkspaceFrameId(move_group, source.workspace_parameters); + + moveit::core::MoveItErrorCode ret = + appendConstraintsAsFetchQueryWithTolerance(query, source.trajectory_constraints.constraints, move_group, + match_tolerance, workspace_id, + name_ + ".trajectory_constraints.constraints"); + + if (ret) + { + RCLCPP_WARN(getLogger(), "Error while appending insert metadata: %s", ret.message.c_str()); + } + return ret; +}; + +} // namespace trajectory_cache +} // namespace moveit_ros