From 6ebd330b82fd970d64424f1e2d3b3d8d50af56ee Mon Sep 17 00:00:00 2001 From: methylDragon Date: Mon, 20 Nov 2023 11:37:13 -0800 Subject: [PATCH] Remove query appending macro Signed-off-by: methylDragon --- .../src/motion_plan_cache.cpp | 142 +++++++++--------- 1 file changed, 67 insertions(+), 75 deletions(-) diff --git a/nexus_motion_planner/src/motion_plan_cache.cpp b/nexus_motion_planner/src/motion_plan_cache.cpp index 3541f9c..e0e4b15 100644 --- a/nexus_motion_planner/src/motion_plan_cache.cpp +++ b/nexus_motion_planner/src/motion_plan_cache.cpp @@ -26,13 +26,18 @@ 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; +// Append a range inclusive query with some tolerance about some center value. +void queryAppendRangeInclusiveWithTolerance( + Query& query, const std::string& name, double center, double tolerance) +{ + query.appendRangeInclusive( + name, center - tolerance / 2, center + tolerance / 2); +} + MotionPlanCache::MotionPlanCache(const rclcpp::Node::SharedPtr& node) : node_(node) { @@ -336,10 +341,10 @@ MotionPlanCache::extract_and_append_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( "start_state.joint_state.position_" + std::to_string(i), - NEXUS_MATCH_RANGE( - current_state_msg.joint_state.position.at(i), match_tolerance) + + current_state_msg.joint_state.position.at(i), match_tolerance ); } } @@ -352,10 +357,10 @@ MotionPlanCache::extract_and_append_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( "start_state.joint_state.position_" + std::to_string(i), - NEXUS_MATCH_RANGE( - plan_request.start_state.joint_state.position.at(i), match_tolerance) + + plan_request.start_state.joint_state.position.at(i), match_tolerance ); } } @@ -398,19 +403,19 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( // auto original = *query; // Copy not supported. - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( "max_velocity_scaling_factor", - NEXUS_MATCH_RANGE( - plan_request.max_velocity_scaling_factor, match_tolerance) + + plan_request.max_velocity_scaling_factor, match_tolerance ); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( "max_acceleration_scaling_factor", - NEXUS_MATCH_RANGE( - plan_request.max_acceleration_scaling_factor, match_tolerance) + + plan_request.max_acceleration_scaling_factor, match_tolerance ); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( "max_cartesian_speed", - NEXUS_MATCH_RANGE(plan_request.max_cartesian_speed, match_tolerance)); + plan_request.max_cartesian_speed, match_tolerance); // Extract constraints (so we don't have cardinality on goal_constraint idx.) std::vector joint_constraints; @@ -440,9 +445,9 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); query.append(meta_name + ".joint_name", constraint.joint_name); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( meta_name + ".position", - NEXUS_MATCH_RANGE(constraint.position, match_tolerance)); + constraint.position, match_tolerance); query.appendGTE( meta_name + ".tolerance_above", constraint.tolerance_above); query.appendLTE( @@ -501,18 +506,18 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( query.append(meta_name + ".link_name", constraint.link_name); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( meta_name + ".target_point_offset.x", - NEXUS_MATCH_RANGE( - x_offset + constraint.target_point_offset.x, match_tolerance)); - query.appendRangeInclusive( + + x_offset + constraint.target_point_offset.x, match_tolerance); + queryAppendRangeInclusiveWithTolerance( meta_name + ".target_point_offset.y", - NEXUS_MATCH_RANGE( - y_offset + constraint.target_point_offset.y, match_tolerance)); - query.appendRangeInclusive( + + y_offset + constraint.target_point_offset.y, match_tolerance); + queryAppendRangeInclusiveWithTolerance( meta_name + ".target_point_offset.z", - NEXUS_MATCH_RANGE( - z_offset + constraint.target_point_offset.z, match_tolerance)); + + z_offset + constraint.target_point_offset.z, match_tolerance); } } @@ -586,18 +591,18 @@ MotionPlanCache::extract_and_append_plan_goal_to_query( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( meta_name + ".target_point_offset.x", - NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); - query.appendRangeInclusive( + final_quat.getX(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( meta_name + ".target_point_offset.y", - NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); - query.appendRangeInclusive( + final_quat.getY(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( meta_name + ".target_point_offset.z", - NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); - query.appendRangeInclusive( + final_quat.getZ(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( meta_name + ".target_point_offset.w", - NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); + final_quat.getW(), match_tolerance); } } @@ -1208,10 +1213,9 @@ MotionPlanCache::extract_and_append_cartesian_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( "start_state.joint_state.position_" + std::to_string(i), - NEXUS_MATCH_RANGE( - current_state_msg.joint_state.position.at(i), match_tolerance)); + current_state_msg.joint_state.position.at(i), match_tolerance); } } else @@ -1223,11 +1227,9 @@ MotionPlanCache::extract_and_append_cartesian_plan_start_to_query( query.append( "start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( "start_state.joint_state.position_" + std::to_string(i), - NEXUS_MATCH_RANGE( - plan_request.start_state.joint_state.position.at(i), - match_tolerance)); + plan_request.start_state.joint_state.position.at(i), match_tolerance); } } @@ -1260,20 +1262,16 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( // auto original = *metadata; // Copy not supported. - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( "max_velocity_scaling_factor", - NEXUS_MATCH_RANGE( - plan_request.max_velocity_scaling_factor, match_tolerance)); - query.appendRangeInclusive( + plan_request.max_velocity_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance( "max_acceleration_scaling_factor", - NEXUS_MATCH_RANGE( - plan_request.max_acceleration_scaling_factor, match_tolerance)); - query.appendRangeInclusive( - "max_step", - NEXUS_MATCH_RANGE(plan_request.max_step, match_tolerance)); - query.appendRangeInclusive( - "jump_threshold", - NEXUS_MATCH_RANGE(plan_request.jump_threshold, match_tolerance)); + plan_request.max_acceleration_scaling_factor, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + "max_step", plan_request.max_step, match_tolerance); + queryAppendRangeInclusiveWithTolerance( + "jump_threshold", plan_request.jump_threshold, match_tolerance); // Waypoints // Restating them in terms of the robot model frame (usually base_link) @@ -1328,15 +1326,15 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( // Apply offsets // Position - query.appendRangeInclusive( + queryAppendRangeInclusiveWithTolerance( meta_name + ".position.x", - NEXUS_MATCH_RANGE(x_offset + waypoint.position.x, match_tolerance)); - query.appendRangeInclusive( + x_offset + waypoint.position.x, match_tolerance); + queryAppendRangeInclusiveWithTolerance( meta_name + ".position.y", - NEXUS_MATCH_RANGE(y_offset+ waypoint.position.y, match_tolerance)); - query.appendRangeInclusive( + y_offset+ waypoint.position.y, match_tolerance); + queryAppendRangeInclusiveWithTolerance( meta_name + ".position.z", - NEXUS_MATCH_RANGE(z_offset+waypoint.position.z, match_tolerance)); + z_offset+waypoint.position.z, match_tolerance); // Orientation tf2::Quaternion tf2_quat_goal_offset( @@ -1349,18 +1347,14 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - query.appendRangeInclusive( - meta_name + ".orientation.x", - NEXUS_MATCH_RANGE(final_quat.getX(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".orientation.y", - NEXUS_MATCH_RANGE(final_quat.getY(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".orientation.z", - NEXUS_MATCH_RANGE(final_quat.getZ(), match_tolerance)); - query.appendRangeInclusive( - meta_name + ".orientation.w", - NEXUS_MATCH_RANGE(final_quat.getW(), match_tolerance)); + queryAppendRangeInclusiveWithTolerance( + meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + queryAppendRangeInclusiveWithTolerance( + meta_name + ".orientation.w", final_quat.getW(), match_tolerance); } query.append("link_name", plan_request.link_name); @@ -1555,7 +1549,5 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata( return true; } -#undef NEXUS_MATCH_RANGE - } // namespace motion_planner } // namespace nexus