From 6a7a3d9d883a3613a0dfa682ba8e6b5e88feabf9 Mon Sep 17 00:00:00 2001 From: methylDragon Date: Tue, 26 Sep 2023 22:56:56 -0700 Subject: [PATCH] Update only use cached plans parameter name Signed-off-by: methylDragon --- .../src/motion_planner_server.cpp | 23 ++++++++++--------- .../src/motion_planner_server.hpp | 2 +- 2 files changed, 13 insertions(+), 12 deletions(-) diff --git a/nexus_motion_planner/src/motion_planner_server.cpp b/nexus_motion_planner/src/motion_planner_server.cpp index 10d8883..ce974cb 100644 --- a/nexus_motion_planner/src/motion_planner_server.cpp +++ b/nexus_motion_planner/src/motion_planner_server.cpp @@ -199,12 +199,12 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) _use_motion_plan_cache ? "True" : "False" ); - _only_use_cached_plans = this->declare_parameter( - "only_use_cached_plans", false); + _use_cached_plans_instead_of_planning = this->declare_parameter( + "use_cached_plans_instead_of_planning", false); RCLCPP_INFO( this->get_logger(), - "Setting parameter only_use_cached_plans to [%s]", - _only_use_cached_plans ? "True" : "False" + "Setting parameter use_cached_plans_instead_of_planning to [%s]", + _use_cached_plans_instead_of_planning ? "True" : "False" ); _overwrite_worse_plans = this->declare_parameter( @@ -235,7 +235,7 @@ MotionPlannerServer::MotionPlannerServer(const rclcpp::NodeOptions& options) ); _cache_exact_match_tolerance = this->declare_parameter( - "cache_exact_match_tolerance", 0.00001); + "cache_exact_match_tolerance", 0.0005); // ~0.028 degrees per joint RCLCPP_INFO( this->get_logger(), "Setting parameter cache_exact_match_tolerance to [%.2e]", @@ -310,11 +310,12 @@ auto MotionPlannerServer::on_configure(const LifecycleState& /*state*/) -> CallbackReturn { RCLCPP_INFO(this->get_logger(), "Configuring..."); - if (!_use_motion_plan_cache && _only_use_cached_plans) + if (!_use_motion_plan_cache && _use_cached_plans_instead_of_planning) { RCLCPP_ERROR( this->get_logger(), - "use_motion_plan_cache must be true if only_use_cached_plans is true." + "use_motion_plan_cache must be true if " + "use_cached_plans_instead_of_planning is true." ); return CallbackReturn::ERROR; } @@ -642,7 +643,7 @@ void MotionPlannerServer::plan_with_move_group( bool plan_is_from_cache = false; interface->constructMotionPlanRequest(plan_req_msg); - if (!_only_use_cached_plans) + if (!_use_cached_plans_instead_of_planning) { res->result.error_code = interface->plan(plan); RCLCPP_INFO( @@ -667,7 +668,7 @@ void MotionPlannerServer::plan_with_move_group( moveit_msgs::msg::MoveItErrorCodes::SUCCESS; RCLCPP_INFO( this->get_logger(), - "Cache fetch took %es, planning time was: %es", + "Cache fetch took %es, planning time of fetched plan was: %es", (fetch_end - fetch_start).seconds(), fetched_plan->lookupDouble("planning_time_s")); } @@ -675,8 +676,8 @@ void MotionPlannerServer::plan_with_move_group( { RCLCPP_ERROR( this->get_logger(), - "only_use_cached_plans was true, and could not find cached plan for " - "plan request: \n\n%s", + "use_cached_plans_instead_of_planning 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; diff --git a/nexus_motion_planner/src/motion_planner_server.hpp b/nexus_motion_planner/src/motion_planner_server.hpp index 24e319a..19dab7d 100644 --- a/nexus_motion_planner/src/motion_planner_server.hpp +++ b/nexus_motion_planner/src/motion_planner_server.hpp @@ -99,7 +99,7 @@ class MotionPlannerServer : public rclcpp_lifecycle::LifecycleNode std::shared_ptr _motion_plan_cache; bool _use_motion_plan_cache; - bool _only_use_cached_plans; + bool _use_cached_plans_instead_of_planning; bool _overwrite_worse_plans; std::string _cache_db_plugin; std::string _cache_db_host;