From 815934f80008cf3c1e32e2d341d4d44f8205bd66 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Thu, 21 Sep 2023 15:28:11 -0700 Subject: [PATCH] Add motion plan cache Signed-off-by: methylDragon --- nexus_motion_planner/CMakeLists.txt | 4 +- .../src/motion_plan_cache.cpp | 918 ++++++++++++++++++ .../src/motion_plan_cache.hpp | 137 +++ .../src/motion_planner_server.cpp | 167 +++- .../src/motion_planner_server.hpp | 19 + 5 files changed, 1238 insertions(+), 7 deletions(-) create mode 100644 nexus_motion_planner/src/motion_plan_cache.cpp create mode 100644 nexus_motion_planner/src/motion_plan_cache.hpp diff --git a/nexus_motion_planner/CMakeLists.txt b/nexus_motion_planner/CMakeLists.txt index 9c69e8d..5f73ce9 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) @@ -28,6 +29,7 @@ set (motion_planner_server_dependencies rclcpp_action rclcpp_lifecycle warehouse_ros + warehouse_ros_sqlite tf2 tf2_ros ) @@ -44,7 +46,7 @@ set(LIBRARY_NAME motion_planner_server_core) set(EXECUTABLE_NAME motion_planner_server) # Server executable -add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp) +add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp src/motion_plan_cache.cpp) ament_target_dependencies(${EXECUTABLE_NAME} ${motion_planner_server_dependencies}) # Test executable 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..60a895a --- /dev/null +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -0,0 +1,918 @@ +#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 { + +#define NEXUS_MATCH_RANGE(arg, tolerance) \ + arg - tolerance / 2, arg + tolerance / 2 + +using warehouse_ros::MessageWithMetadata; +using warehouse_ros::Metadata; +using warehouse_ros::Query; + +MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) +: node_(node) +{ + // TODO: methyldragon - + // Declare or pass in these in the motion_planner_server instead? + if (!node_->has_parameter("warehouse_plugin")) + { + node_->declare_parameter( + "warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + } + + tf_buffer_ = + std::make_unique(node_->get_clock()); + tf_listener_ = + std::make_shared(*tf_buffer_); + + warehouse_ros::DatabaseLoader loader(node_); + db_ = loader.loadDatabase(); +} + +void 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); + + exact_match_precision_ = exact_match_precision; + db_->setParams(db_path, db_port); + db_->connect(); +} + +// TOP LEVEL OPS =============================================================== +std::vector::ConstPtr> +MotionPlanCache::fetchAllMatchingPlans( + 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) +{ + auto coll = db_->openCollection( + "move_group_plan_cache", move_group_namespace); + + Query::Ptr query = coll.createQuery(); + + bool start_ok = this->extractAndAppendStartToQuery( + *query, move_group, plan_request, start_tolerance); + bool goal_ok = this->extractAndAppendGoalToQuery( + *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 */ "execution_time_s", true); +} + +MessageWithMetadata::ConstPtr +MotionPlanCache::fetchBestMatchingPlan( + 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) +{ + // 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->fetchAllMatchingPlans( + move_group, move_group_namespace, + plan_request, start_tolerance, goal_tolerance, + true); + + 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::putPlan( + 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, + bool overwrite) +{ + // Check pre-conditions + if (!plan.multi_dof_joint_trajectory.points.empty()) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping 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 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 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); + + // If start and goal are "exact" match, AND the candidate plan is better, + // overwrite. + Query::Ptr exact_query = coll.createQuery(); + + bool start_query_ok = this->extractAndAppendStartToQuery( + *exact_query, move_group, + plan_request, exact_match_precision_); + bool goal_query_ok = this->extractAndAppendGoalToQuery( + *exact_query, move_group, + plan_request, exact_match_precision_); + + if (!start_query_ok || !goal_query_ok) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping insert: Could not construct overwrite query."); + return false; + } + + auto exact_matches = coll.queryList(exact_query, /* metadata_only */ true); + double best_execution_time_seen = std::numeric_limits::infinity(); + if (!exact_matches.empty()) + { + for (auto& match : exact_matches) + { + double match_execution_time_s = + match->lookupDouble("execution_time_s"); + if (match_execution_time_s < best_execution_time_seen) + { + best_execution_time_seen = match_execution_time_s; + } + + if (match_execution_time_s > execution_time_s) + { + // If we found any "exact" matches that are worse, delete them. + if (overwrite) + { + int delete_id = match->lookupInt("id"); + RCLCPP_ERROR( + 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_seen) + { + Metadata::Ptr insert_metadata = coll.createMetadata(); + + bool start_meta_ok = this->extractAndAppendStartToMetadata( + *insert_metadata, move_group, plan_request); + bool goal_meta_ok = this->extractAndAppendGoalToMetadata( + *insert_metadata, move_group, plan_request); + insert_metadata->append("execution_time_s", execution_time_s); + + if (!start_meta_ok || !goal_meta_ok) + { + RCLCPP_ERROR( + node_->get_logger(), + "Skipping 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_seen); + + coll.insert(plan, insert_metadata); + return true; + } + + RCLCPP_INFO( + node_->get_logger(), + "Skipping insert: New plan execution_time (%es) " + "is worse than best plan's execution_time (%es)", + execution_time_s, best_execution_time_seen); + return false; +} + +// QUERY CONSTRUCTION ========================================================== +bool +MotionPlanCache::extractAndAppendStartToQuery( + 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 after as well. + 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.appendRangeInclusive( + "start_state.joint_state.position_" + std::to_string(i), + NEXUS_MATCH_RANGE( + 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.appendRangeInclusive( + "start_state.joint_state.position_" + std::to_string(i), + NEXUS_MATCH_RANGE( + plan_request.start_state.joint_state.position.at(i), match_tolerance) + ); + } + } + return true; +} + +bool +MotionPlanCache::extractAndAppendGoalToQuery( + 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.appendRangeInclusive( + "max_velocity_scaling_factor", + NEXUS_MATCH_RANGE( + plan_request.max_velocity_scaling_factor, match_tolerance) + ); + query.appendRangeInclusive( + "max_acceleration_scaling_factor", + NEXUS_MATCH_RANGE( + plan_request.max_acceleration_scaling_factor, match_tolerance) + ); + query.appendRangeInclusive( + "max_cartesian_speed", + NEXUS_MATCH_RANGE(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); + } + } + + // 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.appendRangeInclusive( + meta_name + ".position", + NEXUS_MATCH_RANGE(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.appendRangeInclusive( + meta_name + ".target_point_offset.x", + NEXUS_MATCH_RANGE( + x_offset + constraint.target_point_offset.x, match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.y", + NEXUS_MATCH_RANGE( + y_offset + constraint.target_point_offset.y, match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.z", + NEXUS_MATCH_RANGE( + 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.appendRangeInclusive( + meta_name + ".target_point_offset.x", + NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.y", + NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.z", + NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); + query.appendRangeInclusive( + meta_name + ".target_point_offset.w", + NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); + } + } + + return true; +} + +// METADATA CONSTRUCTION ======================================================= +bool +MotionPlanCache::extractAndAppendStartToMetadata( + 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 after as well. + 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::extractAndAppendGoalToMetadata( + 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); + } + } + + // 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; +} + +#undef NEXUS_MATCH_RANGE + +} // namespace motion_planner +} // namespace nexus \ No newline at end of file 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..a865506 --- /dev/null +++ b/nexus_motion_planner/src/motion_plan_cache.hpp @@ -0,0 +1,137 @@ +#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 + +// NEXUS messages +#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. + * + * 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. + */ +class MotionPlanCache +{ +public: + // We explicitly need a Node::SharedPtr because warehouse_ros ONLY supports + // it... + explicit MotionPlanCache(const rclcpp::Node::SharedPtr& node); + + void init( + const std::string& db_path = ":memory:", + uint32_t db_port = 0, + double exact_match_precision = 0.00001); + + // TOP LEVEL OPS ============================================================= + std::vector< + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + > + fetchAllMatchingPlans( + 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); + + warehouse_ros::MessageWithMetadata< + moveit_msgs::msg::RobotTrajectory + >::ConstPtr + fetchBestMatchingPlan( + 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); + + bool putPlan( + 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, + bool overwrite = true); + + // QUERY CONSTRUCTION ======================================================== + bool extractAndAppendStartToQuery( + warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + bool extractAndAppendGoalToQuery( + warehouse_ros::Query& query, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request, + double match_tolerance); + + // METADATA CONSTRUCTION ===================================================== + bool extractAndAppendStartToMetadata( + warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + + bool extractAndAppendGoalToMetadata( + warehouse_ros::Metadata& metadata, + const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::MotionPlanRequest& plan_request); + +private: + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + + double exact_match_precision_ = 0; + + 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 1ecd6f1..67f044f 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -189,6 +189,98 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) "Setting parameter get_state_wait_seconds to [%.3f]", _get_state_wait_seconds ); + + // Motion plan cache params + _use_motion_plan_cache = this->declare_parameter( + "use_motion_plan_cache", true); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter use_motion_plan_cache to [%s]", + _use_motion_plan_cache ? "True" : "False" + ); + + _only_use_cached_plans = this->declare_parameter( + "only_use_cached_plans", false); + RCLCPP_INFO( + this->get_logger(), + "Setting parameter only_use_cached_plans to [%s]", + _only_use_cached_plans ? "True" : "False" + ); + + _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 + ); + + _cache_exact_match_tolerance = this->declare_parameter( + "cache_exact_match_tolerance", 0.025); + 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 (_use_motion_plan_cache) + { + auto cache_node_options = rclcpp::NodeOptions(); + cache_node_options.automatically_declare_parameters_from_overrides(true); + cache_node_options.use_global_arguments(false); + _internal_cache_node = std::make_shared( + "motion_planner_server_internal_cache_node", cache_node_options); + _cache_spin_thread = std::thread( + [this]() + { + while (rclcpp::ok()) + { + rclcpp::spin_some(_internal_cache_node); + } + }); + + // Push warehouse_ros parameters to internal cache node + // This must happen BEFORE the MotionPlanCache is created! + _internal_cache_node->declare_parameter( + "warehouse_plugin", _cache_db_plugin); + + _internal_cache_node->declare_parameter( + "warehouse_host", _cache_db_host); + + _internal_cache_node->declare_parameter( + "warehouse_port", _cache_db_port); + + _motion_plan_cache = + std::make_shared( + _internal_cache_node); + } } //============================================================================== @@ -198,6 +290,11 @@ MotionPlannerServer::~MotionPlannerServer() { _spin_thread.join(); } + + if (_cache_spin_thread.joinable()) + { + _cache_spin_thread.join(); + } } //============================================================================== @@ -205,6 +302,21 @@ auto MotionPlannerServer::on_configure(const LifecycleState& /*state*/) -> CallbackReturn { RCLCPP_INFO(this->get_logger(), "Configuring..."); + if (!_use_motion_plan_cache && _only_use_cached_plans) + { + RCLCPP_ERROR( + this->get_logger(), + "use_motion_plan_cache must be true if only_use_cached_plans is true." + ); + return CallbackReturn::ERROR; + } + + if (_use_motion_plan_cache) + { + _motion_plan_cache->init( + _cache_db_host, _cache_db_port, _cache_exact_match_tolerance); + } + if (_use_move_group_interfaces) { auto ok = initialize_move_group_interfaces(); @@ -488,6 +600,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; @@ -513,24 +626,66 @@ void MotionPlannerServer::plan_with_move_group( else { res->result.error_code.val = 1; - } } else { MoveGroupInterface::Plan plan; - res->result.error_code = interface->plan(plan); + interface->constructMotionPlanRequest(plan_req_msg); + + if (!_only_use_cached_plans) + { + res->result.error_code = interface->plan(plan); + } + else + { + auto fetched_plan = _motion_plan_cache->fetchBestMatchingPlan( + *interface, robot_name, plan_req_msg, + _cache_start_match_tolerance, _cache_goal_match_tolerance); + if (fetched_plan) + { + plan.start_state = plan_req_msg.start_state; + plan.trajectory = *fetched_plan; + plan.planning_time = 0; // ??? + res->result.error_code.val = + moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } + else + { + RCLCPP_ERROR( + this->get_logger(), + "only_use_cached_plans was true, 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; + } + } 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 (_execute_trajectory) { - RCLCPP_INFO( - this->get_logger(), - "Executing trajectory!"); + RCLCPP_INFO(this->get_logger(), "Executing trajectory!"); + + auto exec_start = this->now(); // This is a blocking call. - interface->execute(res->result.trajectory); + // interface->execute(res->result.trajectory); + auto exec_end = this->now(); + + if (_use_motion_plan_cache && !req.cartesian) + { + bool plan_put_ok = _motion_plan_cache->putPlan( + *interface, robot_name, + std::move(plan_req_msg), std::move(res->result.trajectory), + (exec_end - exec_start).seconds(), true); + if (!plan_put_ok) + { + RCLCPP_WARNING(this->get_logger(), "Did not put plan into cache."); + } + } } return; } diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 050929f..5ad8564 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -35,6 +35,9 @@ // NEXUS messages #include +// NEXUS motion plan cache +#include "motion_plan_cache.hpp" + //============================================================================== class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode { @@ -69,6 +72,10 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode rclcpp::Node::SharedPtr _internal_node; std::thread _spin_thread; + rclcpp::Node::SharedPtr _internal_cache_node; + std::thread _cache_spin_thread; + + // MoveIt planning std::vector _manipulators; bool _use_move_group_interfaces; bool _use_namespace; @@ -88,6 +95,18 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode double _workspace_max_y; double _workspace_max_z; + // Motion plan caching + std::shared_ptr _motion_plan_cache; + + bool _use_motion_plan_cache; + bool _only_use_cached_plans; + std::string _cache_db_plugin; + std::string _cache_db_host; + int _cache_db_port; + double _cache_exact_match_tolerance; + double _cache_start_match_tolerance; + double _cache_goal_match_tolerance; + rclcpp::Service::SharedPtr _plan_srv; std::unordered_map>