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

Implement cartesian plan cache #18

Merged
merged 9 commits into from
Dec 4, 2023
Merged

Implement cartesian plan cache #18

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
9 commits
Select commit Hold shift + click to select a range
e4ec917
Add cartesian plan caching interfaces
methylDragon Oct 17, 2023
caada7e
Add construct_get_cartesian_plan_request
methylDragon Oct 17, 2023
019dbfb
Add goal query and metadata
methylDragon Oct 17, 2023
3b70b6e
Add start query and metadata
methylDragon Oct 18, 2023
87fda73
Implement top level cache ops
methylDragon Oct 18, 2023
7d70524
Use motion plan cache for cartesian plans
methylDragon Oct 18, 2023
188215b
Allow mismatched plan frames since we coerce anyway
methylDragon Oct 18, 2023
b2cda90
Fix move bug
methylDragon Oct 18, 2023
66864b9
Plan cache code review fixes (sans unit tests) (#26)
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
11 changes: 11 additions & 0 deletions nexus_capabilities/src/capabilities/plan_motion.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -120,6 +120,17 @@ make_request()
req->max_velocity_scaling_factor = scale_speed;
req->max_acceleration_scaling_factor = scale_speed;

auto maybe_read_only =
this->getInput<bool>("force_cache_mode_execute_read_only");
if (maybe_read_only.has_value())
{
req->force_cache_mode_execute_read_only = maybe_read_only.value();
}
else
{
req->force_cache_mode_execute_read_only = false;
}

RCLCPP_DEBUG_STREAM(this->_logger,
"planning to " << req->goal_pose.pose << " at frame " <<
req->goal_pose.header.frame_id << " with cartesian " << req->cartesian << "and scaled speed of " <<
Expand Down
3 changes: 3 additions & 0 deletions nexus_capabilities/src/capabilities/plan_motion.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,9 @@ public: static BT::PortsList providedPorts()
BT::InputPort<std::vector<moveit_msgs::msg::JointConstraint>>(
"start_constraints",
"OPTIONAL. If provided the the joint_constraints are used as the start condition. Else, the current state of the robot will be used as the start."),
BT::InputPort<bool>(
"force_cache_mode_execute_read_only",
"OPTIONAL. Set true to force cache mode to ExecuteReadOnly for this request."),
BT::OutputPort<moveit_msgs::msg::RobotTrajectory>(
"result", "The resulting trajectory.")};
}
Expand Down
33 changes: 28 additions & 5 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 @@ -43,12 +48,17 @@ set (test_request_dependencies
)

#===============================================================================
set(LIBRARY_NAME motion_planner_server_core)
set(MOTION_PLAN_CACHE_LIBRARY_NAME motion_planner_server_core)
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_plan_cache_dependencies})

# Server executable
add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp src/motion_plan_cache.cpp)
add_executable(${EXECUTABLE_NAME} src/main.cpp src/motion_planner_server.cpp)
ament_target_dependencies(${EXECUTABLE_NAME} ${motion_planner_server_dependencies})
target_link_libraries(${EXECUTABLE_NAME} ${MOTION_PLAN_CACHE_LIBRARY_NAME})

# Test executable
add_executable(test_request src/test_request.cpp)
Expand All @@ -67,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 @@ -75,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
Loading
Loading