diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 02707be298b..452e535d7ca 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 00000000000..8193305ac74 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/features/motion_plan_request_features.hpp @@ -0,0 +1,265 @@ +// 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 + +#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. + * + * @see appendRobotStateJointStateAsFetchQueryWithTolerance + * @see appendRobotStateJointStateAsInsertMetadata + */ +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: + 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. + * + * These features will be extracted as less-than-or-equal features. + * + * NOTE: In accordance with the source message's field descriptions: + * If the max scaling factors are outside the range of (0, 1], they will be set to 1. + * If max_cartesian_speed is <= 0, it will be ignored instead. + */ +class MaxSpeedAndAccelerationFeatures : public FeaturesInterface +{ +public: + MaxSpeedAndAccelerationFeatures(); + + 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 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: + 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: + 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: + 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/include/moveit/trajectory_cache/utils/utils.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp index 3089f9c1baf..2c65767cd5f 100644 --- a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp @@ -80,7 +80,7 @@ void sortOrientationConstraints(std::vector constraints, const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance, - const std::string& workspace_frame_id, const std::string& prefix); + const std::string& reference_frame_id, const std::string& prefix); /** @brief Extracts relevant features from a vector of moveit_msgs::msg::Constraints messages to a cache entry's * metadata. @@ -106,7 +106,7 @@ moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance( * @param[in,out] metadata. The metadata to add features to. * @param[in] constraints. The constraints to extract features from. * @param[in] move_group. The manipulator move group, used to get its state. - * @param[in] workspace_frame_id. The frame to restate constraints in. + * @param[in] reference_frame_id. The frame to restate constraints in. * @param[in] prefix. A prefix to add to feature keys. * @returns moveit::core::MoveItErrorCode::SUCCESS if successfully appended. Otherwise, will return a different error * code, in which case the metadata should not be reused. @@ -115,7 +115,7 @@ moveit::core::MoveItErrorCode appendConstraintsAsInsertMetadata(warehouse_ros::Metadata& metadata, std::vector constraints, const moveit::planning_interface::MoveGroupInterface& move_group, - const std::string& workspace_frame_id, const std::string& prefix); + const std::string& reference_frame_id, const std::string& prefix); // RobotState. ===================================================================================== 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 00000000000..45349f9e800 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/features/motion_plan_request_features.cpp @@ -0,0 +1,347 @@ +// 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 +#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 +{ + return appendRobotStateJointStateAsInsertMetadata(metadata, source.start_state, move_group, name_ + ".start_state"); +}; + +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 +{ + return appendRobotStateJointStateAsFetchQueryWithTolerance(query, source.start_state, move_group, match_tolerance, + name_ + ".start_state"); +}; + +// "Goal" features. ================================================================================ + +// MaxSpeedAndAccelerationFeatures. + +MaxSpeedAndAccelerationFeatures::MaxSpeedAndAccelerationFeatures() : name_("MaxSpeedAndAccelerationFeatures") +{ +} + +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 appendFeaturesAsExactFetchQuery(query, source, move_group, 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 +{ + if (source.max_velocity_scaling_factor <= 0 || source.max_velocity_scaling_factor > 1.0) + { + query.appendLTE(name_ + ".max_velocity_scaling_factor", 1.0); + } + else + { + query.appendLTE(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + } + + if (source.max_acceleration_scaling_factor <= 0 || source.max_acceleration_scaling_factor > 1.0) + { + query.appendLTE(name_ + ".max_acceleration_scaling_factor", 1.0); + } + else + { + query.appendLTE(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + } + + if (source.max_cartesian_speed > 0) + { + query.append(name_ + ".cartesian_speed_limited_link", source.cartesian_speed_limited_link); + query.appendLTE(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + } + + return moveit::core::MoveItErrorCode::SUCCESS; +}; + +moveit::core::MoveItErrorCode MaxSpeedAndAccelerationFeatures::appendFeaturesAsInsertMetadata( + warehouse_ros::Metadata& metadata, const moveit_msgs::msg::MotionPlanRequest& source, + const moveit::planning_interface::MoveGroupInterface& /*move_group*/) const +{ + if (source.max_velocity_scaling_factor <= 0 || source.max_velocity_scaling_factor > 1.0) + { + metadata.append(name_ + ".max_velocity_scaling_factor", 1.0); + } + else + { + metadata.append(name_ + ".max_velocity_scaling_factor", source.max_velocity_scaling_factor); + } + + if (source.max_acceleration_scaling_factor <= 0 || source.max_acceleration_scaling_factor > 1.0) + { + metadata.append(name_ + ".max_acceleration_scaling_factor", 1.0); + } + else + { + metadata.append(name_ + ".max_acceleration_scaling_factor", source.max_acceleration_scaling_factor); + } + + if (source.max_cartesian_speed > 0) + { + metadata.append(name_ + ".cartesian_speed_limited_link", source.cartesian_speed_limited_link); + metadata.append(name_ + ".max_cartesian_speed", source.max_cartesian_speed); + } + + 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); + return appendConstraintsAsInsertMetadata(metadata, source.goal_constraints, move_group, workspace_id, + name_ + ".goal_constraints"); +}; + +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); + return appendConstraintsAsFetchQueryWithTolerance(query, source.goal_constraints, move_group, match_tolerance, + workspace_id, name_ + ".goal_constraints"); +}; + +// 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); + return appendConstraintsAsInsertMetadata(metadata, { source.path_constraints }, move_group, workspace_id, + name_ + ".path_constraints"); +}; + +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); + return appendConstraintsAsFetchQueryWithTolerance(query, { source.path_constraints }, move_group, match_tolerance, + workspace_id, name_ + ".path_constraints"); +}; + +// 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); + return appendConstraintsAsInsertMetadata(metadata, source.trajectory_constraints.constraints, move_group, + workspace_id, name_ + ".trajectory_constraints.constraints"); +}; + +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); + return appendConstraintsAsFetchQueryWithTolerance(query, source.trajectory_constraints.constraints, move_group, + match_tolerance, workspace_id, + name_ + ".trajectory_constraints.constraints"); +}; + +} // namespace trajectory_cache +} // namespace moveit_ros diff --git a/moveit_ros/trajectory_cache/src/utils/utils.cpp b/moveit_ros/trajectory_cache/src/utils/utils.cpp index b2f3aa3cc6a..87fa0ca999a 100644 --- a/moveit_ros/trajectory_cache/src/utils/utils.cpp +++ b/moveit_ros/trajectory_cache/src/utils/utils.cpp @@ -111,7 +111,7 @@ void sortOrientationConstraints(std::vector constraints, const moveit::planning_interface::MoveGroupInterface& move_group, double match_tolerance, - const std::string& workspace_frame_id, const std::string& prefix) + const std::string& reference_frame_id, const std::string& prefix) { const std::shared_ptr tf_buffer = move_group.getTF(); @@ -179,7 +179,7 @@ moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance( if (!constraint.position_constraints.empty()) { // All offsets will be "frozen" and computed wrt. the workspace frame instead. - query.append(constraint_prefix + ".position_constraints.header.frame_id", workspace_frame_id); + query.append(constraint_prefix + ".position_constraints.header.frame_id", reference_frame_id); size_t position_idx = 0; for (auto& position_constraint : constraint.position_constraints) @@ -191,12 +191,12 @@ moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance( double y_offset = 0; double z_offset = 0; - if (workspace_frame_id != position_constraint.header.frame_id) + if (reference_frame_id != position_constraint.header.frame_id) { try { auto transform = - tf_buffer->lookupTransform(position_constraint.header.frame_id, workspace_frame_id, tf2::TimePointZero); + tf_buffer->lookupTransform(position_constraint.header.frame_id, reference_frame_id, tf2::TimePointZero); x_offset = transform.transform.translation.x; y_offset = transform.transform.translation.y; @@ -209,7 +209,7 @@ moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance( // supported. std::stringstream ss; ss << "Skipping " << prefix << " metadata append: " << "Could not get transform for translation " - << workspace_frame_id << " to " << position_constraint.header.frame_id << ": " << ex.what(); + << reference_frame_id << " to " << position_constraint.header.frame_id << ": " << ex.what(); return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE, ss.str()); } } @@ -229,7 +229,7 @@ moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance( if (!constraint.orientation_constraints.empty()) { // All offsets will be "frozen" and computed wrt. the workspace frame instead. - query.append(constraint_prefix + ".orientation_constraints.header.frame_id", workspace_frame_id); + query.append(constraint_prefix + ".orientation_constraints.header.frame_id", reference_frame_id); size_t ori_idx = 0; for (auto& orientation_constraint : constraint.orientation_constraints) @@ -243,11 +243,11 @@ moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance( quat_offset.z = 0; quat_offset.w = 1; - if (workspace_frame_id != orientation_constraint.header.frame_id) + if (reference_frame_id != orientation_constraint.header.frame_id) { try { - auto transform = tf_buffer->lookupTransform(orientation_constraint.header.frame_id, workspace_frame_id, + auto transform = tf_buffer->lookupTransform(orientation_constraint.header.frame_id, reference_frame_id, tf2::TimePointZero); quat_offset = transform.transform.rotation; @@ -259,7 +259,7 @@ moveit::core::MoveItErrorCode appendConstraintsAsFetchQueryWithTolerance( // supported. std::stringstream ss; ss << "Skipping " << prefix << " metadata append: " << "Could not get transform for orientation " - << workspace_frame_id << " to " << orientation_constraint.header.frame_id << ": " << ex.what(); + << reference_frame_id << " to " << orientation_constraint.header.frame_id << ": " << ex.what(); return moveit::core::MoveItErrorCode(moveit_msgs::msg::MoveItErrorCodes::FRAME_TRANSFORM_FAILURE, ss.str()); } } diff --git a/moveit_ros/trajectory_cache/test/CMakeLists.txt b/moveit_ros/trajectory_cache/test/CMakeLists.txt index c3ed6396c85..1c1f919dc84 100644 --- a/moveit_ros/trajectory_cache/test/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/test/CMakeLists.txt @@ -38,7 +38,7 @@ if(BUILD_TESTING) "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" "test_executable:=test_utils_with_move_group") - # Test features library. + # Test constant features library. ament_add_gtest_executable(test_constant_features_with_move_group features/test_constant_features_with_move_group.cpp) target_link_libraries(test_constant_features_with_move_group @@ -49,9 +49,16 @@ if(BUILD_TESTING) "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" "test_executable:=test_constant_features_with_move_group") - # ament_add_pytest_test( - # test_utils_with_move_group_py "utils/test_utils.py" WORKING_DIRECTORY - # "${CMAKE_CURRENT_BINARY_DIR}") + # Test MotionPlanRequest features library. + ament_add_gtest_executable(test_motion_plan_request_features_with_move_group + features/test_motion_plan_request_features_with_move_group.cpp) + target_link_libraries(test_motion_plan_request_features_with_move_group + moveit_ros_trajectory_features_lib + move_group_fixture) + add_ros_test(gtest_with_move_group.py + TARGET test_motion_plan_request_features_with_move_group TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_motion_plan_request_features_with_move_group") # This test executable is run by the pytest_test, since a node is required for # testing move groups diff --git a/moveit_ros/trajectory_cache/test/features/test_motion_plan_request_features_with_move_group.cpp b/moveit_ros/trajectory_cache/test/features/test_motion_plan_request_features_with_move_group.cpp new file mode 100644 index 00000000000..c01713d4a99 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/features/test_motion_plan_request_features_with_move_group.cpp @@ -0,0 +1,109 @@ +// 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 + * @author methylDragon + * + * Sanity tests for the moveit_msgs::msg::MotionPlanRequest feature extractors. + * + * @see motion_plan_request_features.hpp + * + * WARNING: + * These tests currently do not cover the implementation details, they are just the first sanity check for + * ensuring the most basic roundtrip functionality works. + * + * For example, some features might not have any resulting changes to the metadata or query due to + * the nature of what is contained in the MotionPlanRequest passed to them. + */ + +#include + +#include +#include +#include + +#include +#include + +#include "../move_group_fixture.hpp" + +namespace +{ + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +using ::moveit_msgs::msg::MotionPlanRequest; +using ::moveit_ros::trajectory_cache::FeaturesInterface; + +using ::moveit_ros::trajectory_cache::GoalConstraintsFeatures; +using ::moveit_ros::trajectory_cache::MaxSpeedAndAccelerationFeatures; +using ::moveit_ros::trajectory_cache::PathConstraintsFeatures; +using ::moveit_ros::trajectory_cache::StartStateJointStateFeatures; +using ::moveit_ros::trajectory_cache::TrajectoryConstraintsFeatures; +using ::moveit_ros::trajectory_cache::WorkspaceFeatures; + +TEST_F(MoveGroupFixture, MotionPlanRequestRoundTrip) +{ + // Test cases. + double match_tolerance = 0.001; + + std::vector>> features_under_test; + + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique()); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique(match_tolerance)); + features_under_test.push_back(std::make_unique()); + + // Setup. + ASSERT_TRUE(move_group_->setPoseTarget(move_group_->getRandomPose())); + moveit_msgs::msg::MotionPlanRequest msg; + move_group_->constructMotionPlanRequest(msg); + + // Core test. + for (auto& feature : features_under_test) + { + MessageCollection coll = + db_->openCollection("test_db", feature->getName()); + + SCOPED_TRACE(feature->getName()); + + Query::Ptr fuzzy_query = coll.createQuery(); + Query::Ptr exact_query = coll.createQuery(); + Metadata::Ptr metadata = coll.createMetadata(); + + EXPECT_TRUE(feature->appendFeaturesAsInsertMetadata(*metadata, msg, *move_group_)); + coll.insert(msg, metadata); + + EXPECT_TRUE(feature->appendFeaturesAsFuzzyFetchQuery(*fuzzy_query, msg, *move_group_, 0.0)); + EXPECT_TRUE(feature->appendFeaturesAsExactFetchQuery(*exact_query, msg, *move_group_, 0.0)); + + EXPECT_EQ(coll.queryList(fuzzy_query).size(), 1); + EXPECT_EQ(coll.queryList(exact_query).size(), 1); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} \ No newline at end of file