Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Add unit tests for motion plan cache #28

Merged
merged 13 commits into from
Nov 27, 2023
Merged

Add unit tests for motion plan cache #28

Show file tree
Hide file tree
Changes from 12 commits
Commits
Show all changes
13 commits
Select commit Hold shift + click to select a range
b705556
Add count methods
methylDragon Nov 21, 2023
ae9cbc6
Enable shared from this for cache class
methylDragon Nov 21, 2023
0af131f
Add unit test build rules
methylDragon Nov 22, 2023
237fb50
Add tests for motion plan cache (but not cartesian)
methylDragon Nov 22, 2023
c461fd7
Fix bugs in cartesian caching
methylDragon Nov 23, 2023
bcbb978
Add tests for cartesian plan cache
methylDragon Nov 23, 2023
a06c578
Exit if a test fails
methylDragon Nov 23, 2023
3e2bb01
Remove gtest import
methylDragon Nov 23, 2023
bf53bf7
Remove enable_shared_from_this
methylDragon Nov 26, 2023
e23f202
Only check for failure
methylDragon Nov 26, 2023
841a7ca
Test half in-tolerance
methylDragon Nov 26, 2023
776c5fd
Test different robot cache
methylDragon Nov 26, 2023
644b5b4
Add force_cache_mode_execute_read_only (#29)
methylDragon Nov 27, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
26 changes: 22 additions & 4 deletions nexus_motion_planner/CMakeLists.txt
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,12 +28,17 @@ set (motion_planner_server_dependencies
rclcpp
rclcpp_action
rclcpp_lifecycle
warehouse_ros
warehouse_ros_sqlite
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
Expand All @@ -48,7 +53,7 @@ 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_planner_server_dependencies})
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)
Expand All @@ -72,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)
Expand All @@ -80,19 +86,31 @@ 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}
MAX_LINE_LENGTH 80
LANGUAGE CPP
)

ament_add_pytest_test(test_motion_plan_cache_py "test/test_motion_plan_cache.py"
WORKING_DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}"
)

# TODO(YV): Fix these tests.
# add_launch_test(
# test/test_request.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()
6 changes: 6 additions & 0 deletions nexus_motion_planner/package.xml
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -28,8 +28,14 @@
<test_depend>abb_irb1300_10_115_moveit_config</test_depend>
<test_depend>abb_irb1300_support</test_depend>

<test_depend>ament_cmake_pytest</test_depend>
<test_depend>ament_cmake_uncrustify</test_depend>
<test_depend>launch_pytest</test_depend>
<test_depend>launch_testing_ament_cmake</test_depend>

<test_depend>moveit_configs_utils</test_depend>
<test_depend>moveit_resources</test_depend>
<test_depend>python3-pytest</test_depend>
<test_depend>rmf_utils</test_depend>

<export>
Expand Down
46 changes: 31 additions & 15 deletions nexus_motion_planner/src/motion_plan_cache.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -103,6 +103,22 @@ bool MotionPlanCache::init(
return db_->connect();
}

unsigned
MotionPlanCache::count_plans(const std::string& move_group_namespace)
{
auto coll = db_->openCollection<moveit_msgs::msg::RobotTrajectory>(
"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<moveit_msgs::msg::RobotTrajectory>(
"move_group_cartesian_plan_cache", move_group_namespace);
return coll.count();
}

// =============================================================================
// MOTION PLAN CACHING
// =============================================================================
Expand Down Expand Up @@ -204,7 +220,7 @@ MotionPlanCache::put_plan(
auto coll = db_->openCollection<moveit_msgs::msg::RobotTrajectory>(
"move_group_plan_cache", move_group_namespace);

// Pull out "exact" matching plans in cache.
// 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(
Expand Down Expand Up @@ -1059,7 +1075,7 @@ MotionPlanCache::fetch_best_matching_cartesian_plan(
}

auto coll = db_->openCollection<moveit_msgs::msg::RobotTrajectory>(
"move_group_plan_cache", move_group_namespace);
"move_group_cartesian_plan_cache", move_group_namespace);

// Best plan is at first index, since the lookup query was sorted by
// execution_time.
Expand Down Expand Up @@ -1095,15 +1111,14 @@ MotionPlanCache::put_cartesian_plan(
{
RCLCPP_ERROR(
node_->get_logger(),
"Skipping cartesian plan insert: "
"Frame IDs cannot be empty.");
"Skipping cartesian plan insert: Frame IDs cannot be empty.");
return false;
}

auto coll = db_->openCollection<moveit_msgs::msg::RobotTrajectory>(
"move_group_cartesian_plan_cache", move_group_namespace);

// Pull out "exact" matching plans in cache.
// 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(
Expand Down Expand Up @@ -1287,10 +1302,10 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query(
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())
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.");
Expand Down Expand Up @@ -1372,10 +1387,10 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query(
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);
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);
z_offset + waypoint.position.z, match_tolerance);

// Orientation
tf2::Quaternion tf2_quat_goal_offset(
Expand All @@ -1399,6 +1414,7 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_query(
}

query.append("link_name", plan_request.link_name);
query.append("header.frame_id", base_frame);

return true;
}
Expand Down Expand Up @@ -1494,10 +1510,10 @@ MotionPlanCache::extract_and_append_cartesian_plan_goal_to_metadata(
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())
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.");
Expand Down
7 changes: 4 additions & 3 deletions nexus_motion_planner/src/motion_plan_cache.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -37,9 +37,6 @@
#include <moveit/move_group_interface/move_group_interface.h>
#include <moveit_msgs/srv/get_cartesian_path.hpp>

// NEXUS messages
#include <nexus_endpoints.hpp>

namespace nexus {
namespace motion_planner {

Expand Down Expand Up @@ -93,6 +90,10 @@ class MotionPlanCache
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
// ===========================================================================
Expand Down
Loading