From 5c405662f627f6d0a51de7c37edfc7c2a89a6bbd Mon Sep 17 00:00:00 2001 From: AndyZe Date: Thu, 27 Jan 2022 11:58:14 -0600 Subject: [PATCH] [hybrid planning] Add action abortion and test; improve the existing test (#980) * Add action abortion and test; improve the existing test * Add controller run-dependency * Fix the clearing of robot trajectory when a collision would occur * Fix replanning if local planner is stuck * Lambda function everything * Thread safety for stop_hybrid_planning_ * Thread-safe state_ * Clang tidy * Update the planning scene properly * Update Servo test initial_positions.yaml Co-authored-by: Tyler Weaver --- moveit_ros/hybrid_planning/CMakeLists.txt | 29 +- .../hybrid_planning_manager.h | 10 +- .../src/hybrid_planning_manager.cpp | 80 ++++-- .../src/replan_invalidated_trajectory.cpp | 10 +- .../forward_trajectory.h | 4 +- .../src/forward_trajectory.cpp | 4 +- .../local_planner/local_planner_component.h | 4 +- .../src/local_planner_component.cpp | 4 +- moveit_ros/hybrid_planning/package.xml | 7 +- .../hybrid_planning/test/CMakeLists.txt | 25 ++ .../test/hybrid_planning_demo_node.cpp | 9 +- .../test/launch/hybrid_planning_common.py | 27 +- .../launch/hybrid_planning_demo.launch.py | 34 ++- .../launch/test_basic_integration.test.py | 69 +++++ .../test/launch/test_basic_launching.test.py | 73 ----- .../test/test_basic_integration.cpp | 268 ++++++++++++++++++ .../config/collision_start_positions.yaml | 8 + .../config/singularity_start_positions.yaml | 8 + 18 files changed, 502 insertions(+), 171 deletions(-) create mode 100644 moveit_ros/hybrid_planning/test/launch/test_basic_integration.test.py delete mode 100644 moveit_ros/hybrid_planning/test/launch/test_basic_launching.test.py create mode 100644 moveit_ros/hybrid_planning/test/test_basic_integration.cpp diff --git a/moveit_ros/hybrid_planning/CMakeLists.txt b/moveit_ros/hybrid_planning/CMakeLists.txt index 26ee4035d8..82c4294df4 100644 --- a/moveit_ros/hybrid_planning/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(moveit_msgs REQUIRED) find_package(moveit_ros_planning REQUIRED) find_package(moveit_ros_planning_interface REQUIRED) find_package(pluginlib REQUIRED) +find_package(position_controllers REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) find_package(rclcpp_components REQUIRED) @@ -34,13 +35,14 @@ set(LIBRARIES ) set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - rclcpp_components - rclcpp_action moveit_core moveit_ros_planning moveit_ros_planning_interface moveit_msgs + position_controllers + rclcpp + rclcpp_components + rclcpp_action pluginlib std_msgs std_srvs @@ -97,25 +99,4 @@ ament_export_libraries(${LIBRARIES}) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - find_package(ament_cmake_gtest REQUIRED) - find_package(ros_testing REQUIRED) - find_package(Boost REQUIRED COMPONENTS filesystem) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cppcheck_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() - - # Basic launch test - add_ros_test(test/launch/test_basic_launching.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - -endif() - ament_package() diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h index 161ecc2866..7453785e12 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_manager.h @@ -75,13 +75,6 @@ class HybridPlanningManager : public rclcpp::Node */ bool initialize(); - /** - * Hybrid planning goal callback for hybrid planning request server - * @param goal_handle Hybrid planning goal handle to access feedback and response - */ - void hybridPlanningRequestCallback( - std::shared_ptr> goal_handle); - /** * Send global planning request to global planner component * @return Global planner successfully started yes/no @@ -113,6 +106,9 @@ class HybridPlanningManager : public rclcpp::Node // Flag that indicates whether the manager is initialized bool initialized_; + // Flag that indicates hybrid planning has been canceled + std::atomic stop_hybrid_planning_; + // Shared hybrid planning goal handle std::shared_ptr> hybrid_planning_goal_handle_; diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp index 6b7c7fec3e..87f142551c 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/src/hybrid_planning_manager.cpp @@ -45,7 +45,7 @@ namespace moveit::hybrid_planning using namespace std::chrono_literals; HybridPlanningManager::HybridPlanningManager(const rclcpp::NodeOptions& options) - : Node("hybrid_planning_manager", options), initialized_(false) + : Node("hybrid_planning_manager", options), initialized_(false), stop_hybrid_planning_(false) { // Initialize hybrid planning component after after construction // TODO(sjahr) Remove once life cycle component nodes are available @@ -147,16 +147,46 @@ bool HybridPlanningManager::initialize() hybrid_planning_request_server_ = rclcpp_action::create_server( this->get_node_base_interface(), this->get_node_clock_interface(), this->get_node_logging_interface(), this->get_node_waitables_interface(), hybrid_planning_action_name, + // Goal callback [](const rclcpp_action::GoalUUID& /*unused*/, std::shared_ptr /*unused*/) { RCLCPP_INFO(LOGGER, "Received goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, - [](const std::shared_ptr>& /*unused*/) { + // Cancel callback + [this](std::shared_ptr> /*unused*/) { + // Prevent any new global or local requests from going out + stop_hybrid_planning_ = true; + + // Cancel local action + local_planner_action_client_->async_cancel_all_goals(); + + // Cancel global action + global_planner_action_client_->async_cancel_all_goals(); + RCLCPP_INFO(LOGGER, "Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; }, - std::bind(&HybridPlanningManager::hybridPlanningRequestCallback, this, std::placeholders::_1)); + // Request callback + [this](std::shared_ptr> goal_handle) { + // Reset the "stop" flag if it was set previously + stop_hybrid_planning_ = false; + + // Pass goal handle to class member + hybrid_planning_goal_handle_ = std::move(goal_handle); + + // react is defined in a hybrid_planning_manager plugin + ReactionResult reaction_result = + planner_logic_instance_->react(HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED); + if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto result = std::make_shared(); + result->error_code.val = reaction_result.error_code.val; + result->error_message = reaction_result.error_message; + hybrid_planning_goal_handle_->abort(result); + RCLCPP_ERROR_STREAM(LOGGER, "Hybrid Planning Manager failed to react to " << reaction_result.event); + } + }); // Initialize global solution subscriber global_solution_sub_ = create_subscription( @@ -233,6 +263,12 @@ bool HybridPlanningManager::sendGlobalPlannerAction() global_goal_msg.motion_sequence = (hybrid_planning_goal_handle_->get_goal())->motion_sequence; // latest desired motion sequence; global_goal_msg.planning_group = (hybrid_planning_goal_handle_->get_goal())->planning_group; // planning_group_; + + if (stop_hybrid_planning_) + { + return false; + } + // Send global planning goal and wait until it's accepted auto goal_handle_future = global_planner_action_client_->async_send_goal(global_goal_msg, global_goal_options); return true; // return always success TODO(sjahr) add more error checking @@ -301,39 +337,27 @@ bool HybridPlanningManager::sendLocalPlannerAction() // Abort hybrid planning if reaction fails if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { - auto result = std::make_shared(); - result->error_code.val = reaction_result.error_code.val; - result->error_message = reaction_result.error_message; - - hybrid_planning_goal_handle_->abort(result); - RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); + const moveit_msgs::action::HybridPlanner::Result result([reaction_result]() { + moveit_msgs::action::HybridPlanner::Result result; + result.error_code.val = reaction_result.error_code.val; + result.error_message = reaction_result.error_message; + return result; + }()); + hybrid_planning_goal_handle_->abort(std::make_shared(result)); + RCLCPP_ERROR_STREAM(LOGGER, "Hybrid Planning Manager failed to react to " << reaction_result.event); } }; + if (stop_hybrid_planning_) + { + return false; + } + // Send global planning goal auto goal_handle_future = local_planner_action_client_->async_send_goal(local_goal_msg, local_goal_options); return true; // return always success TODO(sjahr) add more error checking } -void HybridPlanningManager::hybridPlanningRequestCallback( - std::shared_ptr> goal_handle) -{ - // Pass goal handle to class member - hybrid_planning_goal_handle_ = std::move(goal_handle); - - // react is defined in a hybrid_planning_manager plugin - ReactionResult reaction_result = - planner_logic_instance_->react(HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED); - if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - auto result = std::make_shared(); - result->error_code.val = reaction_result.error_code.val; - result->error_message = reaction_result.error_message; - hybrid_planning_goal_handle_->abort(result); - RCLCPP_ERROR(LOGGER, "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); - } -} - void HybridPlanningManager::sendHybridPlanningResponse(bool success) { // Return hybrid planning action result dependent on the function's argument diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp index 1a3384695b..380d553028 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/replan_invalidated_trajectory.cpp @@ -43,15 +43,7 @@ namespace moveit::hybrid_planning { ReactionResult ReplanInvalidatedTrajectory::react(const std::string& event) { - if (event == "collision_ahead") - { - if (!hybrid_planning_manager_->sendGlobalPlannerAction()) // Start global planning - { - hybrid_planning_manager_->sendHybridPlanningResponse(false); - } - return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); - } - if (event == "local_planner_stuck") + if ((event == "collision_ahead") || (event == "local_planner_stuck")) { if (!hybrid_planning_manager_->sendGlobalPlannerAction()) // Start global planning { diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h index 8e27edb685..4685bb3f08 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/include/moveit/local_constraint_solver_plugins/forward_trajectory.h @@ -52,12 +52,12 @@ class ForwardTrajectory : public LocalConstraintSolverInterface ~ForwardTrajectory() = default; bool initialize(const rclcpp::Node::SharedPtr& node, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor, - const std::string& group_name) override; + const std::string& /* unused */) override; bool reset() override; moveit_msgs::action::LocalPlanner::Feedback solve(const robot_trajectory::RobotTrajectory& local_trajectory, - const std::shared_ptr local_goal, + const std::shared_ptr /* unused */, trajectory_msgs::msg::JointTrajectory& local_solution) override; private: diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp index 41a9d9dd42..24acd1bad7 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/src/forward_trajectory.cpp @@ -40,7 +40,7 @@ namespace { const rclcpp::Logger LOGGER = rclcpp::get_logger("local_planner_component"); // If stuck for this many iterations or more, abort the local planning action -constexpr size_t STUCK_ITERATIONS_THRESHOLD = 10; +constexpr size_t STUCK_ITERATIONS_THRESHOLD = 5; constexpr double STUCK_THRESHOLD_RAD = 1e-4; // L1-norm sum across all joints } // namespace @@ -132,7 +132,7 @@ ForwardTrajectory::solve(const robot_trajectory::RobotTrajectory& local_trajecto current_state_command.zeroAccelerations(); } robot_command.empty(); - robot_command.addSuffixWayPoint(*current_state, 0.0); + robot_command.addSuffixWayPoint(*current_state, local_trajectory.getWayPointDurationFromPrevious(0)); } // Detect if the local solver is stuck diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h index 47d08d850d..9b5960f952 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/include/moveit/local_planner/local_planner_component.h @@ -167,8 +167,8 @@ class LocalPlannerComponent // Planner configuration LocalPlannerConfig config_; - // Current planner state - LocalPlannerState state_; + // Current planner state. Must be thread-safe + std::atomic state_; // Timer to periodically call executeIteration() rclcpp::TimerBase::SharedPtr timer_; diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp index dde6cd7f16..5321c325b6 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/src/local_planner_component.cpp @@ -159,8 +159,9 @@ bool LocalPlannerComponent::initialize() RCLCPP_INFO(LOGGER, "Received local planning goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, - [](const std::shared_ptr>& /*unused*/) { + [this](const std::shared_ptr>& /*unused*/) { RCLCPP_INFO(LOGGER, "Received request to cancel local planning goal"); + state_ = LocalPlannerState::ABORT; return rclcpp_action::CancelResponse::ACCEPT; }, [this](std::shared_ptr> goal_handle) { @@ -280,6 +281,7 @@ void LocalPlannerComponent::executeIteration() if (!local_planner_feedback_->feedback.empty()) { local_planning_goal_handle_->publish_feedback(local_planner_feedback_); + return; } // Use a configurable message interface like MoveIt servo diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml index 74488a9fda..04a58327e8 100644 --- a/moveit_ros/hybrid_planning/package.xml +++ b/moveit_ros/hybrid_planning/package.xml @@ -16,20 +16,21 @@ moveit_common ament_index_cpp - rclcpp - rclcpp_action - rclcpp_components moveit_msgs moveit_core moveit_ros_planning moveit_ros_planning_interface pluginlib + rclcpp + rclcpp_action + rclcpp_components std_msgs std_srvs tf2_ros trajectory_msgs controller_manager + position_controllers robot_state_publisher rviz2 moveit_resources_panda_moveit_config diff --git a/moveit_ros/hybrid_planning/test/CMakeLists.txt b/moveit_ros/hybrid_planning/test/CMakeLists.txt index 7435522900..2e53a722de 100644 --- a/moveit_ros/hybrid_planning/test/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/test/CMakeLists.txt @@ -1,3 +1,28 @@ add_executable(hybrid_planning_demo_node hybrid_planning_demo_node.cpp) ament_target_dependencies(hybrid_planning_demo_node ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(hybrid_planning_demo_node ${LIBRARIES}) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + find_package(Boost REQUIRED COMPONENTS filesystem) + + # These don't pass yet, disable them for now + set(ament_cmake_copyright_FOUND TRUE) + set(ament_cmake_cppcheck_FOUND TRUE) + set(ament_cmake_cpplint_FOUND TRUE) + set(ament_cmake_flake8_FOUND TRUE) + set(ament_cmake_uncrustify_FOUND TRUE) + + # Run all lint tests in package.xml except those listed above + ament_lint_auto_find_test_dependencies() + + # Basic integration tests + ament_add_gtest_executable(test_basic_integration + test_basic_integration.cpp + ) + ament_target_dependencies(test_basic_integration ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(launch/test_basic_integration.test.py TIMEOUT 50 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + +endif() diff --git a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp index 3946cb3455..4c5eee4195 100644 --- a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp +++ b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp @@ -77,7 +77,6 @@ class HybridPlanningDemo } hp_action_client_ = rclcpp_action::create_client(node_, hybrid_planning_action_name); - robot_state_publisher_ = node_->create_publisher("display_robot_state", 1); collision_object_1_.header.frame_id = "panda_link0"; collision_object_1_.id = "box1"; @@ -193,6 +192,12 @@ class HybridPlanningDemo // Configure a valid robot state robot_state->setToDefaultValues(joint_model_group, "ready"); + robot_state->update(); + // Lock the planning scene as briefly as possible + { + planning_scene_monitor::LockedPlanningSceneRW locked_planning_scene(planning_scene_monitor_); + locked_planning_scene->setCurrentState(*robot_state); + } // Create desired motion goal moveit_msgs::msg::MotionPlanRequest goal_motion_request; @@ -260,7 +265,6 @@ class HybridPlanningDemo private: rclcpp::Node::SharedPtr node_; rclcpp_action::Client::SharedPtr hp_action_client_; - rclcpp::Publisher::SharedPtr robot_state_publisher_; rclcpp::Subscription::SharedPtr global_solution_subscriber_; planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; rclcpp::TimerBase::SharedPtr timer_; @@ -285,6 +289,7 @@ int main(int argc, char** argv) HybridPlanningDemo demo(node); std::thread run_demo([&demo]() { + // This sleep isn't necessary but it gives humans time to process what's going on rclcpp::sleep_for(5s); demo.run(); }); diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py index e9b71a7638..be58f6038c 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -33,8 +33,7 @@ def load_yaml(package_name, file_path): return None -def generate_common_hybrid_launch_description(): - # Component yaml files are grouped in separate namespaces +def get_robot_description(): robot_description_config = xacro.process_file( os.path.join( get_package_share_directory("moveit_resources_panda_moveit_config"), @@ -43,13 +42,23 @@ def generate_common_hybrid_launch_description(): ) ) robot_description = {"robot_description": robot_description_config.toxml()} + return robot_description + +def get_robot_description_semantic(): robot_description_semantic_config = load_file( "moveit_resources_panda_moveit_config", "config/panda.srdf" ) robot_description_semantic = { "robot_description_semantic": robot_description_semantic_config } + return robot_description_semantic + + +def generate_common_hybrid_launch_description(): + robot_description = get_robot_description() + + robot_description_semantic = get_robot_description_semantic() kinematics_yaml = load_yaml( "moveit_resources_panda_moveit_config", "config/kinematics.yaml" @@ -198,25 +207,11 @@ def generate_common_hybrid_launch_description(): ) ] - # Demo node - demo_node = Node( - package="moveit_hybrid_planning", - executable="hybrid_planning_demo_node", - name="hybrid_planning_demo_node", - output="screen", - parameters=[ - robot_description, - robot_description_semantic, - common_hybrid_planning_param, - ], - ) - launched_nodes = [ container, static_tf, rviz_node, robot_state_publisher, - demo_node, ros2_control_node, ] + load_controllers diff --git a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py index bbdc881601..5a1b5de233 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py @@ -1,11 +1,41 @@ import launch import os import sys +import xacro + +from ament_index_python.packages import get_package_share_directory +from launch_ros.actions import Node sys.path.append(os.path.dirname(__file__)) -from hybrid_planning_common import generate_common_hybrid_launch_description +from hybrid_planning_common import ( + generate_common_hybrid_launch_description, + get_robot_description, + get_robot_description_semantic, + load_file, + load_yaml, +) def generate_launch_description(): # generate_common_hybrid_launch_description() returns a list of nodes to launch - return launch.LaunchDescription(generate_common_hybrid_launch_description()) + common_launch = generate_common_hybrid_launch_description() + robot_description = get_robot_description() + robot_description_semantic = get_robot_description_semantic() + + # Demo node + common_hybrid_planning_param = load_yaml( + "moveit_hybrid_planning", "config/common_hybrid_planning_params.yaml" + ) + demo_node = Node( + package="moveit_hybrid_planning", + executable="hybrid_planning_demo_node", + name="hybrid_planning_demo_node", + output="screen", + parameters=[ + get_robot_description(), + get_robot_description_semantic(), + common_hybrid_planning_param, + ], + ) + + return launch.LaunchDescription(common_launch + [demo_node]) diff --git a/moveit_ros/hybrid_planning/test/launch/test_basic_integration.test.py b/moveit_ros/hybrid_planning/test/launch/test_basic_integration.test.py new file mode 100644 index 0000000000..1b8ccefd34 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/launch/test_basic_integration.test.py @@ -0,0 +1,69 @@ +import launch_testing +import os +import sys +import unittest + +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction +from launch.substitutions import LaunchConfiguration, PathJoinSubstitution +from launch_ros.actions import Node + +sys.path.append(os.path.dirname(__file__)) +from hybrid_planning_common import ( + generate_common_hybrid_launch_description, + get_robot_description, + get_robot_description_semantic, + load_file, + load_yaml, +) + + +def generate_test_description(): + # generate_common_hybrid_launch_description() returns a list of nodes to launch + common_launch = generate_common_hybrid_launch_description() + robot_description = get_robot_description() + robot_description_semantic = get_robot_description_semantic() + + common_hybrid_planning_param = load_yaml( + "moveit_hybrid_planning", "config/common_hybrid_planning_params.yaml" + ) + + hybrid_planning_gtest = Node( + executable=PathJoinSubstitution( + [LaunchConfiguration("test_binary_dir"), "test_basic_integration"] + ), + parameters=[ + robot_description, + robot_description_semantic, + common_hybrid_planning_param, + ], + output="screen", + ) + + return LaunchDescription( + [ + DeclareLaunchArgument( + name="test_binary_dir", + description="Binary directory of package " + "containing test executables", + ), + TimerAction(period=2.0, actions=[hybrid_planning_gtest]), + launch_testing.actions.ReadyToTest(), + ] + + common_launch + ), { + "hybrid_planning_gtest": hybrid_planning_gtest, + } + + +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, hybrid_planning_gtest): + self.proc_info.assertWaitForShutdown(hybrid_planning_gtest, 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, hybrid_planning_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=hybrid_planning_gtest) diff --git a/moveit_ros/hybrid_planning/test/launch/test_basic_launching.test.py b/moveit_ros/hybrid_planning/test/launch/test_basic_launching.test.py deleted file mode 100644 index 927a2e31a7..0000000000 --- a/moveit_ros/hybrid_planning/test/launch/test_basic_launching.test.py +++ /dev/null @@ -1,73 +0,0 @@ -# Simply check if the components stay alive after launching -import os -import sys -import time -import unittest - -import launch -import launch.actions -import launch_ros.actions -import launch_testing.actions -import launch_testing.markers -import pytest -import rclpy -from rclpy.node import Node - -sys.path.append(os.path.dirname(__file__)) -from hybrid_planning_common import generate_common_hybrid_launch_description - - -@pytest.mark.launch_test -@launch_testing.markers.keep_alive -def generate_test_description(): - path_to_test = os.path.dirname(__file__) - - # Add the usual demo nodes - ld = launch.LaunchDescription(generate_common_hybrid_launch_description()) - # Python testing requires this marker - ld.add_action(launch_testing.actions.ReadyToTest()) - return ld - - -class TestFixture(unittest.TestCase): - def test_startup(self, proc_output): - rclpy.init() - - # Wait several seconds, allowing any nodes to die - time.sleep(2) - - expected_nodes = [ - "controller_manager", - "global_planner", - "hybrid_planning_container", - "hybrid_planning_demo_node", - "joint_state_broadcaster", - "local_planner", - "robot_state_publisher", - ] - - node = MakeTestNode("test_node") - - try: - for node_name in expected_nodes: - assert node.wait_for_node(node_name, 1.0), ( - "Expected hybrid_planning node not found! Missing node: " - + node_name - ) - finally: - rclpy.shutdown() - - -class MakeTestNode(Node): - def __init__(self, name="test_node"): - super().__init__(name) - - def wait_for_node(self, node_name, timeout=8.0): - start = time.time() - flag = False - print("Waiting for node...") - while time.time() - start < timeout and not flag: - flag = node_name in self.get_node_names() - time.sleep(0.1) - - return flag diff --git a/moveit_ros/hybrid_planning/test/test_basic_integration.cpp b/moveit_ros/hybrid_planning/test/test_basic_integration.cpp new file mode 100644 index 0000000000..20f7e1efe4 --- /dev/null +++ b/moveit_ros/hybrid_planning/test/test_basic_integration.cpp @@ -0,0 +1,268 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, PickNik Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of PickNik Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + *********************************************************************/ + +/* Author: Andy Zelenak + Desc: Launch hybrid planning and test basic functionality +*/ + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace moveit_hybrid_planning +{ +using namespace std::chrono_literals; + +class HybridPlanningFixture : public ::testing::Test +{ +public: + HybridPlanningFixture() : node_(std::make_shared("hybrid_planning_testing")) + { + action_successful_ = false; + action_aborted_ = false; + action_complete_ = false; + + executor_.add_node(node_); + + std::string hybrid_planning_action_name = ""; + node_->declare_parameter("hybrid_planning_action_name", ""); + if (node_->has_parameter("hybrid_planning_action_name")) + { + node_->get_parameter("hybrid_planning_action_name", hybrid_planning_action_name); + } + else + { + RCLCPP_ERROR(node_->get_logger(), "hybrid_planning_action_name parameter was not defined"); + std::exit(EXIT_FAILURE); + } + + hp_action_client_ = + rclcpp_action::create_client(node_, hybrid_planning_action_name); + + // Add new collision object as soon as global trajectory is available. + global_solution_subscriber_ = node_->create_subscription( + "global_trajectory", rclcpp::SystemDefaultsQoS(), + [this](const moveit_msgs::msg::MotionPlanResponse::SharedPtr /* unused */) {}); + + RCLCPP_INFO(node_->get_logger(), "Initialize Planning Scene Monitor"); + tf_buffer_ = std::make_shared(node_->get_clock()); + + planning_scene_monitor_ = std::make_shared(node_, "robot_description", + "planning_scene_monitor"); + if (!planning_scene_monitor_->getPlanningScene()) + { + RCLCPP_ERROR(node_->get_logger(), "The planning scene was not retrieved!"); + std::exit(EXIT_FAILURE); + } + else + { + planning_scene_monitor_->startStateMonitor(); + planning_scene_monitor_->setPlanningScenePublishingFrequency(100); + planning_scene_monitor_->startPublishingPlanningScene(planning_scene_monitor::PlanningSceneMonitor::UPDATE_SCENE, + "/planning_scene"); + planning_scene_monitor_->startSceneMonitor(); + } + + if (!planning_scene_monitor_->waitForCurrentRobotState(node_->now(), 5)) + { + RCLCPP_ERROR(node_->get_logger(), "Timeout when waiting for /joint_states updates. Is the robot running?"); + std::exit(EXIT_FAILURE); + } + + if (!hp_action_client_->wait_for_action_server(20s)) + { + RCLCPP_ERROR(node_->get_logger(), "Hybrid planning action server not available after waiting"); + std::exit(EXIT_FAILURE); + } + + // Setup motion planning goal taken from motion_planning_api tutorial + const std::string planning_group = "panda_arm"; + robot_model_loader::RobotModelLoader robot_model_loader(node_, "robot_description"); + const moveit::core::RobotModelPtr& robot_model = robot_model_loader.getModel(); + + // Create a RobotState and JointModelGroup + const auto robot_state = std::make_shared(robot_model); + const moveit::core::JointModelGroup* joint_model_group = robot_state->getJointModelGroup(planning_group); + + // Configure a valid robot state + robot_state->setToDefaultValues(joint_model_group, "ready"); + robot_state->update(); + // Lock the planning scene as briefly as possible + { + planning_scene_monitor::LockedPlanningSceneRW locked_planning_scene(planning_scene_monitor_); + locked_planning_scene->setCurrentState(*robot_state); + } + + const moveit::core::RobotState goal_state([robot_model, joint_model_group]() { + moveit::core::RobotState goal_state(robot_model); + std::vector joint_values = { 0.0, 0.0, 0.0, 0.0, 0.0, 1.571, 0.785 }; + goal_state.setJointGroupPositions(joint_model_group, joint_values); + return goal_state; + }()); + + // Create desired motion goal + const moveit_msgs::msg::MotionPlanRequest goal_motion_request( + [robot_state, planning_group, goal_state, joint_model_group]() { + moveit_msgs::msg::MotionPlanRequest goal_motion_request; + moveit::core::robotStateToRobotStateMsg(*robot_state, goal_motion_request.start_state); + goal_motion_request.group_name = planning_group; + goal_motion_request.num_planning_attempts = 10; + goal_motion_request.max_velocity_scaling_factor = 0.1; + goal_motion_request.max_acceleration_scaling_factor = 0.1; + goal_motion_request.allowed_planning_time = 2.0; + goal_motion_request.planner_id = "ompl"; + goal_motion_request.pipeline_id = "ompl"; + goal_motion_request.goal_constraints.resize(1); + goal_motion_request.goal_constraints[0] = + kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); + return goal_motion_request; + }()); + + // Create Hybrid Planning action request + moveit_msgs::msg::MotionSequenceItem sequence_item; + sequence_item.req = goal_motion_request; + sequence_item.blend_radius = 0.0; // Single goal + + moveit_msgs::msg::MotionSequenceRequest sequence_request; + sequence_request.items.push_back(sequence_item); + + goal_action_request_ = moveit_msgs::action::HybridPlanner::Goal(); + goal_action_request_.planning_group = planning_group; + goal_action_request_.motion_sequence = sequence_request; + + send_goal_options_ = rclcpp_action::Client::SendGoalOptions(); + send_goal_options_.result_callback = + [this](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { + switch (result.code) + { + case rclcpp_action::ResultCode::SUCCEEDED: + action_successful_ = true; + RCLCPP_INFO(node_->get_logger(), "Hybrid planning goal succeeded"); + break; + case rclcpp_action::ResultCode::ABORTED: + action_aborted_ = true; + RCLCPP_ERROR(node_->get_logger(), "Hybrid planning goal was aborted"); + break; + case rclcpp_action::ResultCode::CANCELED: + RCLCPP_ERROR(node_->get_logger(), "Hybrid planning goal was canceled"); + break; + default: + RCLCPP_ERROR(node_->get_logger(), "Unknown hybrid planning result code"); + break; + } + action_complete_ = true; + return; + }; + send_goal_options_.feedback_callback = + [this](rclcpp_action::ClientGoalHandle::SharedPtr /*unused*/, + const std::shared_ptr feedback) { + RCLCPP_INFO(node_->get_logger(), feedback->feedback.c_str()); + }; + } + + rclcpp::executors::MultiThreadedExecutor executor_; + +protected: + rclcpp::Node::SharedPtr node_; + rclcpp_action::Client::SharedPtr hp_action_client_; + rclcpp::Subscription::SharedPtr global_solution_subscriber_; + std::shared_ptr tf_buffer_; + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + std::atomic_bool action_successful_; + std::atomic_bool action_complete_; + std::atomic_bool action_aborted_; + + // Action request + moveit_msgs::action::HybridPlanner::Goal goal_action_request_; + rclcpp_action::Client::SendGoalOptions send_goal_options_; +}; // class HybridPlanningFixture + +// Make a hybrid planning request and verify it completes +TEST_F(HybridPlanningFixture, ActionCompletion) +{ + std::thread run_thread([this]() { + // Send the goal + auto goal_handle_future = hp_action_client_->async_send_goal(goal_action_request_, send_goal_options_); + }); + + rclcpp::Rate rate(10); + while (!action_complete_) + { + executor_.spin_once(); + rate.sleep(); + } + run_thread.join(); + ASSERT_TRUE(action_successful_); +} + +// Make a hybrid planning request then abort it +TEST_F(HybridPlanningFixture, ActionAbortion) +{ + std::thread run_thread([this]() { + // Send the goal + auto goal_handle_future = hp_action_client_->async_send_goal(goal_action_request_, send_goal_options_); + + // Cancel the goal + hp_action_client_->async_cancel_all_goals(); + }); + + rclcpp::Rate rate(10); + while (!action_complete_) + { + executor_.spin_once(); + rate.sleep(); + } + run_thread.join(); + ASSERT_FALSE(action_successful_); + ASSERT_TRUE(action_aborted_); +} +} // namespace moveit_hybrid_planning + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + + int ret = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return ret; +} diff --git a/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml b/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml index 57bc7b3bcd..a0c8afdfe4 100644 --- a/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml +++ b/moveit_ros/moveit_servo/test/config/collision_start_positions.yaml @@ -6,3 +6,11 @@ initial_positions: panda_joint5: 0.0 panda_joint6: 1.1 panda_joint7: 0.785 +initial_velocities: + panda_joint1: 0.0 + panda_joint2: 0.0 + panda_joint3: 0.0 + panda_joint4: 0.0 + panda_joint5: 0.0 + panda_joint6: 0.0 + panda_joint7: 0.0 diff --git a/moveit_ros/moveit_servo/test/config/singularity_start_positions.yaml b/moveit_ros/moveit_servo/test/config/singularity_start_positions.yaml index 42b95ee145..4199bc3dee 100644 --- a/moveit_ros/moveit_servo/test/config/singularity_start_positions.yaml +++ b/moveit_ros/moveit_servo/test/config/singularity_start_positions.yaml @@ -6,3 +6,11 @@ initial_positions: panda_joint5: 0.117 panda_joint6: 1.439 panda_joint7: -1.286 +initial_velocities: + panda_joint1: 0.0 + panda_joint2: 0.0 + panda_joint3: 0.0 + panda_joint4: 0.0 + panda_joint5: 0.0 + panda_joint6: 0.0 + panda_joint7: 0.0