diff --git a/moveit_ros/trajectory_cache/CMakeLists.txt b/moveit_ros/trajectory_cache/CMakeLists.txt index 49e3a51a91..02707be298 100644 --- a/moveit_ros/trajectory_cache/CMakeLists.txt +++ b/moveit_ros/trajectory_cache/CMakeLists.txt @@ -32,9 +32,24 @@ set(TRAJECTORY_CACHE_DEPENDENCIES # ============================================================================== +set(TRAJECTORY_CACHE_LIBRARIES moveit_ros_trajectory_cache_utils_lib + moveit_ros_trajectory_cache_lib) + +# Utils library +add_library(moveit_ros_trajectory_cache_utils_lib src/utils/utils.cpp) +generate_export_header(moveit_ros_trajectory_cache_utils_lib) +target_include_directories( + moveit_ros_trajectory_cache_utils_lib + PUBLIC $ + $) +ament_target_dependencies(moveit_ros_trajectory_cache_utils_lib + ${TRAJECTORY_CACHE_DEPENDENCIES}) + # Trajectory cache library add_library(moveit_ros_trajectory_cache_lib src/trajectory_cache.cpp) generate_export_header(moveit_ros_trajectory_cache_lib) +target_link_libraries(moveit_ros_trajectory_cache_lib + moveit_ros_trajectory_cache_utils_lib) target_include_directories( moveit_ros_trajectory_cache_lib PUBLIC $ @@ -43,7 +58,7 @@ ament_target_dependencies(moveit_ros_trajectory_cache_lib ${TRAJECTORY_CACHE_DEPENDENCIES}) install( - TARGETS moveit_ros_trajectory_cache_lib + TARGETS ${TRAJECTORY_CACHE_LIBRARIES} EXPORT moveit_ros_trajectory_cacheTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -52,29 +67,14 @@ install( DESTINATION include/moveit_ros_trajectory_cache) install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache) -install( - FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h - DESTINATION include/moveit_ros_trajectory_cache) - -# ============================================================================== - -if(BUILD_TESTING) - find_package(ament_cmake_pytest REQUIRED) - find_package(launch_testing_ament_cmake REQUIRED) - find_package(rmf_utils REQUIRED) - # This test executable is run by the pytest_test, since a node is required for - # testing move groups - add_executable(test_trajectory_cache test/test_trajectory_cache.cpp) - target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib) +# Install export headers for each library +foreach(lib_target ${TRAJECTORY_CACHE_LIBRARIES}) + install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${lib_target}_export.h + DESTINATION include/moveit_ros_trajectory_cache) +endforeach() - install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME}) - - ament_add_pytest_test( - test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY - "${CMAKE_CURRENT_BINARY_DIR}") - -endif() +add_subdirectory(test) ament_export_targets(moveit_ros_trajectory_cacheTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES}) diff --git a/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp new file mode 100644 index 0000000000..27df279f65 --- /dev/null +++ b/moveit_ros/trajectory_cache/include/moveit/trajectory_cache/utils/utils.hpp @@ -0,0 +1,59 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief Utilities used by the trajectory_cache package. + * @author methylDragon + */ + +#pragma once + +#include +#include + +#include +#include + +#include + +// Frames. ========================================================================================= + +/** @brief Gets workspace frame ID. + * If workspace_parameters has no frame ID, fetch it from move_group. + */ +std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::WorkspaceParameters& workspace_parameters); + +/** @brief Gets cartesian path request frame ID. + * If path_request has no frame ID, fetch it from move_group. + */ +std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& path_request); + +// Features. ======================================================================================= + +/** @brief Appends a range inclusive query with some tolerance about some center value. */ +void queryAppendCenterWithTolerance(warehouse_ros::Query& query, const std::string& name, double center, + double tolerance); + +// Constraints. ==================================================================================== + +/** @brief Sorts a vector of joint constraints in-place by joint name. */ +void sortJointConstraints(std::vector& joint_constraints); + +/** @brief Sorts a vector of position constraints in-place by link name. */ +void sortPositionConstraints(std::vector& position_constraints); + +/** @brief Sorts a vector of orientation constraints in-place by link name. */ +void sortOrientationConstraints(std::vector& orientation_constraints); diff --git a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp index 21365cb9e7..2fcdb96821 100644 --- a/moveit_ros/trajectory_cache/src/trajectory_cache.cpp +++ b/moveit_ros/trajectory_cache/src/trajectory_cache.cpp @@ -28,6 +28,7 @@ #include #include #include +#include namespace moveit_ros { @@ -38,63 +39,6 @@ using warehouse_ros::MessageWithMetadata; using warehouse_ros::Metadata; using warehouse_ros::Query; -// Utils ======================================================================= - -// Ensure we always have a workspace frame ID. -std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::msg::WorkspaceParameters& workspace_parameters) -{ - if (workspace_parameters.header.frame_id.empty()) - { - return move_group.getRobotModel()->getModelFrame(); - } - else - { - return workspace_parameters.header.frame_id; - } -} - -// Ensure we always have a cartesian path request frame ID. -std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, - const moveit_msgs::srv::GetCartesianPath::Request& path_request) -{ - if (path_request.header.frame_id.empty()) - { - return move_group.getPoseReferenceFrame(); - } - else - { - return path_request.header.frame_id; - } -} - -// 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); -} - -// Sort constraint components by joint or link name. -void sortConstraints(std::vector& joint_constraints, - std::vector& position_constraints, - std::vector& orientation_constraints) -{ - std::sort(joint_constraints.begin(), joint_constraints.end(), - [](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) { - return l.joint_name < r.joint_name; - }); - - std::sort(position_constraints.begin(), position_constraints.end(), - [](const moveit_msgs::msg::PositionConstraint& l, const moveit_msgs::msg::PositionConstraint& r) { - return l.link_name < r.link_name; - }); - - std::sort(orientation_constraints.begin(), orientation_constraints.end(), - [](const moveit_msgs::msg::OrientationConstraint& l, const moveit_msgs::msg::OrientationConstraint& r) { - return l.link_name < r.link_name; - }); -} - // Trajectory Cache ============================================================ TrajectoryCache::TrajectoryCache(const rclcpp::Node::SharedPtr& node) @@ -595,7 +539,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + queryAppendCenterWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), current_state_msg.joint_state.position.at(i), match_tolerance); } } @@ -604,7 +548,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryStartToQuery( for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + queryAppendCenterWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), plan_request.start_state.joint_state.position.at(i), match_tolerance); } } @@ -641,11 +585,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( "Not supported."); } - queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, + queryAppendCenterWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor", + queryAppendCenterWithTolerance(query, "max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed, + queryAppendCenterWithTolerance(query, "max_cartesian_speed", plan_request.max_cartesian_speed, match_tolerance); // Extract constraints (so we don't have cardinality on goal_constraint idx.) @@ -668,7 +612,9 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( } // Also sort for even less cardinality. - sortConstraints(joint_constraints, position_constraints, orientation_constraints); + sortJointConstraints(joint_constraints); + sortPositionConstraints(position_constraints); + sortOrientationConstraints(orientation_constraints); } // Joint constraints @@ -678,7 +624,7 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( std::string meta_name = "goal_constraints.joint_constraints_" + std::to_string(joint_idx++); query.append(meta_name + ".joint_name", constraint.joint_name); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position", constraint.position, match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".position", constraint.position, match_tolerance); query.appendGTE(meta_name + ".tolerance_above", constraint.tolerance_above); query.appendLTE(meta_name + ".tolerance_below", constraint.tolerance_below); } @@ -727,11 +673,11 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( query.append(meta_name + ".link_name", constraint.link_name); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", + queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.x", x_offset + constraint.target_point_offset.x, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", + queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.y", y_offset + constraint.target_point_offset.y, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", + queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.z", z_offset + constraint.target_point_offset.z, match_tolerance); } } @@ -794,13 +740,13 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToQuery( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), + queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.x", final_quat.getX(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), + queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.y", final_quat.getY(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), + queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.z", final_quat.getZ(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), + queryAppendCenterWithTolerance(query, meta_name + ".target_point_offset.w", final_quat.getW(), match_tolerance); } } @@ -942,7 +888,9 @@ bool TrajectoryCache::extractAndAppendTrajectoryGoalToMetadata( } // Also sort for even less cardinality. - sortConstraints(joint_constraints, position_constraints, orientation_constraints); + sortJointConstraints(joint_constraints); + sortPositionConstraints(position_constraints); + sortOrientationConstraints(orientation_constraints); } // Joint constraints @@ -1135,7 +1083,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( for (size_t i = 0; i < current_state_msg.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), current_state_msg.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + queryAppendCenterWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), current_state_msg.joint_state.position.at(i), match_tolerance); } } @@ -1144,7 +1092,7 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryStartToQuery( for (size_t i = 0; i < plan_request.start_state.joint_state.name.size(); i++) { query.append("start_state.joint_state.name_" + std::to_string(i), plan_request.start_state.joint_state.name.at(i)); - queryAppendRangeInclusiveWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), + queryAppendCenterWithTolerance(query, "start_state.joint_state.position_" + std::to_string(i), plan_request.start_state.joint_state.position.at(i), match_tolerance); } } @@ -1172,12 +1120,12 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( RCLCPP_WARN(logger_, "Ignoring avoid_collisions: Not supported."); } - queryAppendRangeInclusiveWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, + queryAppendCenterWithTolerance(query, "max_velocity_scaling_factor", plan_request.max_velocity_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "max_acceleration_scaling_factor", + queryAppendCenterWithTolerance(query, "max_acceleration_scaling_factor", plan_request.max_acceleration_scaling_factor, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "max_step", plan_request.max_step, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance); + queryAppendCenterWithTolerance(query, "max_step", plan_request.max_step, match_tolerance); + queryAppendCenterWithTolerance(query, "jump_threshold", plan_request.jump_threshold, match_tolerance); // Waypoints // Restating them in terms of the robot model frame (usually base_link) @@ -1229,11 +1177,11 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( // Apply offsets // Position - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, + queryAppendCenterWithTolerance(query, meta_name + ".position.x", x_offset + waypoint.position.x, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, + queryAppendCenterWithTolerance(query, meta_name + ".position.y", y_offset + waypoint.position.y, match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, + queryAppendCenterWithTolerance(query, meta_name + ".position.z", z_offset + waypoint.position.z, match_tolerance); // Orientation @@ -1244,10 +1192,10 @@ bool TrajectoryCache::extractAndAppendCartesianTrajectoryGoalToQuery( auto final_quat = tf2_quat_goal_offset * tf2_quat_frame_offset; final_quat.normalize(); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); - queryAppendRangeInclusiveWithTolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.x", final_quat.getX(), match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.y", final_quat.getY(), match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.z", final_quat.getZ(), match_tolerance); + queryAppendCenterWithTolerance(query, meta_name + ".orientation.w", final_quat.getW(), match_tolerance); } query.append("link_name", plan_request.link_name); diff --git a/moveit_ros/trajectory_cache/src/utils/utils.cpp b/moveit_ros/trajectory_cache/src/utils/utils.cpp new file mode 100644 index 0000000000..2e348fee69 --- /dev/null +++ b/moveit_ros/trajectory_cache/src/utils/utils.cpp @@ -0,0 +1,89 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @brief Implementation of the utilities used by the trajectory_cache package. + * @author methylDragon + */ + +#include +#include + +#include +#include +#include + +#include + +// Frames. ========================================================================================= + +std::string getWorkspaceFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::msg::WorkspaceParameters& workspace_parameters) +{ + if (workspace_parameters.header.frame_id.empty()) + { + return move_group.getRobotModel()->getModelFrame(); + } + else + { + return workspace_parameters.header.frame_id; + } +} + +std::string getCartesianPathRequestFrameId(const moveit::planning_interface::MoveGroupInterface& move_group, + const moveit_msgs::srv::GetCartesianPath::Request& path_request) +{ + if (path_request.header.frame_id.empty()) + { + return move_group.getPoseReferenceFrame(); + } + else + { + return path_request.header.frame_id; + } +} + +// Features. ======================================================================================= + +void queryAppendCenterWithTolerance(warehouse_ros::Query& query, const std::string& name, double center, + double tolerance) +{ + query.appendRangeInclusive(name, center - tolerance / 2, center + tolerance / 2); +} + +// Constraints. ==================================================================================== + +void sortJointConstraints(std::vector& joint_constraints) +{ + std::sort(joint_constraints.begin(), joint_constraints.end(), + [](const moveit_msgs::msg::JointConstraint& l, const moveit_msgs::msg::JointConstraint& r) { + return l.joint_name < r.joint_name; + }); +} + +void sortPositionConstraints(std::vector& position_constraints) +{ + std::sort(position_constraints.begin(), position_constraints.end(), + [](const moveit_msgs::msg::PositionConstraint& l, const moveit_msgs::msg::PositionConstraint& r) { + return l.link_name < r.link_name; + }); +} + +void sortOrientationConstraints(std::vector& orientation_constraints) +{ + std::sort(orientation_constraints.begin(), orientation_constraints.end(), + [](const moveit_msgs::msg::OrientationConstraint& l, const moveit_msgs::msg::OrientationConstraint& r) { + return l.link_name < r.link_name; + }); +} diff --git a/moveit_ros/trajectory_cache/test/CMakeLists.txt b/moveit_ros/trajectory_cache/test/CMakeLists.txt new file mode 100644 index 0000000000..fec11c8ba9 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/CMakeLists.txt @@ -0,0 +1,58 @@ +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake_pytest REQUIRED) + find_package(launch_testing_ament_cmake REQUIRED) + find_package(ros_testing REQUIRED) + + # Fixtures. + add_library(warehouse_fixture warehouse_fixture.cpp) + target_include_directories( + warehouse_fixture + PUBLIC $) + ament_target_dependencies(warehouse_fixture + ${TRAJECTORY_CACHE_DEPENDENCIES} + ament_cmake_gtest) + + add_library(move_group_fixture move_group_fixture.cpp) + target_include_directories( + move_group_fixture + PUBLIC $) + ament_target_dependencies(move_group_fixture + ${TRAJECTORY_CACHE_DEPENDENCIES} + ament_cmake_gtest) + + # Test utils library. + ament_add_gtest(test_utils + utils/test_utils.cpp) + target_link_libraries(test_utils + moveit_ros_trajectory_cache_utils_lib + warehouse_fixture) + + ament_add_gtest_executable(test_utils_with_move_group + utils/test_utils_with_move_group.cpp) + target_link_libraries(test_utils_with_move_group + moveit_ros_trajectory_cache_utils_lib + move_group_fixture) + add_ros_test(gtest_with_move_group.py + TARGET test_utils_with_move_group TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}" + "test_executable:=test_utils_with_move_group") + + # install(TARGETS test_utils_with_move_group RUNTIME DESTINATION lib/${PROJECT_NAME}) + + # ament_add_pytest_test( + # test_utils_with_move_group_py "utils/test_utils.py" WORKING_DIRECTORY + # "${CMAKE_CURRENT_BINARY_DIR}") + + # This test executable is run by the pytest_test, since a node is required for + # testing move groups + add_executable(test_trajectory_cache test_trajectory_cache.cpp) + target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib) + + install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME}) + + # ament_add_pytest_test( + # test_trajectory_cache_py "test_trajectory_cache.py" WORKING_DIRECTORY + # "${CMAKE_CURRENT_BINARY_DIR}") + +endif() diff --git a/moveit_ros/trajectory_cache/test/gtest_with_move_group.py b/moveit_ros/trajectory_cache/test/gtest_with_move_group.py new file mode 100644 index 0000000000..27aa30ff68 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/gtest_with_move_group.py @@ -0,0 +1,122 @@ +import os +import launch +import unittest +import launch_ros +import launch_testing +from ament_index_python.packages import get_package_share_directory +from moveit_configs_utils import MoveItConfigsBuilder +from launch_param_builder import ParameterBuilder + +from ament_index_python.packages import get_package_share_directory +from launch import LaunchDescription +from launch.actions import ExecuteProcess +from launch_pytest.tools import process as process_tools +from launch_ros.actions import Node + +from moveit_configs_utils import MoveItConfigsBuilder + + +def generate_test_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_pipelines("ompl", ["ompl"]) + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .to_moveit_configs() + ) + + run_move_group_node = Node( + package="moveit_ros_move_group", + executable="move_group", + output="log", + parameters=[moveit_config.to_dict()], + ) + + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", + "0.0", "0.0", "world", "panda_link0"], + ) + + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="log", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_resources_panda_moveit_config"), + "config", + "ros2_controllers.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="log", + ) + + load_controllers = [] + for controller in [ + "panda_arm_controller", + "panda_hand_controller", + "joint_state_broadcaster", + ]: + load_controllers += [ + ExecuteProcess( + cmd=["ros2 run controller_manager spawner {}".format( + controller)], + shell=True, + output="log", + ) + ] + + gtest_node = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + launch.substitutions.LaunchConfiguration("test_executable"), + ] + ), + parameters=[ + moveit_config.to_dict() + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + launch.actions.DeclareLaunchArgument( + name="test_binary_dir", + description="Binary directory of package " + "containing test executables", + ), + run_move_group_node, + static_tf, + robot_state_publisher, + ros2_control_node, + *load_controllers, + launch.actions.TimerAction(period=0.1, actions=[gtest_node]), + launch_testing.actions.ReadyToTest(), + ] + ), { + "gtest_node": gtest_node, + } + + +class TestGTestWaitForCompletion(unittest.TestCase): + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, gtest_node): + self.proc_info.assertWaitForShutdown(gtest_node, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, gtest_node): + launch_testing.asserts.assertExitCodes(proc_info, process=gtest_node) diff --git a/moveit_ros/trajectory_cache/test/move_group_fixture.cpp b/moveit_ros/trajectory_cache/test/move_group_fixture.cpp new file mode 100644 index 0000000000..1df43e3d13 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/move_group_fixture.cpp @@ -0,0 +1,67 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + */ + +#include + +#include +#include +#include +#include + +#include "move_group_fixture.hpp" + +MoveGroupFixture::MoveGroupFixture() + : node_(rclcpp::Node::make_shared("move_group_fixture")), move_group_name_("panda_arm") +{ + node_->declare_parameter("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); + move_group_ = std::make_shared(node_, move_group_name_); +} + +MoveGroupFixture::~MoveGroupFixture() +{ + is_spinning_ = false; + if (spin_thread_.joinable()) + { + spin_thread_.join(); + } +} + +void MoveGroupFixture::SetUp() +{ + is_spinning_ = true; + spin_thread_ = std::thread(&MoveGroupFixture::spinNode, this); + + db_ = moveit_warehouse::loadDatabase(node_); + db_->setParams(":memory:", 1); + ASSERT_TRUE(db_->connect()); +} + +void MoveGroupFixture::TearDown() +{ + db_.reset(); + is_spinning_ = false; + spin_thread_.join(); +} + +void MoveGroupFixture::spinNode() +{ + while (is_spinning_ && rclcpp::ok()) + { + rclcpp::spin_some(node_); + } +} diff --git a/moveit_ros/trajectory_cache/test/move_group_fixture.hpp b/moveit_ros/trajectory_cache/test/move_group_fixture.hpp new file mode 100644 index 0000000000..60b5faba48 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/move_group_fixture.hpp @@ -0,0 +1,50 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + */ + +#pragma once + +#include + +#include +#include +#include +#include + +/** @class MoveGroupFixture + * @brief Test fixture to spin up a node to start a move group with. */ +class MoveGroupFixture : public ::testing::Test +{ +public: + MoveGroupFixture(); + ~MoveGroupFixture(); + +protected: + void SetUp() override; + void TearDown() override; + + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + std::shared_ptr move_group_; + std::string move_group_name_; + +private: + void spinNode(); + + std::thread spin_thread_; + std::atomic is_spinning_; +}; diff --git a/moveit_ros/trajectory_cache/test/utils/test_utils.cpp b/moveit_ros/trajectory_cache/test/utils/test_utils.cpp new file mode 100644 index 0000000000..f024acb587 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/utils/test_utils.cpp @@ -0,0 +1,112 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + */ + +#include + +#include +#include +#include +#include +#include + +#include "../warehouse_fixture.hpp" + +namespace +{ + +using ::warehouse_ros::MessageCollection; +using ::warehouse_ros::Metadata; +using ::warehouse_ros::Query; + +TEST_F(WarehouseFixture, QueryAppendCenterWithToleranceWorks) +{ + MessageCollection coll = + db_->openCollection("test_db", "test_collection"); + + Metadata::Ptr metadata = coll.createMetadata(); + metadata->append("test_metadata", 5.0); + metadata->append("unrelated_metadata", 1.0); + coll.insert(geometry_msgs::msg::Point(), metadata); + + Query::Ptr unrelated_query = coll.createQuery(); + queryAppendCenterWithTolerance(*unrelated_query, "unrnelated_metadata", 1.0, 10.0); + EXPECT_TRUE(coll.queryList(unrelated_query).empty()); + + Query::Ptr related_query_too_low = coll.createQuery(); + queryAppendCenterWithTolerance(*related_query_too_low, "test_metadata", 4.45, 1.0); + EXPECT_TRUE(coll.queryList(related_query_too_low).empty()); + + Query::Ptr related_query_too_high = coll.createQuery(); + queryAppendCenterWithTolerance(*related_query_too_high, "test_metadata", 5.55, 1.0); + EXPECT_TRUE(coll.queryList(related_query_too_high).empty()); + + Query::Ptr related_query_in_range = coll.createQuery(); + queryAppendCenterWithTolerance(*related_query_in_range, "test_metadata", 5.0, 1.0); + EXPECT_EQ(coll.queryList(related_query_in_range).size(), 1); +} + +TEST(TestUtils, ConstraintSortingWorks) +{ + // Joint constraints. + { + moveit_msgs::msg::JointConstraint jc1; + jc1.joint_name = "joint2"; + moveit_msgs::msg::JointConstraint jc2; + jc2.joint_name = "joint1"; + std::vector joint_constraints = { jc1, jc2 }; + sortJointConstraints(joint_constraints); + + EXPECT_EQ(joint_constraints[0].joint_name, "joint1"); + EXPECT_EQ(joint_constraints[1].joint_name, "joint2"); + } + + // Position constraints. + { + moveit_msgs::msg::PositionConstraint pc1; + pc1.link_name = "link2"; + moveit_msgs::msg::PositionConstraint pc2; + pc2.link_name = "link1"; + std::vector position_constraints = { pc1, pc2 }; + sortPositionConstraints(position_constraints); + EXPECT_EQ(position_constraints[0].link_name, "link1"); + EXPECT_EQ(position_constraints[1].link_name, "link2"); + } + + // Orientation constraints. + { + moveit_msgs::msg::OrientationConstraint oc1; + oc1.link_name = "link2"; + moveit_msgs::msg::OrientationConstraint oc2; + oc2.link_name = "link1"; + std::vector orientation_constraints = { oc1, oc2 }; + sortOrientationConstraints(orientation_constraints); + EXPECT_EQ(orientation_constraints[0].link_name, "link1"); + EXPECT_EQ(orientation_constraints[1].link_name, "link2"); + } +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} \ No newline at end of file diff --git a/moveit_ros/trajectory_cache/test/utils/test_utils_with_move_group.cpp b/moveit_ros/trajectory_cache/test/utils/test_utils_with_move_group.cpp new file mode 100644 index 0000000000..426b7baca1 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/utils/test_utils_with_move_group.cpp @@ -0,0 +1,56 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + */ + +#include + +#include +#include + +#include "../move_group_fixture.hpp" + +namespace +{ + +TEST_F(MoveGroupFixture, GetWorkspaceFrameIdWorks) +{ + moveit_msgs::msg::WorkspaceParameters workspace_parameters; + EXPECT_EQ(getWorkspaceFrameId(*move_group_, workspace_parameters), move_group_->getRobotModel()->getModelFrame()); + + workspace_parameters.header.frame_id = "test_frame"; + EXPECT_EQ(getWorkspaceFrameId(*move_group_, workspace_parameters), "test_frame"); +} + +TEST_F(MoveGroupFixture, GetCartesianPathRequestFrameId) +{ + moveit_msgs::srv::GetCartesianPath::Request path_request; + EXPECT_EQ(getCartesianPathRequestFrameId(*move_group_, path_request), move_group_->getPoseReferenceFrame()); + + path_request.header.frame_id = "test_frame"; + EXPECT_EQ(getCartesianPathRequestFrameId(*move_group_, path_request), "test_frame"); +} + +} // namespace + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} \ No newline at end of file diff --git a/moveit_ros/trajectory_cache/test/warehouse_fixture.cpp b/moveit_ros/trajectory_cache/test/warehouse_fixture.cpp new file mode 100644 index 0000000000..745f0bd769 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/warehouse_fixture.cpp @@ -0,0 +1,63 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + */ + +#include + +#include +#include +#include + +#include "warehouse_fixture.hpp" + +WarehouseFixture::WarehouseFixture() : node_(rclcpp::Node::make_shared("warehouse_fixture")) +{ + node_->declare_parameter("warehouse_plugin", "warehouse_ros_sqlite::DatabaseConnection"); +} + +WarehouseFixture::~WarehouseFixture() +{ + is_spinning_ = false; + if (spin_thread_.joinable()) { + spin_thread_.join(); + } +} + +void WarehouseFixture::SetUp() +{ + is_spinning_ = true; + spin_thread_ = std::thread(&WarehouseFixture::spinNode, this); + + db_ = moveit_warehouse::loadDatabase(node_); + db_->setParams(":memory:", 1); + ASSERT_TRUE(db_->connect()); +} + +void WarehouseFixture::TearDown() +{ + db_.reset(); + is_spinning_ = false; + spin_thread_.join(); +} + +void WarehouseFixture::spinNode() +{ + while (is_spinning_ && rclcpp::ok()) + { + rclcpp::spin_some(node_); + } +} diff --git a/moveit_ros/trajectory_cache/test/warehouse_fixture.hpp b/moveit_ros/trajectory_cache/test/warehouse_fixture.hpp new file mode 100644 index 0000000000..8bde8f78e4 --- /dev/null +++ b/moveit_ros/trajectory_cache/test/warehouse_fixture.hpp @@ -0,0 +1,46 @@ +// Copyright 2024 Intrinsic Innovation LLC. +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** @file + * @author methylDragon + */ + +#pragma once + +#include +#include +#include + + +/** @class WarehouseFixture + * @brief Test fixture to spin up a node to start a warehouse_ros connection with. */ +class WarehouseFixture : public ::testing::Test +{ +public: + WarehouseFixture(); + ~WarehouseFixture(); + +protected: + void SetUp() override; + void TearDown() override; + + rclcpp::Node::SharedPtr node_; + warehouse_ros::DatabaseConnection::Ptr db_; + +private: + void spinNode(); + + std::thread spin_thread_; + std::atomic is_spinning_; +}; \ No newline at end of file