diff --git a/.github/workflows/nexus_integration_tests.yaml b/.github/workflows/nexus_integration_tests.yaml index af7b067..f9aadcd 100644 --- a/.github/workflows/nexus_integration_tests.yaml +++ b/.github/workflows/nexus_integration_tests.yaml @@ -40,6 +40,8 @@ jobs: rosdep update rosdep install --from-paths . -yir - name: build - run: /ros_entrypoint.sh colcon build --packages-up-to nexus_integration_tests --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache - - name: test + run: /ros_entrypoint.sh colcon build --packages-up-to nexus_integration_tests nexus_motion_planner --mixin release lld --cmake-args -DCMAKE_CXX_COMPILER_LAUNCHER=ccache + - name: Test - Unit Tests + run: . ./install/setup.bash && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh colcon test --packages-select nexus_motion_planner --event-handlers=console_direct+ + - name: Test - Integration Test run: . ./install/setup.bash && cd nexus_integration_tests && RMW_IMPLEMENTATION=rmw_cyclonedds_cpp /ros_entrypoint.sh python3 -m unittest diff --git a/README.md b/README.md index 9d4228d..c9db9f4 100644 --- a/README.md +++ b/README.md @@ -1,6 +1,6 @@ # NEXUS -![](https://github.com/OpenSourceRobotics/nexus/workflows/style/badge.svg) -![](https://github.com/OpenSourceRobotics/nexus/workflows/integration_tests/badge.svg) +![](https://github.com/osrf/nexus/workflows/style/badge.svg) +![](https://github.com/osrf/nexus/workflows/integration_tests/badge.svg) ![](./docs/media/nexus_architecture.png) @@ -30,7 +30,7 @@ curl --proto '=https' --tlsv1.2 -sSf https://sh.rustup.rs | sh ```bash mkdir ~/ws_nexus/src -p cd ~/ws_nexus/src/ -git clone git@github.com:OpenSourceRobotics/nexus +git clone git@github.com:osrf/nexus vcs import . < nexus/abb.repos cd ~/ws_nexus rosdep install --from-paths src --ignore-src --rosdistro iron -y -r @@ -50,7 +50,7 @@ See sample BTs for [system_orchestrator](nexus_integration_tests/config/system_o At present, capabilities are registered as plugins with the `orchestrator` at runtime. We support [these capabilities](./nexus_capabilities/src/capabilities/plugins.xml) out of the bo Each plugin may register one or more behavior tree nodes which can then be used to define processes as seen in the `place_on_conveyor.xml`. -The framework to register capabilities and map them to processes that can be performed in ongoing work. See [this discussion](https://github.com/OpenSourceRobotics/nexus/discussions/369) for more details. +The framework to register capabilities and map them to processes that can be performed in ongoing work. See [this discussion](https://github.com/osrf/nexus/discussions/32) for more details. ### Visualization [experimental] ![](./docs/media/bt_example.png) @@ -77,7 +77,7 @@ TODO: Add a dedicated demo package. The `nexus_endpoints` package contains ROS topics, services and actions used by NEXUS. The package is generated from [nexus_endpoints.redf.yaml](./nexus_endpoints.redf.yaml) using `redf`. rust is required to generated the package, the easiest way to install rust is via [rustup](https://rustup.rs/). -With rust installed, clone the redf repo at https://github.com/OpenSourceRobotics/redf, then run +With rust installed, clone the redf repo at https://github.com/osrf/redf, then run ```bash cargo install --path . ``` diff --git a/nexus_capabilities/src/capabilities/plan_motion.cpp b/nexus_capabilities/src/capabilities/plan_motion.cpp index 4d83079..b762a93 100644 --- a/nexus_capabilities/src/capabilities/plan_motion.cpp +++ b/nexus_capabilities/src/capabilities/plan_motion.cpp @@ -120,6 +120,17 @@ make_request() req->max_velocity_scaling_factor = scale_speed; req->max_acceleration_scaling_factor = scale_speed; + auto maybe_read_only = + this->getInput("force_cache_mode_execute_read_only"); + if (maybe_read_only.has_value()) + { + req->force_cache_mode_execute_read_only = maybe_read_only.value(); + } + else + { + req->force_cache_mode_execute_read_only = false; + } + RCLCPP_DEBUG_STREAM(this->_logger, "planning to " << req->goal_pose.pose << " at frame " << req->goal_pose.header.frame_id << " with cartesian " << req->cartesian << "and scaled speed of " << diff --git a/nexus_capabilities/src/capabilities/plan_motion.hpp b/nexus_capabilities/src/capabilities/plan_motion.hpp index ce72073..57dcc58 100644 --- a/nexus_capabilities/src/capabilities/plan_motion.hpp +++ b/nexus_capabilities/src/capabilities/plan_motion.hpp @@ -66,6 +66,9 @@ public: static BT::PortsList providedPorts() BT::InputPort>( "start_constraints", "OPTIONAL. If provided the the joint_constraints are used as the start condition. Else, the current state of the robot will be used as the start."), + BT::InputPort( + "force_cache_mode_execute_read_only", + "OPTIONAL. Set true to force cache mode to ExecuteReadOnly for this request."), BT::OutputPort( "result", "The resulting trajectory.")}; } diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt index 2ed33c7..e3d53a1 100644 --- a/nexus_motion_planner/CMakeLists.txt +++ b/nexus_motion_planner/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) find_package(trajectory_msgs REQUIRED) find_package(warehouse_ros REQUIRED) +find_package(warehouse_ros_sqlite REQUIRED) include_directories(include) @@ -27,11 +28,17 @@ set (motion_planner_server_dependencies rclcpp rclcpp_action rclcpp_lifecycle - warehouse_ros tf2 tf2_ros ) +set (motion_plan_cache_dependencies + moveit_ros_planning_interface + rclcpp + warehouse_ros + warehouse_ros_sqlite +) + set (test_request_dependencies moveit_msgs nexus_endpoints @@ -41,12 +48,17 @@ set (test_request_dependencies ) #=============================================================================== -set(LIBRARY_NAME motion_planner_server_core) +set(MOTION_PLAN_CACHE_LIBRARY_NAME motion_planner_server_core) set(EXECUTABLE_NAME motion_planner_server) +# Motion plan cache library +add_library(${MOTION_PLAN_CACHE_LIBRARY_NAME} src/motion_plan_cache.cpp) +ament_target_dependencies(${MOTION_PLAN_CACHE_LIBRARY_NAME} ${motion_plan_cache_dependencies}) + # Server executable add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp) ament_target_dependencies(${EXECUTABLE_NAME} ${motion_planner_server_dependencies}) +target_link_libraries(${EXECUTABLE_NAME} ${MOTION_PLAN_CACHE_LIBRARY_NAME}) # Test executable add_executable(test_request src/test_request.cpp) @@ -65,6 +77,7 @@ install( ) if(BUILD_TESTING) + find_package(ament_cmake_pytest REQUIRED) find_package(ament_cmake_uncrustify REQUIRED) find_package(launch_testing_ament_cmake REQUIRED) find_package(rmf_utils REQUIRED) @@ -73,6 +86,14 @@ if(BUILD_TESTING) NAMES "rmf_code_style.cfg" PATHS "${rmf_utils_DIR}/../../../../share/rmf_utils/") + add_executable(test_motion_plan_cache src/test_motion_plan_cache.cpp) + target_link_libraries(test_motion_plan_cache ${MOTION_PLAN_CACHE_LIBRARY_NAME}) + + install(TARGETS + test_motion_plan_cache + RUNTIME DESTINATION lib/${PROJECT_NAME} + ) + ament_uncrustify( ARGN include src test CONFIG_FILE ${uncrustify_config_file} @@ -80,12 +101,22 @@ if(BUILD_TESTING) LANGUAGE CPP ) - # TODO(YV): Fix these tests. - # add_launch_test( - # test/test_request.test.py - # TIMEOUT 60 - # ) + ament_add_pytest_test(test_motion_plan_cache_py "test/test_motion_plan_cache.py" + WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}" + ) + + # Motion planner server test with cache mode unset + add_launch_test( + test/test_request.test.py + TIMEOUT 60 + ) + + # Motion planner server test with cache mode set (so it uses the cache) + add_launch_test( + test/test_request_with_cache.test.py + TIMEOUT 60 + ) endif() -ament_export_dependencies(${motion_planner_server_dependencies}) +ament_export_dependencies(${motion_planner_server_dependencies} ${motion_plan_cache_dependencies}) ament_package() diff --git a/nexus_motion_planner/config/planner_params.yaml b/nexus_motion_planner/config/planner_params.yaml index 128c1f7..cfca6a5 100644 --- a/nexus_motion_planner/config/planner_params.yaml +++ b/nexus_motion_planner/config/planner_params.yaml @@ -41,18 +41,4 @@ motion_planner_server: # The max_z of workspace bounding box. workspace_max_z: 1.0 # The seconds within which the current robot state should be valid. - get_state_wait_seconds: 0.01 - # Namespaced parameters for each robot when use_namespace is true. - # Is important to ensure move_group, robot_state_publisher and - # joint_state broadcaster all have the same namespace. - # Note: If use_namespace is false, pass these parameters directly. - abb_irb1300: - robot_description: "" - robot_description_semantic: "" - group_name: "manipulator" - # Group specific kinematic properties. - manipulator: - kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin - kinematics_solver_search_resolution: 0.005 - kinematics_solver_timeout: 0.005 - kinematics_solver_attempts: 3 + get_state_wait_seconds: 0.01 \ No newline at end of file diff --git a/nexus_motion_planner/config/planner_params_with_cache.yaml b/nexus_motion_planner/config/planner_params_with_cache.yaml new file mode 100644 index 0000000..976ecb5 --- /dev/null +++ b/nexus_motion_planner/config/planner_params_with_cache.yaml @@ -0,0 +1,76 @@ +motion_planner_server: + ros__parameters: + # List of robots to generate motion plans for + manipulators: ["abb_irb1300"] + # The default moveit group for each manipulator + default_group_name: "manipulator" + # Seconds to wait to connect to an action or service server + timeout_duration: 5 + # Configure the planner_server to query motion plans directly from a move_group + # node that is running by initializing a move_group_interface. + # When set to false, the planner_server will initialize planning pipelines + # to directly compute plans but this has not been implemented yet. + use_move_group_interfaces: true + # When planning for multiple robots, set this to true. + use_namespace: false + # Tolerance for goal position. + goal_tolerance: 0.005 + # Maximum seconds to generate a plan. + planning_time: 1.0 + # Maximum number of replanning attempts + replan_attempts: 10 + # If true, the cartesian interpolator will check for collisions. + collision_aware_cartesian_path: false + # The value of the max_step parameter used in cartesian interpolation. + cartesian_max_step: 0.001 + # The value of the jump_threshold parameter used in cartesian interpolation. + cartesian_jump_threshold: 0.0 + # Set true if trajectory should be executed after a plan is generated. + # The execution is a blocking event. + execute_trajectory: false + # The min_x of workspace bounding box. + workspace_min_x: -1.0 + # The min_y of workspace bounding box. + workspace_min_y: -1.0 + # The min_z of workspace bounding box. + workspace_min_z: -1.0 + # The max_x of workspace bounding box. + workspace_max_x: 1.0 + # The max_y of workspace bounding box. + workspace_max_y: 1.0 + # The max_z of workspace bounding box. + workspace_max_z: 1.0 + # The seconds within which the current robot state should be valid. + get_state_wait_seconds: 0.01 + + ## Motion Plan Cache Parameters + # Planner database mode. Valid values: + # - Unset: Always plan. No caching. + # - TrainingOverwrite: Always plan, overwriting existing cache plans if better plan was found. + # - TrainingAppendOnly: Always plan, append to cache if better plan was found (no overwriting). + # - ExecuteBestEffort: Prioritize cached plans. Only plan if no cached plans found. + # - ExecuteReadOnly: Only use cached plans. Fail if no cached plans found. + planner_database_mode: "TrainingOverwrite" + # Database type and location + cache_db_plugin: "warehouse_ros_sqlite::DatabaseConnection" + cache_db_host: ":memory:" + cache_db_port: 0 # Isn't used for SQLite3 + + # The cache keys cache entries on certain properties of the motion plan request's + # starting joint state, goal constraints, and more. + + # For float comparisons, what tolerance counts as an exact match (to prevent floating point precision errors) + cache_exact_match_tolerance: 0.001 # ~0.05 degrees per joint + # Query range thresholds for matching attributes of the starting robot state for cache hits. + # This typically applies to joint states, per joint, in radians + # + # It should be acceptable to be more lenient on the start state, because the robot will be commanded + # to go to the first trajectory point from it's current start state. + cache_start_match_tolerance: 0.025 + # Query range thresholds for matching attributes of the requested planning goals for cache hits. + # This typically applies to the x, y, z coordinates of the goal pose, in metres + # And also the individual quaternion components of the goal orientation + # + # Goal tolerances should be more strict so the end point of the robot remains consistent between + # the fetched plan and the desired goal. + cache_goal_match_tolerance: 0.001 \ No newline at end of file diff --git a/nexus_motion_planner/package.xml b/nexus_motion_planner/package.xml index d23de1d..40ef807 100644 --- a/nexus_motion_planner/package.xml +++ b/nexus_motion_planner/package.xml @@ -28,9 +28,17 @@ abb_irb1300_10_115_moveit_config abb_irb1300_support + ament_cmake_pytest ament_cmake_uncrustify + launch_pytest launch_testing_ament_cmake + + moveit_configs_utils + moveit_resources + python3-pytest rmf_utils + robot_state_publisher + ros2_control ament_cmake diff --git a/nexus_motion_planner/src/main.cpp b/nexus_motion_planner/src/main.cpp index 655af9f..c1d7353 100644 --- a/nexus_motion_planner/src/main.cpp +++ b/nexus_motion_planner/src/main.cpp @@ -27,7 +27,7 @@ int main(int argc, char** argv) setvbuf(stdout, NULL, _IONBF, BUFSIZ); rclcpp::init(argc, argv); - auto node = std::make_shared(); + auto node = std::make_shared(); rclcpp::spin(node->get_node_base_interface()); rclcpp::shutdown(); return 0; diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp new file mode 100644 index 0000000..7712b24 --- /dev/null +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -0,0 +1,1617 @@ +// Copyright 2022 Johnson & Johnson +// +// 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. + +#include +#include + +#include "moveit/robot_state/conversions.h" +#include "moveit/robot_state/robot_state.h" +#include "warehouse_ros/database_loader.h" + +#include "rclcpp/rclcpp.hpp" +#include "rclcpp/logging.hpp" +#include "motion_plan_cache.hpp" + +namespace nexus { +namespace motion_planner { + +using warehouse_ros::MessageWithMetadata; +using warehouse_ros::Metadata; +using warehouse_ros::Query; + +// Utils ======================================================================= + +// Append a range inclusive query with some tolerance about some center value. +void query_append_range_inclusive_with_tolerance( + Query& query, const std::string& name, double center, double tolerance) +{ + query.appendRangeInclusive( + name, center - tolerance / 2, center + tolerance / 2); +} + +// Sort constraint components by joint or link name. +void sort_constraints( + std::vector& joint_constraints, + std::vector& position_constraints, + std::vector& orientation_constraints) +{ + std::sort( + joint_constraints.begin(), joint_constraints.end(), + []( + const moveit_msgs::msg::JointConstraint& l, + const moveit_msgs::msg::JointConstraint& r) + { + return l.joint_name < r.joint_name; + }); + + std::sort( + position_constraints.begin(), position_constraints.end(), + []( + const moveit_msgs::msg::PositionConstraint& l, + const moveit_msgs::msg::PositionConstraint& r) + { + return l.link_name < r.link_name; + }); + + std::sort( + orientation_constraints.begin(), orientation_constraints.end(), + []( + const moveit_msgs::msg::OrientationConstraint& l, + const moveit_msgs::msg::OrientationConstraint& r) + { + return l.link_name < r.link_name; + }); +} + +// Motion Plan Cache =========================================================== + +MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) +: node_(node) +{ + tf_buffer_ = + std::make_unique(node_->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); +} + +bool MotionPlanCache::init( + const std::string& db_path, uint32_t db_port, double exact_match_precision) +{ + RCLCPP_INFO( + node_->get_logger(), + "Opening motion plan cache database at: %s (Port: %d, Precision: %f)", + db_path.c_str(), db_port, exact_match_precision); + + // If the `warehouse_plugin` parameter isn't set, defaults to warehouse_ros' + // default. + warehouse_ros::DatabaseLoader loader(node_); + db_ = loader.loadDatabase(); + + exact_match_precision_ = exact_match_precision; + db_->setParams(db_path, db_port); + return db_->connect(); +} + +unsigned +MotionPlanCache::count_plans(const std::string& move_group_namespace) +{ + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + return coll.count(); +} + +unsigned +MotionPlanCache::count_cartesian_plans(const std::string& move_group_namespace) +{ + auto coll = db_->openCollection( + "move_group_cartesian_plan_cache", move_group_namespace); + return coll.count(); +} + +// ============================================================================= +// MOTION PLAN CACHING +// ============================================================================= +// MOTION PLAN CACHING: TOP LEVEL OPS +std::vector::ConstPtr> +MotionPlanCache::fetch_all_matching_plans( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) +{ + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = this->extract_and_append_plan_start_to_query( + *query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extract_and_append_plan_goal_to_query( + *query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR(node_->get_logger(), "Could not construct plan query."); + return {}; + } + + return coll.queryList(query, metadata_only, sort_by, true); +} + +MessageWithMetadata::ConstPtr +MotionPlanCache::fetch_best_matching_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_plans = this->fetch_all_matching_plans( + move_group, move_group_namespace, + plan_request, start_tolerance, goal_tolerance, true, sort_by); + + if (matching_plans.empty()) + { + RCLCPP_INFO(node_->get_logger(), "No matching plans found."); + return nullptr; + } + + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_plan_id = matching_plans.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_plan_id); + + return coll.findOne(best_query, metadata_only); +} + +bool +MotionPlanCache::put_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& plan, + double execution_time_s, double planning_time_s, bool delete_worse_plans) +{ + // Check pre-conditions + if (!plan.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping plan insert: Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.workspace_parameters.header.frame_id.empty() || + plan.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), "Skipping plan insert: Frame IDs cannot be empty."); + return false; + } + if (plan_request.workspace_parameters.header.frame_id != + plan.joint_trajectory.header.frame_id) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping plan insert: " + "Plan request frame (%s) does not match plan frame (%s).", + plan_request.workspace_parameters.header.frame_id.c_str(), + plan.joint_trajectory.header.frame_id.c_str()); + return false; + } + + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + + // Pull out plans "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = this->extract_and_append_plan_start_to_query( + *exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extract_and_append_plan_goal_to_query( + *exact_query, move_group, plan_request, 0); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping plan insert: Could not construct lookup query."); + return false; + } + + auto exact_matches = coll.queryList( + exact_query, /* metadata_only */ true, /* sort_by */ "execution_time_s"); + + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (delete_worse_plans) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_INFO( + node_->get_logger(), + "Overwriting plan (id: %d): " + "execution_time (%es) > new plan's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); + + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); + } + } + } + } + + // Insert if candidate is best seen. + if (execution_time_s < best_execution_time) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = this->extract_and_append_plan_start_to_metadata( + *insert_metadata, move_group, plan_request); + bool goal_meta_ok = this->extract_and_append_plan_goal_to_metadata( + *insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + insert_metadata->append("planning_time_s", planning_time_s); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping plan insert: Could not construct insert metadata."); + return false; + } + + RCLCPP_INFO( + node_->get_logger(), + "Inserting plan: New plan execution_time (%es) " + "is better than best plan's execution_time (%es)", + execution_time_s, best_execution_time); + + coll.insert(plan, insert_metadata); + return true; + } + + RCLCPP_INFO( + node_->get_logger(), + "Skipping plan insert: New plan execution_time (%es) " + "is worse than best plan's execution_time (%es)", + execution_time_s, best_execution_time); + return false; +} + +// MOTION PLAN CACHING: QUERY CONSTRUCTION +bool +MotionPlanCache::extract_and_append_plan_start_to_query( + Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *query; // Copy not supported. + + query.append("group_name", plan_request.group_name); + + // Workspace params + // Match anything within our specified workspace limits. + query.append( + "workspace_parameters.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + query.appendGTE( + "workspace_parameters.min_corner.x", + plan_request.workspace_parameters.min_corner.x); + query.appendGTE( + "workspace_parameters.min_corner.y", + plan_request.workspace_parameters.min_corner.y); + query.appendGTE( + "workspace_parameters.min_corner.z", + plan_request.workspace_parameters.min_corner.z); + query.appendLTE( + "workspace_parameters.max_corner.x", + plan_request.workspace_parameters.max_corner.x); + query.appendLTE( + "workspace_parameters.max_corner.y", + plan_request.workspace_parameters.max_corner.y); + query.appendLTE( + "workspace_parameters.max_corner.z", + plan_request.workspace_parameters.max_corner.z); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.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. + // + // When upstreaming this class to MoveIt, 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( + node_->get_logger(), + "Skipping start query append: Could not get robot state."); + // *query = original; // Undo our changes. (Can't. Copy not supported.) + return false; + } + + 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( + "start_state.joint_state.name_" + std::to_string(i), + current_state_msg.joint_state.name.at(i)); + query_append_range_inclusive_with_tolerance( + query, "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 < plan_request.start_state.joint_state.name.size(); i++ + ) + { + query.append( + "start_state.joint_state.name_" + std::to_string(i), + plan_request.start_state.joint_state.name.at(i)); + query_append_range_inclusive_with_tolerance( + query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); + } + } + return true; +} + +bool +MotionPlanCache::extract_and_append_plan_goal_to_query( + Query& query, + const moveit::planning_interface::MoveGroupInterface& /* move_group */, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + bool emit_position_constraint_warning = false; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& position_constraint : constraint.position_constraints) + { + if (!position_constraint.constraint_region.primitives.empty()) + { + emit_position_constraint_warning = true; + break; + } + } + if (emit_position_constraint_warning) + { + break; + } + } + if (emit_position_constraint_warning) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring goal_constraints.position_constraints.constraint_region: " + "Not supported."); + } + + // auto original = *query; // Copy not supported. + + query_append_range_inclusive_with_tolerance( + query, "max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, "max_cartesian_speed", + plan_request.max_cartesian_speed, match_tolerance); + + // Extract constraints (so we don't have cardinality on goal_constraint idx.) + std::vector joint_constraints; + std::vector position_constraints; + std::vector orientation_constraints; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& joint_constraint : constraint.joint_constraints) + { + joint_constraints.push_back(joint_constraint); + } + for (auto& position_constraint : constraint.position_constraints) + { + position_constraints.push_back(position_constraint); + } + for (auto& orientation_constraint : constraint.orientation_constraints) + { + orientation_constraints.push_back(orientation_constraint); + } + + // Also sort for even less cardinality. + sort_constraints( + joint_constraints, position_constraints, orientation_constraints); + } + + // Joint constraints + size_t joint_idx = 0; + for (auto& constraint : joint_constraints) + { + std::string meta_name = + "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); + + query.append(meta_name + ".joint_name", constraint.joint_name); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".position", constraint.position, match_tolerance); + query.appendGTE(meta_name + ".tolerance_above", constraint.tolerance_above); + query.appendLTE(meta_name + ".tolerance_below", constraint.tolerance_below); + } + + // Position constraints + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + if (!position_constraints.empty()) + { + query.append( + "goal_constraints.position_constraints.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + + size_t pos_idx = 0; + for (auto& constraint : position_constraints) + { + std::string meta_name = + "goal_constraints.position_constraints_" + std::to_string(pos_idx++); + + // Compute offsets wrt. to workspace frame. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + if (plan_request.workspace_parameters.header.frame_id != + constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, + plan_request.workspace_parameters.header.frame_id, + tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN( + node_->get_logger(), + "Skipping goal query append: " + "Could not get goal transform for translation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), + constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *query = original; // Undo our changes. + return false; + } + } + + query.append(meta_name + ".link_name", constraint.link_name); + + query_append_range_inclusive_with_tolerance( + query, meta_name + ".target_point_offset.x", + x_offset + constraint.target_point_offset.x, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".target_point_offset.y", + y_offset + constraint.target_point_offset.y, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".target_point_offset.z", + z_offset + constraint.target_point_offset.z, match_tolerance); + } + } + + // Orientation constraints + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + if (!orientation_constraints.empty()) + { + query.append( + "goal_constraints.orientation_constraints.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + + size_t ori_idx = 0; + for (auto& constraint : orientation_constraints) + { + std::string meta_name = + "goal_constraints.orientation_constraints_" + std::to_string(ori_idx++); + + // Compute offsets wrt. to workspace frame. + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (plan_request.workspace_parameters.header.frame_id != + constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, + plan_request.workspace_parameters.header.frame_id, + tf2::TimePointZero); + + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN( + node_->get_logger(), + "Skipping goal query append: " + "Could not get goal transform for orientation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), + constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *query = original; // Undo our changes. + return false; + } + } + + query.append(meta_name + ".link_name", constraint.link_name); + + // Orientation of constraint frame wrt. workspace frame + tf2::Quaternion tf2_quat_frame_offset( + quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + + // Added offset on top of the constraint frame's orientation stated in + // the constraint. + tf2::Quaternion tf2_quat_goal_offset( + constraint.orientation.x, + constraint.orientation.y, + constraint.orientation.z, + constraint.orientation.w); + + tf2_quat_frame_offset.normalize(); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + query_append_range_inclusive_with_tolerance( + query, meta_name + ".target_point_offset.x", + final_quat.getX(), match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".target_point_offset.y", + final_quat.getY(), match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".target_point_offset.z", + final_quat.getZ(), match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".target_point_offset.w", + final_quat.getW(), match_tolerance); + } + } + + return true; +} + +// MOTION PLAN CACHING: METADATA CONSTRUCTION +bool +MotionPlanCache::extract_and_append_plan_start_to_metadata( + Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request) +{ + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("group_name", plan_request.group_name); + + // Workspace params + metadata.append( + "workspace_parameters.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + metadata.append( + "workspace_parameters.min_corner.x", + plan_request.workspace_parameters.min_corner.x); + metadata.append( + "workspace_parameters.min_corner.y", + plan_request.workspace_parameters.min_corner.y); + metadata.append( + "workspace_parameters.min_corner.z", + plan_request.workspace_parameters.min_corner.z); + metadata.append( + "workspace_parameters.max_corner.x", + plan_request.workspace_parameters.max_corner.x); + metadata.append( + "workspace_parameters.max_corner.y", + plan_request.workspace_parameters.max_corner.y); + metadata.append( + "workspace_parameters.max_corner.z", + plan_request.workspace_parameters.max_corner.z); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.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. + // + // When upstreaming this class to MoveIt, 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( + node_->get_logger(), + "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy not supported + return false; + } + + 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( + "start_state.joint_state.name_" + std::to_string(i), + current_state_msg.joint_state.name.at(i)); + metadata.append( + "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i)); + } + } + else + { + for ( + size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++ + ) + { + metadata.append( + "start_state.joint_state.name_" + std::to_string(i), + plan_request.start_state.joint_state.name.at(i)); + metadata.append( + "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i)); + } + } + + return true; +} + +bool +MotionPlanCache::extract_and_append_plan_goal_to_metadata( + Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& /* move_group */, + const moveit_msgs::msg::MotionPlanRequest& plan_request) +{ + // Make ignored members explicit + bool emit_position_constraint_warning = false; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& position_constraint : constraint.position_constraints) + { + if (!position_constraint.constraint_region.primitives.empty()) + { + emit_position_constraint_warning = true; + break; + } + } + if (emit_position_constraint_warning) + { + break; + } + } + if (emit_position_constraint_warning) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring goal_constraints.position_constraints.constraint_region: " + "Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor); + metadata.append("max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor); + metadata.append("max_cartesian_speed", plan_request.max_cartesian_speed); + + // Extract constraints (so we don't have cardinality on goal_constraint idx.) + std::vector joint_constraints; + std::vector position_constraints; + std::vector orientation_constraints; + for (auto& constraint : plan_request.goal_constraints) + { + for (auto& joint_constraint : constraint.joint_constraints) + { + joint_constraints.push_back(joint_constraint); + } + for (auto& position_constraint : constraint.position_constraints) + { + position_constraints.push_back(position_constraint); + } + for (auto& orientation_constraint : constraint.orientation_constraints) + { + orientation_constraints.push_back(orientation_constraint); + } + + // Also sort for even less cardinality. + sort_constraints( + joint_constraints, position_constraints, orientation_constraints); + } + + // Joint constraints + size_t joint_idx = 0; + for (auto& constraint : joint_constraints) + { + std::string meta_name = + "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); + + metadata.append(meta_name + ".joint_name", constraint.joint_name); + metadata.append(meta_name + ".position", constraint.position); + metadata.append( + meta_name + ".tolerance_above", constraint.tolerance_above); + metadata.append( + meta_name + ".tolerance_below", constraint.tolerance_below); + } + + // Position constraints + if (!position_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + metadata.append( + "goal_constraints.position_constraints.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + + size_t position_idx = 0; + for (auto& constraint : position_constraints) + { + std::string meta_name = + "goal_constraints.position_constraints_" + + std::to_string(position_idx++); + + // Compute offsets wrt. to workspace frame. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + if (plan_request.workspace_parameters.header.frame_id != + constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, + plan_request.workspace_parameters.header.frame_id, + tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN( + node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for translation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), + constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } + + metadata.append(meta_name + ".link_name", constraint.link_name); + + metadata.append( + meta_name + ".target_point_offset.x", + x_offset + constraint.target_point_offset.x); + metadata.append( + meta_name + ".target_point_offset.y", + y_offset + constraint.target_point_offset.y); + metadata.append( + meta_name + ".target_point_offset.z", + z_offset + constraint.target_point_offset.z); + } + } + + // Orientation constraints + if (!orientation_constraints.empty()) + { + // All offsets will be "frozen" and computed wrt. the workspace frame + // instead. + metadata.append( + "goal_constraints.orientation_constraints.header.frame_id", + plan_request.workspace_parameters.header.frame_id); + + size_t ori_idx = 0; + for (auto& constraint : orientation_constraints) + { + std::string meta_name = + "goal_constraints.orientation_constraints_" + std::to_string(ori_idx++); + + // Compute offsets wrt. to workspace frame. + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (plan_request.workspace_parameters.header.frame_id != + constraint.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + constraint.header.frame_id, + plan_request.workspace_parameters.header.frame_id, + tf2::TimePointZero); + + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN( + node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for orientation %s to %s: %s", + plan_request.workspace_parameters.header.frame_id.c_str(), + constraint.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } + + metadata.append(meta_name + ".link_name", constraint.link_name); + + // Orientation of constraint frame wrt. workspace frame + tf2::Quaternion tf2_quat_frame_offset( + quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + + // Added offset on top of the constraint frame's orientation stated in + // the constraint. + tf2::Quaternion tf2_quat_goal_offset( + constraint.orientation.x, + constraint.orientation.y, + constraint.orientation.z, + constraint.orientation.w); + + tf2_quat_frame_offset.normalize(); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + metadata.append(meta_name + ".target_point_offset.x", + final_quat.getX()); + metadata.append(meta_name + ".target_point_offset.y", + final_quat.getY()); + metadata.append(meta_name + ".target_point_offset.z", + final_quat.getZ()); + metadata.append(meta_name + ".target_point_offset.w", + final_quat.getW()); + } + } + + return true; +} + +// ============================================================================= +// CARTESIAN PLAN CACHING +// ============================================================================= +// CARTESIAN PLAN CACHING: TOP LEVEL OPS +moveit_msgs::srv::GetCartesianPath::Request +MotionPlanCache::construct_get_cartesian_plan_request( + moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, + double step, double jump_threshold, bool avoid_collisions) +{ + moveit_msgs::srv::GetCartesianPath::Request out; + + // Some of these parameters need us to pull PRIVATE values out of the + // move_group elsewhere... Yes, it is very cursed and I hate it. + // Fixing it requires fixing it in MoveIt. + moveit_msgs::msg::MotionPlanRequest tmp; + move_group.constructMotionPlanRequest(tmp); + + out.start_state = std::move(tmp.start_state); + out.group_name = std::move(tmp.group_name); + out.max_velocity_scaling_factor = tmp.max_velocity_scaling_factor; + out.max_acceleration_scaling_factor = tmp.max_acceleration_scaling_factor; + + out.header.frame_id = move_group.getPoseReferenceFrame(); + out.waypoints = waypoints; + out.max_step = step; + out.jump_threshold = jump_threshold; + out.path_constraints = moveit_msgs::msg::Constraints(); + out.avoid_collisions = avoid_collisions; + out.link_name = move_group.getEndEffectorLink(); + + // We were already cursed before, now we are double cursed... + // move_group.getNodeHandle() is UNIMPLEMENTED upstream. + out.header.stamp = node_->now(); + + return out; +} + +std::vector::ConstPtr> +MotionPlanCache::fetch_all_matching_cartesian_plans( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double min_fraction, + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) +{ + auto coll = db_->openCollection( + "move_group_cartesian_plan_cache", move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = this->extract_and_append_cartesian_plan_start_to_query( + *query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extract_and_append_cartesian_plan_goal_to_query( + *query, move_group, plan_request, goal_tolerance); + + if (!start_ok || !goal_ok) + { + RCLCPP_ERROR( + node_->get_logger(), "Could not construct cartesian plan query."); + return {}; + } + + query->appendGTE("fraction", min_fraction); + return coll.queryList(query, metadata_only, sort_by, true); +} + +MessageWithMetadata::ConstPtr +MotionPlanCache::fetch_best_matching_cartesian_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double min_fraction, + double start_tolerance, double goal_tolerance, bool metadata_only, + const std::string& sort_by) +{ + // First find all matching, but metadata only. + // Then use the ID metadata of the best plan to pull the actual message. + auto matching_plans = this->fetch_all_matching_cartesian_plans( + move_group, move_group_namespace, + plan_request, min_fraction, + start_tolerance, goal_tolerance, true, sort_by); + + if (matching_plans.empty()) + { + RCLCPP_INFO(node_->get_logger(), "No matching cartesian plans found."); + return nullptr; + } + + auto coll = db_->openCollection( + "move_group_cartesian_plan_cache", move_group_namespace); + + // Best plan is at first index, since the lookup query was sorted by + // execution_time. + int best_plan_id = matching_plans.at(0)->lookupInt("id"); + Query::Ptr best_query = coll.createQuery(); + best_query->append("id", best_plan_id); + + return coll.findOne(best_query, metadata_only); +} + +bool +MotionPlanCache::put_cartesian_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& plan, + double execution_time_s, + double planning_time_s, + double fraction, + bool delete_worse_plans) +{ + // Check pre-conditions + if (!plan.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping cartesian plan insert: " + "Multi-DOF trajectory plans are not supported."); + return false; + } + if (plan_request.header.frame_id.empty() || + plan.joint_trajectory.header.frame_id.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping cartesian plan insert: Frame IDs cannot be empty."); + return false; + } + + auto coll = db_->openCollection( + "move_group_cartesian_plan_cache", move_group_namespace); + + // Pull out plans "exactly" keyed by request in cache. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = this->extract_and_append_cartesian_plan_start_to_query( + *exact_query, move_group, plan_request, 0); + bool goal_query_ok = this->extract_and_append_cartesian_plan_goal_to_query( + *exact_query, move_group, plan_request, 0); + exact_query->append("fraction", fraction); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping cartesian plan insert: Could not construct lookup query."); + return false; + } + + auto exact_matches = coll.queryList( + exact_query, /* metadata_only */ true, /* sort_by */ "execution_time_s"); + double best_execution_time = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + best_execution_time = exact_matches.at(0)->lookupDouble("execution_time_s"); + + if (delete_worse_plans) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = match->lookupDouble("execution_time_s"); + if (execution_time_s < match_execution_time_s) + { + int delete_id = match->lookupInt("id"); + RCLCPP_INFO( + node_->get_logger(), + "Overwriting cartesian plan (id: %d): " + "execution_time (%es) > new plan's execution_time (%es)", + delete_id, match_execution_time_s, execution_time_s); + + Query::Ptr delete_query = coll.createQuery(); + delete_query->append("id", delete_id); + coll.removeMessages(delete_query); + } + } + } + } + + // Insert if candidate is best seen. + if (execution_time_s < best_execution_time) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = + this->extract_and_append_cartesian_plan_start_to_metadata( + *insert_metadata, move_group, plan_request); + bool goal_meta_ok = + this->extract_and_append_cartesian_plan_goal_to_metadata( + *insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + insert_metadata->append("planning_time_s", planning_time_s); + insert_metadata->append("fraction", fraction); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping cartesian plan insert: Could not construct insert metadata."); + return false; + } + + RCLCPP_INFO( + node_->get_logger(), + "Inserting cartesian plan: New plan execution_time (%es) " + "is better than best plan's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); + + coll.insert(plan, insert_metadata); + return true; + } + + RCLCPP_INFO( + node_->get_logger(), + "Skipping cartesian plan insert: New plan execution_time (%es) " + "is worse than best plan's execution_time (%es) at fraction (%es)", + execution_time_s, best_execution_time, fraction); + return false; +} + +// // CARTESIAN PLAN CACHING: QUERY CONSTRUCTION +bool +MotionPlanCache::extract_and_append_cartesian_plan_start_to_query( + Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + query.append("group_name", plan_request.group_name); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.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. + // + // When upstreaming this class to MoveIt, 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( + node_->get_logger(), + "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy not supported + return false; + } + + 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( + "start_state.joint_state.name_" + std::to_string(i), + current_state_msg.joint_state.name.at(i)); + query_append_range_inclusive_with_tolerance( + query, "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 < plan_request.start_state.joint_state.name.size(); i++ + ) + { + query.append( + "start_state.joint_state.name_" + std::to_string(i), + plan_request.start_state.joint_state.name.at(i)); + query_append_range_inclusive_with_tolerance( + query, "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i), match_tolerance); + } + } + + return true; +} + +bool +MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( + Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double match_tolerance) +{ + match_tolerance += exact_match_precision_; + + // Make ignored members explicit + if (!plan_request.path_constraints.joint_constraints.empty() || + !plan_request.path_constraints.position_constraints.empty() || + !plan_request.path_constraints.orientation_constraints.empty() || + !plan_request.path_constraints.visibility_constraints.empty()) + { + RCLCPP_WARN( + node_->get_logger(), "Ignoring path_constraints: Not supported."); + } + if (plan_request.avoid_collisions) + { + RCLCPP_WARN(node_->get_logger(), + "Ignoring avoid_collisions: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + query_append_range_inclusive_with_tolerance( + query, "max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, "max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, "max_step", plan_request.max_step, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, "jump_threshold", plan_request.jump_threshold, match_tolerance); + + // Waypoints + // Restating them in terms of the robot model frame (usually base_link) + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + // Compute offsets. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (base_frame != plan_request.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + plan_request.header.frame_id, base_frame, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN( + node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for %s to %s: %s", + base_frame.c_str(), plan_request.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } + + tf2::Quaternion tf2_quat_frame_offset( + quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); + + size_t waypoint_idx = 0; + for (auto& waypoint : plan_request.waypoints) + { + std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); + + // Apply offsets + // Position + query_append_range_inclusive_with_tolerance( + query, meta_name + ".position.x", + x_offset + waypoint.position.x, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".position.y", + y_offset + waypoint.position.y, match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".position.z", + z_offset + waypoint.position.z, match_tolerance); + + // Orientation + tf2::Quaternion tf2_quat_goal_offset( + waypoint.orientation.x, + waypoint.orientation.y, + waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + query_append_range_inclusive_with_tolerance( + query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + query_append_range_inclusive_with_tolerance( + query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); + } + + query.append("link_name", plan_request.link_name); + query.append("header.frame_id", base_frame); + + return true; +} + +// CARTESIAN PLAN CACHING: METADATA CONSTRUCTION +bool +MotionPlanCache::extract_and_append_cartesian_plan_start_to_metadata( + Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) +{ + // Make ignored members explicit + if (!plan_request.start_state.multi_dof_joint_state.joint_names.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring start_state.multi_dof_joint_states: Not supported."); + } + if (!plan_request.start_state.attached_collision_objects.empty()) + { + RCLCPP_WARN( + node_->get_logger(), + "Ignoring start_state.attached_collision_objects: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("group_name", plan_request.group_name); + + // Joint state + // Only accounts for joint_state position. Ignores velocity and effort. + if (plan_request.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. + // + // When upstreaming this class to MoveIt, 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( + node_->get_logger(), + "Skipping start metadata append: Could not get robot state."); + // *metadata = original; // Undo our changes. // Copy not supported + return false; + } + + 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( + "start_state.joint_state.name_" + std::to_string(i), + current_state_msg.joint_state.name.at(i)); + metadata.append( + "start_state.joint_state.position_" + std::to_string(i), + current_state_msg.joint_state.position.at(i)); + } + } + else + { + for ( + size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++ + ) + { + metadata.append( + "start_state.joint_state.name_" + std::to_string(i), + plan_request.start_state.joint_state.name.at(i)); + metadata.append( + "start_state.joint_state.position_" + std::to_string(i), + plan_request.start_state.joint_state.position.at(i)); + } + } + + return true; +} + +bool +MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( + Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request) +{ + // Make ignored members explicit + if (!plan_request.path_constraints.joint_constraints.empty() || + !plan_request.path_constraints.position_constraints.empty() || + !plan_request.path_constraints.orientation_constraints.empty() || + !plan_request.path_constraints.visibility_constraints.empty()) + { + RCLCPP_WARN( + node_->get_logger(), "Ignoring path_constraints: Not supported."); + } + if (plan_request.avoid_collisions) + { + RCLCPP_WARN(node_->get_logger(), + "Ignoring avoid_collisions: Not supported."); + } + + // auto original = *metadata; // Copy not supported. + + metadata.append("max_velocity_scaling_factor", + plan_request.max_velocity_scaling_factor); + metadata.append("max_acceleration_scaling_factor", + plan_request.max_acceleration_scaling_factor); + metadata.append("max_step", plan_request.max_step); + metadata.append("jump_threshold", plan_request.jump_threshold); + + // Waypoints + // Restating them in terms of the robot model frame (usually base_link) + std::string base_frame = move_group.getRobotModel()->getModelFrame(); + + // Compute offsets. + double x_offset = 0; + double y_offset = 0; + double z_offset = 0; + + geometry_msgs::msg::Quaternion quat_offset; + quat_offset.x = 0; + quat_offset.y = 0; + quat_offset.z = 0; + quat_offset.w = 1; + + if (base_frame != plan_request.header.frame_id) + { + try + { + auto transform = tf_buffer_->lookupTransform( + plan_request.header.frame_id, base_frame, tf2::TimePointZero); + + x_offset = transform.transform.translation.x; + y_offset = transform.transform.translation.y; + z_offset = transform.transform.translation.z; + quat_offset = transform.transform.rotation; + } + catch (tf2::TransformException& ex) + { + RCLCPP_WARN( + node_->get_logger(), + "Skipping goal metadata append: " + "Could not get goal transform for %s to %s: %s", + base_frame.c_str(), plan_request.header.frame_id.c_str(), + ex.what()); + + // (Can't. Copy not supported.) + // *metadata = original; // Undo our changes. + return false; + } + } + + tf2::Quaternion tf2_quat_frame_offset( + quat_offset.x, quat_offset.y, quat_offset.z, quat_offset.w); + tf2_quat_frame_offset.normalize(); + + size_t waypoint_idx = 0; + for (auto& waypoint : plan_request.waypoints) + { + std::string meta_name = "waypoints_" + std::to_string(waypoint_idx++); + + // Apply offsets + // Position + metadata.append(meta_name + ".position.x", x_offset + waypoint.position.x); + metadata.append(meta_name + ".position.y", y_offset + waypoint.position.y); + metadata.append(meta_name + ".position.z", z_offset + waypoint.position.z); + + // Orientation + tf2::Quaternion tf2_quat_goal_offset( + waypoint.orientation.x, + waypoint.orientation.y, + waypoint.orientation.z, + waypoint.orientation.w); + tf2_quat_goal_offset.normalize(); + + auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; + final_quat.normalize(); + + metadata.append(meta_name + ".orientation.x", final_quat.getX()); + metadata.append(meta_name + ".orientation.y", final_quat.getY()); + metadata.append(meta_name + ".orientation.z", final_quat.getZ()); + metadata.append(meta_name + ".orientation.w", final_quat.getW()); + } + + metadata.append("link_name", plan_request.link_name); + metadata.append("header.frame_id", base_frame); + + return true; +} + +} // namespace motion_planner +} // namespace nexus diff --git a/nexus_motion_planner/src/motion_plan_cache.hpp b/nexus_motion_planner/src/motion_plan_cache.hpp new file mode 100644 index 0000000..fc6143c --- /dev/null +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -0,0 +1,275 @@ +// Copyright 2022 Johnson & Johnson +// +// 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. + +#ifndef SRC__MOTION_PLAN_CACHE_HPP +#define SRC__MOTION_PLAN_CACHE_HPP + +#include +#include +#include + +#include +#include + +#include + +// TF2 +#include +#include + +// ROS2 Messages +#include +#include +#include + +// moveit modules +#include +#include + +namespace nexus { +namespace motion_planner { + +/** + * Cache manager for the Nexus motion planner. + * + * This manager facilitates cache management for MoveIt 2's `MoveGroupInterface` + * by using `warehouse_ros` to manage a database of executed motion plans, and + * how long they took to execute. This allows for the lookup and reuse of the + * best performing plans found so far. + * + * WARNING: + * This cache does NOT support collision detection! + * Plans will be put into and fetched from the cache IGNORING collision. + * If your planning scene is expected to change between cache lookups, do NOT + * use this cache, fetched plans are likely to result in collision then. + * + * To handle collisions this class will need to hash the planning scene world + * msg (after zeroing out std_msgs/Header timestamps and sequences) and do an + * appropriate lookup. + * + * Relevant ROS Parameters: + * - `warehouse_plugin`: What database to use + * - `warehouse_host`: Where the database should be created + * - `warehouse_port`: The port used for the database + * + * Motion plans are stored in the `move_group_plan_cache` database within the + * database file, with plans for each move group stored in a collection named + * after the relevant move group's name. + * + * For example, the "my_move_group" move group will have its cache stored in + * `move_group_plan_cache@my_move_group` + * + * Motion plans are keyed on: + * - Plan Start: robot joint state + * - Plan Goal (either of): + * - Final pose (wrt. `planning_frame` (usually `base_link`)) + * - Final robot joint states + * + * Motion plans may be looked up with some tolerance at call time. + */ +class MotionPlanCache +{ +public: + // We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports + // it... + explicit MotionPlanCache(const rclcpp::Node::SharedPtr& node); + + bool init( + const std::string& db_path = ":memory:", + uint32_t db_port = 0, + double exact_match_precision = 1e-6); + + unsigned count_plans(const std::string& move_group_namespace); + + unsigned count_cartesian_plans(const std::string& move_group_namespace); + + // =========================================================================== + // MOTION PLAN CACHING + // =========================================================================== + // TOP LEVEL OPS + + // Fetches all plans that fit within the requested tolerances for start and + // goal conditions, returning them as a vector, sorted by some db column. + std::vector< + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + > + fetch_all_matching_plans( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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"); + + // Fetches the best plan that fits within the requested tolerances for start + // and goal conditions, by some db column. + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + fetch_best_matching_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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"); + + // Put a plan into the database if it is the best matching plan seen so far. + // + // Plans are matched based off their start and goal states. + // And are considered "better" if they have a smaller planned execution time + // than matching plans. + // + // Optionally deletes all worse plans by default to prune the cache. + bool put_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + const moveit_msgs::msg::RobotTrajectory& plan, + double execution_time_s, + double planning_time_s, + bool delete_worse_plans = true); + + // QUERY CONSTRUCTION + bool extract_and_append_plan_start_to_query( + warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + bool extract_and_append_plan_goal_to_query( + warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + // METADATA CONSTRUCTION + bool extract_and_append_plan_start_to_metadata( + warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + bool extract_and_append_plan_goal_to_metadata( + warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + // =========================================================================== + // CARTESIAN PLAN CACHING + // =========================================================================== + // TOP LEVEL OPS + + // This mimics the move group computeCartesianPath signature (without path + // constraints). + moveit_msgs::srv::GetCartesianPath::Request + construct_get_cartesian_plan_request( + moveit::planning_interface::MoveGroupInterface& move_group, + const std::vector& waypoints, + double step, + double jump_threshold, + bool avoid_collisions = true); + + // Fetches all cartesian plans that fit within the requested tolerances for + // start and goal conditions, returning them as a vector, sorted by some db + // column. + std::vector< + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + > + fetch_all_matching_cartesian_plans( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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"); + + // Fetches the best cartesian plan that fits within the requested tolerances + // for start and goal conditions, by some db column. + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + fetch_best_matching_cartesian_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_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"); + + // Put a cartesian plan into the database if it is the best matching cartesian + // plan seen so far. + // + // Cartesian plans are matched based off their start and goal states. + // And are considered "better" if they have a smaller planned execution time + // than matching cartesian plans. + // + // Optionally deletes all worse cartesian plans by default to prune the cache. + bool put_cartesian_plan( + const moveit::planning_interface::MoveGroupInterface& move_group, + const std::string& move_group_namespace, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + const moveit_msgs::msg::RobotTrajectory& plan, + double execution_time_s, + double planning_time_s, + double fraction, + bool delete_worse_plans = true); + + // QUERY CONSTRUCTION + bool extract_and_append_cartesian_plan_start_to_query( + warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double match_tolerance); + + bool extract_and_append_cartesian_plan_goal_to_query( + warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request, + double match_tolerance); + + // METADATA CONSTRUCTION + bool extract_and_append_cartesian_plan_start_to_metadata( + warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + + bool extract_and_append_cartesian_plan_goal_to_metadata( + warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& plan_request); + +private: + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + + double exact_match_precision_ = 1e-6; + + std::unique_ptr tf_buffer_; + std::shared_ptr tf_listener_; +}; + +} // namespace planning_interface +} // namespace moveit + +#endif // SRC__MOTION_PLAN_CACHE_HPP \ No newline at end of file diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 9c53046..2b7ced2 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -12,8 +12,62 @@ // See the License for the specific language governing permissions and // limitations under the License. +// TODO: Catch warehouse_ros exceptions if they are thrown. + #include "motion_planner_server.hpp" +std::string str_tolower(std::string s) +{ + std::transform(s.begin(), s.end(), s.begin(), + [](unsigned char c) { return std::tolower(c); } + ); + return s; +} + +namespace nexus { +namespace motion_planner { + +constexpr bool cache_mode_is_execute(PlannerDatabaseMode mode) +{ + return mode == PlannerDatabaseMode::ExecuteBestEffort || + mode == PlannerDatabaseMode::ExecuteReadOnly; +} + +constexpr bool cache_mode_is_training(PlannerDatabaseMode mode) +{ + return mode == PlannerDatabaseMode::TrainingOverwrite || + mode == PlannerDatabaseMode::TrainingAppendOnly; +} + +// Convert planner database string param to PlannerDatabaseMode enum values. +PlannerDatabaseMode str_to_planner_database_mode(std::string s) +{ + std::string s_lower = str_tolower(s); + + // Using a switch-case here is... inconvenient (needs constexpr hashing or a + // map), so we don't. + if (s_lower == "training_overwrite" || s_lower == "trainingoverwrite") + { + return PlannerDatabaseMode::TrainingOverwrite; + } + else if (s_lower == "training_append_only" || s_lower == "trainingappendonly") + { + return PlannerDatabaseMode::TrainingAppendOnly; + } + else if (s_lower == "execute_best_effort" || s_lower == "executebesteffort") + { + return PlannerDatabaseMode::ExecuteBestEffort; + } + else if (s_lower == "execute_read_only" || s_lower == "executereadonly") + { + return PlannerDatabaseMode::ExecuteReadOnly; + } + else + { + return PlannerDatabaseMode::Unset; + } +} + //============================================================================== MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) : rclcpp_lifecycle::LifecycleNode("motion_planner_server", options) @@ -162,6 +216,82 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) "Setting parameter get_state_wait_seconds to [%.3f]", _get_state_wait_seconds ); + + // Motion plan cache params + // unset, training, training_append_only, execute_best_effort, execute_read_only + _planner_database_mode = this->declare_parameter( + "planner_database_mode", "unset"); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter planner_database_mode to [%s]", + _planner_database_mode.c_str() + ); + _cache_mode = str_to_planner_database_mode(_planner_database_mode); + + _cache_db_plugin = this->declare_parameter( + "cache_db_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_db_plugin to [%s]", _cache_db_plugin.c_str() + ); + + _cache_db_host = this->declare_parameter("cache_db_host", ":memory:"); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_db_host to [%s]", _cache_db_host.c_str() + ); + + _cache_db_port = this->declare_parameter("cache_db_port", 0); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_db_port to [%d]", _cache_db_port + ); + + // For floating point comparison, what counts as an "exact" match. + _cache_exact_match_tolerance = this->declare_parameter( + "cache_exact_match_tolerance", 0.0005); // ~0.028 degrees per joint + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_exact_match_tolerance to [%.2e]", + _cache_exact_match_tolerance + ); + + _cache_start_match_tolerance = this->declare_parameter( + "cache_start_match_tolerance", 0.025); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_start_match_tolerance to [%.5f]", + _cache_start_match_tolerance + ); + + _cache_goal_match_tolerance = this->declare_parameter( + "cache_goal_match_tolerance", 0.001); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter cache_goal_match_tolerance to [%.5f]", + _cache_goal_match_tolerance + ); + + if (_cache_mode != PlannerDatabaseMode::Unset) + { + // Push warehouse_ros parameters to internal node + // This must happen BEFORE the MotionPlanCache is created! + _internal_node->declare_parameter( + "warehouse_plugin", _cache_db_plugin); + + _internal_node->declare_parameter( + "warehouse_host", _cache_db_host); + + _internal_node->declare_parameter( + "warehouse_port", _cache_db_port); + + } + + // We need to construct the cache even if we are not using the DB, since it + // has a utility function that uses the internal node that we use to generate + // a trajectory start point for cartesian planning. + _motion_plan_cache = + std::make_unique(_internal_node); } //============================================================================== @@ -178,6 +308,17 @@ auto MotionPlannerServer::on_configure(const LifecycleState& /*state*/) -> CallbackReturn { RCLCPP_INFO(this->get_logger(), "Configuring..."); + + if (_cache_mode != PlannerDatabaseMode::Unset) + { + if (!_motion_plan_cache->init( + _cache_db_host, _cache_db_port, _cache_exact_match_tolerance)) + { + RCLCPP_ERROR(this->get_logger(), "Could not init motion plan cache."); + return CallbackReturn::ERROR; + } + } + if (_use_move_group_interfaces) { auto ok = initialize_move_group_interfaces(); @@ -322,8 +463,7 @@ auto MotionPlannerServer::on_shutdown(const LifecycleState& /*state*/) //============================================================================== void MotionPlannerServer::plan_with_move_group( - const GetMotionPlanService::ServiceType::Request& req, - Response res) + const GetMotionPlanService::ServiceType::Request& req, Response res) { RCLCPP_INFO( this->get_logger(), @@ -461,6 +601,7 @@ void MotionPlannerServer::plan_with_move_group( interface->setMaxVelocityScalingFactor(vel_scale); interface->setMaxAccelerationScalingFactor(acc_scale); moveit::core::MoveItErrorCode error; + moveit_msgs::msg::MotionPlanRequest plan_req_msg; // For caching if (req.cartesian) { std::vector waypoints; @@ -468,42 +609,236 @@ void MotionPlannerServer::plan_with_move_group( // TODO(YV): For now we use simple cartesian interpolation. OMPL supports // constrained planning so we can consider adding orientation or line // constraints to the end-effector link instead and call plan(). - const double error = interface->computeCartesianPath( - waypoints, - _cartesian_max_step, - _cartesian_jump_threshold, - res->result.trajectory, - _collision_aware_cartesian_path - ); - RCLCPP_INFO( - this->get_logger(), - "Cartesian interpolation returned a fraction of [%.2f]", error - ); - if (error <= 0.0) + + moveit_msgs::msg::RobotTrajectory cartesian_plan; + double fraction; + bool cartesian_plan_is_from_cache = false; + auto cartesian_plan_req_msg = + _motion_plan_cache->construct_get_cartesian_plan_request( + *interface, waypoints, _cartesian_max_step, _cartesian_jump_threshold, + _collision_aware_cartesian_path); + + // Fetch if in execute mode. + if (cache_mode_is_execute(_cache_mode) + || req.force_cache_mode_execute_read_only) + { + auto fetch_start = this->now(); + auto fetched_cartesian_plan = + _motion_plan_cache->fetch_best_matching_cartesian_plan( + *interface, robot_name, cartesian_plan_req_msg, + /* min_fraction */ 1.0, + _cache_start_match_tolerance, _cache_goal_match_tolerance); + auto fetch_end = this->now(); + // Set plan if a cached cartesian plan was fetched. + if (fetched_cartesian_plan) + { + fraction = fetched_cartesian_plan->lookupDouble("fraction"); + cartesian_plan_is_from_cache = true; + cartesian_plan = *fetched_cartesian_plan; + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + res->result.planning_time = (fetch_end - fetch_start).seconds(); + RCLCPP_INFO( + this->get_logger(), + "Cache fetch took %es, planning time of fetched plan was: %es", + (fetch_end - fetch_start).seconds(), + fetched_cartesian_plan->lookupDouble("planning_time_s")); + } + // Fail if ReadOnly mode and no cached cartesian plan was fetched. + else if (_cache_mode == PlannerDatabaseMode::ExecuteReadOnly + || req.force_cache_mode_execute_read_only) + { + RCLCPP_ERROR( + this->get_logger(), + "Cache mode was ExecuteReadOnly, and could not find " + "cached cartesian plan for cartesian plan request: \n\n%s", + moveit_msgs::srv::to_yaml(cartesian_plan_req_msg).c_str()); + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + } + + // Plan if needed. + // This is if we didn't fetch a cartesian plan from the cache. + // (In training or unset mode we never attempt to fetch, so it will always + // plan.) + if (!cartesian_plan_is_from_cache) { - res->result.error_code.val = -1; + auto cartesian_plan_start = this->now(); + fraction = interface->computeCartesianPath( + waypoints, + _cartesian_max_step, + _cartesian_jump_threshold, + cartesian_plan, + _collision_aware_cartesian_path + ); + auto cartesian_plan_end = this->now(); + + RCLCPP_INFO( + this->get_logger(), + "Cartesian interpolation returned a fraction of [%.2f]", fraction + ); + if (fraction <= 0.0) + { + res->result.error_code.val = -1; + } + else + { + res->result.error_code.val = 1; + } + + res->result.planning_time = + (cartesian_plan_end - cartesian_plan_start).seconds(); + + RCLCPP_INFO( + this->get_logger(), + "Plan status: %d, planning time: %es", + res->result.error_code.val, res->result.planning_time); } else { - res->result.error_code.val = 1; + if (fraction <= 0) + { + res->result.error_code.val = -1; + } + else + { + res->result.error_code.val = 1; + } + } + + // Do NOT move this. We use the cartesian_plan_req_msg later. + res->result.trajectory_start = cartesian_plan_req_msg.start_state; + res->result.trajectory = std::move(cartesian_plan); + + if (res->result.error_code.val != + moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_WARN( + this->get_logger(), + "Cartesian planning did not succeed: %d", res->result.error_code.val); + return; + } + // Put plan if in training mode. + // Make sure we check if the plan we have was fetched (so we don't have + // duplicate caches.) + if (cache_mode_is_training(_cache_mode) && !cartesian_plan_is_from_cache) + { + if (!_motion_plan_cache->put_cartesian_plan( + *interface, robot_name, + std::move(cartesian_plan_req_msg), std::move(res->result.trajectory), + rclcpp::Duration( + res->result.trajectory.joint_trajectory.points.back() + .time_from_start + ).seconds(), + res->result.planning_time, fraction, + _cache_mode == PlannerDatabaseMode::TrainingOverwrite)) + { + RCLCPP_WARN( + this->get_logger(), "Did not put cartesian plan into cache."); + } } } else { MoveGroupInterface::Plan plan; - res->result.error_code = interface->plan(plan); + bool plan_is_from_cache = false; + interface->constructMotionPlanRequest(plan_req_msg); + + // Fetch if in execute mode. + if (cache_mode_is_execute(_cache_mode) + || req.force_cache_mode_execute_read_only) + { + auto fetch_start = this->now(); + auto fetched_plan = _motion_plan_cache->fetch_best_matching_plan( + *interface, robot_name, plan_req_msg, + _cache_start_match_tolerance, _cache_goal_match_tolerance); + auto fetch_end = this->now(); + // Set plan if a cached plan was fetched. + if (fetched_plan) + { + plan_is_from_cache = true; + plan.start_state = plan_req_msg.start_state; + plan.trajectory = *fetched_plan; + plan.planning_time = (fetch_end - fetch_start).seconds(); + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + RCLCPP_INFO( + this->get_logger(), + "Cache fetch took %es, planning time of fetched plan was: %es", + (fetch_end - fetch_start).seconds(), + fetched_plan->lookupDouble("planning_time_s")); + } + // Fail if ReadOnly mode and no cached plan was fetched. + else if (_cache_mode == PlannerDatabaseMode::ExecuteReadOnly + || req.force_cache_mode_execute_read_only) + { + RCLCPP_ERROR( + this->get_logger(), + "Cache mode was ExecuteReadOnly, and could not find " + "cached plan for plan request: \n\n%s", + moveit_msgs::msg::to_yaml(plan_req_msg).c_str()); + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + } + + // Plan if needed. + // This is if we didn't fetch a plan from the cache. + // (In training or unset mode we never attempt to fetch, so it will always + // plan.) + if (!plan_is_from_cache) + { + res->result.error_code = interface->plan(plan); + RCLCPP_INFO( + this->get_logger(), + "Plan status: %d, planning time: %es", + res->result.error_code.val, plan.planning_time); + } + res->result.trajectory_start = std::move(plan.start_state); res->result.trajectory = std::move(plan.trajectory); res->result.planning_time = std::move(plan.planning_time); + + if (res->result.error_code.val != + moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + RCLCPP_WARN( + this->get_logger(), + "Planning did not succeed: %d", res->result.error_code.val); + return; + } + + // Put plan if in training mode. + // Make sure we check if the plan we have was fetched (so we don't have + // duplicate caches.) + if (cache_mode_is_training(_cache_mode) && !plan_is_from_cache) + { + if (!_motion_plan_cache->put_plan( + *interface, robot_name, + std::move(plan_req_msg), std::move(res->result.trajectory), + rclcpp::Duration( + res->result.trajectory.joint_trajectory.points.back() + .time_from_start + ).seconds(), + res->result.planning_time, + _cache_mode == PlannerDatabaseMode::TrainingOverwrite)) + { + RCLCPP_WARN(this->get_logger(), "Did not put plan into cache."); + } + } } if (_execute_trajectory) { - RCLCPP_INFO( - this->get_logger(), - "Executing trajectory!"); + RCLCPP_INFO(this->get_logger(), "Executing trajectory!"); // This is a blocking call. interface->execute(res->result.trajectory); } return; } + +} // namespace planning_interface +} // namespace moveit diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 050929f..7451d7a 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -35,6 +35,36 @@ // NEXUS messages #include +// NEXUS motion plan cache +#include "motion_plan_cache.hpp" + +namespace nexus { +namespace motion_planner { + +enum class PlannerDatabaseMode : uint8_t +{ + // Unset. Planner will not use a cache. + Unset = 0, + + // Planner will always generate a plan. + // It will add plans to the database if they are the best seen so far. + // Will overwrite close-enough entries if a better plan was found. + TrainingOverwrite = 10, + + // Planner will always generate a plan. + // It will add plans to the database if they are the best seen so far. + // Will not overwrite any existing entries. + TrainingAppendOnly = 11, + + // Planner will prioritize returning plans in the cache. + // If no plan was found, it will instead attempt to generate a plan live, but will not update the + // cache. + ExecuteBestEffort = 20, + + // Planner will return a plan only if present in the database cache. + ExecuteReadOnly = 21, +}; + //============================================================================== class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode { @@ -69,6 +99,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode rclcpp::Node::SharedPtr _internal_node; std::thread _spin_thread; + // MoveIt planning std::vector _manipulators; bool _use_move_group_interfaces; bool _use_namespace; @@ -88,6 +119,18 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode double _workspace_max_y; double _workspace_max_z; + // Motion plan caching + std::unique_ptr _motion_plan_cache; + std::string _planner_database_mode; + PlannerDatabaseMode _cache_mode = PlannerDatabaseMode::Unset; + + std::string _cache_db_plugin; + std::string _cache_db_host; + int _cache_db_port; + double _cache_exact_match_tolerance; // for floating point comparison + double _cache_start_match_tolerance; + double _cache_goal_match_tolerance; + rclcpp::Service::SharedPtr _plan_srv; std::unordered_map> @@ -102,4 +145,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode Response res); }; // class MotionPlannerServer +} // namespace planning_interface +} // namespace moveit + #endif // SRC__MOTION_PLANNER_SERVER_HPP diff --git a/nexus_motion_planner/src/test_motion_plan_cache.cpp b/nexus_motion_planner/src/test_motion_plan_cache.cpp new file mode 100644 index 0000000..64e71c7 --- /dev/null +++ b/nexus_motion_planner/src/test_motion_plan_cache.cpp @@ -0,0 +1,1317 @@ +// Copyright 2023 Johnson & Johnson +// +// 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. + +#include + +#include "moveit/robot_state/conversions.h" +#include "moveit/robot_state/robot_state.h" +#include "motion_plan_cache.hpp" + +#include + +using moveit::planning_interface::MoveGroupInterface; +using nexus::motion_planner::MotionPlanCache; + +const std::string g_robot_name = "panda"; +const std::string g_robot_frame = "world"; + +// UTILS ======================================================================= +// Utility function to emit a pass or fail statement. +void check_and_emit( + bool predicate, const std::string& prefix, const std::string& label) +{ + if (predicate) + { + std::cout << "[PASS] " << prefix << ": " << label << std::endl; + } + else + { + std::cout << "[FAIL] " << prefix << ": " << label << std::endl; + exit(1); + } +} + +moveit_msgs::msg::RobotTrajectory get_dummy_panda_plan() +{ + static moveit_msgs::msg::RobotTrajectory out = []() + { + moveit_msgs::msg::RobotTrajectory plan; + + auto traj = &plan.joint_trajectory; + traj->header.frame_id = g_robot_frame; + + traj->joint_names.push_back(g_robot_name + "_joint1"); + traj->joint_names.push_back(g_robot_name + "_joint2"); + traj->joint_names.push_back(g_robot_name + "_joint3"); + traj->joint_names.push_back(g_robot_name + "_joint4"); + traj->joint_names.push_back(g_robot_name + "_joint5"); + traj->joint_names.push_back(g_robot_name + "_joint6"); + traj->joint_names.push_back(g_robot_name + "_joint7"); + + traj->points.emplace_back(); + traj->points.at(0).positions = {0, 0, 0, 0, 0, 0}; + traj->points.at(0).velocities = {0, 0, 0, 0, 0, 0}; + traj->points.at(0).accelerations = {0, 0, 0, 0, 0, 0}; + traj->points.at(0).time_from_start.sec = 999999; + + return plan; + }(); + return out; +} + +std::vector get_dummy_waypoints() +{ + static std::vector out = []() + { + std::vector waypoints; + for (size_t i = 0; i < 3; i++) + { + waypoints.emplace_back(); + waypoints.at(i).position.x = i; + waypoints.at(i).position.y = i; + waypoints.at(i).position.z = i; + waypoints.at(i).orientation.w = i; + waypoints.at(i).orientation.x = i + 0.1; + waypoints.at(i).orientation.y = i + 0.1; + waypoints.at(i).orientation.z = i + 0.1; + } + return waypoints; + }(); + return out; +} + + +void test_motion_plans( + std::shared_ptr move_group, + std::shared_ptr cache) +{ + // Setup ===================================================================== + // All variants are copies. + + /// MotionPlanRequest + + // Plain start + moveit_msgs::msg::MotionPlanRequest plan_req; + move_group->constructMotionPlanRequest(plan_req); + plan_req.workspace_parameters.header.frame_id = g_robot_frame; + plan_req.workspace_parameters.max_corner.x = 10; + plan_req.workspace_parameters.max_corner.y = 10; + plan_req.workspace_parameters.max_corner.z = 10; + plan_req.workspace_parameters.min_corner.x = -10; + plan_req.workspace_parameters.min_corner.y = -10; + plan_req.workspace_parameters.min_corner.z = -10; + plan_req.start_state.multi_dof_joint_state.joint_names.clear(); + plan_req.start_state.multi_dof_joint_state.transforms.clear(); + plan_req.start_state.multi_dof_joint_state.twist.clear(); + plan_req.start_state.multi_dof_joint_state.wrench.clear(); + + // Empty frame start + moveit_msgs::msg::MotionPlanRequest empty_frame_plan_req = plan_req; + empty_frame_plan_req.workspace_parameters.header.frame_id = ""; + + // is_diff = true + auto is_diff_plan_req = plan_req; + is_diff_plan_req.start_state.is_diff = true; + is_diff_plan_req.start_state.joint_state.header.frame_id = ""; + is_diff_plan_req.start_state.joint_state.name.clear(); + is_diff_plan_req.start_state.joint_state.position.clear(); + is_diff_plan_req.start_state.joint_state.velocity.clear(); + is_diff_plan_req.start_state.joint_state.effort.clear(); + + // Something close enough (mod 0.1 away) + auto close_matching_plan_req = plan_req; + close_matching_plan_req.workspace_parameters.max_corner.x += 0.05; + close_matching_plan_req.workspace_parameters.min_corner.x -= 0.05; + close_matching_plan_req.start_state.joint_state.position.at(0) -= 0.05; + close_matching_plan_req.start_state.joint_state.position.at(1) += 0.05; + close_matching_plan_req.start_state.joint_state.position.at(2) -= 0.05; + close_matching_plan_req.goal_constraints + .at(0).joint_constraints.at(0).position -= 0.05; + close_matching_plan_req.goal_constraints + .at(0).joint_constraints.at(1).position += 0.05; + close_matching_plan_req.goal_constraints + .at(0).joint_constraints.at(2).position -= 0.05; + + // Close with swapped constraints (mod 0.1 away) + auto swapped_close_matching_plan_req = close_matching_plan_req; + std::swap( + swapped_close_matching_plan_req.goal_constraints.at( + 0).joint_constraints.at(0), + swapped_close_matching_plan_req.goal_constraints.at( + 0).joint_constraints.at(1)); + + // Smaller workspace start + auto smaller_workspace_plan_req = plan_req; + smaller_workspace_plan_req.workspace_parameters.max_corner.x = 1; + smaller_workspace_plan_req.workspace_parameters.max_corner.y = 1; + smaller_workspace_plan_req.workspace_parameters.max_corner.z = 1; + smaller_workspace_plan_req.workspace_parameters.min_corner.x = -1; + smaller_workspace_plan_req.workspace_parameters.min_corner.y = -1; + smaller_workspace_plan_req.workspace_parameters.min_corner.z = -1; + + // Larger workspace start + auto larger_workspace_plan_req = plan_req; + larger_workspace_plan_req.workspace_parameters.max_corner.x = 50; + larger_workspace_plan_req.workspace_parameters.max_corner.y = 50; + larger_workspace_plan_req.workspace_parameters.max_corner.z = 50; + larger_workspace_plan_req.workspace_parameters.min_corner.x = -50; + larger_workspace_plan_req.workspace_parameters.min_corner.y = -50; + larger_workspace_plan_req.workspace_parameters.min_corner.z = -50; + + // Different + auto different_plan_req = plan_req; + different_plan_req.workspace_parameters.max_corner.x += 1.05; + different_plan_req.workspace_parameters.min_corner.x -= 2.05; + different_plan_req.start_state.joint_state.position.at(0) -= 3.05; + different_plan_req.start_state.joint_state.position.at(1) += 4.05; + different_plan_req.start_state.joint_state.position.at(2) -= 5.05; + different_plan_req.goal_constraints + .at(0).joint_constraints.at(0).position -= 6.05; + different_plan_req.goal_constraints + .at(0).joint_constraints.at(1).position += 7.05; + different_plan_req.goal_constraints + .at(0).joint_constraints.at(2).position -= 8.05; + + /// RobotTrajectory + + // Plan + auto plan = get_dummy_panda_plan(); + + // Plan with no frame_id in its trajectory header + auto empty_frame_plan = plan; + empty_frame_plan.joint_trajectory.header.frame_id = ""; + + auto different_plan = plan; + different_plan.joint_trajectory.points.at(0).positions.at(0) = 999; + different_plan.joint_trajectory.points.at(0).positions.at(1) = 999; + different_plan.joint_trajectory.points.at(0).positions.at(2) = 999; + + // Test Utils + + std::string prefix; + + // Checks ==================================================================== + + // Initially empty + prefix = "test_motion_plans.empty"; + + check_and_emit( + cache->count_plans(g_robot_name) == 0, + prefix, "Plan cache initially empty"); + + check_and_emit( + cache->fetch_all_matching_plans( + *move_group, g_robot_name, plan_req, 999, 999).empty(), + prefix, "Fetch all plans on empty cache returns empty"); + + check_and_emit( + cache->fetch_best_matching_plan( + *move_group, g_robot_name, plan_req, 999, 999) == nullptr, + prefix, "Fetch best plan on empty cache returns nullptr"); + + // Put plan empty frame + // + // Plan must have frame in joint trajectory, expect put fail + prefix = "test_motion_plans.put_plan_empty_frame"; + double execution_time = 999; + double planning_time = 999; + + check_and_emit( + !cache->put_plan( + *move_group, g_robot_name, plan_req, empty_frame_plan, + execution_time, planning_time, false), + prefix, "Put empty frame plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 0, prefix, "No plans in cache"); + + // Put plan req empty frame + // + // Plan request must have frame in workspace, expect put fail + prefix = "test_motion_plans.put_plan_req_empty_frame"; + execution_time = 999; + planning_time = 999; + + check_and_emit( + !cache->put_plan( + *move_group, g_robot_name, empty_frame_plan_req, plan, + execution_time, planning_time, false), + prefix, "Put empty frame req plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 0, prefix, "No plans in cache"); + + // Put first, no delete_worse_plans + prefix = "test_motion_plans.put_first"; + execution_time = 999; + planning_time = 999; + + check_and_emit( + cache->put_plan( + *move_group, g_robot_name, plan_req, plan, + execution_time, planning_time, false), + prefix, "Put first valid plan, no delete_worse_plans, ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + prefix = "test_motion_plans.put_first.fetch_matching_no_tolerance"; + + auto fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, plan_req, 0, 0); + + auto fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Fetch with is_diff + // + // is_diff key should be equivalent to exact match if robot state did not + // change, hence should have cache hit + prefix = "test_motion_plans.put_first.fetch_is_diff_no_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, is_diff_plan_req, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, is_diff_plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + prefix = "test_motion_plans.put_first.fetch_non_matching_out_of_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, close_matching_plan_req, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, close_matching_plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + prefix = + "test_motion_plans.put_first.fetch_non_matching_only_start_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, close_matching_plan_req, 999, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, close_matching_plan_req, 999, 0); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + prefix = + "test_motion_plans.put_first.fetch_non_matching_only_goal_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, close_matching_plan_req, 0, 999); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, close_matching_plan_req, 0, 999); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + prefix = "test_motion_plans.put_first.fetch_non_matching_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, close_matching_plan_req, 0.1, 0.1); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Fetch swapped + // + // Matches should be mostly invariant to constraint ordering + prefix = "test_motion_plans.put_first.fetch_swapped"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, swapped_close_matching_plan_req, 0.1, 0.1); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Fetch with smaller workspace + // + // Matching plans with more restrictive workspace requirements should not + // pull up plans cached for less restrictive workspace requirements + prefix = "test_motion_plans.put_first.fetch_smaller_workspace"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, smaller_workspace_plan_req, 999, 999); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch with larger workspace + // + // Matching plans with less restrictive workspace requirements should pull up + // plans cached for more restrictive workspace requirements + prefix = "test_motion_plans.put_first.fetch_larger_workspace"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, larger_workspace_plan_req, 999, 999); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, larger_workspace_plan_req, 999, 999); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble( + "workspace_parameters.max_corner.x") + <= larger_workspace_plan_req.workspace_parameters.max_corner.x, + prefix, "Fetched plan has more restrictive workspace max_corner"); + + check_and_emit( + fetched_plan->lookupDouble( + "workspace_parameters.max_corner.x") + >= larger_workspace_plan_req.workspace_parameters.min_corner.x, + prefix, "Fetched plan has more restrictive workspace min_corner"); + + // Put worse, no delete_worse_plans + // + // Worse plans should not be inserted + prefix = "test_motion_plans.put_worse"; + double worse_execution_time = execution_time + 100; + + check_and_emit( + !cache->put_plan( + *move_group, g_robot_name, plan_req, plan, + worse_execution_time, planning_time, false), + prefix, "Put worse plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); + + // Put better, no delete_worse_plans + // + // Better plans should be inserted + prefix = "test_motion_plans.put_better"; + double better_execution_time = execution_time - 100; + + check_and_emit( + cache->put_plan( + *move_group, g_robot_name, plan_req, plan, + better_execution_time, planning_time, false), + prefix, "Put better plan, no delete_worse_plans, ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 2, prefix, "Two plans in cache"); + + // Fetch sorted + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_motion_plans.put_better.fetch_sorted"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, plan_req, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 2, prefix, "Fetch all returns two"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plans.at(0)->lookupDouble( + "execution_time_s") == better_execution_time + && fetched_plans.at(1)->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plans are sorted correctly"); + + // Put better, delete_worse_plans + // + // Better, different, plans should be inserted + prefix = "test_motion_plans.put_better_delete_worse_plans"; + double even_better_execution_time = better_execution_time - 100; + + check_and_emit( + cache->put_plan( + *move_group, g_robot_name, plan_req, different_plan, + even_better_execution_time, planning_time, true), + prefix, "Put better plan, delete_worse_plans, ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 1, prefix, "One plan in cache"); + + // Fetch better plan + prefix = "test_motion_plans.put_better_delete_worse_plans.fetch"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, plan_req, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == different_plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble( + "execution_time_s") == even_better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Put different req, delete_worse_plans + // + // Unrelated plan requests should live alongside pre-existing plans + prefix = "test_motion_plans.put_different_req"; + + check_and_emit( + cache->put_plan( + *move_group, g_robot_name, different_plan_req, different_plan, + better_execution_time, planning_time, true), + prefix, "Put different plan req, delete_worse_plans, ok"); + + check_and_emit( + cache->count_plans(g_robot_name) == 2, prefix, "Two plans in cache"); + + // Fetch with different plan req + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_motion_plans.put_different_req.fetch"; + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, different_plan_req, 0, 0); + + fetched_plan = cache->fetch_best_matching_plan( + *move_group, g_robot_name, different_plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == different_plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble( + "execution_time_s") == better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + prefix = "test_motion_plans.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + check_and_emit( + cache->count_cartesian_plans(different_robot_name) == 0, + prefix, "No plans in cache"); + + // Put first for different robot, delete_worse_plans + // + // A different robot's cache should not interact with the original cache + prefix = "test_motion_plans.different_robot.put_first"; + check_and_emit( + cache->put_plan( + *move_group, different_robot_name, different_plan_req, + different_plan, better_execution_time, planning_time, true), + prefix, "Put different plan req, delete_worse_plans, ok"); + + check_and_emit( + cache->count_plans(different_robot_name) == 1, + prefix, "One plans in cache"); + + check_and_emit( + cache->count_plans(different_robot_name) == 1, + prefix, "One plans in cache"); + + check_and_emit( + cache->count_plans(g_robot_name) == 2, + prefix, "Two plans in original robot's cache"); + + fetched_plans = cache->fetch_all_matching_plans( + *move_group, g_robot_name, different_plan_req, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, + "Fetch all on original still returns one"); +} + +void test_cartesian_plans( + std::shared_ptr move_group, + std::shared_ptr cache) +{ + std::string prefix; + + /// First, test if construction even works... + + // Construct get cartesian plan request + prefix = "test_cartesian_plans.construct_get_cartesian_path_request"; + + int test_step = 1; + int test_jump = 2; + auto test_waypoints = get_dummy_waypoints(); + auto cartesian_plan_req_under_test = + cache->construct_get_cartesian_plan_request( + *move_group, test_waypoints, test_step, test_jump, false); + + check_and_emit( + cartesian_plan_req_under_test.waypoints == test_waypoints + && static_cast( + cartesian_plan_req_under_test.max_step) == test_step + && static_cast( + cartesian_plan_req_under_test.jump_threshold) == test_jump + && !cartesian_plan_req_under_test.avoid_collisions, + prefix, "Ok"); + + // Setup ===================================================================== + // All variants are copies. + + /// GetCartesianPath::Request + + // Plain start + auto waypoints = get_dummy_waypoints(); + auto cartesian_plan_req = cache + ->construct_get_cartesian_plan_request(*move_group, waypoints, 1, 1, false); + cartesian_plan_req.start_state.multi_dof_joint_state.joint_names.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.transforms.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.twist.clear(); + cartesian_plan_req.start_state.multi_dof_joint_state.wrench.clear(); + cartesian_plan_req.path_constraints.joint_constraints.clear(); + cartesian_plan_req.path_constraints.position_constraints.clear(); + cartesian_plan_req.path_constraints.orientation_constraints.clear(); + cartesian_plan_req.path_constraints.visibility_constraints.clear(); + + // Empty frame start + auto empty_frame_cartesian_plan_req = cartesian_plan_req; + empty_frame_cartesian_plan_req.header.frame_id = ""; + + // is_diff = true + auto is_diff_cartesian_plan_req = cartesian_plan_req; + is_diff_cartesian_plan_req.start_state.is_diff = true; + is_diff_cartesian_plan_req.start_state.joint_state.header.frame_id = ""; + is_diff_cartesian_plan_req.start_state.joint_state.name.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.position.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.velocity.clear(); + is_diff_cartesian_plan_req.start_state.joint_state.effort.clear(); + + // Something close enough (mod 0.1 away) + auto close_matching_cartesian_plan_req = cartesian_plan_req; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(0) -= + 0.05; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(1) += + 0.05; + close_matching_cartesian_plan_req.start_state.joint_state.position.at(2) -= + 0.05; + close_matching_cartesian_plan_req.waypoints.at(0).position.x -= 0.05; + close_matching_cartesian_plan_req.waypoints.at(1).position.x += 0.05; + close_matching_cartesian_plan_req.waypoints.at(2).position.x -= 0.05; + + // Different + auto different_cartesian_plan_req = cartesian_plan_req; + different_cartesian_plan_req.start_state.joint_state.position.at(0) -= 1.05; + different_cartesian_plan_req.start_state.joint_state.position.at(1) += 2.05; + different_cartesian_plan_req.start_state.joint_state.position.at(2) -= 3.05; + different_cartesian_plan_req.waypoints.at(0).position.x -= 1.05; + different_cartesian_plan_req.waypoints.at(1).position.x += 2.05; + different_cartesian_plan_req.waypoints.at(2).position.x -= 3.05; + + /// RobotTrajectory + + // Plan + auto plan = get_dummy_panda_plan(); + + // Plan with no frame_id in its trajectory header + auto empty_frame_plan = plan; + empty_frame_plan.joint_trajectory.header.frame_id = ""; + + auto different_plan = plan; + different_plan.joint_trajectory.points.at(0).positions.at(0) = 999; + different_plan.joint_trajectory.points.at(0).positions.at(1) = 999; + different_plan.joint_trajectory.points.at(0).positions.at(2) = 999; + + // Checks ==================================================================== + + // Initially empty + prefix = "test_cartesian_plans.empty"; + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 0, + prefix, "Plan cache initially empty"); + + check_and_emit( + cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, 0, 999, 999).empty(), + prefix, "Fetch all plans on empty cache returns empty"); + + check_and_emit( + cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, 0, 999, 999) == nullptr, + prefix, "Fetch best plan on empty cache returns nullptr"); + + // Put plan empty frame + // + // Plan must have frame in joint trajectory, expect put fail + prefix = "test_cartesian_plans.put_plan_empty_frame"; + double execution_time = 999; + double planning_time = 999; + double fraction = 0.5; + + check_and_emit( + !cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, empty_frame_plan, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 0, + prefix, "No plans in cache"); + + // Put plan req empty frame + // + // Plan request must have frame in workspace, expect put fail + prefix = "test_cartesian_plans.put_plan_req_empty_frame"; + execution_time = 999; + planning_time = 999; + + check_and_emit( + !cache->put_cartesian_plan( + *move_group, g_robot_name, empty_frame_cartesian_plan_req, plan, + execution_time, planning_time, fraction, false), + prefix, "Put empty frame req plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 0, + prefix, "No plans in cache"); + + // Put first, no delete_worse_plans + prefix = "test_cartesian_plans.put_first"; + execution_time = 999; + planning_time = 999; + + check_and_emit( + cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, plan, + execution_time, planning_time, fraction, false), + prefix, "Put first valid plan, no delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 1, + prefix, "One plan in cache"); + + // Fetch matching, no tolerance + // + // Exact key match should have cache hit + prefix = "test_cartesian_plans.put_first.fetch_matching_no_tolerance"; + + auto fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + auto fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Fetch with is_diff + // + // is_diff key should be equivalent to exact match if robot state did not + // change, hence should have cache hit + prefix = "test_cartesian_plans.put_first.fetch_is_diff_no_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, is_diff_cartesian_plan_req, fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, is_diff_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Fetch non-matching, out of tolerance + // + // Non-matching key should not have cache hit + prefix = "test_cartesian_plans.put_first.fetch_non_matching_out_of_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, only start in tolerance (but not goal) + // + // Non-matching key should not have cache hit + prefix = + "test_motion_plans.put_first.fetch_non_matching_only_start_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 999, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 999, 0); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, only goal in tolerance (but not start) + // + // Non-matching key should not have cache hit + prefix = + "test_motion_plans.put_first.fetch_non_matching_only_goal_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0, 999); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0, 999); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch non-matching, in tolerance + // + // Close key within tolerance limit should have cache hit + prefix = "test_cartesian_plans.put_first.fetch_non_matching_in_tolerance"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0.1, 0.1); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, close_matching_cartesian_plan_req, + fraction, 0.1, 0.1); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Fetch with higher fraction + // + // Matching plans with more restrictive fraction requirements should not + // pull up plans cached for less restrictive fraction requirements + prefix = "test_cartesian_plans.put_first.fetch_higher_fraction"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, 1, 999, 999); + + check_and_emit(fetched_plans.size() == 0, prefix, "Fetch all returns empty"); + check_and_emit( + fetched_plan == nullptr, prefix, "Fetch best plan is nullptr"); + + // Fetch with lower fraction + // + // Matching plans with less restrictive fraction requirements should pull up + // plans cached for more restrictive fraction requirements + prefix = "test_cartesian_plans.put_first.fetch_lower_fraction"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, 0, 999, 999); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Put worse, no delete_worse_plans + // + // Worse plans should not be inserted + prefix = "test_cartesian_plans.put_worse"; + double worse_execution_time = execution_time + 100; + + check_and_emit( + !cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, plan, + worse_execution_time, planning_time, fraction, false), + prefix, "Put worse plan, no delete_worse_plans, not ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 1, + prefix, "One plan in cache"); + + // Put better, no delete_worse_plans + // + // Better plans should be inserted + prefix = "test_cartesian_plans.put_better"; + double better_execution_time = execution_time - 100; + + check_and_emit( + cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, plan, + better_execution_time, planning_time, fraction, false), + prefix, "Put better plan, no delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 2, + prefix, "Two plans in cache"); + + // Fetch sorted + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_cartesian_plans.put_better.fetch_sorted"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 2, prefix, "Fetch all returns two"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble("execution_time_s") == better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + check_and_emit( + fetched_plans.at(0)->lookupDouble( + "execution_time_s") == better_execution_time + && fetched_plans.at(1)->lookupDouble("execution_time_s") == execution_time, + prefix, "Fetched plans are sorted correctly"); + + // Put better, delete_worse_plans + // + // Better, different, plans should be inserted + prefix = "test_cartesian_plans.put_better_delete_worse_plans"; + double even_better_execution_time = better_execution_time - 100; + + check_and_emit( + cache->put_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, different_plan, + even_better_execution_time, planning_time, fraction, true), + prefix, "Put better plan, delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 1, + prefix, "One plan in cache"); + + // Fetch better plan + prefix = "test_cartesian_plans.put_better_delete_worse_plans.fetch"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == different_plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble( + "execution_time_s") == even_better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Put different req, delete_worse_plans + // + // Unrelated plan requests should live alongside pre-existing plans + prefix = "test_cartesian_plans.put_different_req"; + + check_and_emit( + cache->put_cartesian_plan( + *move_group, g_robot_name, different_cartesian_plan_req, different_plan, + better_execution_time, planning_time, fraction, true), + prefix, "Put different plan req, delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 2, + prefix, "Two plans in cache"); + + // Fetch with different plan req + // + // With multiple plans in cache, fetches should be sorted accordingly + prefix = "test_cartesian_plans.put_different_req.fetch"; + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, different_cartesian_plan_req, fraction, 0, 0); + + fetched_plan = cache->fetch_best_matching_cartesian_plan( + *move_group, g_robot_name, different_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, "Fetch all returns one"); + check_and_emit( + fetched_plan != nullptr, prefix, "Fetch best plan is not nullptr"); + + check_and_emit( + *fetched_plans.at(0) == *fetched_plan, + prefix, "Fetched plan on both fetches match"); + + check_and_emit( + *fetched_plan == different_plan, prefix, "Fetched plan matches original"); + + check_and_emit( + fetched_plan->lookupDouble( + "execution_time_s") == better_execution_time, + prefix, "Fetched plan has correct execution time"); + + check_and_emit( + fetched_plan->lookupDouble("planning_time_s") == planning_time, + prefix, "Fetched plan has correct planning time"); + + check_and_emit( + fetched_plan->lookupDouble("fraction") == fraction, + prefix, "Fetched plan has correct fraction"); + + // Fetch different robot + // + // Since we didn't populate anything, we should expect empty + prefix = "test_cartesian_plans.different_robot.empty"; + std::string different_robot_name = "different_robot"; + + check_and_emit( + cache->count_cartesian_plans(different_robot_name) == 0, + prefix, "No plans in cache"); + + // Put first for different robot, delete_worse_plans + // + // A different robot's cache should not interact with the original cache + prefix = "test_cartesian_plans.different_robot.put_first"; + check_and_emit( + cache->put_cartesian_plan( + *move_group, different_robot_name, different_cartesian_plan_req, + different_plan, better_execution_time, planning_time, fraction, true), + prefix, "Put different plan req, delete_worse_plans, ok"); + + check_and_emit( + cache->count_cartesian_plans(different_robot_name) == 1, + prefix, "One plans in cache"); + + check_and_emit( + cache->count_cartesian_plans(different_robot_name) == 1, + prefix, "One plans in cache"); + + check_and_emit( + cache->count_cartesian_plans(g_robot_name) == 2, + prefix, "Two plans in original robot's cache"); + + fetched_plans = cache->fetch_all_matching_cartesian_plans( + *move_group, g_robot_name, different_cartesian_plan_req, fraction, 0, 0); + + check_and_emit(fetched_plans.size() == 1, prefix, + "Fetch all on original still returns one"); +} + +int main(int argc, char** argv) +{ + // Setup + rclcpp::init(argc, argv); + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + + auto test_node = rclcpp::Node::make_shared("test_node", node_options); + std::thread spin_thread( + [&]() + { + while (rclcpp::ok()) + { + rclcpp::spin_some(test_node); + } + } + ); + + // This is necessary + test_node->declare_parameter( + "warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + + // Test proper + { + auto cache = std::make_shared(test_node); + check_and_emit(cache->init(":memory:", 0, 1e-4), "init", "Cache init"); + + auto move_group = + std::make_shared(test_node, "panda_arm"); + + auto curr_state = move_group->getCurrentState(); + move_group->setStartState(*curr_state); + + test_motion_plans(move_group, cache); + test_cartesian_plans(move_group, cache); + } + + rclcpp::shutdown(); + spin_thread.join(); + + return 0; +} + +// CARTESIAN PLANS === + +// Check construct plan request +// Remember to check fraction too! \ No newline at end of file diff --git a/nexus_motion_planner/test/test_motion_plan_cache.py b/nexus_motion_planner/test/test_motion_plan_cache.py new file mode 100755 index 0000000..e73cbb1 --- /dev/null +++ b/nexus_motion_planner/test/test_motion_plan_cache.py @@ -0,0 +1,148 @@ +# Copyright 2023 Johnson & Johnson +# +# 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. + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_pytest.tools import process as process_tools +from launch_ros.actions import Node + +from moveit_configs_utils import MoveItConfigsBuilder + +import launch_pytest + +import pytest +import os + +# This would have been a gtest, but we need a move_group instance, which +# requires some parameters loaded and a separate node started. + + +@pytest.fixture +def moveit_config(): + return ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines("ompl", ["ompl"]) # Sadly necessary + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_moveit_configs() + ) + + +# We need this so the move_group has something to interact with +@pytest.fixture +def robot_fixture(moveit_config): + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="log", + parameters=[moveit_config.to_dict()], + ) + + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", + "0.0", "0.0", "world", "panda_link0"], + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="log", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="log", + ) + + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format( + controller)], + shell=True, + output="log", + ) + ] + + return [ + run_move_group_node, + static_tf, + robot_state_publisher, + ros2_control_node, + *load_controllers + ] + + +@pytest.fixture +def motion_cache_test_runner_node(moveit_config): + return Node( + package="nexus_motion_planner", + executable="test_motion_plan_cache", + name="test_motion_plan_cache_node", + output="screen", + cached_output=True, + parameters=[moveit_config.to_dict()] + ) + + +@launch_pytest.fixture +def launch_description(motion_cache_test_runner_node, robot_fixture): + return LaunchDescription( + robot_fixture + + [ + motion_cache_test_runner_node, + launch_pytest.actions.ReadyToTest() + ] + ) + + +def validate_stream(expected): + def wrapped(output): + assert expected in output.splitlines( + ), f"Did not get expected: {expected} in output:\n\n{output}" + return wrapped + + +@pytest.mark.launch(fixture=launch_description) +def test_all_tests_pass(motion_cache_test_runner_node, launch_context): + # Check no occurrences of [FAIL] in output + assert not process_tools.wait_for_output_sync( + launch_context, + motion_cache_test_runner_node, + lambda x: "[FAIL]" in x, + timeout=10 + ) + + # Wait for process to end and check for graceful exit + yield + assert motion_cache_test_runner_node.return_code == 0 diff --git a/nexus_motion_planner/test/test_request.test.py b/nexus_motion_planner/test/test_request.test.py index 71c16a9..a4ab8a4 100644 --- a/nexus_motion_planner/test/test_request.test.py +++ b/nexus_motion_planner/test/test_request.test.py @@ -139,6 +139,14 @@ def generate_test_description(): "robot_description_semantic": ParameterValue(robot_description_semantic_content, value_type=str) } + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="log", + parameters=[robot_description], + ) + kinematics_yaml = load_yaml( moveit_config_package, "config/kinematics.yaml") @@ -179,6 +187,34 @@ def generate_test_description(): "publish_transforms_updates": True, } + # Load ros2_control controller (so joint states are published) + # This needs to be loaded in tandem with the MoveIt controller manager + ros2_controllers_path = os.path.join( + get_package_share_directory(moveit_config_package), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_path], + output="log", + ) + + load_controllers = [] + for controller in [ + "manipulator_controller", + "joint_state_broadcaster" + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format( + controller)], + shell=True, + output="log", + ) + ] + # Start the actual move_group node/action server move_group_node = Node( package="moveit_ros_move_group", @@ -307,6 +343,7 @@ def generate_test_description(): } return launch.LaunchDescription([ + robot_state_publisher, motion_planner, move_group_node, static_tf_node_1, @@ -318,6 +355,8 @@ def generate_test_description(): trigger_lifecycle_5, trigger_lifecycle_6, trigger_get_plan_irb1300_poses, + ros2_control_node, + *load_controllers, launch_testing.actions.ReadyToTest() ]), node_mapping diff --git a/nexus_motion_planner/test/test_request_with_cache.test.py b/nexus_motion_planner/test/test_request_with_cache.test.py new file mode 100644 index 0000000..ec92c44 --- /dev/null +++ b/nexus_motion_planner/test/test_request_with_cache.test.py @@ -0,0 +1,411 @@ +# Copyright 2023 Johnson & Johnson +# +# 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. + +import os +import pytest +import unittest +import xacro +import yaml + +import launch +from launch.actions import ( + ExecuteProcess, + LogInfo, + RegisterEventHandler, + SetEnvironmentVariable, + TimerAction +) +from launch.event_handlers import ( + OnExecutionComplete, + OnProcessExit, + OnProcessStart, +) +from launch.substitutions import ( + Command, + FindExecutable, + PathJoinSubstitution, +) + +from launch_ros.actions import Node +from launch_ros.descriptions import ParameterValue +from launch_ros.substitutions import FindPackageShare + +import launch_testing + +from ament_index_python.packages import get_package_share_directory + +def set_lifecycle(server_name, transition): + """Executes service to transition lifecycle states + + Parameters + ---------- + server_name : str + Name of lifecycle node + transition : str + lifecycle state to transition to + + Returns + ------- + launch.actions.ExecuteProcess + An ExecuteProcess action that executes the lifecycle transition + """ + return ExecuteProcess( + cmd=[[ + FindExecutable(name='ros2'), + ' lifecycle set ', + server_name + ' ', + transition + ]], + name=server_name + '_set_lifecycle_to_' + transition, + shell=True, + output='screen', + ) + +def load_yaml(package_name, file_path): + package_path = get_package_share_directory(package_name) + absolute_file_path = os.path.join(package_path, file_path) + + try: + with open(absolute_file_path, "r") as file: + return yaml.safe_load(file) + except EnvironmentError: # parent of IOError, OSError *and* WindowsError where available + return None + +@pytest.mark.launch_test +def generate_test_description(): + """This test sequence tests if the Motion Planner Server is able to generate a + valid plan from the GetMotionPlan interface. + + Returns + ------- + launch.LaunchDescription + Nodes to be launched for testing + """ + SetEnvironmentVariable('RCUTILS_LOGGING_BUFFERED_STREAM', '1') + SetEnvironmentVariable('RCUTILS_LOGGING_USE_STDOUT', '0') + + # This is necessary to get unbuffered output from the process under test + proc_env = os.environ.copy() + proc_env['PYTHONUNBUFFERED'] = '1' + + robot_xacro_file = "irb1300_10_115.xacro" + moveit_config_file = "abb_irb1300_10_115.srdf.xacro" + planner_config_file = "planner_params_with_cache.yaml" + + moveit_config_package = "abb_irb1300_10_115_moveit_config" + support_package = "abb_irb1300_support" + runtime_config_package = "nexus_motion_planner" + + planner_server_params = PathJoinSubstitution( + [ + FindPackageShare(runtime_config_package), + "config", + planner_config_file, + ] + ) + + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(support_package), "urdf", robot_xacro_file] + ), + ] + ) + robot_description = {"robot_description": ParameterValue(robot_description_content, value_type=str)} + + robot_description_semantic_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [FindPackageShare(moveit_config_package), "config", moveit_config_file] + ), + ] + ) + robot_description_semantic = { + "robot_description_semantic": ParameterValue(robot_description_semantic_content, value_type=str) + } + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="log", + parameters=[robot_description], + ) + + kinematics_yaml = load_yaml( + moveit_config_package, "config/kinematics.yaml") + + # Planning Functionality + ompl_planning_pipeline_config = { + "move_group": { + "planning_plugin": "ompl_interface/OMPLPlanner", + "request_adapters": """default_planner_request_adapters/AddTimeOptimalParameterization default_planner_request_adapters/ResolveConstraintFrames default_planner_request_adapters/FixWorkspaceBounds default_planner_request_adapters/FixStartStateBounds default_planner_request_adapters/FixStartStateCollision default_planner_request_adapters/FixStartStatePathConstraints""", + "start_state_max_bounds_error": 0.1, + } + } + ompl_planning_yaml = load_yaml( + moveit_config_package, "config/ompl_planning.yaml" + ) + ompl_planning_pipeline_config["move_group"].update(ompl_planning_yaml) + + # Trajectory Execution Functionality + moveit_simple_controllers_yaml = load_yaml( + moveit_config_package, "config/moveit_controllers.yaml" + ) + moveit_controllers = { + "moveit_simple_controller_manager": moveit_simple_controllers_yaml, + "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", + } + + trajectory_execution = { + # MoveIt does not handle controller switching automatically + "moveit_manage_controllers": False, + "trajectory_execution.allowed_execution_duration_scaling": 1.2, + "trajectory_execution.allowed_goal_duration_margin": 0.5, + "trajectory_execution.allowed_start_tolerance": 0.01, + } + + planning_scene_monitor_parameters = { + "publish_planning_scene": True, + "publish_geometry_updates": True, + "publish_state_updates": True, + "publish_transforms_updates": True, + } + + # Load ros2_control controller (so joint states are published) + # This needs to be loaded in tandem with the MoveIt controller manager + ros2_controllers_path = os.path.join( + get_package_share_directory(moveit_config_package), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[robot_description, ros2_controllers_path], + output="log", + ) + + load_controllers = [] + for controller in [ + "manipulator_controller", + "joint_state_broadcaster" + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format( + controller)], + shell=True, + output="log", + ) + ] + + # Start the actual move_group node/action server + move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + ompl_planning_pipeline_config, + trajectory_execution, + moveit_controllers, + planning_scene_monitor_parameters, + ], + ) + + # Start the nexus_motion_planner server + motion_planner = Node( + package = "nexus_motion_planner", + executable = "motion_planner_server", + name = "motion_planner_server", + output="both", + parameters=[ + robot_description, + robot_description_semantic, + kinematics_yaml, + planner_server_params, + ], + ) + + # Static TF + static_tf_node_1 = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher_world_t_base_link", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "base_link"], + ) + + # Static TF representing item pickup pose + static_tf_node_2 = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher_base_link_t_item", + output="log", + arguments=["0.3", "0.2", "0.1", "0.0", "0.0", "0.0", "base_link", "item"], + ) + + # Send get motion plan requests with specified start and target + # pose values + get_plan_irb1300_poses = Node( + package='nexus_motion_planner', + executable='test_request', + arguments=[ + "-name", "abb_irb1300", + "-frame_id", "item", + "-goal_type", "0", + "-t", "0.794981,0.000000,0.063564,0.000000,0.707107,0.000000,0.707107", + ], + output='both', + ) + + # The following set of lifecycle state transitions tests the lifecycle + # capabilities of the motion planner server + + lifecycle_set_configure = set_lifecycle('/motion_planner_server', 'configure') + lifecycle_set_activate = set_lifecycle('/motion_planner_server', 'activate') + lifecycle_set_deactivate = set_lifecycle('/motion_planner_server', 'deactivate') + lifecycle_set_cleanup = set_lifecycle('/motion_planner_server', 'cleanup') + lifecycle_set_configure2 = set_lifecycle('/motion_planner_server', 'configure') + lifecycle_set_activate2 = set_lifecycle('/motion_planner_server', 'activate') + + trigger_lifecycle_1 = RegisterEventHandler( + OnProcessStart( + target_action=motion_planner, + on_start=[ + TimerAction( + period=5.0, + actions=[lifecycle_set_configure] + ) + ] + ) + ) + + trigger_lifecycle_2 = RegisterEventHandler( + OnProcessExit( + target_action=lifecycle_set_configure, + on_exit=[lifecycle_set_activate])) + + trigger_lifecycle_3 = RegisterEventHandler( + OnProcessExit( + target_action=lifecycle_set_activate, + on_exit=[lifecycle_set_deactivate])) + + trigger_lifecycle_4 = RegisterEventHandler( + OnProcessExit( + target_action=lifecycle_set_deactivate, + on_exit=[lifecycle_set_cleanup])) + + trigger_lifecycle_5 = RegisterEventHandler( + OnProcessExit( + target_action=lifecycle_set_cleanup, + on_exit=[lifecycle_set_configure2])) + + trigger_lifecycle_6 = RegisterEventHandler( + OnProcessExit( + target_action=lifecycle_set_configure2, + on_exit=[lifecycle_set_activate2])) + + trigger_get_plan_irb1300_poses = RegisterEventHandler( + OnProcessExit( + target_action=lifecycle_set_activate2, + on_exit=[ + LogInfo(msg='Sending motion planning task with end-effector poses'), + TimerAction( + period=5.0, + actions=[get_plan_irb1300_poses] + ) + ] + ) + ) + + node_mapping = { + 'motion_planner': motion_planner, + 'get_plan_irb1300_poses': get_plan_irb1300_poses, + } + + return launch.LaunchDescription([ + robot_state_publisher, + motion_planner, + move_group_node, + static_tf_node_1, + static_tf_node_2, + trigger_lifecycle_1, + trigger_lifecycle_2, + trigger_lifecycle_3, + trigger_lifecycle_4, + trigger_lifecycle_5, + trigger_lifecycle_6, + trigger_get_plan_irb1300_poses, + ros2_control_node, + *load_controllers, + launch_testing.actions.ReadyToTest() + ]), node_mapping + +class TestGetMotionPlanService(unittest.TestCase): + def test_read_task_client_stderr( + self, + proc_output, + get_plan_irb1300_poses): + """Check if task client has sent service request and received successful response""" + + get_plan_poses_str = [ + "Target pose specified", + "Moveit Error Code = 1", + ] + + for expected_output in get_plan_poses_str: + proc_output.assertWaitFor( + expected_output, + process=get_plan_irb1300_poses, timeout=40, stream='stderr') + + def test_read_server_stderr( + self, + proc_output, + motion_planner): + """Check if server has successfully computed motion plan""" + + motion_planner_server_str = [ + "Planning request accepted", + "Inserting plan", + "Planning request complete!", + ] + + for expected_output in motion_planner_server_str: + proc_output.assertWaitFor( + expected_output, + process=motion_planner, timeout=60, stream='stderr') + +class TestWait(unittest.TestCase): + def test_wait_for_get_plan_clients(self, proc_info, get_plan_irb1300_poses): + """Wait until process ends""" + proc_info.assertWaitForShutdown(process=get_plan_irb1300_poses, timeout=40) + +@launch_testing.post_shutdown_test() +class TestShutdown(unittest.TestCase): + def test_read_task_client_stderr( + self, + proc_info, + get_plan_irb1300_poses): + """Check if the processes exited normally.""" + + launch_testing.asserts.assertExitCodes(proc_info, [0], + process=get_plan_irb1300_poses) diff --git a/nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv b/nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv index 87cc18f..a883d18 100644 --- a/nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv +++ b/nexus_msgs/nexus_motion_planner_msgs/srv/GetMotionPlan.srv @@ -52,6 +52,11 @@ string payload float64 max_velocity_scaling_factor float64 max_acceleration_scaling_factor +# Set to true in order to force the server to use the motion cache as if the +# cache mode were set to ExecuteReadOnly, regardless of the original setting +# for this request. +bool force_cache_mode_execute_read_only + --- # Motion planning result diff --git a/nexus_tree_nodes.xml b/nexus_tree_nodes.xml index e9e7efa..9001d42 100644 --- a/nexus_tree_nodes.xml +++ b/nexus_tree_nodes.xml @@ -52,6 +52,7 @@ An optional fraction [0,1] to scale the acceleraton and speed of the generated trajectory True if a cartesian plan is required If provided the joint_constraints will be used as the start state of the robot. By default the current state is set as start + True to force cache mode to ExecuteReadOnly for this request. The generated motion plan @@ -141,7 +142,7 @@ The geometry_msgs::msg::PoseStamped pose at the end of the offset OPTIONAL, rclcpp::Time timepoint to lookup on, if not provided, the current time is used OPTIONAL, if true, return t such that `target_pose = base_pose * t` - The geometry_msgs::msg::Transform offset that transforms base_pose to target_pose + The geometry_msgs::msg::Transform offset that transforms base_pose to target_pose