Skip to content

Commit

Permalink
Switch to snake case for function names
Browse files Browse the repository at this point in the history
Signed-off-by: methylDragon <[email protected]>
  • Loading branch information
methylDragon committed Sep 26, 2023
1 parent c07e8bd commit af5827a
Show file tree
Hide file tree
Showing 3 changed files with 26 additions and 26 deletions.
34 changes: 17 additions & 17 deletions nexus_motion_planner/src/motion_plan_cache.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -54,7 +54,7 @@ void MotionPlanCache::init(

// TOP LEVEL OPS ===============================================================
std::vector<MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr>
MotionPlanCache::fetchAllMatchingPlans(
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,
Expand All @@ -65,9 +65,9 @@ MotionPlanCache::fetchAllMatchingPlans(

Query::Ptr query = coll.createQuery();

bool start_ok = this->extractAndAppendStartToQuery(
bool start_ok = this->extract_and_append_start_to_query(
*query, move_group, plan_request, start_tolerance);
bool goal_ok = this->extractAndAppendGoalToQuery(
bool goal_ok = this->extract_and_append_goal_to_query(
*query, move_group, plan_request, goal_tolerance);

if (!start_ok || !goal_ok)
Expand All @@ -81,15 +81,15 @@ MotionPlanCache::fetchAllMatchingPlans(
}

MessageWithMetadata<moveit_msgs::msg::RobotTrajectory>::ConstPtr
MotionPlanCache::fetchBestMatchingPlan(
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)
{
// 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(
auto matching_plans = this->fetch_all_matching_plans(
move_group, move_group_namespace,
plan_request, start_tolerance, goal_tolerance,
true);
Expand All @@ -113,7 +113,7 @@ MotionPlanCache::fetchBestMatchingPlan(
}

bool
MotionPlanCache::putPlan(
MotionPlanCache::put_plan(
const moveit::planning_interface::MoveGroupInterface& move_group,
const std::string& move_group_namespace,
const moveit_msgs::msg::MotionPlanRequest& plan_request,
Expand Down Expand Up @@ -155,10 +155,10 @@ MotionPlanCache::putPlan(
// overwrite.
Query::Ptr exact_query = coll.createQuery();

bool start_query_ok = this->extractAndAppendStartToQuery(
bool start_query_ok = this->extract_and_append_start_to_query(
*exact_query, move_group,
plan_request, exact_match_precision_);
bool goal_query_ok = this->extractAndAppendGoalToQuery(
bool goal_query_ok = this->extract_and_append_goal_to_query(
*exact_query, move_group,
plan_request, exact_match_precision_);

Expand Down Expand Up @@ -208,9 +208,9 @@ MotionPlanCache::putPlan(
{
Metadata::Ptr insert_metadata = coll.createMetadata();

bool start_meta_ok = this->extractAndAppendStartToMetadata(
bool start_meta_ok = this->extract_and_append_start_to_metadata(
*insert_metadata, move_group, plan_request);
bool goal_meta_ok = this->extractAndAppendGoalToMetadata(
bool goal_meta_ok = this->extract_and_append_goal_to_metadata(
*insert_metadata, move_group, plan_request);
insert_metadata->append("execution_time_s", execution_time_s);

Expand Down Expand Up @@ -242,7 +242,7 @@ MotionPlanCache::putPlan(

// QUERY CONSTRUCTION ==========================================================
bool
MotionPlanCache::extractAndAppendStartToQuery(
MotionPlanCache::extract_and_append_start_to_query(
Query& query,
const moveit::planning_interface::MoveGroupInterface& move_group,
const moveit_msgs::msg::MotionPlanRequest& plan_request,
Expand Down Expand Up @@ -302,7 +302,7 @@ MotionPlanCache::extractAndAppendStartToQuery(
// 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.
// apply the joint states as offsets as well.
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
if (!current_state)
{
Expand Down Expand Up @@ -348,7 +348,7 @@ MotionPlanCache::extractAndAppendStartToQuery(
}

bool
MotionPlanCache::extractAndAppendGoalToQuery(
MotionPlanCache::extract_and_append_goal_to_query(
Query& query,
const moveit::planning_interface::MoveGroupInterface& /* move_group */,
const moveit_msgs::msg::MotionPlanRequest& plan_request,
Expand Down Expand Up @@ -591,7 +591,7 @@ MotionPlanCache::extractAndAppendGoalToQuery(

// METADATA CONSTRUCTION =======================================================
bool
MotionPlanCache::extractAndAppendStartToMetadata(
MotionPlanCache::extract_and_append_start_to_metadata(
Metadata& metadata,
const moveit::planning_interface::MoveGroupInterface& move_group,
const moveit_msgs::msg::MotionPlanRequest& plan_request)
Expand Down Expand Up @@ -647,7 +647,7 @@ MotionPlanCache::extractAndAppendStartToMetadata(
// 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.
// apply the joint states as offsets as well.
moveit::core::RobotStatePtr current_state = move_group.getCurrentState();
if (!current_state)
{
Expand Down Expand Up @@ -690,7 +690,7 @@ MotionPlanCache::extractAndAppendStartToMetadata(
}

bool
MotionPlanCache::extractAndAppendGoalToMetadata(
MotionPlanCache::extract_and_append_goal_to_metadata(
Metadata& metadata,
const moveit::planning_interface::MoveGroupInterface& /* move_group */,
const moveit_msgs::msg::MotionPlanRequest& plan_request)
Expand Down Expand Up @@ -915,4 +915,4 @@ MotionPlanCache::extractAndAppendGoalToMetadata(
#undef NEXUS_MATCH_RANGE

} // namespace motion_planner
} // namespace nexus
} // namespace nexus
14 changes: 7 additions & 7 deletions nexus_motion_planner/src/motion_plan_cache.hpp
Original file line number Diff line number Diff line change
Expand Up @@ -74,7 +74,7 @@ class MotionPlanCache
moveit_msgs::msg::RobotTrajectory
>::ConstPtr
>
fetchAllMatchingPlans(
fetch_all_matching_plans(
const moveit::planning_interface::MoveGroupInterface& move_group,
const std::string& move_group_namespace,
const moveit_msgs::msg::MotionPlanRequest& plan_request,
Expand All @@ -83,13 +83,13 @@ class MotionPlanCache
warehouse_ros::MessageWithMetadata<
moveit_msgs::msg::RobotTrajectory
>::ConstPtr
fetchBestMatchingPlan(
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);

bool putPlan(
bool put_plan(
const moveit::planning_interface::MoveGroupInterface& move_group,
const std::string& move_group_namespace,
const moveit_msgs::msg::MotionPlanRequest& plan_request,
Expand All @@ -98,25 +98,25 @@ class MotionPlanCache
bool overwrite = true);

// QUERY CONSTRUCTION ========================================================
bool extractAndAppendStartToQuery(
bool extract_and_append_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 extractAndAppendGoalToQuery(
bool extract_and_append_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 extractAndAppendStartToMetadata(
bool extract_and_append_start_to_metadata(
warehouse_ros::Metadata& metadata,
const moveit::planning_interface::MoveGroupInterface& move_group,
const moveit_msgs::msg::MotionPlanRequest& plan_request);

bool extractAndAppendGoalToMetadata(
bool extract_and_append_goal_to_metadata(
warehouse_ros::Metadata& metadata,
const moveit::planning_interface::MoveGroupInterface& move_group,
const moveit_msgs::msg::MotionPlanRequest& plan_request);
Expand Down
4 changes: 2 additions & 2 deletions nexus_motion_planner/src/motion_planner_server.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -639,7 +639,7 @@ void MotionPlannerServer::plan_with_move_group(
}
else
{
auto fetched_plan = _motion_plan_cache->fetchBestMatchingPlan(
auto fetched_plan = _motion_plan_cache->fetch_best_matching_plan(
*interface, robot_name, plan_req_msg,
_cache_start_match_tolerance, _cache_goal_match_tolerance);
if (fetched_plan)
Expand Down Expand Up @@ -677,7 +677,7 @@ void MotionPlannerServer::plan_with_move_group(

if (_use_motion_plan_cache && !req.cartesian)
{
bool plan_put_ok = _motion_plan_cache->putPlan(
bool plan_put_ok = _motion_plan_cache->put_plan(
*interface, robot_name,
std::move(plan_req_msg), std::move(res->result.trajectory),
(exec_end - exec_start).seconds(), true);
Expand Down

0 comments on commit af5827a

Please sign in to comment.