From e129fb94ed2366d025098e253c7e1da981a80b9e Mon Sep 17 00:00:00 2001 From: Henning Kayser Date: Fri, 14 Jun 2024 22:15:46 +0200 Subject: [PATCH] Imported upstream version '2.10.0' of 'upstream' --- .clang-tidy | 2 +- .docker/ci-testing/Dockerfile | 2 +- .docker/ci/Dockerfile | 2 +- .docker/release/Dockerfile | 2 +- .docker/source/Dockerfile | 4 +- .docker/tutorial-source/Dockerfile | 4 +- .github/ISSUE_TEMPLATE/first_timers_only.md | 2 +- .github/PULL_REQUEST_TEMPLATE.md | 4 +- .github/workflows/ci.yaml | 39 +- .github/workflows/docker.yaml | 98 +-- .github/workflows/format.yaml | 3 +- .github/workflows/sonar.yaml | 15 +- .github/workflows/tutorial_docker.yaml | 13 +- .pre-commit-config.yaml | 9 + README.md | 89 +- moveit/CHANGELOG.rst | 11 + moveit/CMakeLists.txt | 6 - moveit/package.xml | 10 +- moveit/scripts/create_maintainer_table.py | 2 +- moveit2.repos | 8 +- moveit2_humble.repos | 5 + moveit2_iron.repos | 5 + moveit_common/CHANGELOG.rst | 11 + moveit_common/CMakeLists.txt | 14 +- moveit_common/cmake/moveit_package.cmake | 75 +- moveit_common/moveit_common-extras.cmake | 34 +- moveit_common/package.xml | 9 +- moveit_configs_utils/CHANGELOG.rst | 19 + .../moveit_configs_utils/launches.py | 28 +- .../moveit_configs_builder.py | 20 +- moveit_configs_utils/package.xml | 5 +- moveit_configs_utils/setup.py | 2 +- moveit_core/CHANGELOG.rst | 43 + moveit_core/CMakeLists.txt | 94 +- moveit_core/ConfigExtras.cmake | 7 +- .../collision_detection/CMakeLists.txt | 59 +- .../src/allvalid/collision_env_allvalid.cpp | 2 +- .../src/collision_common.cpp | 2 +- .../collision_detection/src/collision_env.cpp | 2 +- .../src/collision_matrix.cpp | 2 +- .../src/collision_octomap_filter.cpp | 8 +- .../src/collision_plugin_cache.cpp | 2 +- moveit_core/collision_detection/src/world.cpp | 3 +- .../collision_detection_bullet/CMakeLists.txt | 88 +- .../bullet_integration/ros_bullet_utils.cpp | 2 +- ...t_bullet_continuous_collision_checking.cpp | 2 +- .../collision_detection_fcl/CMakeLists.txt | 83 +- .../src/collision_common.cpp | 2 +- .../src/collision_env_fcl.cpp | 2 +- .../test/test_fcl_env.cpp | 2 +- .../collision_distance_field/CMakeLists.txt | 63 +- .../collision_distance_field_types.h | 5 +- .../src/collision_common_distance_field.cpp | 2 +- .../src/collision_distance_field_types.cpp | 2 +- .../src/collision_env_distance_field.cpp | 5 +- .../constraint_samplers/CMakeLists.txt | 66 +- .../src/constraint_sampler.cpp | 2 +- .../src/constraint_sampler_manager.cpp | 2 +- .../src/constraint_sampler_tools.cpp | 2 +- .../src/default_constraint_samplers.cpp | 2 +- .../src/union_constraint_sampler.cpp | 2 +- .../constraint_samplers/test/pr2_arm_ik.cpp | 4 +- .../test/pr2_arm_kinematics_plugin.cpp | 2 +- moveit_core/distance_field/CMakeLists.txt | 28 +- .../distance_field/src/distance_field.cpp | 2 +- .../src/propagation_distance_field.cpp | 2 +- .../test/test_distance_field.cpp | 3 - .../doc/_templates/custom-class-template.rst | 34 - .../doc/_templates/custom-module-template.rst | 66 -- moveit_core/doc/api.rst | 6 - moveit_core/doc/conf.py | 200 ----- moveit_core/doc/index.rst | 13 - moveit_core/dynamics_solver/CMakeLists.txt | 25 +- .../dynamics_solver/src/dynamics_solver.cpp | 2 +- moveit_core/exceptions/CMakeLists.txt | 19 +- moveit_core/filter_plugin_acceleration.xml | 8 + .../kinematic_constraints/CMakeLists.txt | 52 +- .../src/kinematic_constraint.cpp | 2 +- .../kinematic_constraints/src/utils.cpp | 2 +- moveit_core/kinematics_base/CMakeLists.txt | 24 +- .../moveit/kinematics_base/kinematics_base.h | 9 +- .../kinematics_base/src/kinematics_base.cpp | 2 +- moveit_core/kinematics_metrics/CMakeLists.txt | 24 +- .../src/kinematics_metrics.cpp | 2 +- moveit_core/macros/CMakeLists.txt | 7 +- .../online_signal_smoothing/CMakeLists.txt | 100 ++- moveit_core/online_signal_smoothing/README.md | 12 + .../acceleration_filter.h | 162 ++++ .../butterworth_filter.h | 3 +- .../res/acceleration_limiting_diagram.png | Bin 0 -> 68588 bytes .../src/acceleration_filter.cpp | 349 ++++++++ .../src/acceleration_filter_parameters.yaml | 16 + .../test/test_acceleration_filter.cpp | 207 +++++ moveit_core/package.xml | 24 +- moveit_core/planning_interface/CMakeLists.txt | 33 +- .../src/planning_interface.cpp | 2 +- moveit_core/planning_scene/CMakeLists.txt | 59 +- .../moveit/planning_scene/planning_scene.h | 19 +- .../planning_scene/src/planning_scene.cpp | 174 ++-- moveit_core/robot_model/CMakeLists.txt | 48 +- .../moveit/robot_model/joint_model_group.h | 17 +- .../robot_model/src/floating_joint_model.cpp | 2 +- .../robot_model/src/joint_model_group.cpp | 61 +- .../robot_model/src/prismatic_joint_model.cpp | 2 +- .../robot_model/src/revolute_joint_model.cpp | 2 +- moveit_core/robot_model/src/robot_model.cpp | 2 +- moveit_core/robot_state/CMakeLists.txt | 95 +- .../src/cartesian_interpolator.cpp | 2 +- moveit_core/robot_state/src/conversions.cpp | 2 +- moveit_core/robot_state/src/robot_state.cpp | 2 +- .../test/robot_state_benchmark.cpp | 331 +++---- .../robot_state/test/robot_state_test.cpp | 2 +- moveit_core/robot_trajectory/CMakeLists.txt | 28 +- .../robot_trajectory/robot_trajectory.h | 11 +- .../robot_trajectory/src/robot_trajectory.cpp | 136 ++- .../test/test_robot_trajectory.cpp | 32 + .../trajectory_processing/CMakeLists.txt | 71 +- .../trajectory_processing/trajectory_tools.h | 8 + .../src/ruckig_traj_smoothing.cpp | 2 +- .../time_optimal_trajectory_generation.cpp | 4 +- .../src/trajectory_tools.cpp | 45 + moveit_core/transforms/CMakeLists.txt | 17 +- moveit_core/transforms/src/transforms.cpp | 2 +- moveit_core/utils/CMakeLists.txt | 41 +- .../utils/src/robot_model_test_utils.cpp | 2 +- moveit_core/utils/test/CMakeLists.txt | 20 +- moveit_core/utils/test/logger_dut.cpp | 5 +- moveit_core/version/CMakeLists.txt | 41 +- moveit_core/version/version.cmake | 51 +- moveit_core/version/version.cpp | 2 +- moveit_kinematics/CHANGELOG.rst | 15 + moveit_kinematics/CMakeLists.txt | 76 +- .../CMakeLists.txt | 84 +- .../cached_ik_kinematics_plugin/README.md | 4 +- .../cached_ik_kinematics_plugin.h | 2 +- .../src/ik_cache.cpp | 2 +- .../ikfast_kinematics_plugin/CMakeLists.txt | 24 +- .../ikfast_kinematics_plugin/README.md | 2 +- .../auto_create_ikfast_moveit_plugin.sh | 2 +- .../templates/CMakeLists.txt | 22 +- .../ikfast61_moveit_plugin_template.cpp | 5 +- .../kdl_kinematics_plugin/CMakeLists.txt | 23 +- .../src/kdl_kinematics_plugin.cpp | 2 +- moveit_kinematics/package.xml | 8 +- .../srv_kinematics_plugin/CMakeLists.txt | 14 +- .../src/srv_kinematics_plugin.cpp | 2 +- moveit_kinematics/test/CMakeLists.txt | 55 +- moveit_kinematics/test/test_ikfast_plugins.sh | 2 +- .../test/test_kinematics_plugin.cpp | 2 +- moveit_planners/chomp/README.md | 2 +- .../chomp/chomp_interface/CHANGELOG.rst | 7 + .../chomp/chomp_interface/CMakeLists.txt | 64 +- .../chomp/chomp_interface/package.xml | 5 +- .../chomp_interface/src/chomp_interface.cpp | 2 +- .../chomp_interface/src/chomp_plugin.cpp | 2 +- .../chomp/chomp_motion_planner/CHANGELOG.rst | 7 + .../chomp/chomp_motion_planner/CMakeLists.txt | 45 +- .../chomp/chomp_motion_planner/package.xml | 5 +- .../src/chomp_optimizer.cpp | 2 +- .../src/chomp_planner.cpp | 2 +- moveit_planners/moveit_planners/CHANGELOG.rst | 11 + .../moveit_planners/CMakeLists.txt | 5 - moveit_planners/moveit_planners/package.xml | 9 +- moveit_planners/ompl/CHANGELOG.rst | 20 + moveit_planners/ompl/CMakeLists.txt | 37 +- .../ompl/ompl_interface/CMakeLists.txt | 101 ++- .../detail/state_validity_checker.h | 2 +- .../src/detail/constrained_goal_sampler.cpp | 2 +- .../src/detail/constraints_library.cpp | 2 +- .../src/detail/ompl_constraints.cpp | 2 +- .../src/detail/state_validity_checker.cpp | 2 +- .../src/model_based_planning_context.cpp | 11 +- .../ompl_interface/src/ompl_interface.cpp | 2 +- .../src/ompl_planner_manager.cpp | 2 +- .../model_based_state_space.cpp | 2 +- .../work_space/pose_model_state_space.cpp | 2 +- .../src/planning_context_manager.cpp | 2 +- .../test_constrained_planning_state_space.cpp | 2 +- ...est_constrained_state_validity_checker.cpp | 2 +- .../test/test_ompl_constraints.cpp | 4 +- .../test/test_planning_context_manager.cpp | 2 +- .../ompl_interface/test/test_state_space.cpp | 2 +- .../test/test_state_validity_checker.cpp | 2 +- moveit_planners/ompl/package.xml | 9 +- .../CHANGELOG.rst | 15 + .../CMakeLists.txt | 267 +++--- .../package.xml | 9 +- .../src/command_list_manager.cpp | 2 +- .../src/joint_limits_aggregator.cpp | 2 +- .../src/joint_limits_container.cpp | 18 +- .../src/limits_container.cpp | 2 +- .../src/move_group_sequence_action.cpp | 2 +- .../src/move_group_sequence_service.cpp | 2 +- .../src/pilz_industrial_motion_planner.cpp | 2 +- .../src/planning_context_loader_circ.cpp | 2 +- .../src/planning_context_loader_lin.cpp | 2 +- .../src/planning_context_loader_ptp.cpp | 2 +- .../trajectory_blender_transition_window.cpp | 2 +- .../src/trajectory_generator.cpp | 2 +- .../src/trajectory_generator_circ.cpp | 2 +- .../src/trajectory_generator_lin.cpp | 2 +- .../src/trajectory_generator_ptp.cpp | 2 +- .../test/CMakeLists.txt | 17 +- .../test/integration_tests/CMakeLists.txt | 1 + .../test/test_utils.cpp | 6 +- .../test/test_utils.h | 2 +- .../test/unit_tests/CMakeLists.txt | 193 ++-- .../CHANGELOG.rst | 11 + .../CMakeLists.txt | 61 +- .../package.xml | 9 +- moveit_planners/stomp/CHANGELOG.rst | 26 + moveit_planners/stomp/CMakeLists.txt | 43 +- .../include/stomp_moveit/cost_functions.hpp | 105 ++- moveit_planners/stomp/package.xml | 9 +- .../stomp/src/stomp_moveit_planner_plugin.cpp | 2 +- .../src/stomp_moveit_planning_context.cpp | 2 +- moveit_planners/stomp/test/CMakeLists.txt | 8 + .../stomp/test/test_cost_functions.cpp | 117 +++ .../stomp/test/test_noise_generator.cpp | 68 +- .../CHANGELOG.rst | 11 + .../CMakeLists.txt | 35 +- .../package.xml | 6 +- .../prbt_moveit_config/CHANGELOG.rst | 11 + .../prbt_moveit_config/CMakeLists.txt | 4 +- .../prbt_moveit_config/package.xml | 6 +- .../prbt_pg70_support/CHANGELOG.rst | 11 + .../prbt_pg70_support/CMakeLists.txt | 6 +- .../prbt_pg70_support/package.xml | 6 +- .../test_configs/prbt_support/CHANGELOG.rst | 11 + .../test_configs/prbt_support/CMakeLists.txt | 8 +- .../test_configs/prbt_support/package.xml | 6 +- moveit_plugins/moveit_plugins/CHANGELOG.rst | 5 + moveit_plugins/moveit_plugins/CMakeLists.txt | 5 - moveit_plugins/moveit_plugins/package.xml | 5 +- .../CHANGELOG.rst | 14 + .../CMakeLists.txt | 114 +-- .../moveit_ros_control_interface/package.xml | 5 +- .../src/controller_manager_plugin.cpp | 19 +- .../CHANGELOG.rst | 12 + .../CMakeLists.txt | 51 +- .../package.xml | 9 +- .../src/moveit_simple_controller_manager.cpp | 2 +- moveit_py/CHANGELOG.rst | 17 + moveit_py/CITATION.cff | 2 +- moveit_py/CMakeLists.txt | 127 +-- moveit_py/README.md | 2 +- moveit_py/moveit/planning.pyi | 6 + moveit_py/package.xml | 2 +- .../moveit_core/robot_state/robot_state.h | 5 + .../src/moveit/moveit_py_utils/CMakeLists.txt | 13 +- .../moveit_ros/moveit_cpp/moveit_cpp.cpp | 4 +- .../trajectory_execution_manager.cpp | 41 +- moveit_ros/benchmarks/CHANGELOG.rst | 30 + moveit_ros/benchmarks/CMakeLists.txt | 65 +- moveit_ros/benchmarks/README.md | 2 +- moveit_ros/benchmarks/package.xml | 9 +- .../benchmarks/src/BenchmarkExecutor.cpp | 22 +- .../benchmarks/src/BenchmarkOptions.cpp | 2 +- moveit_ros/hybrid_planning/CHANGELOG.rst | 15 + moveit_ros/hybrid_planning/CMakeLists.txt | 107 ++- moveit_ros/hybrid_planning/README.md | 2 +- .../global_planner_component/CMakeLists.txt | 9 +- .../src/global_planner_component.cpp | 10 +- .../global_planner_plugins/CMakeLists.txt | 9 +- .../CMakeLists.txt | 11 +- .../hybrid_planning_events.h | 14 + .../hybrid_planning_manager.h | 35 +- .../planner_logic_interface.h | 36 +- .../res/hp_manager_parameters.yaml | 6 + .../src/hybrid_planning_manager.cpp | 212 ++--- .../planner_logic_plugins/CMakeLists.txt | 26 +- .../single_plan_execution.h | 1 - .../src/replan_invalidated_trajectory.cpp | 7 +- .../src/single_plan_execution.cpp | 30 +- .../CMakeLists.txt | 10 +- .../local_planner_component/CMakeLists.txt | 12 +- .../local_planner/local_planner_component.h | 73 +- .../res/local_planner_parameters.yaml | 61 ++ .../src/local_planner_component.cpp | 65 +- .../CMakeLists.txt | 7 +- moveit_ros/hybrid_planning/package.xml | 14 +- .../hybrid_planning/test/CMakeLists.txt | 27 +- .../config/common_hybrid_planning_params.yaml | 6 - .../test/config/global_planner.yaml | 2 +- .../test/hybrid_planning_demo_node.cpp | 321 ------- .../test/launch/hybrid_planning_common.py | 176 +--- .../launch/hybrid_planning_demo.launch.py | 38 - .../launch/test_basic_integration.test.py | 78 +- .../test/test_basic_integration.cpp | 24 +- moveit_ros/move_group/CHANGELOG.rst | 21 + moveit_ros/move_group/CMakeLists.txt | 133 ++- moveit_ros/move_group/ConfigExtras.cmake | 9 +- ...efault_capabilities_plugin_description.xml | 6 + .../moveit/move_group/capability_names.h | 2 + moveit_ros/move_group/package.xml | 13 +- ...pply_planning_scene_service_capability.cpp | 2 +- .../cartesian_path_service_capability.cpp | 10 +- .../clear_octomap_service_capability.cpp | 2 +- .../execute_trajectory_action_capability.cpp | 2 +- .../get_group_urdf_capability.cpp | 168 ++++ .../get_group_urdf_capability.h} | 56 +- .../kinematics_service_capability.cpp | 2 +- .../move_action_capability.cpp | 2 +- .../plan_service_capability.cpp | 2 +- .../query_planners_service_capability.cpp | 2 +- .../tf_publisher_capability.cpp | 4 +- moveit_ros/move_group/src/move_group.cpp | 3 +- .../move_group/src/move_group_capability.cpp | 2 +- .../move_group/src/move_group_context.cpp | 2 +- moveit_ros/moveit_ros/CHANGELOG.rst | 11 + moveit_ros/moveit_ros/CMakeLists.txt | 5 - moveit_ros/moveit_ros/package.xml | 9 +- moveit_ros/moveit_servo/CHANGELOG.rst | 28 + moveit_ros/moveit_servo/CMakeLists.txt | 167 ++-- moveit_ros/moveit_servo/README.md | 2 +- .../config/panda_simulated_config.yaml | 4 +- .../config/test_config_panda.yaml | 2 +- .../moveit_servo/collision_monitor.hpp | 3 +- .../launch/demo_joint_jog.launch.py | 10 +- .../moveit_servo/launch/demo_pose.launch.py | 10 +- .../launch/demo_ros_api.launch.py | 14 +- .../moveit_servo/launch/demo_twist.launch.py | 10 +- moveit_ros/moveit_servo/package.xml | 6 +- .../moveit_servo/src/collision_monitor.cpp | 14 +- moveit_ros/moveit_servo/src/servo.cpp | 28 +- moveit_ros/moveit_servo/src/servo_node.cpp | 25 +- moveit_ros/moveit_servo/src/utils/command.cpp | 2 +- moveit_ros/moveit_servo/src/utils/common.cpp | 2 +- .../occupancy_map_monitor/CHANGELOG.rst | 14 + .../occupancy_map_monitor/CMakeLists.txt | 74 +- moveit_ros/occupancy_map_monitor/package.xml | 15 +- .../src/occupancy_map_monitor.cpp | 2 +- ...ccupancy_map_monitor_middleware_handle.cpp | 4 +- .../src/occupancy_map_server.cpp | 2 +- .../src/occupancy_map_updater.cpp | 2 +- moveit_ros/perception/CHANGELOG.rst | 14 + moveit_ros/perception/CMakeLists.txt | 90 +- .../CMakeLists.txt | 27 +- .../src/depth_image_octomap_updater.cpp | 2 +- .../lazy_free_space_updater/CMakeLists.txt | 20 +- .../src/lazy_free_space_updater.cpp | 2 +- .../perception/mesh_filter/CMakeLists.txt | 61 +- .../mesh_filter/src/gl_renderer.cpp | 4 +- moveit_ros/perception/package.xml | 8 +- .../point_containment_filter/CMakeLists.txt | 18 +- .../src/shape_mask.cpp | 2 +- .../pointcloud_octomap_updater/CMakeLists.txt | 35 +- .../src/pointcloud_octomap_updater.cpp | 2 +- .../perception/semantic_world/CMakeLists.txt | 9 +- .../semantic_world/src/semantic_world.cpp | 2 +- moveit_ros/planning/CHANGELOG.rst | 68 ++ moveit_ros/planning/CMakeLists.txt | 136 +-- moveit_ros/planning/ConfigExtras.cmake | 6 +- .../collision_plugin_loader/CMakeLists.txt | 13 +- .../src/collision_plugin_loader.cpp | 2 +- .../CMakeLists.txt | 16 +- .../src/constraint_sampler_manager_loader.cpp | 2 +- .../kinematics_plugin_loader/CMakeLists.txt | 24 +- .../kinematics_plugin_loader.h | 4 +- moveit_ros/planning/moveit_cpp/CMakeLists.txt | 34 +- .../planning/moveit_cpp/src/moveit_cpp.cpp | 2 +- .../moveit_cpp/src/planning_component.cpp | 2 +- moveit_ros/planning/package.xml | 26 +- .../planning/plan_execution/CMakeLists.txt | 23 +- .../plan_execution/src/plan_execution.cpp | 2 +- .../planning_components_tools/CMakeLists.txt | 92 +- .../src/evaluate_collision_checking_speed.cpp | 2 +- .../planning/planning_pipeline/CMakeLists.txt | 65 +- .../src/planning_pipeline.cpp | 27 +- .../CMakeLists.txt | 31 +- .../src/planning_pipeline_interfaces.cpp | 10 +- .../CMakeLists.txt | 26 +- .../res/default_request_adapter_params.yaml | 2 +- .../src/check_for_stacked_constraints.cpp | 2 +- .../src/check_start_state_bounds.cpp | 2 +- .../src/check_start_state_collision.cpp | 14 +- .../src/resolve_constraint_frames.cpp | 2 +- .../src/validate_workspace_bounds.cpp | 2 +- .../CMakeLists.txt | 25 +- .../src/add_ruckig_traj_smoothing.cpp | 2 +- .../src/add_time_optimal_parameterization.cpp | 2 +- .../src/display_motion_path.cpp | 2 +- .../src/validate_path.cpp | 2 +- .../planning_scene_monitor/CMakeLists.txt | 68 +- .../src/current_state_monitor.cpp | 2 +- .../src/planning_scene_monitor.cpp | 50 +- .../src/trajectory_monitor.cpp | 2 +- .../launch/planning_scene_monitor.test.py | 94 ++ .../test/planning_scene_monitor_test.cpp | 154 ++++ moveit_ros/planning/rdf_loader/CMakeLists.txt | 50 +- .../include/moveit/rdf_loader/rdf_loader.h | 6 + .../planning/rdf_loader/src/rdf_loader.cpp | 2 +- .../robot_model_loader/CMakeLists.txt | 18 +- .../src/robot_model_loader.cpp | 4 +- .../srdf_publisher_node/CMakeLists.txt | 9 + .../src/srdf_publisher_node.cpp | 101 +++ .../test/srdf_publisher_test.py | 124 +++ .../CMakeLists.txt | 47 +- .../trajectory_execution_manager.h | 19 + .../src/trajectory_execution_manager.cpp | 153 +++- moveit_ros/planning_interface/CHANGELOG.rst | 20 + moveit_ros/planning_interface/CMakeLists.txt | 94 +- .../planning_interface/ConfigExtras.cmake | 21 +- .../CMakeLists.txt | 13 +- .../move_group_interface/CMakeLists.txt | 40 +- .../move_group_interface.h | 2 +- .../src/move_group_interface.cpp | 11 +- .../src/wrap_python_move_group.cpp | 821 ------------------ moveit_ros/planning_interface/package.xml | 8 +- .../planning_scene_interface/CMakeLists.txt | 33 +- .../src/planning_scene_interface.cpp | 9 +- .../wrap_python_planning_scene_interface.cpp | 137 --- .../py_bindings_tools/CMakeLists.txt | 26 - .../py_bindings_tools/dox/py_bindings.dox | 48 - .../moveit/py_bindings_tools/gil_releaser.h | 96 -- .../moveit/py_bindings_tools/py_conversions.h | 96 -- .../moveit/py_bindings_tools/serialize_msg.h | 137 --- .../src/roscpp_initializer.cpp | 157 ---- .../src/wrap_python_roscpp_initializer.cpp | 53 -- .../moveit_ros_planning_interface/__init__.py | 0 .../robot_interface/CMakeLists.txt | 15 - .../src/wrap_python_robot_interface.cpp | 430 --------- moveit_ros/planning_interface/setup.py | 9 - .../planning_interface/test/CMakeLists.txt | 47 +- moveit_ros/planning_interface/test/cleanup.py | 42 - .../planning_interface/test/cleanup.test | 5 - .../test/dual_arm_robot_state_update.py | 59 -- .../test/dual_arm_robot_state_update.test | 5 - .../test/move_group_interface_cpp_test.cpp | 2 +- .../test/move_group_pick_place_test.cpp | 2 +- .../test/python_move_group.py | 112 --- .../test/python_move_group.test | 9 - .../test/python_move_group_ns.py | 124 --- .../test/python_move_group_ns.test | 9 - .../test/robot_state_update.py | 58 -- .../test/robot_state_update.test | 5 - .../planning_interface/test/serialize_msg.py | 116 --- .../test/serialize_msg_python_cpp_helpers.cpp | 134 --- .../test/subframes_test.cpp | 2 +- .../planning_interface/test/test_cleanup.py | 8 - moveit_ros/robot_interaction/CHANGELOG.rst | 13 + moveit_ros/robot_interaction/CMakeLists.txt | 52 +- moveit_ros/robot_interaction/package.xml | 8 +- .../src/interaction_handler.cpp | 7 +- .../src/kinematic_options.cpp | 2 +- .../src/robot_interaction.cpp | 2 +- moveit_ros/tests/CHANGELOG.rst | 95 ++ moveit_ros/tests/CMakeLists.txt | 19 + .../tests/include/parameter_name_list.hpp | 495 +++++++++++ .../tests/launch/move_group_api.test.py | 137 +++ moveit_ros/tests/package.xml | 40 + moveit_ros/tests/src/move_group_api_test.cpp | 105 +++ moveit_ros/visualization/CHANGELOG.rst | 13 + moveit_ros/visualization/CMakeLists.txt | 109 ++- .../CMakeLists.txt | 79 +- .../src/motion_planning_display.cpp | 5 +- .../src/motion_planning_frame.cpp | 2 +- moveit_ros/visualization/package.xml | 9 +- .../planning_scene_rviz_plugin/CMakeLists.txt | 57 +- .../src/background_processing.cpp | 8 +- .../src/planning_scene_display.cpp | 2 +- .../robot_state_rviz_plugin/CMakeLists.txt | 47 +- .../rviz_plugin_render_tools/CMakeLists.txt | 51 +- .../src/octomap_render.cpp | 3 - .../src/robot_state_visualization.cpp | 2 +- .../src/trajectory_visualization.cpp | 2 +- .../trajectory_rviz_plugin/CMakeLists.txt | 57 +- .../src/trajectory_display.cpp | 2 +- moveit_ros/warehouse/CHANGELOG.rst | 13 + moveit_ros/warehouse/CMakeLists.txt | 81 +- moveit_ros/warehouse/ConfigExtras.cmake | 6 +- moveit_ros/warehouse/package.xml | 9 +- .../warehouse/src/constraints_storage.cpp | 2 +- moveit_ros/warehouse/src/import_from_text.cpp | 2 +- .../warehouse/src/planning_scene_storage.cpp | 2 +- .../src/planning_scene_world_storage.cpp | 3 +- .../warehouse/src/save_to_warehouse.cpp | 2 +- moveit_ros/warehouse/src/state_storage.cpp | 2 +- .../src/trajectory_constraints_storage.cpp | 3 +- .../warehouse/src/warehouse_connector.cpp | 2 +- .../warehouse/src/warehouse_services.cpp | 2 +- moveit_runtime/CHANGELOG.rst | 5 + moveit_runtime/CMakeLists.txt | 5 - moveit_runtime/package.xml | 5 +- .../moveit_setup_app_plugins/CHANGELOG.rst | 5 + .../moveit_setup_app_plugins/CMakeLists.txt | 70 +- .../moveit_setup_app_plugins/package.xml | 6 +- .../moveit_setup_assistant/CHANGELOG.rst | 12 + .../moveit_setup_assistant/CMakeLists.txt | 88 +- .../moveit_setup_assistant/package.xml | 13 +- .../moveit_setup_controllers/CHANGELOG.rst | 6 + .../moveit_setup_controllers/CMakeLists.txt | 87 +- .../moveit_setup_controllers/package.xml | 6 +- .../src/ros2_controllers_config.cpp | 1 + .../moveit_setup_core_plugins/CHANGELOG.rst | 5 + .../moveit_setup_core_plugins/CMakeLists.txt | 70 +- .../moveit_setup_core_plugins/package.xml | 7 +- .../moveit_setup_framework/CHANGELOG.rst | 14 + .../moveit_setup_framework/CMakeLists.txt | 81 +- .../data/srdf_config.hpp | 1 + .../moveit_setup_framework/package.xml | 7 +- .../moveit_setup_framework/src/templates.cpp | 2 +- .../templates/CMakeLists.txt | 4 +- .../templates/package.xml.template | 4 +- .../moveit_setup_simulation/CMakeLists.txt | 59 +- .../moveit_setup_simulation/package.xml | 5 - .../moveit_setup_srdf_plugins/CHANGELOG.rst | 8 + .../moveit_setup_srdf_plugins/CMakeLists.txt | 112 ++- .../moveit_setup_srdf_plugins/package.xml | 6 +- .../src/collision_linear_model.cpp | 4 +- .../src/compute_default_collisions.cpp | 2 +- 511 files changed, 7667 insertions(+), 8169 deletions(-) create mode 100644 moveit2_humble.repos create mode 100644 moveit2_iron.repos delete mode 100644 moveit_core/doc/_templates/custom-class-template.rst delete mode 100644 moveit_core/doc/_templates/custom-module-template.rst delete mode 100644 moveit_core/doc/api.rst delete mode 100644 moveit_core/doc/conf.py delete mode 100644 moveit_core/doc/index.rst create mode 100644 moveit_core/filter_plugin_acceleration.xml create mode 100644 moveit_core/online_signal_smoothing/README.md create mode 100644 moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h create mode 100644 moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png create mode 100644 moveit_core/online_signal_smoothing/src/acceleration_filter.cpp create mode 100644 moveit_core/online_signal_smoothing/src/acceleration_filter_parameters.yaml create mode 100644 moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp create mode 100644 moveit_planners/stomp/test/CMakeLists.txt create mode 100644 moveit_planners/stomp/test/test_cost_functions.cpp rename moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h => moveit_planners/stomp/test/test_noise_generator.cpp (54%) create mode 100644 moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/res/hp_manager_parameters.yaml create mode 100644 moveit_ros/hybrid_planning/local_planner/local_planner_component/res/local_planner_parameters.yaml delete mode 100644 moveit_ros/hybrid_planning/test/config/common_hybrid_planning_params.yaml delete mode 100644 moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp delete mode 100644 moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py create mode 100644 moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp rename moveit_ros/{hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/moveit_error_code_interface.h => move_group/src/default_capabilities/get_group_urdf_capability.h} (72%) create mode 100644 moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py create mode 100644 moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp create mode 100644 moveit_ros/planning/srdf_publisher_node/CMakeLists.txt create mode 100644 moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp create mode 100644 moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py delete mode 100644 moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp delete mode 100644 moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp delete mode 100644 moveit_ros/planning_interface/py_bindings_tools/src/wrap_python_roscpp_initializer.cpp delete mode 100644 moveit_ros/planning_interface/python/moveit_ros_planning_interface/__init__.py delete mode 100644 moveit_ros/planning_interface/robot_interface/CMakeLists.txt delete mode 100644 moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp delete mode 100644 moveit_ros/planning_interface/setup.py delete mode 100755 moveit_ros/planning_interface/test/cleanup.py delete mode 100644 moveit_ros/planning_interface/test/cleanup.test delete mode 100755 moveit_ros/planning_interface/test/dual_arm_robot_state_update.py delete mode 100644 moveit_ros/planning_interface/test/dual_arm_robot_state_update.test delete mode 100755 moveit_ros/planning_interface/test/python_move_group.py delete mode 100644 moveit_ros/planning_interface/test/python_move_group.test delete mode 100755 moveit_ros/planning_interface/test/python_move_group_ns.py delete mode 100644 moveit_ros/planning_interface/test/python_move_group_ns.test delete mode 100755 moveit_ros/planning_interface/test/robot_state_update.py delete mode 100644 moveit_ros/planning_interface/test/robot_state_update.test delete mode 100644 moveit_ros/planning_interface/test/serialize_msg.py delete mode 100644 moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp delete mode 100755 moveit_ros/planning_interface/test/test_cleanup.py create mode 100644 moveit_ros/tests/CHANGELOG.rst create mode 100644 moveit_ros/tests/CMakeLists.txt create mode 100644 moveit_ros/tests/include/parameter_name_list.hpp create mode 100644 moveit_ros/tests/launch/move_group_api.test.py create mode 100644 moveit_ros/tests/package.xml create mode 100644 moveit_ros/tests/src/move_group_api_test.cpp diff --git a/.clang-tidy b/.clang-tidy index 83fc26e052..6e9e400699 100644 --- a/.clang-tidy +++ b/.clang-tidy @@ -1,6 +1,7 @@ --- Checks: '-*, performance-*, + -performance-enum-size, llvm-namespace-comment, modernize-redundant-void-arg, modernize-use-nullptr, @@ -21,7 +22,6 @@ Checks: '-*, readability-static-definition-in-anonymous-namespace, ' HeaderFilterRegex: '' -AnalyzeTemporaryDtors: false CheckOptions: - key: llvm-namespace-comment.ShortNamespaceLines value: '10' diff --git a/.docker/ci-testing/Dockerfile b/.docker/ci-testing/Dockerfile index 45c18e07d6..99e89cba4b 100644 --- a/.docker/ci-testing/Dockerfile +++ b/.docker/ci-testing/Dockerfile @@ -1,4 +1,4 @@ -# ghcr.io/ros-planning/moveit2:${OUR_ROS_DISTRO}-ci-testing +# ghcr.io/moveit/moveit2:${OUR_ROS_DISTRO}-ci-testing # CI image using the ROS testing repository FROM osrf/ros2:testing diff --git a/.docker/ci/Dockerfile b/.docker/ci/Dockerfile index c6aa5ee42c..827bae17d9 100644 --- a/.docker/ci/Dockerfile +++ b/.docker/ci/Dockerfile @@ -1,4 +1,4 @@ -# ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-ci +# ghcr.io/moveit/moveit2:${ROS_DISTRO}-ci # ROS base image augmented with all MoveIt dependencies to use for CI ARG ROS_DISTRO=rolling diff --git a/.docker/release/Dockerfile b/.docker/release/Dockerfile index 8a1b6992c4..0c02814cbe 100644 --- a/.docker/release/Dockerfile +++ b/.docker/release/Dockerfile @@ -1,4 +1,4 @@ -# ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-release +# ghcr.io/moveit/moveit2:${ROS_DISTRO}-release # Full debian-based install of MoveIt using apt-get ARG ROS_DISTRO=rolling diff --git a/.docker/source/Dockerfile b/.docker/source/Dockerfile index 1c6a0dce38..bf2c845071 100644 --- a/.docker/source/Dockerfile +++ b/.docker/source/Dockerfile @@ -1,10 +1,10 @@ # syntax = docker/dockerfile:1.3 -# ghcr.io/ros-planning/moveit2:${ROS_DISTRO}-source +# ghcr.io/moveit/moveit2:${ROS_DISTRO}-source # Downloads the moveit source code and install remaining debian dependencies ARG ROS_DISTRO=rolling -FROM moveit/moveit2:${ROS_DISTRO}-ci-testing +FROM moveit/moveit2:${ROS_DISTRO}-ci LABEL maintainer Robert Haschke rhaschke@techfak.uni-bielefeld.de # Export ROS_UNDERLAY for downstream docker containers diff --git a/.docker/tutorial-source/Dockerfile b/.docker/tutorial-source/Dockerfile index 325949b2ac..e4f875e91b 100644 --- a/.docker/tutorial-source/Dockerfile +++ b/.docker/tutorial-source/Dockerfile @@ -1,6 +1,6 @@ # syntax = docker/dockerfile:1.3 -# ghcr.io/ros-planning/moveit2:main-${ROS_DISTRO}-tutorial-source +# ghcr.io/moveit/moveit2:main-${ROS_DISTRO}-tutorial-source # Source build of the repos file from the tutorail site ARG ROS_DISTRO=rolling @@ -20,7 +20,7 @@ RUN --mount=type=cache,target=/root/.ccache/,sharing=locked \ # Enable ccache PATH=/usr/lib/ccache:$PATH && \ # Checkout the tutorial repo - git clone https://github.com/ros-planning/moveit2_tutorials src/moveit2_tutorials && \ + git clone https://github.com/moveit/moveit2_tutorials src/moveit2_tutorials && \ # Fetch required upstream sources for building vcs import --skip-existing src < src/moveit2_tutorials/moveit2_tutorials.repos && \ # Source ROS install diff --git a/.github/ISSUE_TEMPLATE/first_timers_only.md b/.github/ISSUE_TEMPLATE/first_timers_only.md index 10f1b1acac..ecd89f9455 100644 --- a/.github/ISSUE_TEMPLATE/first_timers_only.md +++ b/.github/ISSUE_TEMPLATE/first_timers_only.md @@ -18,7 +18,7 @@ We're interested in helping you take the first step, and can answer questions an We know that creating a pull request is the biggest barrier for new contributors. This issue is for you 💝 -If you have contributed before, **consider leaving this PR for someone new**, and looking through our general [bug](https://github.com/ros-planning/moveit2/labels/bug) issues. Thanks! +If you have contributed before, **consider leaving this PR for someone new**, and looking through our general [bug](https://github.com/moveit/moveit2/labels/bug) issues. Thanks! ### 🤔 What you will need to know. diff --git a/.github/PULL_REQUEST_TEMPLATE.md b/.github/PULL_REQUEST_TEMPLATE.md index 65a7329b72..e073217871 100644 --- a/.github/PULL_REQUEST_TEMPLATE.md +++ b/.github/PULL_REQUEST_TEMPLATE.md @@ -5,9 +5,9 @@ Please explain the changes you made, including a reference to the related issue ### Checklist - [ ] **Required by CI**: Code is auto formatted using [clang-format](http://moveit.ros.org/documentation/contributing/code) - [ ] Extend the tutorials / documentation [reference](http://moveit.ros.org/documentation/contributing/) -- [ ] Document API changes relevant to the user in the [MIGRATION.md](https://github.com/ros-planning/moveit2/blob/main/MIGRATION.md) notes +- [ ] Document API changes relevant to the user in the [MIGRATION.md](https://github.com/moveit/moveit2/blob/main/MIGRATION.md) notes - [ ] Create tests, which fail without this PR [reference](https://moveit.picknik.ai/humble/doc/examples/tests/tests_tutorial.html) - [ ] Include a screenshot if changing a GUI -- [ ] While waiting for someone to review your request, please help review [another open pull request](https://github.com/ros-planning/moveit2/pulls) to support the maintainers +- [ ] While waiting for someone to review your request, please help review [another open pull request](https://github.com/moveit/moveit2/pulls) to support the maintainers [//]: # "You can expect a response from a maintainer within 7 days. If you haven't heard anything by then, feel free to ping the thread. Thank you!" diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 05dbc402f6..9dd1dca56a 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -6,6 +6,7 @@ name: CI on: workflow_dispatch: pull_request: + merge_group: push: branches: - main @@ -17,9 +18,9 @@ jobs: matrix: env: - IMAGE: rolling-ci - CCOV: true + CCOV: false # Disabled: https://github.com/moveit/moveit2/issues/2866 ROS_DISTRO: rolling - - IMAGE: rolling-ci-testing + - IMAGE: rolling-ci ROS_DISTRO: rolling IKFAST_TEST: true CLANG_TIDY: pedantic @@ -27,6 +28,8 @@ jobs: ROS_DISTRO: humble - IMAGE: humble-ci-testing ROS_DISTRO: humble + - IMAGE: jazzy-ci + ROS_DISTRO: jazzy env: CXXFLAGS: >- -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls @@ -60,6 +63,7 @@ jobs: (cd $TARGET_REPO_PATH; clang-tidy --list-checks) # Disable clang-tidy for ikfast plugins as we cannot fix the generated code find $BASEDIR/target_ws/build -iwholename "*_ikfast_plugin/compile_commands.json" -exec rm {} \; + find $BASEDIR/target_ws/build -iwholename "*_ikfast_manipulator_plugin/compile_commands.json" -exec rm {} \; CC: ${{ matrix.env.CLANG_TIDY && 'clang' }} CXX: ${{ matrix.env.CLANG_TIDY && 'clang++' }} ADDITIONAL_DEBS: lld @@ -82,10 +86,12 @@ jobs: sudo rm -rf /usr/local df -h - uses: actions/checkout@v4 - - uses: testspace-com/setup-testspace@v1 - if: github.repository == 'ros-planning/moveit2' - with: - domain: ros-planning +# NOTE: Testspace is temporarily disabled and needs to be installed for the MoveIt org +# See: https://github.com/moveit/moveit2/issues/2852 +# - uses: testspace-com/setup-testspace@v1 +# if: github.repository == 'moveit/moveit2' +# with: +# domain: moveit - name: Get latest release date for rosdistro id: rosdistro_release_date uses: JafarAbdi/latest-rosdistro-release-date-action@main @@ -97,32 +103,35 @@ jobs: with: file: moveit2.repos - name: Cache upstream workspace - uses: pat-s/always-upload-cache@v3.0.11 + uses: actions/cache@v4 with: path: ${{ env.BASEDIR }}/upstream_ws key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} + save-always: true env: CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-${{ matrix.env.IMAGE }}-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }} # The target directory cache doesn't include the source directory because # that comes from the checkout. See "prepare target_ws for cache" task below - name: Cache target workspace if: "!matrix.env.CCOV" - uses: pat-s/always-upload-cache@v3.0.11 + uses: actions/cache@v4 with: path: ${{ env.BASEDIR }}/target_ws key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} + save-always: true env: CACHE_PREFIX: target_ws${{ matrix.env.CCOV && '-ccov' || '' }}-${{ matrix.env.IMAGE }}-${{ hashFiles('**/CMakeLists.txt', '**/package.xml', '.github/workflows/ci.yaml') }} - name: Cache ccache - uses: pat-s/always-upload-cache@v3.0.11 + uses: actions/cache@v4 with: path: ${{ env.CCACHE_DIR }} key: ${{ env.CACHE_PREFIX }}-${{ github.sha }}-${{ github.run_id }} restore-keys: | ${{ env.CACHE_PREFIX }}-${{ github.sha }} ${{ env.CACHE_PREFIX }} + save-always: true env: CACHE_PREFIX: ccache-${{ matrix.env.IMAGE }}${{ matrix.env.CCOV && '-ccov' || '' }} - name: Configure ccache @@ -136,10 +145,12 @@ jobs: name: Run industrial_ci uses: ros-industrial/industrial_ci@master env: ${{ matrix.env }} - - name: Push result to Testspace - if: always() && (github.repository == 'ros-planning/moveit2') - run: | - testspace "[ ${{ matrix.env.IMAGE }} ]${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml" +# NOTE: Testspace is temporarily disabled and needs to be installed for the MoveIt org +# See: https://github.com/moveit/moveit2/issues/2852 +# - name: Push result to Testspace +# if: always() && (github.repository == 'moveit/moveit2') +# run: | +# testspace "[ ${{ matrix.env.IMAGE }} ]${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml" - name: Upload test artifacts (on failure) uses: actions/upload-artifact@v4 if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results) @@ -154,7 +165,7 @@ jobs: workdir: ${{ env.BASEDIR }}/target_ws ignore: '"*/target_ws/build/*" "*/target_ws/install/*" "*/test/*"' - name: Upload codecov report - uses: codecov/codecov-action@v3 + uses: codecov/codecov-action@v4 if: always() && matrix.env.CCOV && steps.ici.outputs.target_test_results == '0' with: files: ${{ env.BASEDIR }}/target_ws/coverage.info diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml index f1f0090512..f597653f9d 100644 --- a/.github/workflows/docker.yaml +++ b/.github/workflows/docker.yaml @@ -19,17 +19,27 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [rolling, jazzy] runs-on: ubuntu-latest permissions: packages: write contents: read env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} + GH_IMAGE: ghcr.io/moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} steps: + - uses: rhaschke/docker-run-action@v5 + name: Check for apt updates + continue-on-error: true + id: apt + with: + image: ${{ env.IMAGE }} + run: | + apt-get update + have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true) + echo "no_cache=$have_updates" >> "$GITHUB_OUTPUT" - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 - name: Login to Github Container Registry @@ -51,7 +61,9 @@ jobs: file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} push: ${{ env.PUSH }} - no-cache: true + no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }} + cache-from: type=registry,ref=${{ env.GH_IMAGE }} + cache-to: type=inline tags: | ${{ env.GH_IMAGE }} ${{ env.DH_IMAGE }} @@ -60,58 +72,27 @@ jobs: strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [rolling, jazzy] runs-on: ubuntu-latest permissions: packages: write contents: read env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} + GH_IMAGE: ghcr.io/moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} steps: - - name: Set up Docker Buildx - uses: docker/setup-buildx-action@v3 - - name: Login to Github Container Registry - if: env.PUSH == 'true' - uses: docker/login-action@v3 + - uses: rhaschke/docker-run-action@v5 + name: Check for apt updates + continue-on-error: true + id: apt with: - registry: ghcr.io - username: ${{ github.repository_owner }} - password: ${{ secrets.GITHUB_TOKEN }} - - name: Login to DockerHub - if: env.PUSH == 'true' - uses: docker/login-action@v3 - with: - username: ${{ secrets.DOCKERHUB_USERNAME }} - password: ${{ secrets.DOCKERHUB_TOKEN }} - - name: Build and Push - uses: docker/build-push-action@v5 - with: - file: .docker/${{ github.job }}/Dockerfile - build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} - push: ${{ env.PUSH }} - no-cache: true - tags: | - ${{ env.GH_IMAGE }} - ${{ env.DH_IMAGE }} - - ci-testing: - strategy: - fail-fast: false - matrix: - ROS_DISTRO: [rolling] - runs-on: ubuntu-latest - permissions: - packages: write - contents: read - env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} - - steps: + image: ${{ env.IMAGE }} + run: | + apt-get update + have_updates=$(apt-get --simulate upgrade | grep -q "^0 upgraded, 0 newly installed, 0 to remove and 0 not upgraded.$" && echo false || echo true) + echo "no_cache=$have_updates" >> "$GITHUB_OUTPUT" - name: Set up Docker Buildx uses: docker/setup-buildx-action@v3 - name: Login to Github Container Registry @@ -131,27 +112,31 @@ jobs: uses: docker/build-push-action@v5 with: file: .docker/${{ github.job }}/Dockerfile - build-args: OUR_ROS_DISTRO=${{ matrix.ROS_DISTRO }} + build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} push: ${{ env.PUSH }} - no-cache: true + no-cache: ${{ steps.apt.outputs.no_cache || github.event_name == 'workflow_dispatch' }} + cache-from: type=registry,ref=${{ env.GH_IMAGE }} + cache-to: type=inline tags: | ${{ env.GH_IMAGE }} + ${{ env.GH_IMAGE }}-testing ${{ env.DH_IMAGE }} + ${{ env.DH_IMAGE }}-testing source: - needs: ci-testing + needs: ci strategy: fail-fast: false matrix: - ROS_DISTRO: [rolling] + ROS_DISTRO: [rolling, jazzy] runs-on: ubuntu-latest permissions: packages: write contents: read env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} + GH_IMAGE: ghcr.io/moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} DH_IMAGE: moveit/moveit2:${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} steps: - uses: actions/checkout@v4 @@ -179,7 +164,8 @@ jobs: file: .docker/${{ github.job }}/Dockerfile build-args: ROS_DISTRO=${{ matrix.ROS_DISTRO }} push: ${{ env.PUSH }} - no-cache: true + cache-from: type=registry,ref=${{ env.GH_IMAGE }} + cache-to: type=inline tags: | ${{ env.GH_IMAGE }} ${{ env.DH_IMAGE }} @@ -191,7 +177,7 @@ jobs: - source steps: - name: Delete Untagged Images - if: (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') + if: (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') uses: actions/github-script@v7 with: github-token: ${{ secrets.DELETE_PACKAGES_TOKEN }} @@ -207,6 +193,6 @@ jobs: } } env: - OWNER: ros-planning + OWNER: moveit PACKAGE_NAME: moveit2 PER_PAGE: 100 diff --git a/.github/workflows/format.yaml b/.github/workflows/format.yaml index b8c178f046..b55c526b64 100644 --- a/.github/workflows/format.yaml +++ b/.github/workflows/format.yaml @@ -6,6 +6,7 @@ name: Formatting (pre-commit) on: workflow_dispatch: pull_request: + merge_group: push: branches: - main @@ -21,7 +22,7 @@ jobs: python-version: '3.10' - name: Install clang-format-14 run: sudo apt-get install clang-format-14 - - uses: pre-commit/action@v3.0.0 + - uses: pre-commit/action@v3.0.1 id: precommit - name: Upload pre-commit changes if: failure() && steps.precommit.outcome == 'failure' diff --git a/.github/workflows/sonar.yaml b/.github/workflows/sonar.yaml index 281894af23..083d6c07ca 100644 --- a/.github/workflows/sonar.yaml +++ b/.github/workflows/sonar.yaml @@ -56,10 +56,11 @@ jobs: sudo rm -rf /usr/local df -h - uses: actions/checkout@v4 - - uses: testspace-com/setup-testspace@v1 - if: github.repository == 'ros-planning/moveit2' - with: - domain: ros-planning +# Testspace disabled temporarily: https://github.com/moveit/moveit2/issues/2852 +# - uses: testspace-com/setup-testspace@v1 +# if: github.repository == 'moveit/moveit2' +# with: +# domain: ros-planning - name: Get latest release date for rosdistro id: rosdistro_release_date uses: JafarAbdi/latest-rosdistro-release-date-action@main @@ -71,21 +72,23 @@ jobs: with: file: moveit2.repos - name: Cache upstream workspace - uses: pat-s/always-upload-cache@v3.0.11 + uses: actions/cache@v4 with: path: ${{ env.BASEDIR }}/upstream_ws key: ${{ env.CACHE_PREFIX }}-${{ github.run_id }} restore-keys: ${{ env.CACHE_PREFIX }} + save-always: true env: CACHE_PREFIX: ${{ steps.rosdistro_release_date.outputs.date }}-upstream_ws-${{ steps.repos_edit_timestamp.outputs.timestamp }}-rolling-ci-${{ hashFiles('moveit2*.repos', '.github/workflows/ci.yaml') }} - name: Cache ccache - uses: pat-s/always-upload-cache@v3.0.11 + uses: actions/cache@v4 with: path: ${{ env.CCACHE_DIR }} key: ccache-rolling-ci-ccov-${{ github.sha }}-${{ github.run_id }} restore-keys: | ccache-rolling-ci-ccov-${{ github.sha }} ccache-rolling-ci-ccov + save-always: true - name: Configure ccache run: | mkdir -p ${{ env.CCACHE_DIR }} diff --git a/.github/workflows/tutorial_docker.yaml b/.github/workflows/tutorial_docker.yaml index 482d8c2501..a07f55d707 100644 --- a/.github/workflows/tutorial_docker.yaml +++ b/.github/workflows/tutorial_docker.yaml @@ -6,6 +6,7 @@ on: - cron: '0 17 * * 6' workflow_dispatch: pull_request: + merge_group: push: branches: - main @@ -21,9 +22,9 @@ jobs: packages: write contents: read env: - GH_IMAGE: ghcr.io/ros-planning/moveit2:main-${{ matrix.ROS_DISTRO }}-${{ github.job }} + GH_IMAGE: ghcr.io/moveit/moveit2:main-${{ matrix.ROS_DISTRO }}-${{ github.job }} DH_IMAGE: moveit/moveit2:main-${{ matrix.ROS_DISTRO }}-${{ github.job }} - PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') }} + PUSH: ${{ (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') }} steps: - uses: actions/checkout@v4 @@ -45,12 +46,12 @@ jobs: - name: "Remove .dockerignore" run: rm .dockerignore # enforce full source context - name: Cache ccache - uses: actions/cache@v3 + uses: actions/cache@v4 with: path: .ccache key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }} - name: inject ccache into docker - uses: reproducible-containers/buildkit-cache-dance@v2.1.3 + uses: reproducible-containers/buildkit-cache-dance@v3.1.0 with: cache-source: .ccache cache-target: /root/.ccache/ @@ -73,7 +74,7 @@ jobs: - tutorial-source steps: - name: Delete Untagged Images - if: (github.event_name != 'pull_request') && (github.repository == 'ros-planning/moveit2') + if: (github.event_name != 'pull_request') && (github.repository == 'moveit/moveit2') uses: actions/github-script@v7 with: github-token: ${{ secrets.DELETE_PACKAGES_TOKEN }} @@ -89,6 +90,6 @@ jobs: } } env: - OWNER: ros-planning + OWNER: moveit PACKAGE_NAME: moveit2 PER_PAGE: 100 diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml index b18718e8cc..129be497ef 100644 --- a/.pre-commit-config.yaml +++ b/.pre-commit-config.yaml @@ -52,3 +52,12 @@ repos: - id: codespell args: ['--write-changes', '--ignore-words=.codespell_words', '--skip="*.eps"'] exclude: CHANGELOG.rst + + - repo: https://github.com/cheshirekow/cmake-format-precommit + rev: v0.6.10 + hooks: + - id: cmake-format + - id: cmake-lint + args: + - "--disabled-codes=C0301" # Disable Line too long lint + - "--suppress-decorations" diff --git a/README.md b/README.md index efc43c2afa..fa3d75cc09 100644 --- a/README.md +++ b/README.md @@ -1,13 +1,13 @@ MoveIt Logo -The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the ROS 1 repository see [MoveIt 1](https://github.com/ros-planning/moveit). +The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the ROS 1 repository see [MoveIt 1](https://github.com/moveit/moveit). For the commercially supported version see [MoveIt Pro](http://picknik.ai/pro). *Easy-to-use open source robotics manipulation platform for developing commercial applications, prototyping designs, and benchmarking algorithms.* ## Continuous Integration Status -[![Formatting (pre-commit)](https://github.com/ros-planning/moveit2/actions/workflows/format.yaml/badge.svg?branch=main)](https://github.com/ros-planning/moveit2/actions/workflows/format.yaml?query=branch%3Amain) -[![CI (Rolling and Humble)](https://github.com/ros-planning/moveit2/actions/workflows/ci.yaml/badge.svg?branch=main)](https://github.com/ros-planning/moveit2/actions/workflows/ci.yaml?query=branch%3Amain) +[![Formatting (pre-commit)](https://github.com/moveit/moveit2/actions/workflows/format.yaml/badge.svg?branch=main)](https://github.com/moveit/moveit2/actions/workflows/format.yaml?query=branch%3Amain) +[![CI (Rolling and Humble)](https://github.com/moveit/moveit2/actions/workflows/ci.yaml/badge.svg?branch=main)](https://github.com/moveit/moveit2/actions/workflows/ci.yaml?query=branch%3Amain) [![Code Coverage](https://codecov.io/gh/ros-planning/moveit2/branch/main/graph/badge.svg?token=W7uHKcY0ly)](https://codecov.io/gh/ros-planning/moveit2) ## Getting Started @@ -56,47 +56,42 @@ research and innovation programme under grant agreement no. 732287. See [How To Generate API Doxygen Reference Locally](https://moveit.picknik.ai/main/doc/how_to_guides/how_to_generate_api_doxygen_locally.html). # Buildfarm -| Package | Humble Binary | Iron Binary | Rolling Binary | -|:---:|:---:|:---:|:---:| -| geometric_shapes | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | -| moveit | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit__ubuntu_jammy_amd64__binary) | -| moveit_common | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_common__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | -| moveit_core | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_core__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | -| moveit_hybrid_planning | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | -| moveit_kinematics | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | -| moveit_msgs | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | -| moveit_planners | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | -| moveit_planners_ompl | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | -| moveit_planners_stomp | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | -| moveit_plugins | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | -| moveit_resources | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | -| moveit_resources_fanuc_description | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_fanuc_description__ubuntu_jammy_amd64__binary) | -| moveit_resources_fanuc_moveit_config | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_fanuc_moveit_config__ubuntu_jammy_amd64__binary) | -| moveit_resources_panda_description | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_panda_description__ubuntu_jammy_amd64__binary) | -| moveit_resources_panda_moveit_config | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_panda_moveit_config__ubuntu_jammy_amd64__binary) | -| moveit_resources_pr2_description | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_resources_pr2_description__ubuntu_jammy_amd64__binary) | -| moveit_ros | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | -| moveit_ros_benchmarks | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | -| moveit_ros_move_group | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | -| moveit_ros_occupancy_map_monitor | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | -| moveit_ros_perception | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | -| moveit_ros_planning | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | -| moveit_ros_planning_interface | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | -| moveit_ros_robot_interaction | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | -| moveit_ros_visualization | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | -| moveit_ros_warehouse | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | -| moveit_runtime | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | -| moveit_servo | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | -| moveit_setup_app_plugins | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | -| moveit_setup_assistant | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | -| moveit_setup_controllers | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | -| moveit_setup_core_plugins | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | -| moveit_setup_framework | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | -| moveit_setup_srdf_plugins | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | -| moveit_simple_controller_manager | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | -| pilz_industrial_motion_planner | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | -| pilz_industrial_motion_planner_testutils | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | -| random_numbers | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__random_numbers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | -| srdfdom | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__srdfdom__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | -| warehouse_ros | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | -| warehouse_ros_sqlite | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | +| Package | Humble Binary | Iron Binary | Rolling Binary | Jazzy Binary | +|:---:|:---:|:---:|:---:|:---:| +| geometric_shapes | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__geometric_shapes__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__geometric_shapes__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__geometric_shapes__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__geometric_shapes__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__geometric_shapes__ubuntu_noble_amd64__binary) | +| moveit | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit__ubuntu_noble_amd64__binary) | +| moveit_common | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_common__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_common__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_common__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_common__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_common__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_common__ubuntu_noble_amd64__binary) | +| moveit_core | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_core__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_core__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_core__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_core__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_core__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_core__ubuntu_noble_amd64__binary) | +| moveit_hybrid_planning | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_hybrid_planning__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_hybrid_planning__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_hybrid_planning__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_hybrid_planning__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_hybrid_planning__ubuntu_noble_amd64__binary) | +| moveit_kinematics | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_kinematics__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_kinematics__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_kinematics__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_kinematics__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_kinematics__ubuntu_noble_amd64__binary) | +| moveit_msgs | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_msgs__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_msgs__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_msgs__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_msgs__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_msgs__ubuntu_noble_amd64__binary) | +| moveit_planners | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_planners__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_planners__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_planners__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_planners__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_planners__ubuntu_noble_amd64__binary) | +| moveit_planners_ompl | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_planners_ompl__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_planners_ompl__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_planners_ompl__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_planners_ompl__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_planners_ompl__ubuntu_noble_amd64__binary) | +| moveit_planners_stomp | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_planners_stomp__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_planners_stomp__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_planners_stomp__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_planners_stomp__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_planners_stomp__ubuntu_noble_amd64__binary) | +| moveit_plugins | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_plugins__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_plugins__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_plugins__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_plugins__ubuntu_noble_amd64__binary) | +| moveit_resources | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_resources__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_resources__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_resources__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_resources__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_resources__ubuntu_noble_amd64__binary) | +| moveit_ros | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros__ubuntu_noble_amd64__binary) | +| moveit_ros_benchmarks | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_benchmarks__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_benchmarks__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_benchmarks__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_benchmarks__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_benchmarks__ubuntu_noble_amd64__binary) | +| moveit_ros_move_group | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_move_group__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_move_group__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_move_group__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_move_group__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_move_group__ubuntu_noble_amd64__binary) | +| moveit_ros_occupancy_map_monitor | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_occupancy_map_monitor__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_occupancy_map_monitor__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_occupancy_map_monitor__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_occupancy_map_monitor__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_occupancy_map_monitor__ubuntu_noble_amd64__binary) | +| moveit_ros_perception | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_perception__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_perception__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_perception__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_perception__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_perception__ubuntu_noble_amd64__binary) | +| moveit_ros_planning | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_planning__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_planning__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_planning__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_planning__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_planning__ubuntu_noble_amd64__binary) | +| moveit_ros_planning_interface | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_planning_interface__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_planning_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_planning_interface__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_planning_interface__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_planning_interface__ubuntu_noble_amd64__binary) | +| moveit_ros_robot_interaction | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_robot_interaction__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_robot_interaction__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_robot_interaction__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_robot_interaction__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_robot_interaction__ubuntu_noble_amd64__binary) | +| moveit_ros_visualization | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_visualization__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_visualization__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_visualization__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_visualization__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_visualization__ubuntu_noble_amd64__binary) | +| moveit_ros_warehouse | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_ros_warehouse__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_ros_warehouse__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_ros_warehouse__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_ros_warehouse__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_ros_warehouse__ubuntu_noble_amd64__binary) | +| moveit_runtime | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_runtime__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_runtime__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_runtime__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_runtime__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_runtime__ubuntu_noble_amd64__binary) | +| moveit_servo | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_servo__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_servo__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_servo__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_servo__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_servo__ubuntu_noble_amd64__binary) | +| moveit_setup_app_plugins | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_app_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_setup_app_plugins__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_setup_app_plugins__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_setup_app_plugins__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_setup_app_plugins__ubuntu_noble_amd64__binary) | +| moveit_setup_assistant | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_assistant__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_setup_assistant__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_setup_assistant__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_setup_assistant__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_setup_assistant__ubuntu_noble_amd64__binary) | +| moveit_setup_controllers | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_controllers__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_setup_controllers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_setup_controllers__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_setup_controllers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_setup_controllers__ubuntu_noble_amd64__binary) | +| moveit_setup_core_plugins | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_core_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_setup_core_plugins__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_setup_core_plugins__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_setup_core_plugins__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_setup_core_plugins__ubuntu_noble_amd64__binary) | +| moveit_setup_framework | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_framework__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_setup_framework__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_setup_framework__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_setup_framework__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_setup_framework__ubuntu_noble_amd64__binary) | +| moveit_setup_srdf_plugins | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_setup_srdf_plugins__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_setup_srdf_plugins__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_setup_srdf_plugins__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_setup_srdf_plugins__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_setup_srdf_plugins__ubuntu_noble_amd64__binary) | +| moveit_simple_controller_manager | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__moveit_simple_controller_manager__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__moveit_simple_controller_manager__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__moveit_simple_controller_manager__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__moveit_simple_controller_manager__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__moveit_simple_controller_manager__ubuntu_noble_amd64__binary) | +| pilz_industrial_motion_planner | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__pilz_industrial_motion_planner__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__pilz_industrial_motion_planner__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__pilz_industrial_motion_planner__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__pilz_industrial_motion_planner__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__pilz_industrial_motion_planner__ubuntu_noble_amd64__binary) | +| pilz_industrial_motion_planner_testutils | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__pilz_industrial_motion_planner_testutils__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__pilz_industrial_motion_planner_testutils__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__pilz_industrial_motion_planner_testutils__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__pilz_industrial_motion_planner_testutils__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__pilz_industrial_motion_planner_testutils__ubuntu_noble_amd64__binary) | +| random_numbers | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__random_numbers__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__random_numbers__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__random_numbers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__random_numbers__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__random_numbers__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__random_numbers__ubuntu_noble_amd64__binary) | +| srdfdom | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__srdfdom__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__srdfdom__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__srdfdom__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__srdfdom__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__srdfdom__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__srdfdom__ubuntu_noble_amd64__binary) | +| warehouse_ros | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__warehouse_ros__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__warehouse_ros__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__warehouse_ros__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__warehouse_ros__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__warehouse_ros__ubuntu_noble_amd64__binary) | +| warehouse_ros_sqlite | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Hbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Hbin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Ibin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary)](https://build.ros2.org/job/Ibin_uJ64__warehouse_ros_sqlite__ubuntu_jammy_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Rbin_uN64__warehouse_ros_sqlite__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Rbin_uN64__warehouse_ros_sqlite__ubuntu_noble_amd64__binary) | [![Build Status](https://build.ros2.org/buildStatus/icon?job=Jbin_uN64__warehouse_ros_sqlite__ubuntu_noble_amd64__binary)](https://build.ros2.org/job/Jbin_uN64__warehouse_ros_sqlite__ubuntu_noble_amd64__binary) | diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst index 0e984538ce..a20ae58929 100644 --- a/moveit/CHANGELOG.rst +++ b/moveit/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit/CMakeLists.txt b/moveit/CMakeLists.txt index 6ed3f1dc2d..8edc48c14a 100644 --- a/moveit/CMakeLists.txt +++ b/moveit/CMakeLists.txt @@ -2,10 +2,4 @@ cmake_minimum_required(VERSION 3.22) project(moveit) find_package(ament_cmake REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - set(ament_cmake_flake8_FOUND TRUE) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/moveit/package.xml b/moveit/package.xml index bb055d43ed..d87906c7c4 100644 --- a/moveit/package.xml +++ b/moveit/package.xml @@ -2,7 +2,7 @@ moveit - 2.9.0 + 2.10.0 Meta package that contains all essential packages of MoveIt 2 Henning Kayser Tyler Weaver @@ -12,8 +12,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2 - https://github.com/ros-planning/moveit2/issues + https://github.com/moveit/moveit2 + https://github.com/moveit/moveit2/issues Ioan Sucan Sachin Chitta @@ -30,10 +30,6 @@ moveit_ros moveit_setup_assistant - - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit/scripts/create_maintainer_table.py b/moveit/scripts/create_maintainer_table.py index c1d1ca0a44..2cd7999fd0 100644 --- a/moveit/scripts/create_maintainer_table.py +++ b/moveit/scripts/create_maintainer_table.py @@ -161,7 +161,7 @@ def get_first_folder(path): def populate_package_data(path, package): output = ( - "" + package.name diff --git a/moveit2.repos b/moveit2.repos index 866f7ccdc7..aec6f2851e 100644 --- a/moveit2.repos +++ b/moveit2.repos @@ -1,10 +1,14 @@ repositories: # Keep moveit_* repos here because they are released with moveit + geometric_shapes: + type: git + url: https://github.com/moveit/geometric_shapes.git + version: ros2 moveit_msgs: type: git - url: https://github.com/ros-planning/moveit_msgs.git + url: https://github.com/moveit/moveit_msgs.git version: ros2 moveit_resources: type: git - url: https://github.com/ros-planning/moveit_resources.git + url: https://github.com/moveit/moveit_resources.git version: ros2 diff --git a/moveit2_humble.repos b/moveit2_humble.repos new file mode 100644 index 0000000000..71a9316f22 --- /dev/null +++ b/moveit2_humble.repos @@ -0,0 +1,5 @@ +repositories: + generate_parameter_library: + type: git + url: https://github.com/PickNikRobotics/generate_parameter_library.git + version: 0.3.7 diff --git a/moveit2_iron.repos b/moveit2_iron.repos new file mode 100644 index 0000000000..71a9316f22 --- /dev/null +++ b/moveit2_iron.repos @@ -0,0 +1,5 @@ +repositories: + generate_parameter_library: + type: git + url: https://github.com/PickNikRobotics/generate_parameter_library.git + version: 0.3.7 diff --git a/moveit_common/CHANGELOG.rst b/moveit_common/CHANGELOG.rst index 4c9f32767a..3817d0958d 100644 --- a/moveit_common/CHANGELOG.rst +++ b/moveit_common/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_common ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit_common/CMakeLists.txt b/moveit_common/CMakeLists.txt index 825539f025..c263c6d363 100644 --- a/moveit_common/CMakeLists.txt +++ b/moveit_common/CMakeLists.txt @@ -3,16 +3,6 @@ project(moveit_common NONE) find_package(ament_cmake REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() +ament_package(CONFIG_EXTRAS "moveit_common-extras.cmake") -ament_package( - CONFIG_EXTRAS "moveit_common-extras.cmake" -) - -install( - DIRECTORY cmake - DESTINATION share/moveit_common -) +install(DIRECTORY cmake DESTINATION share/moveit_common) diff --git a/moveit_common/cmake/moveit_package.cmake b/moveit_common/cmake/moveit_package.cmake index 332334d5f1..0906b4726c 100644 --- a/moveit_common/cmake/moveit_package.cmake +++ b/moveit_common/cmake/moveit_package.cmake @@ -3,31 +3,30 @@ # 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 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. +# * 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 the 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. +# * Neither the name of the 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 HOLDER 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. +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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. - -macro(moveit_package) +# Module for common settings in MoveIt packages. +macro(MOVEIT_PACKAGE) # Set ${PROJECT_NAME}_VERSION find_package(ament_cmake REQUIRED) ament_package_xml() @@ -43,8 +42,16 @@ macro(moveit_package) if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") # Enable warnings - add_compile_options(-Wall -Wextra - -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual -Wold-style-cast -Wformat=2) + add_compile_options( + -Wall + -Wextra + -Wwrite-strings + -Wunreachable-code + -Wpointer-arith + -Wredundant-decls + -Wcast-qual + -Wold-style-cast + -Wformat=2) else() # Defaults for Microsoft C++ compiler add_compile_options(/W3 /wd4251 /wd4068 /wd4275) @@ -54,27 +61,41 @@ macro(moveit_package) # Enable Math Constants # https://docs.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=vs-2019 - add_compile_definitions( - _USE_MATH_DEFINES - ) + add_compile_definitions(_USE_MATH_DEFINES) endif() option(MOVEIT_CI_WARNINGS "Enable all warnings used by CI" ON) if(MOVEIT_CI_WARNINGS) if(CMAKE_CXX_COMPILER_ID STREQUAL "GNU") - add_compile_options(-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual) + add_compile_options( + -Wall + -Wextra + -Wwrite-strings + -Wunreachable-code + -Wpointer-arith + -Wredundant-decls + -Wcast-qual) # This too often has false-positives add_compile_options(-Wno-maybe-uninitialized) endif() if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") - add_compile_options(-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wcast-qual) + add_compile_options( + -Wall + -Wextra + -Wwrite-strings + -Wunreachable-code + -Wpointer-arith + -Wredundant-decls + -Wcast-qual) endif() endif() set(CMAKE_EXPORT_COMPILE_COMMANDS ON) if(NOT CMAKE_CONFIGURATION_TYPES AND NOT CMAKE_BUILD_TYPE) - message("${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance") + message( + "${PROJECT_NAME}: You did not request a specific build type: Choosing 'Release' for maximum performance" + ) set(CMAKE_BUILD_TYPE Release) endif() endmacro() diff --git a/moveit_common/moveit_common-extras.cmake b/moveit_common/moveit_common-extras.cmake index b2fc2d01a0..08e01eb31b 100644 --- a/moveit_common/moveit_common-extras.cmake +++ b/moveit_common/moveit_common-extras.cmake @@ -3,28 +3,26 @@ # 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 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. +# * 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 the 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. +# * Neither the name of the 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 HOLDER 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. - +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE +# DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER 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. include("${moveit_common_DIR}/moveit_package.cmake") diff --git a/moveit_common/package.xml b/moveit_common/package.xml index 2ef95923a0..497a394c9c 100644 --- a/moveit_common/package.xml +++ b/moveit_common/package.xml @@ -2,7 +2,7 @@ moveit_common - 2.9.0 + 2.10.0 Common support functionality used throughout MoveIt Henning Kayser Tyler Weaver @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Lior Lustgarten @@ -20,9 +20,6 @@ backward_ros - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_configs_utils/CHANGELOG.rst b/moveit_configs_utils/CHANGELOG.rst index 076e9b1415..ef4153feb6 100644 --- a/moveit_configs_utils/CHANGELOG.rst +++ b/moveit_configs_utils/CHANGELOG.rst @@ -2,6 +2,25 @@ Changelog for package moveit_configs_utils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Fix xacro args loading issue (`#2684 `_) + * Fixed xacro args loading issue + * Formatting fixes with pre-commit action +* Pass along move_group_capabilities parameters (`#2587 `_) + * Pass along move_group_capabilities parameters + * fix lint check + * Use move_group_capabilities as default launch argument +* CMake format and lint in pre-commit (`#2683 `_) +* Use different packages for launch and config packages in generate_demo_launch (`#2647 `_) +* Contributors: Alex Navarro, Forrest Rogers-Marcovitz, Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Update ros2_control usage (`#2620 `_) diff --git a/moveit_configs_utils/moveit_configs_utils/launches.py b/moveit_configs_utils/moveit_configs_utils/launches.py index 808d520d86..c8e3bb533c 100644 --- a/moveit_configs_utils/moveit_configs_utils/launches.py +++ b/moveit_configs_utils/moveit_configs_utils/launches.py @@ -197,7 +197,12 @@ def generate_move_group_launch(moveit_config): DeclareBooleanLaunchArg("publish_monitored_planning_scene", default_value=True) ) # load non-default MoveGroup capabilities (space separated) - ld.add_action(DeclareLaunchArgument("capabilities", default_value="")) + ld.add_action( + DeclareLaunchArgument( + "capabilities", + default_value=moveit_config.move_group_capabilities["capabilities"], + ) + ) # inhibit these default MoveGroup capabilities (space separated) ld.add_action(DeclareLaunchArgument("disable_capabilities", default_value="")) @@ -244,10 +249,12 @@ def generate_move_group_launch(moveit_config): return ld -def generate_demo_launch(moveit_config): +def generate_demo_launch(moveit_config, launch_package_path=None): """ Launches a self contained demo + launch_package_path is optional to use different launch and config packages + Includes * static_virtual_joint_tfs * robot_state_publisher @@ -256,6 +263,9 @@ def generate_demo_launch(moveit_config): * warehouse_db (optional) * ros2_control_node + controller spawners """ + if launch_package_path == None: + launch_package_path = moveit_config.package_path + ld = LaunchDescription() ld.add_action( DeclareBooleanLaunchArg( @@ -272,11 +282,11 @@ def generate_demo_launch(moveit_config): ) ) ld.add_action(DeclareBooleanLaunchArg("use_rviz", default_value=True)) - # If there are virtual joints, broadcast static tf by including virtual_joints launch virtual_joints_launch = ( - moveit_config.package_path / "launch/static_virtual_joint_tfs.launch.py" + launch_package_path / "launch/static_virtual_joint_tfs.launch.py" ) + if virtual_joints_launch.exists(): ld.add_action( IncludeLaunchDescription( @@ -288,7 +298,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/rsp.launch.py") + str(launch_package_path / "launch/rsp.launch.py") ), ) ) @@ -296,7 +306,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/move_group.launch.py") + str(launch_package_path / "launch/move_group.launch.py") ), ) ) @@ -305,7 +315,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/moveit_rviz.launch.py") + str(launch_package_path / "launch/moveit_rviz.launch.py") ), condition=IfCondition(LaunchConfiguration("use_rviz")), ) @@ -315,7 +325,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/warehouse_db.launch.py") + str(launch_package_path / "launch/warehouse_db.launch.py") ), condition=IfCondition(LaunchConfiguration("db")), ) @@ -338,7 +348,7 @@ def generate_demo_launch(moveit_config): ld.add_action( IncludeLaunchDescription( PythonLaunchDescriptionSource( - str(moveit_config.package_path / "launch/spawn_controllers.launch.py") + str(launch_package_path / "launch/spawn_controllers.launch.py") ), ) ) diff --git a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py index 41e154c25a..04cb077514 100644 --- a/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py +++ b/moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py @@ -173,20 +173,20 @@ def __init__( if setup_assistant_file.exists(): setup_assistant_yaml = load_yaml(setup_assistant_file) config = setup_assistant_yaml.get("moveit_setup_assistant_config", {}) - urdf_config = config.get("urdf", config.get("URDF")) - if urdf_config and self.__urdf_package is None: - self.__urdf_package = Path( - get_package_share_directory(urdf_config["package"]) - ) - self.__urdf_file_path = Path(urdf_config["relative_path"]) - if (xacro_args := urdf_config.get("xacro_args")) is not None: + if urdf_config := config.get("urdf", config.get("URDF")): + if self.__urdf_package is None: + self.__urdf_package = Path( + get_package_share_directory(urdf_config["package"]) + ) + self.__urdf_file_path = Path(urdf_config["relative_path"]) + + if xacro_args := urdf_config.get("xacro_args"): self.__urdf_xacro_args = dict( arg.split(":=") for arg in xacro_args.split(" ") if arg ) - srdf_config = config.get("srdf", config.get("SRDF")) - if srdf_config: + if srdf_config := config.get("srdf", config.get("SRDF")): self.__srdf_file_path = Path(srdf_config["relative_path"]) if not self.__urdf_package or not self.__urdf_file_path: @@ -414,7 +414,7 @@ def sensors_3d(self, file_path: Optional[str] = None): if sensors_path.exists(): sensors_data = load_yaml(sensors_path) # TODO(mikeferguson): remove the second part of this check once - # https://github.com/ros-planning/moveit_resources/pull/141 has made through buildfarm + # https://github.com/moveit/moveit_resources/pull/141 has made through buildfarm if len(sensors_data["sensors"]) > 0 and sensors_data["sensors"][0]: self.__moveit_configs.sensors_3d = sensors_data return self diff --git a/moveit_configs_utils/package.xml b/moveit_configs_utils/package.xml index 05bf3e5619..f72fd7510a 100644 --- a/moveit_configs_utils/package.xml +++ b/moveit_configs_utils/package.xml @@ -2,7 +2,7 @@ moveit_configs_utils - 2.9.0 + 2.10.0 Python library for loading moveit config parameters in launch files MoveIt Release Team BSD-3-Clause @@ -15,9 +15,6 @@ launch_ros srdfdom - ament_lint_auto - ament_lint_common - ament_python diff --git a/moveit_configs_utils/setup.py b/moveit_configs_utils/setup.py index 8007a515af..8f6c23d322 100644 --- a/moveit_configs_utils/setup.py +++ b/moveit_configs_utils/setup.py @@ -5,7 +5,7 @@ setup( name=package_name, - version="2.9.0", + version="2.10.0", packages=find_packages(), data_files=[ ("share/ament_index/resource_index/packages", ["resource/" + package_name]), diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst index d8708fde06..2b4633600b 100644 --- a/moveit_core/CHANGELOG.rst +++ b/moveit_core/CHANGELOG.rst @@ -2,6 +2,49 @@ Changelog for package moveit_core ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Enforce liboctomap-dev by using a cmake version range +* Add utility functions to get limits and trajectory message (`#2861 `_) +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Use std::optional instead of nullptr checking (`#2454 `_) +* Enable mdof trajectory execution (`#2740 `_) + * Add RobotTrajectory conversion from MDOF to joints + * Convert MDOF trajectories to joint trajectories in planning interfaces + * Treat mdof joint variables as common joints in + TrajectoryExecutionManager + * Convert multi-DOF trajectories to joints in TEM + * Revert "Convert MDOF trajectories to joint trajectories in planning interfaces" + This reverts commit 885ee2718594859555b73dc341311a859d31216e. + * Handle multi-DOF variables in TEM's bound checking + * Add parameter to optionally enable multi-dof conversion + * Improve error message about unknown controllers + * Fix name ordering in JointTrajectory conversion + * Improve DEBUG output in TEM + * Comment RobotTrajectory test + * add acceleration to avoid out of bounds read +* Fix doc reference to non-existent function (`#2765 `_) +* (core) Remove unused python docs folder (`#2746 `_) +* Unify log names (`#2720 `_) +* (core) Install collision_detector_fcl_plugin (`#2699 `_) + FCL version of acda563 +* Simplify Isometry multiplication benchmarks (`#2628 `_) + With the benchmark library, there is no need to specify an iteration count. + Interestingly, 4x4 matrix multiplication is faster than affine*matrix +* CMake format and lint in pre-commit (`#2683 `_) +* Merge pull request `#2660 `_ from MarqRazz/pr-fix_model_with_collision + Fix getLinkModelNamesWithCollisionGeometry to include the base link +* validate link has parent +* pre-commit +* Fix getLinkModelNamesWithCollisionGeometry to include the base link of the planning group +* Acceleration Limited Smoothing Plugin for Servo (`#2651 `_) +* Contributors: Henning Kayser, Marq Rasmussen, Matthijs van der Burgh, Paul Gesel, Robert Haschke, Sebastian Jahr, Shobuj Paul, Tyler Weaver, marqrazz + 2.9.0 (2024-01-09) ------------------ * (core) Remove all references to python wrapper from the core pkg (`#2623 `_) diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt index 107ce7fa6f..90b2851ded 100644 --- a/moveit_core/CMakeLists.txt +++ b/moveit_core/CMakeLists.txt @@ -18,8 +18,11 @@ find_package(geometric_shapes REQUIRED) find_package(geometry_msgs REQUIRED) find_package(kdl_parser REQUIRED) find_package(moveit_msgs REQUIRED) -find_package(OCTOMAP REQUIRED) +# Enforce system version liboctomap-dev +# https://github.com/moveit/moveit2/issues/2862 +find_package(octomap 1.9.7...<1.10.0 REQUIRED) find_package(octomap_msgs REQUIRED) +find_package(osqp REQUIRED) find_package(pluginlib REQUIRED) find_package(random_numbers REQUIRED) find_package(rclcpp REQUIRED) @@ -65,39 +68,39 @@ add_subdirectory(transforms) add_subdirectory(utils) add_subdirectory(version) - install( - TARGETS - collision_detector_bullet_plugin - moveit_butterworth_filter - moveit_butterworth_parameters - moveit_collision_detection - moveit_collision_detection_bullet - moveit_collision_detection_fcl - moveit_collision_distance_field - moveit_constraint_samplers - moveit_distance_field - moveit_dynamics_solver - moveit_exceptions - moveit_kinematic_constraints - moveit_kinematics_base - moveit_kinematics_metrics - moveit_macros - moveit_planning_interface - moveit_planning_scene - moveit_robot_model - moveit_robot_state - moveit_robot_trajectory - moveit_smoothing_base - moveit_test_utils - moveit_trajectory_processing - moveit_transforms - moveit_utils + TARGETS collision_detector_bullet_plugin + collision_detector_fcl_plugin + moveit_acceleration_filter + moveit_acceleration_filter_parameters + moveit_butterworth_filter + moveit_butterworth_filter_parameters + moveit_collision_detection + moveit_collision_detection_bullet + moveit_collision_detection_fcl + moveit_collision_distance_field + moveit_constraint_samplers + moveit_distance_field + moveit_dynamics_solver + moveit_exceptions + moveit_kinematic_constraints + moveit_kinematics_base + moveit_kinematics_metrics + moveit_macros + moveit_planning_interface + moveit_planning_scene + moveit_robot_model + moveit_robot_state + moveit_robot_trajectory + moveit_smoothing_base + moveit_test_utils + moveit_trajectory_processing + moveit_transforms + moveit_utils EXPORT moveit_coreTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) ament_export_targets(moveit_coreTargets HAS_LIBRARY_TARGET) ament_export_dependencies( @@ -113,8 +116,9 @@ ament_export_dependencies( geometry_msgs kdl_parser moveit_msgs - OCTOMAP + octomap octomap_msgs + osqp pluginlib random_numbers rclcpp @@ -131,26 +135,16 @@ ament_export_dependencies( urdf urdfdom urdfdom_headers - visualization_msgs -) + visualization_msgs) # Plugin exports -pluginlib_export_plugin_description_file(moveit_core collision_detector_fcl_description.xml) -pluginlib_export_plugin_description_file(moveit_core collision_detector_bullet_description.xml) -pluginlib_export_plugin_description_file(moveit_core filter_plugin_butterworth.xml) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() -endif() +pluginlib_export_plugin_description_file(moveit_core + collision_detector_fcl_description.xml) +pluginlib_export_plugin_description_file( + moveit_core collision_detector_bullet_description.xml) +pluginlib_export_plugin_description_file(moveit_core + filter_plugin_butterworth.xml) +pluginlib_export_plugin_description_file(moveit_core + filter_plugin_acceleration.xml) ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_core/ConfigExtras.cmake b/moveit_core/ConfigExtras.cmake index 66d6e50b17..8475da4b4a 100644 --- a/moveit_core/ConfigExtras.cmake +++ b/moveit_core/ConfigExtras.cmake @@ -1,6 +1,8 @@ # Extras module needed for dependencies to find boost components -find_package(Boost REQUIRED +find_package( + Boost + REQUIRED chrono date_time filesystem @@ -9,5 +11,4 @@ find_package(Boost REQUIRED regex serialization system - thread -) + thread) diff --git a/moveit_core/collision_detection/CMakeLists.txt b/moveit_core/collision_detection/CMakeLists.txt index 2bf8b6bfa2..cd90e4d5c5 100644 --- a/moveit_core/collision_detection/CMakeLists.txt +++ b/moveit_core/collision_detection/CMakeLists.txt @@ -1,4 +1,5 @@ -add_library(moveit_collision_detection SHARED +add_library( + moveit_collision_detection SHARED src/allvalid/collision_env_allvalid.cpp src/collision_common.cpp src/collision_matrix.cpp @@ -7,16 +8,16 @@ add_library(moveit_collision_detection SHARED src/world.cpp src/world_diff.cpp src/collision_env.cpp - src/collision_plugin_cache.cpp -) -target_include_directories(moveit_collision_detection PUBLIC - $ - $ -) + src/collision_plugin_cache.cpp) +target_include_directories( + moveit_collision_detection + PUBLIC $ + $) include(GenerateExportHeader) generate_export_header(moveit_collision_detection) -ament_target_dependencies(moveit_collision_detection +ament_target_dependencies( + moveit_collision_detection pluginlib rclcpp rmw_implementation @@ -27,38 +28,44 @@ ament_target_dependencies(moveit_collision_detection visualization_msgs tf2_eigen geometric_shapes - OCTOMAP -) -target_include_directories(moveit_collision_detection BEFORE PUBLIC $) -target_include_directories(moveit_collision_detection PUBLIC $) + octomap) +target_include_directories( + moveit_collision_detection BEFORE + PUBLIC $) +target_include_directories( + moveit_collision_detection + PUBLIC $) -set_target_properties(moveit_collision_detection PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) +set_target_properties(moveit_collision_detection + PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) -target_link_libraries(moveit_collision_detection - moveit_robot_state - moveit_utils -) +target_link_libraries(moveit_collision_detection moveit_robot_state + moveit_utils) # unit tests if(BUILD_TESTING) if(WIN32) # TODO add window paths else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils") + set(APPEND_LIBRARY_DIRS + "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils" + ) endif() - ament_add_gtest(test_world test/test_world.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") + ament_add_gtest(test_world test/test_world.cpp APPEND_LIBRARY_DIRS + "${APPEND_LIBRARY_DIRS}") target_link_libraries(test_world moveit_collision_detection) - ament_add_gtest(test_world_diff test/test_world_diff.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") + ament_add_gtest(test_world_diff test/test_world_diff.cpp APPEND_LIBRARY_DIRS + "${APPEND_LIBRARY_DIRS}") target_link_libraries(test_world_diff moveit_collision_detection) - ament_add_gtest(test_all_valid test/test_all_valid.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - target_link_libraries(test_all_valid moveit_collision_detection moveit_robot_model) + ament_add_gtest(test_all_valid test/test_all_valid.cpp APPEND_LIBRARY_DIRS + "${APPEND_LIBRARY_DIRS}") + target_link_libraries(test_all_valid moveit_collision_detection + moveit_robot_model) endif() install(DIRECTORY include/ DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_export.h DESTINATION include/moveit_core) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_export.h + DESTINATION include/moveit_core) diff --git a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp index 8c140ee3c9..2605289cab 100644 --- a/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp +++ b/moveit_core/collision_detection/src/allvalid/collision_env_allvalid.cpp @@ -46,7 +46,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection.world_allvalid"); + return moveit::getLogger("moveit.core.collision_detection.env_allvalid"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_common.cpp b/moveit_core/collision_detection/src/collision_common.cpp index d869e61f31..8ee2090959 100644 --- a/moveit_core/collision_detection/src/collision_common.cpp +++ b/moveit_core/collision_detection/src/collision_common.cpp @@ -46,7 +46,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection_common"); + return moveit::getLogger("moveit.core.collision_detection.collision_common"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_env.cpp b/moveit_core/collision_detection/src/collision_env.cpp index 338567527d..a353ec54cc 100644 --- a/moveit_core/collision_detection/src/collision_env.cpp +++ b/moveit_core/collision_detection/src/collision_env.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection_env"); + return moveit::getLogger("moveit.core.collision_detection_env"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_matrix.cpp b/moveit_core/collision_detection/src/collision_matrix.cpp index 1d727f0bb5..eb2ef1fa1a 100644 --- a/moveit_core/collision_detection/src/collision_matrix.cpp +++ b/moveit_core/collision_detection/src/collision_matrix.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection_matrix"); + return moveit::getLogger("moveit.core.collision_detection_matrix"); } } // namespace diff --git a/moveit_core/collision_detection/src/collision_octomap_filter.cpp b/moveit_core/collision_detection/src/collision_octomap_filter.cpp index 327a9789a0..aca52fa2c8 100644 --- a/moveit_core/collision_detection/src/collision_octomap_filter.cpp +++ b/moveit_core/collision_detection/src/collision_octomap_filter.cpp @@ -49,7 +49,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection_octomap_filter"); + return moveit::getLogger("moveit.core.collision_detection_octomap_filter"); } } // namespace @@ -126,15 +126,15 @@ int collision_detection::refineContactNormals(const World::ObjectConstPtr& objec octree->begin_leafs_bbx(bbx_min, bbx_max); octomap::OcTreeBaseImpl::leaf_bbx_iterator leafs_end = octree->end_leafs_bbx(); - int count = 0; + // int count = 0; for (; it != leafs_end; ++it) { const octomap::point3d pt = it.getCoordinate(); - // double prob = it->getOccupancy(); if (octree->isNodeOccupied(*it)) // magic number! { - count++; node_centers.push_back(pt); + // count++; + // double prob = it->getOccupancy(); // RCLCPP_INFO(getLogger(), "Adding point %d with prob %.3f at [%.3f, %.3f, %.3f]", // count, prob, pt.x(), pt.y(), pt.z()); } diff --git a/moveit_core/collision_detection/src/collision_plugin_cache.cpp b/moveit_core/collision_detection/src/collision_plugin_cache.cpp index cad03b98cf..619bece8f4 100644 --- a/moveit_core/collision_detection/src/collision_plugin_cache.cpp +++ b/moveit_core/collision_detection/src/collision_plugin_cache.cpp @@ -45,7 +45,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection_plugin_cache"); + return moveit::getLogger("moveit.core.collision_detection_plugin_cache"); } } // namespace diff --git a/moveit_core/collision_detection/src/world.cpp b/moveit_core/collision_detection/src/world.cpp index ddd154aefa..eae07cf01c 100644 --- a/moveit_core/collision_detection/src/world.cpp +++ b/moveit_core/collision_detection/src/world.cpp @@ -46,7 +46,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection_world"); + return moveit::getLogger("moveit.core.collision_detection_world"); } } // namespace @@ -109,6 +109,7 @@ void World::addToObject(const std::string& object_id, const Eigen::Isometry3d& p std::vector World::getObjectIds() const { std::vector ids; + ids.reserve(objects_.size()); for (const auto& object : objects_) ids.push_back(object.first); return ids; diff --git a/moveit_core/collision_detection_bullet/CMakeLists.txt b/moveit_core/collision_detection_bullet/CMakeLists.txt index 0572a9183a..feadeb6533 100644 --- a/moveit_core/collision_detection_bullet/CMakeLists.txt +++ b/moveit_core/collision_detection_bullet/CMakeLists.txt @@ -1,66 +1,68 @@ -add_library(moveit_collision_detection_bullet SHARED +add_library( + moveit_collision_detection_bullet SHARED src/bullet_integration/bullet_utils.cpp src/bullet_integration/bullet_discrete_bvh_manager.cpp src/bullet_integration/bullet_cast_bvh_manager.cpp src/collision_env_bullet.cpp src/bullet_integration/bullet_bvh_manager.cpp src/bullet_integration/contact_checker_common.cpp - src/bullet_integration/ros_bullet_utils.cpp -) -target_include_directories(moveit_collision_detection_bullet PUBLIC - $ - $ -) + src/bullet_integration/ros_bullet_utils.cpp) +target_include_directories( + moveit_collision_detection_bullet + PUBLIC $ + $) include(GenerateExportHeader) generate_export_header(moveit_collision_detection_bullet) -target_include_directories(moveit_collision_detection_bullet PUBLIC $) -set_target_properties(moveit_collision_detection_bullet PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_collision_detection_bullet SYSTEM - BULLET -) -ament_target_dependencies(moveit_collision_detection_bullet +target_include_directories( + moveit_collision_detection_bullet + PUBLIC $) +set_target_properties(moveit_collision_detection_bullet + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_collision_detection_bullet SYSTEM BULLET) +ament_target_dependencies( + moveit_collision_detection_bullet rclcpp rmw_implementation urdf urdfdom urdfdom_headers visualization_msgs - octomap_msgs -) + octomap_msgs) target_link_libraries(moveit_collision_detection_bullet - moveit_collision_detection - moveit_utils -) + moveit_collision_detection moveit_utils) -add_library(collision_detector_bullet_plugin SHARED src/collision_detector_bullet_plugin_loader.cpp) -set_target_properties(collision_detector_bullet_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(collision_detector_bullet_plugin SYSTEM - BULLET -) -ament_target_dependencies(collision_detector_bullet_plugin +add_library(collision_detector_bullet_plugin SHARED + src/collision_detector_bullet_plugin_loader.cpp) +set_target_properties(collision_detector_bullet_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(collision_detector_bullet_plugin SYSTEM BULLET) +ament_target_dependencies( + collision_detector_bullet_plugin rclcpp urdf visualization_msgs pluginlib rmw_implementation - octomap_msgs -) -target_link_libraries(collision_detector_bullet_plugin - moveit_collision_detection_bullet - moveit_planning_scene - moveit_utils -) + octomap_msgs) +target_link_libraries( + collision_detector_bullet_plugin moveit_collision_detection_bullet + moveit_planning_scene moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_bullet_export.h DESTINATION include/moveit_core) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_bullet_export.h + DESTINATION include/moveit_core) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) if(WIN32) - # set(append_library_dirs "$;$") + # set(APPEND_LIBRARY_DIRS + # "$;$") else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils") + set(APPEND_LIBRARY_DIRS + "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils" + ) endif() # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished @@ -68,12 +70,18 @@ if(BUILD_TESTING) add_compile_options(-Wno-deprecated-declarations) endif() - ament_add_gtest(test_bullet_collision_detection test/test_bullet_collision_detection_pr2.cpp) - target_link_libraries(test_bullet_collision_detection moveit_test_utils moveit_collision_detection_bullet) + ament_add_gtest(test_bullet_collision_detection + test/test_bullet_collision_detection_pr2.cpp) + target_link_libraries(test_bullet_collision_detection moveit_test_utils + moveit_collision_detection_bullet) - ament_add_gtest(test_bullet_collision_detection_panda test/test_bullet_collision_detection_panda.cpp) - target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils moveit_collision_detection_bullet) + ament_add_gtest(test_bullet_collision_detection_panda + test/test_bullet_collision_detection_panda.cpp) + target_link_libraries(test_bullet_collision_detection_panda moveit_test_utils + moveit_collision_detection_bullet) - ament_add_gtest(test_bullet_continuous_collision_checking test/test_bullet_continuous_collision_checking.cpp) - target_link_libraries(test_bullet_continuous_collision_checking moveit_test_utils moveit_collision_detection_bullet) + ament_add_gtest(test_bullet_continuous_collision_checking + test/test_bullet_continuous_collision_checking.cpp) + target_link_libraries(test_bullet_continuous_collision_checking + moveit_test_utils moveit_collision_detection_bullet) endif() diff --git a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp index 9f9e713430..b326780991 100644 --- a/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp +++ b/moveit_core/collision_detection_bullet/src/bullet_integration/ros_bullet_utils.cpp @@ -115,6 +115,6 @@ Eigen::Isometry3d urdfPose2Eigen(const urdf::Pose& pose) rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection_bullet"); + return moveit::getLogger("moveit.core.collision_detection_bullet"); } } // namespace collision_detection_bullet diff --git a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp index 94853d4d57..16c62799ff 100644 --- a/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp +++ b/moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp @@ -56,7 +56,7 @@ namespace cb = collision_detection_bullet; rclcpp::Logger getLogger() { - return moveit::getLogger("collision_detection.bullet_test"); + return moveit::getLogger("moveit.core.collision_detection.bullet_test"); } /** \brief Brings the panda robot in user defined home position */ diff --git a/moveit_core/collision_detection_fcl/CMakeLists.txt b/moveit_core/collision_detection_fcl/CMakeLists.txt index e449297e0f..ece406480c 100644 --- a/moveit_core/collision_detection_fcl/CMakeLists.txt +++ b/moveit_core/collision_detection_fcl/CMakeLists.txt @@ -1,52 +1,50 @@ -add_library(moveit_collision_detection_fcl SHARED - src/collision_common.cpp - src/collision_env_fcl.cpp -) -target_include_directories(moveit_collision_detection_fcl PUBLIC - $ - $ -) +add_library(moveit_collision_detection_fcl SHARED src/collision_common.cpp + src/collision_env_fcl.cpp) +target_include_directories( + moveit_collision_detection_fcl + PUBLIC $ + $) include(GenerateExportHeader) generate_export_header(moveit_collision_detection_fcl) -target_include_directories(moveit_collision_detection_fcl PUBLIC $) -set_target_properties(moveit_collision_detection_fcl PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_collision_detection_fcl +target_include_directories( + moveit_collision_detection_fcl + PUBLIC $) +set_target_properties(moveit_collision_detection_fcl + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_collision_detection_fcl rclcpp rmw_implementation urdf urdfdom urdfdom_headers - visualization_msgs -) -target_link_libraries(moveit_collision_detection_fcl - moveit_collision_detection - moveit_utils - fcl -) + visualization_msgs) +target_link_libraries(moveit_collision_detection_fcl moveit_collision_detection + moveit_utils fcl) -add_library(collision_detector_fcl_plugin SHARED src/collision_detector_fcl_plugin_loader.cpp) -set_target_properties(collision_detector_fcl_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(collision_detector_fcl_plugin - rclcpp - urdf - visualization_msgs - pluginlib - rmw_implementation -) -target_link_libraries(collision_detector_fcl_plugin - moveit_collision_detection_fcl - moveit_planning_scene - moveit_utils -) +add_library(collision_detector_fcl_plugin SHARED + src/collision_detector_fcl_plugin_loader.cpp) +set_target_properties(collision_detector_fcl_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(collision_detector_fcl_plugin rclcpp urdf + visualization_msgs pluginlib rmw_implementation) +target_link_libraries( + collision_detector_fcl_plugin moveit_collision_detection_fcl + moveit_planning_scene moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_fcl_export.h DESTINATION include/moveit_core) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_detection_fcl_export.h + DESTINATION include/moveit_core) if(BUILD_TESTING) if(WIN32) - # set(append_library_dirs "$;$") + # set(APPEND_LIBRARY_DIRS + # "$;$") else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils") + set(APPEND_LIBRARY_DIRS + "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../collision_detection;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../utils" + ) endif() # TODO: remove if transition to gtest's new API TYPED_TEST_SUITE_P is finished @@ -55,11 +53,16 @@ if(BUILD_TESTING) endif() ament_add_gtest(test_fcl_collision_env test/test_fcl_env.cpp) - target_link_libraries(test_fcl_collision_env moveit_test_utils moveit_collision_detection_fcl) + target_link_libraries(test_fcl_collision_env moveit_test_utils + moveit_collision_detection_fcl) - ament_add_gtest(test_fcl_collision_detection test/test_fcl_collision_detection_pr2.cpp) - target_link_libraries(test_fcl_collision_detection moveit_test_utils moveit_collision_detection_fcl) + ament_add_gtest(test_fcl_collision_detection + test/test_fcl_collision_detection_pr2.cpp) + target_link_libraries(test_fcl_collision_detection moveit_test_utils + moveit_collision_detection_fcl) - ament_add_gtest(test_fcl_collision_detection_panda test/test_fcl_collision_detection_panda.cpp) - target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils moveit_collision_detection_fcl) + ament_add_gtest(test_fcl_collision_detection_panda + test/test_fcl_collision_detection_panda.cpp) + target_link_libraries(test_fcl_collision_detection_panda moveit_test_utils + moveit_collision_detection_fcl) endif() diff --git a/moveit_core/collision_detection_fcl/src/collision_common.cpp b/moveit_core/collision_detection_fcl/src/collision_common.cpp index de0be9ce66..faf5e8eab4 100644 --- a/moveit_core/collision_detection_fcl/src/collision_common.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_common.cpp @@ -60,7 +60,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_collision_detection_fcl"); + return moveit::getLogger("moveit.core.moveit_collision_detection_fcl"); } } // namespace diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp index e1406dc499..54a007d325 100644 --- a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp +++ b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp @@ -55,7 +55,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_collision_detection_fcl"); + return moveit::getLogger("moveit.core.collision_detection_fcl"); } // Check whether this FCL version supports the requested computations diff --git a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp index ff6458b044..e1a59bbddb 100644 --- a/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp +++ b/moveit_core/collision_detection_fcl/test/test_fcl_env.cpp @@ -66,7 +66,7 @@ inline void setToHome(moveit::core::RobotState& panda_state) rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_collision_detection_fcl.test.test_fcl_env"); + return moveit::getLogger("moveit.core.collision_detection_fcl.test_fcl_env"); } class CollisionDetectionEnvTest : public testing::Test diff --git a/moveit_core/collision_distance_field/CMakeLists.txt b/moveit_core/collision_distance_field/CMakeLists.txt index 5c116aa689..63b63a013d 100644 --- a/moveit_core/collision_distance_field/CMakeLists.txt +++ b/moveit_core/collision_distance_field/CMakeLists.txt @@ -1,48 +1,42 @@ -add_library(moveit_collision_distance_field SHARED +add_library( + moveit_collision_distance_field SHARED src/collision_distance_field_types.cpp - src/collision_common_distance_field.cpp - src/collision_env_distance_field.cpp - src/collision_env_hybrid.cpp -) -target_include_directories(moveit_collision_distance_field PUBLIC - $ - $ -) + src/collision_common_distance_field.cpp src/collision_env_distance_field.cpp + src/collision_env_hybrid.cpp) +target_include_directories( + moveit_collision_distance_field + PUBLIC $ + $) include(GenerateExportHeader) generate_export_header(moveit_collision_distance_field) -target_include_directories(moveit_collision_distance_field PUBLIC $) -set_target_properties(moveit_collision_distance_field PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_include_directories( + moveit_collision_distance_field + PUBLIC $) +set_target_properties(moveit_collision_distance_field + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_collision_distance_field - urdf - visualization_msgs - tf2_eigen - geometric_shapes - OCTOMAP -) +ament_target_dependencies(moveit_collision_distance_field urdf + visualization_msgs tf2_eigen geometric_shapes octomap) -target_link_libraries(moveit_collision_distance_field - moveit_planning_scene - moveit_distance_field - moveit_collision_detection - moveit_robot_state -) +target_link_libraries( + moveit_collision_distance_field moveit_planning_scene moveit_distance_field + moveit_collision_detection moveit_robot_state) install(DIRECTORY include/ DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_distance_field_export.h DESTINATION include/moveit_core) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_collision_distance_field_export.h + DESTINATION include/moveit_core) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(resource_retriever REQUIRED) - ament_add_gtest(test_collision_distance_field test/test_collision_distance_field.cpp) - ament_target_dependencies(test_collision_distance_field - geometric_shapes - OCTOMAP - srdfdom - resource_retriever - ) - target_link_libraries(test_collision_distance_field + ament_add_gtest(test_collision_distance_field + test/test_collision_distance_field.cpp) + ament_target_dependencies(test_collision_distance_field geometric_shapes + octomap srdfdom resource_retriever) + target_link_libraries( + test_collision_distance_field moveit_collision_distance_field moveit_collision_detection moveit_distance_field @@ -50,6 +44,5 @@ if(BUILD_TESTING) moveit_robot_state moveit_test_utils moveit_transforms - moveit_planning_scene - ) + moveit_planning_scene) endif() diff --git a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h index b0f4a2ccd7..5887a9cdab 100644 --- a/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h +++ b/moveit_core/collision_distance_field/include/moveit/collision_distance_field/collision_distance_field_types.h @@ -373,7 +373,8 @@ class PosedBodySphereDecompositionVector public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PosedBodySphereDecompositionVector() : logger_(moveit::getLogger("posed_body_sphere_decomposition_vector")) + PosedBodySphereDecompositionVector() + : logger_(moveit::getLogger("moveit.core.posed_body_sphere_decomposition_vector")) { } @@ -447,7 +448,7 @@ class PosedBodyPointDecompositionVector public: EIGEN_MAKE_ALIGNED_OPERATOR_NEW - PosedBodyPointDecompositionVector() : logger_(moveit::getLogger("posed_body_point_decomposition_vector")) + PosedBodyPointDecompositionVector() : logger_(moveit::getLogger("moveit.core.posed_body_point_decomposition_vector")) { } diff --git a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp index 3cba64aadb..4402a4e44c 100644 --- a/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_common_distance_field.cpp @@ -50,7 +50,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_common_distance_field"); + return moveit::getLogger("moveit.core.collision_common_distance_field"); } } // namespace diff --git a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp index 40bde7f13c..1754a74b4d 100644 --- a/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp +++ b/moveit_core/collision_distance_field/src/collision_distance_field_types.cpp @@ -52,7 +52,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_distance_field_types"); + return moveit::getLogger("moveit.core.collision_distance_field_types"); } } // namespace diff --git a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp index c9bb341352..947ce31263 100644 --- a/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp +++ b/moveit_core/collision_distance_field/src/collision_env_distance_field.cpp @@ -61,7 +61,7 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double /*padding*/, double /*scale*/) - : CollisionEnv(robot_model), logger_(moveit::getLogger("collision_robot_distance_field")) + : CollisionEnv(robot_model), logger_(moveit::getLogger("moveit.core.collision_robot_distance_field")) { initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance); @@ -80,7 +80,8 @@ CollisionEnvDistanceField::CollisionEnvDistanceField( const std::map>& link_body_decompositions, double size_x, double size_y, double size_z, const Eigen::Vector3d& origin, bool use_signed_distance_field, double resolution, double collision_tolerance, double max_propogation_distance, double padding, double scale) - : CollisionEnv(robot_model, world, padding, scale), logger_(moveit::getLogger("collision_robot_distance_field")) + : CollisionEnv(robot_model, world, padding, scale) + , logger_(moveit::getLogger("moveit.core.collision_robot_distance_field")) { initialize(link_body_decompositions, Eigen::Vector3d(size_x, size_y, size_z), origin, use_signed_distance_field, resolution, collision_tolerance, max_propogation_distance); diff --git a/moveit_core/constraint_samplers/CMakeLists.txt b/moveit_core/constraint_samplers/CMakeLists.txt index caedd76c9a..25cda7dc66 100644 --- a/moveit_core/constraint_samplers/CMakeLists.txt +++ b/moveit_core/constraint_samplers/CMakeLists.txt @@ -1,29 +1,24 @@ -add_library(moveit_constraint_samplers SHARED - src/constraint_sampler.cpp - src/constraint_sampler_manager.cpp - src/constraint_sampler_tools.cpp - src/default_constraint_samplers.cpp - src/union_constraint_sampler.cpp -) -target_include_directories(moveit_constraint_samplers PUBLIC - $ - $ -) -set_target_properties(moveit_constraint_samplers PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_constraint_samplers - urdf - urdfdom - urdfdom_headers - visualization_msgs -) -target_link_libraries(moveit_constraint_samplers +add_library( + moveit_constraint_samplers SHARED + src/constraint_sampler.cpp src/constraint_sampler_manager.cpp + src/constraint_sampler_tools.cpp src/default_constraint_samplers.cpp + src/union_constraint_sampler.cpp) +target_include_directories( + moveit_constraint_samplers + PUBLIC $ + $) +set_target_properties(moveit_constraint_samplers + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_constraint_samplers urdf urdfdom + urdfdom_headers visualization_msgs) +target_link_libraries( + moveit_constraint_samplers moveit_robot_trajectory moveit_robot_state moveit_kinematic_constraints moveit_kinematics_base moveit_planning_scene - moveit_utils -) + moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -32,26 +27,23 @@ if(BUILD_TESTING) find_package(orocos_kdl REQUIRED) find_package(angles REQUIRED) find_package(tf2_kdl REQUIRED) - include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} ${tf2_kdl_INCLUDE_DIRS}) + include_directories(SYSTEM ${orocos_kdl_INCLUDE_DIRS} ${angles_INCLUDE_DIRS} + ${tf2_kdl_INCLUDE_DIRS}) if(WIN32) - # set(append_library_dirs "$;$") + # set(APPEND_LIBRARY_DIRS + # "$;$") else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../planning_scene;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../kinematics_constraint") + set(APPEND_LIBRARY_DIRS + "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../planning_scene;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../kinematics_constraint" + ) endif() - ament_add_gmock(test_constraint_samplers - test/test_constraint_samplers.cpp - test/pr2_arm_kinematics_plugin.cpp - test/pr2_arm_ik.cpp - ) - target_include_directories(test_constraint_samplers PUBLIC - ${geometry_msgs_INCLUDE_DIRS} - ) + ament_add_gmock(test_constraint_samplers test/test_constraint_samplers.cpp + test/pr2_arm_kinematics_plugin.cpp test/pr2_arm_ik.cpp) + target_include_directories(test_constraint_samplers + PUBLIC ${geometry_msgs_INCLUDE_DIRS}) ament_target_dependencies(test_constraint_samplers kdl_parser) - target_link_libraries(test_constraint_samplers - moveit_test_utils - moveit_utils - moveit_constraint_samplers - ) + target_link_libraries(test_constraint_samplers moveit_test_utils moveit_utils + moveit_constraint_samplers) endif() diff --git a/moveit_core/constraint_samplers/src/constraint_sampler.cpp b/moveit_core/constraint_samplers/src/constraint_sampler.cpp index 654e47bd7f..0f855b14d4 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler.cpp @@ -45,7 +45,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("constraint_sampler"); + return moveit::getLogger("moveit.core.constraint_sampler"); } } // namespace diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp index 16d401b16c..a3fea06024 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_manager.cpp @@ -48,7 +48,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("constraint_sampler_manager"); + return moveit::getLogger("moveit.core.constraint_sampler_manager"); } } // namespace diff --git a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp index 5cdba1adb4..2aaec85145 100644 --- a/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp +++ b/moveit_core/constraint_samplers/src/constraint_sampler_tools.cpp @@ -49,7 +49,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("constraint_sampler_tools"); + return moveit::getLogger("moveit.core.constraint_sampler_tools"); } } // namespace diff --git a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp index 0935213ac9..f5db2a6968 100644 --- a/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp +++ b/moveit_core/constraint_samplers/src/default_constraint_samplers.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("default_constraint_samplers"); + return moveit::getLogger("moveit.core.default_constraint_samplers"); } } // namespace diff --git a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp index 47db822504..13983d2ff5 100644 --- a/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp +++ b/moveit_core/constraint_samplers/src/union_constraint_sampler.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("union_constraint_sampler"); + return moveit::getLogger("moveit.core.union_constraint_sampler"); } } // namespace diff --git a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp index 88a387ab70..b35496a1a4 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_ik.cpp @@ -54,7 +54,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_constaint_samplers.test.pr2_arm_ik"); + return moveit::getLogger("moveit.core.moveit_constaint_samplers.test.pr2_arm_ik"); } } // namespace @@ -804,6 +804,6 @@ bool PR2ArmIK::checkJointLimits(const double joint_value, const int joint_num) c jv = angles::normalize_angle(joint_value * angle_multipliers_[joint_num]); } - return !(jv < min_angles_[joint_num] || jv > max_angles_[joint_num]); + return jv >= min_angles_[joint_num] && jv <= max_angles_[joint_num]; } } // namespace pr2_arm_kinematics diff --git a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp index a1e1978a35..e32ee40eda 100644 --- a/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp +++ b/moveit_core/constraint_samplers/test/pr2_arm_kinematics_plugin.cpp @@ -53,7 +53,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_constraint_samplers.test.pr2_arm_kinematics_plugin"); + return moveit::getLogger("moveit.core.moveit_constraint_samplers.test.pr2_arm_kinematics_plugin"); } } // namespace diff --git a/moveit_core/distance_field/CMakeLists.txt b/moveit_core/distance_field/CMakeLists.txt index 27a12fe87a..c70886f12f 100644 --- a/moveit_core/distance_field/CMakeLists.txt +++ b/moveit_core/distance_field/CMakeLists.txt @@ -1,23 +1,23 @@ -add_library(moveit_distance_field SHARED - src/distance_field.cpp - src/find_internal_points.cpp - src/propagation_distance_field.cpp -) -target_include_directories(moveit_distance_field PUBLIC - $ - $ -) +add_library( + moveit_distance_field SHARED + src/distance_field.cpp src/find_internal_points.cpp + src/propagation_distance_field.cpp) +target_include_directories( + moveit_distance_field + PUBLIC $ + $) target_link_libraries(moveit_distance_field moveit_macros moveit_utils) -set_target_properties(moveit_distance_field PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_distance_field +set_target_properties(moveit_distance_field + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_distance_field Boost urdfdom urdfdom_headers visualization_msgs geometric_shapes tf2_eigen - OCTOMAP -) + octomap) install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -28,5 +28,5 @@ if(BUILD_TESTING) target_link_libraries(test_voxel_grid moveit_distance_field) ament_add_gtest(test_distance_field test/test_distance_field.cpp) - target_link_libraries(test_distance_field moveit_distance_field) + target_link_libraries(test_distance_field moveit_distance_field octomap) endif() diff --git a/moveit_core/distance_field/src/distance_field.cpp b/moveit_core/distance_field/src/distance_field.cpp index c4a94708f7..c619f95d51 100644 --- a/moveit_core/distance_field/src/distance_field.cpp +++ b/moveit_core/distance_field/src/distance_field.cpp @@ -51,7 +51,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("distance_field"); + return moveit::getLogger("moveit.core.distance_field"); } } // namespace diff --git a/moveit_core/distance_field/src/propagation_distance_field.cpp b/moveit_core/distance_field/src/propagation_distance_field.cpp index fcaa85b3b1..6b70134120 100644 --- a/moveit_core/distance_field/src/propagation_distance_field.cpp +++ b/moveit_core/distance_field/src/propagation_distance_field.cpp @@ -49,7 +49,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("propagation_distance_field"); + return moveit::getLogger("moveit.core.propagation_distance_field"); } } // namespace diff --git a/moveit_core/distance_field/test/test_distance_field.cpp b/moveit_core/distance_field/test/test_distance_field.cpp index 5792eaa336..62cbfe6d63 100644 --- a/moveit_core/distance_field/test/test_distance_field.cpp +++ b/moveit_core/distance_field/test/test_distance_field.cpp @@ -807,20 +807,17 @@ TEST(TestSignedPropagationDistanceField, TestPerformance) PERF_ORIGIN_Z, PERF_MAX_DIST, true); EigenSTL::vector_Vector3d bad_vec; - unsigned int count = 0; for (unsigned int z = UNIFORM_DISTANCE; z < worstdfu.getZNumCells() - UNIFORM_DISTANCE; z += UNIFORM_DISTANCE) { for (unsigned int x = UNIFORM_DISTANCE; x < worstdfu.getXNumCells() - UNIFORM_DISTANCE; x += UNIFORM_DISTANCE) { for (unsigned int y = UNIFORM_DISTANCE; y < worstdfu.getYNumCells() - UNIFORM_DISTANCE; y += UNIFORM_DISTANCE) { - count++; Eigen::Vector3d loc; bool valid = worstdfu.gridToWorld(x, y, z, loc.x(), loc.y(), loc.z()); if (!valid) { - // RCLCPP_WARN("distance_field", "Something wrong"); continue; } bad_vec.push_back(loc); diff --git a/moveit_core/doc/_templates/custom-class-template.rst b/moveit_core/doc/_templates/custom-class-template.rst deleted file mode 100644 index f73eda50ec..0000000000 --- a/moveit_core/doc/_templates/custom-class-template.rst +++ /dev/null @@ -1,34 +0,0 @@ -{{ fullname | escape | underline}} - -.. currentmodule:: {{ module }} - -.. autoclass:: {{ objname }} - :members: - :show-inheritance: - :inherited-members: - :special-members: __call__, __add__, __mul__ - - {% block methods %} - {% if methods %} - .. rubric:: {{ _('Methods') }} - - .. autosummary:: - :nosignatures: - {% for item in methods %} - {%- if not item.startswith('_') %} - ~{{ name }}.{{ item }} - {%- endif -%} - {%- endfor %} - {% endif %} - {% endblock %} - - {% block attributes %} - {% if attributes %} - .. rubric:: {{ _('Attributes') }} - - .. autosummary:: - {% for item in attributes %} - ~{{ name }}.{{ item }} - {%- endfor %} - {% endif %} - {% endblock %} diff --git a/moveit_core/doc/_templates/custom-module-template.rst b/moveit_core/doc/_templates/custom-module-template.rst deleted file mode 100644 index d066d0e4dc..0000000000 --- a/moveit_core/doc/_templates/custom-module-template.rst +++ /dev/null @@ -1,66 +0,0 @@ -{{ fullname | escape | underline}} - -.. automodule:: {{ fullname }} - - {% block attributes %} - {% if attributes %} - .. rubric:: Module attributes - - .. autosummary:: - :toctree: - {% for item in attributes %} - {{ item }} - {%- endfor %} - {% endif %} - {% endblock %} - - {% block functions %} - {% if functions %} - .. rubric:: {{ _('Functions') }} - - .. autosummary:: - :toctree: - :nosignatures: - {% for item in functions %} - {{ item }} - {%- endfor %} - {% endif %} - {% endblock %} - - {% block classes %} - {% if classes %} - .. rubric:: {{ _('Classes') }} - - .. autosummary:: - :toctree: - :template: custom-class-template.rst - :nosignatures: - {% for item in classes %} - {{ item }} - {%- endfor %} - {% endif %} - {% endblock %} - - {% block exceptions %} - {% if exceptions %} - .. rubric:: {{ _('Exceptions') }} - - .. autosummary:: - :toctree: - {% for item in exceptions %} - {{ item }} - {%- endfor %} - {% endif %} - {% endblock %} - -{% block modules %} -{% if modules %} -.. autosummary:: - :toctree: - :template: custom-module-template.rst - :recursive: -{% for item in modules %} - {{ item }} -{%- endfor %} -{% endif %} -{% endblock %} diff --git a/moveit_core/doc/api.rst b/moveit_core/doc/api.rst deleted file mode 100644 index 58c6bd1760..0000000000 --- a/moveit_core/doc/api.rst +++ /dev/null @@ -1,6 +0,0 @@ -.. autosummary:: - :toctree: _autosummary - :template: custom-module-template.rst - :recursive: - - moveit.core diff --git a/moveit_core/doc/conf.py b/moveit_core/doc/conf.py deleted file mode 100644 index 0ae380d607..0000000000 --- a/moveit_core/doc/conf.py +++ /dev/null @@ -1,200 +0,0 @@ -# -*- coding: utf-8 -*- -# -# Configuration file for the Sphinx documentation builder. -# -# This file does only contain a selection of the most common options. For a -# full list see the documentation: -# http://www.sphinx-doc.org/en/master/config - -# -- Path setup -------------------------------------------------------------- - -# If extensions (or modules to document with autodoc) are in another directory, -# add these directories to sys.path here. If the directory is relative to the -# documentation root, use os.path.abspath to make it absolute, like shown here. -# -import os -import sys -import sphinx_rtd_theme - - -# -- Project information ----------------------------------------------------- - -project = "moveit.core" -copyright = "2021, MoveIt maintainer team" -author = "MoveIt maintainer team" - -# The short X.Y version -version = "" -# The full version, including alpha/beta/rc tags -release = "" - - -# -- General configuration --------------------------------------------------- - -# If your documentation needs a minimal Sphinx version, state it here. -# -# needs_sphinx = '1.0' - -# Add any Sphinx extension module names here, as strings. They can be -# extensions coming with Sphinx (named 'sphinx.ext.*') or your custom -# ones. -extensions = [ - "sphinx.ext.autodoc", - "sphinx.ext.autosummary", - "sphinx.ext.intersphinx", - "sphinx_rtd_theme", -] - -# NOTE: Important variables that make auto-generation work, -# see https://www.sphinx-doc.org/en/master/usage/extensions/autodoc.html -autosummary_generate = True -autosummary_imported_members = True -autoclass_content = "both" # Add __init__ doc (ie. params) to class summaries - -# Customization, not as important -html_show_sourcelink = ( - True # Remove 'view source code' from top of page (for html, not python) -) -autodoc_inherit_docstrings = True # If no docstring, inherit from base class -set_type_checking_flag = True # Enable 'expensive' imports for sphinx_autodoc_typehints -add_module_names = False - - -# Add any paths that contain templates here, relative to this directory. -templates_path = ["_templates"] - -# The suffix(es) of source filenames. -# You can specify multiple suffix as a list of string: -# -# source_suffix = ['.rst', '.md'] -source_suffix = ".rst" - -# The master toctree document. -master_doc = "index" - -# The language for content autogenerated by Sphinx. Refer to documentation -# for a list of supported languages. -# -# This is also used if you do content translation via gettext catalogs. -# Usually you set "language" from the command line for these cases. -language = None - -# List of patterns, relative to source directory, that match files and -# directories to ignore when looking for source files. -# This pattern also affects html_static_path and html_extra_path. -exclude_patterns = ["_build", "Thumbs.db", ".DS_Store", "_templates"] - -# The name of the Pygments (syntax highlighting) style to use. -pygments_style = None - - -# -- Options for HTML output ------------------------------------------------- - -# The theme to use for HTML and HTML Help pages. See the documentation for -# a list of builtin themes. -# -html_theme = "sphinx_rtd_theme" - - -# Theme options are theme-specific and customize the look and feel of a theme -# further. For a list of options available for each theme, see the -# documentation. -# -# html_theme_options = {} - -# Add any paths that contain custom static files (such as style sheets) here, -# relative to this directory. They are copied after the builtin static files, -# so a file named "default.css" will overwrite the builtin "default.css". -html_static_path = ["_static"] - -# Custom sidebar templates, must be a dictionary that maps document names -# to template names. -# -# The default sidebars (for documents that don't match any pattern) are -# defined by theme itself. Builtin themes are using these templates by -# default: ``['localtoc.html', 'relations.html', 'sourcelink.html', -# 'searchbox.html']``. -# -# html_sidebars = {} - - -# -- Options for HTMLHelp output --------------------------------------------- - -# Output file base name for HTML help builder. -htmlhelp_basename = "moveit_coredoc" - - -# -- Options for LaTeX output ------------------------------------------------ - -latex_elements = { - # The paper size ('letterpaper' or 'a4paper'). - # - # 'papersize': 'letterpaper', - # The font size ('10pt', '11pt' or '12pt'). - # - # 'pointsize': '10pt', - # Additional stuff for the LaTeX preamble. - # - # 'preamble': '', - # Latex figure (float) alignment - # - # 'figure_align': 'htbp', -} - -# Grouping the document tree into LaTeX files. List of tuples -# (source start file, target name, title, -# author, documentclass [howto, manual, or own class]). -latex_documents = [ - (master_doc, "moveit_core.tex", "moveit\\_core Documentation", "peter", "manual"), -] - - -# -- Options for manual page output ------------------------------------------ - -# One entry per manual page. List of tuples -# (source start file, name, description, authors, manual section). -man_pages = [(master_doc, "moveit_core", "moveit_core Documentation", [author], 1)] - - -# -- Options for Texinfo output ---------------------------------------------- - -# Grouping the document tree into Texinfo files. List of tuples -# (source start file, target name, title, author, -# dir menu entry, description, category) -texinfo_documents = [ - ( - master_doc, - "moveit_core", - "moveit_core Documentation", - author, - "moveit_core", - "One line description of project.", - "Miscellaneous", - ), -] - - -# -- Options for Epub output ------------------------------------------------- - -# Bibliographic Dublin Core info. -epub_title = project - -# The unique identifier of the text. This can be a ISBN number -# or the project homepage. -# -# epub_identifier = '' - -# A unique identification for the text. -# -# epub_uid = '' - -# A list of files that should not be packed into the epub file. -epub_exclude_files = ["search.html"] - - -# -- Extension configuration ------------------------------------------------- - -# -- Options for intersphinx extension --------------------------------------- - -# Example configuration for intersphinx: refer to the Python standard library. -intersphinx_mapping = {"https://docs.python.org/3": None} diff --git a/moveit_core/doc/index.rst b/moveit_core/doc/index.rst deleted file mode 100644 index 9c719062c5..0000000000 --- a/moveit_core/doc/index.rst +++ /dev/null @@ -1,13 +0,0 @@ -Python API Reference: moveit_core -======================================= - -.. note:: - These docs are autogenerated. You may see reference to module names like pymoveit_core, - but you should not use these in your code. Instead, use the modules under the `moveit` namespace. - For example, you should write ``import moveit.core`` instead of ``import pymoveit_core``. - The latter will still work, but will not contain the full API that ``import moveit.core`` does. - -.. toctree:: - - Home page - API reference diff --git a/moveit_core/dynamics_solver/CMakeLists.txt b/moveit_core/dynamics_solver/CMakeLists.txt index 7e83f476a5..04afe66ef0 100644 --- a/moveit_core/dynamics_solver/CMakeLists.txt +++ b/moveit_core/dynamics_solver/CMakeLists.txt @@ -1,20 +1,13 @@ add_library(moveit_dynamics_solver SHARED src/dynamics_solver.cpp) -target_include_directories(moveit_dynamics_solver PUBLIC - $ - $ -) -set_target_properties(moveit_dynamics_solver PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_include_directories( + moveit_dynamics_solver + PUBLIC $ + $) +set_target_properties(moveit_dynamics_solver + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_dynamics_solver - urdf - urdfdom_headers - orocos_kdl - visualization_msgs - kdl_parser -) -target_link_libraries(moveit_dynamics_solver - moveit_robot_state - moveit_utils -) +ament_target_dependencies(moveit_dynamics_solver urdf urdfdom_headers + orocos_kdl visualization_msgs kdl_parser) +target_link_libraries(moveit_dynamics_solver moveit_robot_state moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/dynamics_solver/src/dynamics_solver.cpp b/moveit_core/dynamics_solver/src/dynamics_solver.cpp index 8806f82345..9c39863869 100644 --- a/moveit_core/dynamics_solver/src/dynamics_solver.cpp +++ b/moveit_core/dynamics_solver/src/dynamics_solver.cpp @@ -49,7 +49,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("dynamics_solver"); + return moveit::getLogger("moveit.core.dynamics_solver"); } } // namespace diff --git a/moveit_core/exceptions/CMakeLists.txt b/moveit_core/exceptions/CMakeLists.txt index aca217c569..26591d955a 100644 --- a/moveit_core/exceptions/CMakeLists.txt +++ b/moveit_core/exceptions/CMakeLists.txt @@ -1,15 +1,12 @@ add_library(moveit_exceptions SHARED src/exceptions.cpp) -target_include_directories(moveit_exceptions PUBLIC - $ - $ -) -set_target_properties(moveit_exceptions PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_exceptions - Boost - rclcpp - urdfdom - urdfdom_headers -) +target_include_directories( + moveit_exceptions + PUBLIC $ + $) +set_target_properties(moveit_exceptions PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_exceptions Boost rclcpp urdfdom + urdfdom_headers) target_link_libraries(moveit_exceptions moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/filter_plugin_acceleration.xml b/moveit_core/filter_plugin_acceleration.xml new file mode 100644 index 0000000000..943ecf3f55 --- /dev/null +++ b/moveit_core/filter_plugin_acceleration.xml @@ -0,0 +1,8 @@ + + + + Limits acceleration of commands to generate smooth motion. + + + diff --git a/moveit_core/kinematic_constraints/CMakeLists.txt b/moveit_core/kinematic_constraints/CMakeLists.txt index 5157908c68..51f33e4950 100644 --- a/moveit_core/kinematic_constraints/CMakeLists.txt +++ b/moveit_core/kinematic_constraints/CMakeLists.txt @@ -1,36 +1,34 @@ if(WIN32) - # set(append_library_dirs "$;$") + # set(APPEND_LIBRARY_DIRS + # "$;$") else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl") + set(APPEND_LIBRARY_DIRS + "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils;${CMAKE_CURRENT_BINARY_DIR}/../robot_model;${CMAKE_CURRENT_BINARY_DIR}/../robot_state;${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl" + ) endif() -add_library(moveit_kinematic_constraints SHARED - src/kinematic_constraint.cpp - src/utils.cpp -) -target_include_directories(moveit_kinematic_constraints PUBLIC - $ - $ -) -set_target_properties(moveit_kinematic_constraints PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +add_library(moveit_kinematic_constraints SHARED src/kinematic_constraint.cpp + src/utils.cpp) +target_include_directories( + moveit_kinematic_constraints + PUBLIC $ + $) +set_target_properties(moveit_kinematic_constraints + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_kinematic_constraints +ament_target_dependencies( + moveit_kinematic_constraints urdf urdfdom urdfdom_headers tf2_geometry_msgs geometry_msgs visualization_msgs - tf2_eigen -) + tf2_eigen) -target_link_libraries(moveit_kinematic_constraints - moveit_collision_detection_fcl - moveit_kinematics_base - moveit_robot_state - moveit_robot_model - moveit_utils -) +target_link_libraries( + moveit_kinematic_constraints moveit_collision_detection_fcl + moveit_kinematics_base moveit_robot_state moveit_robot_model moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -38,10 +36,12 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_constraints test/test_constraints.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}" - ) - target_link_libraries(test_constraints moveit_test_utils moveit_kinematic_constraints) + APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}") + target_link_libraries(test_constraints moveit_test_utils + moveit_kinematic_constraints) - ament_add_gtest(test_orientation_constraints test/test_orientation_constraints.cpp) - target_link_libraries(test_orientation_constraints moveit_test_utils moveit_kinematic_constraints) + ament_add_gtest(test_orientation_constraints + test/test_orientation_constraints.cpp) + target_link_libraries(test_orientation_constraints moveit_test_utils + moveit_kinematic_constraints) endif() diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp index 13a4cc5865..33e4efbbde 100644 --- a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp +++ b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp @@ -60,7 +60,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("kinematic_constraints"); + return moveit::getLogger("moveit.core.kinematic_constraints"); } } // namespace diff --git a/moveit_core/kinematic_constraints/src/utils.cpp b/moveit_core/kinematic_constraints/src/utils.cpp index a776d08df6..6a66f6826f 100644 --- a/moveit_core/kinematic_constraints/src/utils.cpp +++ b/moveit_core/kinematic_constraints/src/utils.cpp @@ -57,7 +57,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_kinematic_constraints"); + return moveit::getLogger("moveit.core.kinematic_constraints"); } } // namespace diff --git a/moveit_core/kinematics_base/CMakeLists.txt b/moveit_core/kinematics_base/CMakeLists.txt index 13dc3c9c9c..1802539000 100644 --- a/moveit_core/kinematics_base/CMakeLists.txt +++ b/moveit_core/kinematics_base/CMakeLists.txt @@ -1,12 +1,15 @@ add_library(moveit_kinematics_base SHARED src/kinematics_base.cpp) -target_include_directories(moveit_kinematics_base PUBLIC - $ - $ -) -target_link_libraries(moveit_kinematics_base moveit_macros moveit_robot_model moveit_utils) +target_include_directories( + moveit_kinematics_base + PUBLIC $ + $) +target_link_libraries(moveit_kinematics_base moveit_macros moveit_robot_model + moveit_utils) include(GenerateExportHeader) generate_export_header(moveit_kinematics_base) -target_include_directories(moveit_kinematics_base PUBLIC $) # for this library +target_include_directories( + moveit_kinematics_base PUBLIC $ +)# for this library # This line ensures that messages are built before the library is built ament_target_dependencies( @@ -14,10 +17,11 @@ ament_target_dependencies( rclcpp urdf urdfdom_headers - srdfdom moveit_msgs + srdfdom + moveit_msgs geometric_shapes - geometry_msgs -) + geometry_msgs) install(DIRECTORY include/ DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_kinematics_base_export.h DESTINATION include/moveit_core) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_kinematics_base_export.h + DESTINATION include/moveit_core) diff --git a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h index c51cd4572e..1963d7c389 100644 --- a/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h +++ b/moveit_core/kinematics_base/include/moveit/kinematics_base/kinematics_base.h @@ -322,7 +322,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase } // Otherwise throw error because this function should have been implemented - RCLCPP_ERROR(moveit::getLogger("kinematics_base"), + RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), "This kinematic solver does not support searchPositionIK with multiple poses"); return false; } @@ -363,7 +363,7 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase return searchPositionIK(ik_poses, ik_seed_state, timeout, consistency_limits, solution, solution_callback, error_code, options, context_state); } - RCLCPP_ERROR(moveit::getLogger("kinematics_base"), + RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), "This kinematic solver does not support IK solution cost functions"); return false; } @@ -437,8 +437,9 @@ class MOVEIT_KINEMATICS_BASE_EXPORT KinematicsBase { if (tip_frames_.size() > 1) { - RCLCPP_ERROR(moveit::getLogger("kinematics_base"), "This kinematic solver has more than one tip frame, " - "do not call getTipFrame()"); + RCLCPP_ERROR(moveit::getLogger("moveit.core.kinematics_base"), + "This kinematic solver has more than one tip frame, " + "do not call getTipFrame()"); } return tip_frames_[0]; diff --git a/moveit_core/kinematics_base/src/kinematics_base.cpp b/moveit_core/kinematics_base/src/kinematics_base.cpp index 7b333109b3..3b8d61854d 100644 --- a/moveit_core/kinematics_base/src/kinematics_base.cpp +++ b/moveit_core/kinematics_base/src/kinematics_base.cpp @@ -45,7 +45,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("kinematics_base"); + return moveit::getLogger("moveit.core.kinematics_base"); } } // namespace diff --git a/moveit_core/kinematics_metrics/CMakeLists.txt b/moveit_core/kinematics_metrics/CMakeLists.txt index 52fbbd8e29..e760b834ea 100644 --- a/moveit_core/kinematics_metrics/CMakeLists.txt +++ b/moveit_core/kinematics_metrics/CMakeLists.txt @@ -1,19 +1,15 @@ add_library(moveit_kinematics_metrics SHARED src/kinematics_metrics.cpp) -target_include_directories(moveit_kinematics_metrics PUBLIC - $ - $ -) -set_target_properties(moveit_kinematics_metrics PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_include_directories( + moveit_kinematics_metrics + PUBLIC $ + $) +set_target_properties(moveit_kinematics_metrics + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_kinematics_metrics - urdf - urdfdom_headers - visualization_msgs) +ament_target_dependencies(moveit_kinematics_metrics urdf urdfdom_headers + visualization_msgs) -target_link_libraries(moveit_kinematics_metrics - moveit_robot_model - moveit_robot_state - moveit_utils -) +target_link_libraries(moveit_kinematics_metrics moveit_robot_model + moveit_robot_state moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp index d0c4625918..c6ab4eaf44 100644 --- a/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp +++ b/moveit_core/kinematics_metrics/src/kinematics_metrics.cpp @@ -49,7 +49,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("kinematics_metrics"); + return moveit::getLogger("moveit.core.kinematics_metrics"); } } // namespace diff --git a/moveit_core/macros/CMakeLists.txt b/moveit_core/macros/CMakeLists.txt index 12eb608040..c5cd95113d 100644 --- a/moveit_core/macros/CMakeLists.txt +++ b/moveit_core/macros/CMakeLists.txt @@ -1,7 +1,6 @@ add_library(moveit_macros INTERFACE) -target_include_directories(moveit_macros INTERFACE - $ - $ -) +target_include_directories( + moveit_macros INTERFACE $ + $) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/online_signal_smoothing/CMakeLists.txt b/moveit_core/online_signal_smoothing/CMakeLists.txt index 6fb01e2aad..bcfca1c75e 100644 --- a/moveit_core/online_signal_smoothing/CMakeLists.txt +++ b/moveit_core/online_signal_smoothing/CMakeLists.txt @@ -1,62 +1,76 @@ # Base class -add_library(moveit_smoothing_base SHARED - src/smoothing_base_class.cpp -) -target_include_directories(moveit_smoothing_base PUBLIC - $ - $ - $ -) -target_link_libraries(moveit_smoothing_base - ${Eigen_LIBRARIES} - moveit_macros -) +add_library(moveit_smoothing_base SHARED src/smoothing_base_class.cpp) +target_include_directories( + moveit_smoothing_base + PUBLIC $ + $ + $) +target_link_libraries(moveit_smoothing_base ${Eigen_LIBRARIES} moveit_macros) include(GenerateExportHeader) generate_export_header(moveit_smoothing_base) -set_target_properties(moveit_smoothing_base PROPERTIES VERSION - "${${PROJECT_NAME}_VERSION}" -) -ament_target_dependencies(moveit_smoothing_base - rclcpp - tf2_eigen -) +set_target_properties(moveit_smoothing_base + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_smoothing_base rclcpp tf2_eigen) # Plugin implementations -add_library(moveit_butterworth_filter SHARED - src/butterworth_filter.cpp -) +add_library(moveit_butterworth_filter SHARED src/butterworth_filter.cpp) generate_export_header(moveit_butterworth_filter) -target_include_directories(moveit_butterworth_filter PUBLIC - $ -) -set_target_properties(moveit_butterworth_filter PROPERTIES VERSION - "${${PROJECT_NAME}_VERSION}" -) - -generate_parameter_library(moveit_butterworth_parameters src/butterworth_parameters.yaml) - -target_link_libraries(moveit_butterworth_filter - moveit_butterworth_parameters - moveit_robot_model - moveit_smoothing_base -) -ament_target_dependencies(moveit_butterworth_filter - srdfdom # include dependency from moveit_robot_model - pluginlib -) +target_include_directories( + moveit_butterworth_filter + PUBLIC $) +set_target_properties(moveit_butterworth_filter + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +generate_parameter_library(moveit_butterworth_filter_parameters + src/butterworth_parameters.yaml) + +target_link_libraries( + moveit_butterworth_filter moveit_butterworth_filter_parameters + moveit_robot_model moveit_smoothing_base) +ament_target_dependencies( + moveit_butterworth_filter + srdfdom # include dependency from moveit_robot_model + pluginlib) + +add_library(moveit_acceleration_filter SHARED src/acceleration_filter.cpp) +generate_export_header(moveit_acceleration_filter) +target_include_directories( + moveit_acceleration_filter + PUBLIC $) +set_target_properties(moveit_acceleration_filter + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + +generate_parameter_library(moveit_acceleration_filter_parameters + src/acceleration_filter_parameters.yaml) + +target_link_libraries( + moveit_acceleration_filter moveit_acceleration_filter_parameters + moveit_robot_state moveit_smoothing_base osqp::osqp) +ament_target_dependencies( + moveit_acceleration_filter srdfdom # include dependency from + # moveit_robot_model + pluginlib) # Installation install(DIRECTORY include/ DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_smoothing_base_export.h DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_butterworth_filter_export.h DESTINATION include/moveit_core) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_smoothing_base_export.h + DESTINATION include/moveit_core) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_butterworth_filter_export.h + DESTINATION include/moveit_core) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_acceleration_filter_export.h + DESTINATION include/moveit_core) # Testing if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) find_package(ament_cmake_gtest REQUIRED) # Lowpass filter unit test ament_add_gtest(test_butterworth_filter test/test_butterworth_filter.cpp) target_link_libraries(test_butterworth_filter moveit_butterworth_filter) + + # Acceleration filter unit test + ament_add_gtest(test_acceleration_filter test/test_acceleration_filter.cpp) + target_link_libraries(test_acceleration_filter moveit_acceleration_filter + moveit_test_utils) endif() diff --git a/moveit_core/online_signal_smoothing/README.md b/moveit_core/online_signal_smoothing/README.md new file mode 100644 index 0000000000..f6197856c1 --- /dev/null +++ b/moveit_core/online_signal_smoothing/README.md @@ -0,0 +1,12 @@ +### AccelerationLimitedPlugin +Applies smoothing by limiting the acceleration between consecutive commands. +The purpose of the plugin is to prevent the robot's acceleration limits from being violated by instantaneous changes +to the servo command topics. + +There are three cases considered for acceleration-limiting illustrated in the following figures: +![acceleration_limiting_diagram.png](res/acceleration_limiting_diagram.png) +In the figures, the red arrows show the displacement that will occur given the current velocity. The blue line shows the displacement between the current position and the desired position. The black dashed line shows the maximum possible displacements that are within the acceleration limits. The green line shows the acceleration commands that will be used by this acceleration-limiting plugin. + +- Figure A: The desired position is within the acceleration limits. The next commanded point will be exactly the desired point. +- Figure B: The line between the current position and the desired position intersects the acceleration limits, but the reference position is not within the bounds. The next commanded point will be the point on the displacement line that is closest to the reference. +- Figure C: Neither the displacement line intersects the acceleration limits nor does the reference point lie within the limits. In this case, the next commanded point will be the one that minimizes the robot's velocity while maintaining its direction. diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h new file mode 100644 index 0000000000..361b14852e --- /dev/null +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.h @@ -0,0 +1,162 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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: Paul Gesel +Description: applies smoothing by limiting the acceleration between consecutive commands. +The purpose of the plugin is to prevent the robot's acceleration limits from being violated by instantaneous changes +to the servo command topics. + + In the diagrams below, the c-v lines show the displacement that will occur given the current velocity. The t-c lines + shows the displacement between the current position and the desired position. The dashed lines shows the maximum + possible displacements that are within the acceleration limits. The v-t lines shows the acceleration commands that + will be used by this acceleration-limiting plugin. The x point shows the position that will be used for each scenario. + +Scenario A: The desired position is within the acceleration limits. The next commanded point will be exactly the +desired point. + ________ + | | +c --|-----xt | + \__|__ v | + |________| + +Scenario B: The line between the current position and the desired position intersects the acceleration limits, but the +reference position is not within the bounds. The next commanded point will be the point on the displacement line that +is closest to the reference. + ________ + | | +c --|--------x------t + \__|__ v | + |________| + +Scenario C: Neither the displacement line intersects the acceleration limits nor does the reference point lie within +the limits. In this case, the next commanded point will be the one that minimizes the robot's velocity while +maintaining its direction. + ________ + | | +c --------x--- v | + \ | | + \ |________| + t + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace online_signal_smoothing +{ +MOVEIT_STRUCT_FORWARD(OSQPDataWrapper); + +// Plugin +class AccelerationLimitedPlugin : public SmoothingBaseClass +{ +public: + /** + * Initialize the acceleration based smoothing plugin + * @param node ROS node, used for parameter retrieval + * @param robot_model typically used to retrieve vel/accel/jerk limits + * @param num_joints number of actuated joints in the JointGroup Servo controls + * @return True if initialization was successful + */ + bool initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) override; + + /** + * Smooth the command signals for all DOF. This function limits the change in velocity using the acceleration + * specified in the robot model. + * @param positions array of joint position commands + * @param velocities array of joint velocity commands + * @param accelerations (unused) + * @return True if smoothing was successful + */ + bool doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, Eigen::VectorXd& accelerations) override; + + /** + * Reset to a given joint state. This method must be called before doSmoothing. + * @param positions reset the filters to the joint positions + * @param velocities reset the filters to the joint velocities + * @param accelerations (unused) + * @return True if reset was successful + */ + bool reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& accelerations) override; + + /** + * memory allocated by osqp is freed in destructor + */ + ~AccelerationLimitedPlugin() override + { + if (osqp_workspace_ != nullptr) + { + osqp_cleanup(osqp_workspace_); + } + } + +private: + /** \brief Pointer to rclcpp node handle. */ + rclcpp::Node::SharedPtr node_; + /** \brief Parameters for plugin. */ + online_signal_smoothing::Params params_; + /** \brief The number of joints in the robot's planning group. */ + size_t num_joints_; + /** \brief Last velocities and positions received */ + Eigen::VectorXd last_velocities_; + Eigen::VectorXd last_positions_; + /** \brief Intermediate variables used in calculations */ + Eigen::VectorXd cur_acceleration_; + Eigen::VectorXd positions_offset_; + Eigen::VectorXd velocities_offset_; + /** \brief Extracted joint limits from robot model */ + Eigen::VectorXd max_acceleration_limits_; + Eigen::VectorXd min_acceleration_limits_; + /** \brief Pointer to robot model */ + moveit::core::RobotModelConstPtr robot_model_; + /** \brief Constraint matrix for optimization problem */ + Eigen::SparseMatrix constraints_sparse_; + /** \brief osqp types used for optimization problem */ + OSQPDataWrapperPtr osqp_data_; + OSQPWorkspace* osqp_workspace_ = nullptr; + OSQPSettings osqp_settings_; +}; +} // namespace online_signal_smoothing diff --git a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h index bdcf6a7e5b..80212d2fd2 100644 --- a/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h +++ b/moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h @@ -41,8 +41,7 @@ #include -// Auto-generated -#include +#include #include #include diff --git a/moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png b/moveit_core/online_signal_smoothing/res/acceleration_limiting_diagram.png new file mode 100644 index 0000000000000000000000000000000000000000..fe32aa78d99b0c1b70cba43f85c531f21f5bcb9b GIT binary patch literal 68588 zcmcG$2RN30A2)ny*<_O$g>14j$_i3*i%Jbg^ENX(dy}H8Ieev z_L4|sy%d}9FVCY=&*F~_o*H__6nI^rI2D1vb9iZ+dL4GP^*U$eahhc3;_7@_+|$P6 z^l2AQdsnZi^_6P)CUN4MG(ApRd7W`};WIwte43==afVM?hELzhg-=FGMwU-XR#{d~ zd8ZuTq5XXO4;!D`{OJ&h#7EN8+GFgKGWqeGv2oYD{IuP~{tZ@}IfYp_)LuC;`Izs9 zC3QW|KG_@H`(>Akv@V}u-t#Eil>e7Y1cfl`Bt-;(VY0~r8jcj8PmWcm)y{5Q4Oc(TD|1jZc?Gf@{e}By^xc9&Qnr&e9 zfAh<2BHu~>zFU-;hlfn?rAu*fab%*RqEDTAo|I_v^JlCL_7*w%%nWaQT2y3M8Q^bv z{CHr;OrMg5*55A+6<1f+lIPE>>*_XgD!7E)zRlhK?p^dQF@FA~4f}40T{Ozkx_{vI z9iyDq0xL}})$?(;NgI(>R0Cnu+s*tTsOelO2Aq-%z&`PlKjabWoS)y5_&FV7ko z8R@h9EB7i_TW4q8lhbcnAKUDGbfS#4x3`z~?17sGVR!EEZP>VRYv9MnTKr@|vCn_I z#@@zz-!=Lyq~iEkJDPQmV7&kIS#|kOzCS*(IHaV_n)&NCg>u+wXjGG1%-h&VOe#|U zu$%q(LHp;AMc1z1-AU0)Yldas-v&Q@`jyslkoWuh+~e6t9^SBjWSFUwZB`^=^2n4d zFfedvV9h^?Yb*a>uM9~O7Z-`ug%NtM?|sK)FCHs$I5PU_lScZX3&tMX`1#jGvq?%s z9f~&4&l##p5hOo#(huPneEOjo|6(XcHyfT1a z#$xNEHMj9<<0(9zpcgOv-NTIp@!X8Rz1Sru$AYDu=zcsxzFmj(pJL5s*3;9gN>%ln zo*mm)TVG%O;lr{86W8L{(^*x&Uo zlUsM~;=}`Y^YRJ_3#<2w%`q>1(%X&4JM-x+XVdLn^Oy2}uRh%T_q7}5+_-){#H`3+ zsOjcTQqa?Q;Fi!;LoU*KJ{v| zp~m}~n)RNanl744NY-alb^ZL>_1LLLM%Jl&vvcp$#k-#67Zw?7!s){+E7hCczExjP z{Odj|EOT;lx(n^-bz;~rx>hW1a_q{}*4Ni}57*YxTC?!>mOqfDcG;>XEPZb*Pm+Qw z&+zbYh+w`}XjMlCbCE+Q$|I3z5-y zv=h-q{>$DaWo4R2kFu4PmUh4JQ@9~^EI6E=m-U>zmevN#H#fv5dJ2Wa#Kq&1lcTBG zBrIPBktzBw&Fu}W5)#>mw{OFicIq$Zs0pKGsxN%_pF&wS5AlU%o-Uyigb7wTsPmzC|((9~pLWxe++P(W0a`p2iYH3|b4F4bS=44gaL z<4H8&3F@EYX@S*^jg-%yKgU!N7pK`GOY!h%etn9PXMxw}_CVFWhYoGVo{f?{prBB! zu~9d%cnXP-}m-f9qi#m*+ z_X;JG;2_?#j%%x7BMTGLdOXK4TF$8YckS)dlRZ0QcvQ{4C5<%1W};zaL>8m|Yu>*7 zb}4zjBGy<${Hb1dwTaQlQdzY=6?0KLBeDPOQoKGjao}wp{A6r={kj@IGBhzkv2*9n zqLF7VAEP3fgsM?*?9_C0I#`oCIXR2@U8n!dlnLsy6RlhODg$}hB+NJ0qjnBGw$|3t z*4~qTsC<5XW?9+tI7MNr$~=#-T<*%dcG>imxQBXhzxEt`#lW@8 zo95exVm7(x?;pz)-shc&j)|dRXJ==&3DMeq)ZSirZf>rFu^8K2+sKH~+}u1wIzc~F zGn{UyE}BjFXdV~3dg!vt$LFEg4gq-ukr5NF9-f}=GsD`LHJKWrR2j>RjEtMPxi2eu zexleFOyUy~B47UXO*2vAgjOU2Uw*hAHo5vlufdh;3~g;~6jW5f<`UuY@l3bmoz42T zqHP#59DnYW)BC_C%P6v`$)F?KaO6cKgQR4B23>4~{>@jfbYGp4$-5<{WfAyWZX07{ z@3OnCtspL3EvHeoZQ!(>olSF!(!xlrdh63Ohn;$#QYtGen?5;3x$^sW8!=i;A8X{+ z`{`Y~aU%>DNsP?auC9jF-^=b;A!3UiKhD{ecY^8G!s{^)dwcsk-rssjvrDm0ojG0)9zK3YWcuLv?pIJ7eSq^fir21{<3Z`d)6sc1#C;adBGx zEJl>-t5>gHVnT$}@!!08Q+NOVjikxGQYK<}cH}UftPG&#=H^aHO0pcOzie9Oohxj$ zj+B4$MfK|hQFTRD!M(48_9aSem5`tt@So>K_YSM@U(VQ{EN_ZI?;fs$39p@PUg}{# zQ1QZEOjVV8XlSVWnezv~*vm07V%4uAvSqI|p2B)ti+|G?9ULT)lam*@^YZfIjuR&u z9YnaFJb5yWRkMEo=$Pq?^K!qwe~g|PX{hPx;Xs3JSeO}UyQ{~9?X?l}OZ2-N_0w}x zC&i9Gqp7HYs7dW`U9_}KJq;=7 z-aSDy6eA;}s2^uGGx%RuUtL&_{))?1RZ&%KD{?#vd}96eeXjk7XX4Fi>al%&<`^qS z#rGV#78)K-R5SZ7Z?P{OIhPLKKah?VUY#O$HF2=Se_5Vn*=DECXtN9%F1fr z^JKeOu?y|qy$$Q~frx(7UkPuFlQh@S*@T8T^YPpO8R=1OuKU<)mX3T2UO=MS5K5*f zB@KY745lqxqNb8JnA%dHqx4g$@pX>3m2*M<)?1obzb$XP3oH4ZZrE^FP0kW*I#+!)W%P zJPZLmR#um%P(<2wQ7qKId!?nN?b|XoUg6#qX8ZP*f4kDUb?c;jXHTW5`EQ}!B1^hD zuV(GyLbq?yCGFq8za#fJ z8vvb-xjEM+IxY%LP0h*PB5I=?=ACxU%;ybmcmXO_;~pQ|weZNEY2QF<%{CP1$UDIa zFhG6bPdDxwYK58X-2b(x4!T@H{_?cdpGHUNp2Kz1D;-? z4X<8Nyt{vJZ|8#}=~-E!`UxWbb4nP*gWtYg9vrmn?(SyZbKUp`c5w!JK*mQs>?u-E ze#H{i?*5R&8XD`i$T=8KbS3I%P0!3Mj#56Vt<{>F8sG&S*euu9dS> zJlJDrY00Ob1SIAk`OK+@|M|z<U|J>KEDfJTrZ{Oy&F4Hor!a@R?4b+!&8*up*%yziO-+Y58PJPJ$P_0;8cgnu-&OscQ7jn;F6M((s3p5=NFcH z2h*->-*-bwS|e0r-#+SrfaP3!r^VSZ0Ga!(v6;dgTeehpcK(cy#m-f}79Zb+huG$W zZl95n(OT?kQDnA^MtvzOYx@J^oY}^%?ffDK&`(VZZ7<6WOX09rdcwO^e786_qmxVOR$5Kq*NarD#M z^h7}il=O%j&e@j~NV9hlvVyw?Q`6BAx{^Wo; zJ2E@FamEiZi-%u#F_YIHE5SH^9eY?Xp5=@0+R5fF3=4uRbU0q$(Ae1Mv$7zKMX_{p z5_>*{=Mr({N?2}gZm+vY0u?%8P2PzzwWHa_I^|_==5Rq2^z;pQ7p>UF=4OdF(b%<> z9|ut|MYo0UyCN=M)}HEr(RroRZD?J<@1I?g_sn@25>y2R1*Oo7UEYj&tV{*2X~y!X z_8(s-t)|9fXlN*y;~4))qc~ zM=k2=>IUWI<+U<_Kj_S@E&=;%?cFPY{rL8l+$H>!OJ4~YFq87xFE6XB*X{0K#yBZ! zd**!H^w_b;JOYcLzSGgG9Haq`fyMH4nfk>Z9(hfmoOr?g8n}nHd;2-D_X6gWFuJ!U zzIlAA@fv@`RY1TfeH6hJE~Q`|)$?R|VT|j{2wYorn9sGSCpX`wgD zF@C!9bLYdOrhp@E%RPm5-6ie3UX{qzjeCfY3_SFdhX30PrYo}X$! z3zqU2)ph#tY`@pXWChpiH*fwF_9020SE4_PPQPJoN>O6M7l^j%{k<;|BiyPjL%X}c z>e7VD=g*%>cis!VZD=50T3XVOnXipxBFzeRqbX^P5g;lGv?H+n>s~EmMxuSA?>j^f z&hFd0-dFE?Tjt)H=4LwXUEZ}AfYPU5Gk5p)W}N-{-p$>;3N;im-w#MTg1evSf3d5* zy|F0~8n zzkdC~R$&p}L`h4#kwn9-Z2ZlP*oHT69=kZUIp3o43SdLvQuI7o#lmO=xXxXp9Vj9i zlF#fYCDDDc6J5*+43i*)3^NQJWxol+Y*Ie4qr#JNJg5z=&>>szALN~wn6R9i zXj{N!H@tsRoV7uQQ)Sa1_Fd#d|%dN#juwHho~+NDoXn3*a?)z75Fb#3+c zL~oI(ZF5Q_8s1A(9DqXA{6JuAoWNddpy6sec@lwz0GI6(W()0F3QjdLdVT9Y>nql%N|oZe%MM;|nMg!9KNf z-?=VaxWE^uJB^)BunN#eVT%eCSvk43a$nC!CtqwK8bM*djQQco(OEe zs|UG9+idXyG+-PyZeW z6Yq05S5{W2RDNNt3G#4-M>Pt-EHKO-oJVt;py2Re)g?l~NKNH2$uo~U`Qm(}NpNdJ z+?z4`&Ic_075>rS1-MFP^km`}jCv>m^bP`emztmKOJB_#Z%M0p|DF{~zb8vSLCUpX zWnp<)6A&@Y#=yYfP`buO+-kYc%sPypy`fYr#J}inV>He`$&HQ;B$PLC(A+%5jMf8K z)njq?wAW-e14ev35DnR=+!_E638cGJ>ZCR;Od2yl^-p{9!2K~QHS;+KG$w*{EI-lJUl$O z{W`$q0_P9%U}@`%T?cdy9H6UMTlHH3{Q{{XzgjFT04f4?3o~mkCbIT^PbMZND_h&J z&+qP2+Z;?&V-f!n85`Tsnywj_k`lutbl?u?CTZ4$yrHfx9m0{K+aQUHi%SX@V{bpY zf8Q7=)vjH~7b@42AZ}btNMOMnSjU$?)teT$N(q7Nl&vlQ&Yeu?TP)Y&&>M(r3M%J* zV%L&dSK>YrHJk;QAaiLGZS^fV$E$cxkj&cJT2jz-03`_!v>Gip^nFS-}TA@#l@7c4b zptzXrwVt}~?1p>y?q!?g_6A-EtwTrE1)3$6-0;MAjE<_lzH;`=CD#l7{#H&-G;C~a zZ3`$E_g|AeflmF!XxR{lwT+8`r=yM%J~$FEQ=G>b5#5<(cGeA$IcDuKe|+zKpjx~7JP6vnAlSD1bl#Pki% zy*~3l&=?vS5xC{#*pbZu8T_$L1D$cM*^#8QtGe5glspB4%3IC=Q!##v!Hi%=C2tp5 zJY#Pkk(9)aMn8z_mfE?qCW1kLfD0?rHS~)3wr8#b5(bk19R!E&t_)O1BN5df-pr7| zN{scX$~8LBuo#Y=o3O~ci(NjtEzO-lpAa)I**x|-fk|#hi)1?2=FKGRvbH`q0|Oci zZbIm5JL^5wPYbiau!X_G!U9)t2V!}(>FW1u;4|P~0KaGrAs?VH?FQ!TT#6jsiS|}p zTr8Yk2)sY{?E@R6qrGq*(Cb1A3Z!tiETjU=`rSeG$Y+@nC!RCoc?Gy;%G@;A^p_~k($6IPvai(B%GGr( zt)%2Al}ARRnhT9t84`p+ep)*^YO@V9Nu>Ah-vcAO zq*q@F0%%4ffRN6qpY_D}=X*tuQ3~6pq>E_9)RjlkU76;>e}&?>s3 z3_I5v@^Z#hGH66_SeVYmuBKva0L*=}67j@0ovb520jE&MgfNd;Oo$jS(HdZ4=zx}1 zS635-v-KDHk2TOH(d0#pvq=XJw#53dsB^^&WEtR=t*xy))VN5#qDP=sXqQ=IhOW+6 zu2uE-^Q^9}R{KD5g4(z$CoSVUJ6i3(PRm%N@steYLDUY>4eGd?*~ULE^|zDwC05iDv*68VraP(3tr__!f;vhTA3cInfcNw zN+2&;9yEH;dxKlDda_U=JXOxmQT!NhSvyFuPABN__1Mv&A6y42;4@G?KQ~2eRFdz{ zyMV^jhY!CSruvbID}&l*19`$hRO-hYRp#p?N=iyYKMNs|9C_FN7!c;jdSF`$Qbyxr zf}RQrZbCcOhFk?~)As1t&TZSb4-xthHWt)!^hod#8a`K|Ax3>^wkY-3xMRl-`}dDH zPL}&z#O2x4Mlhre-DG4)P^kw4Qsdlx(x?PN#FXlR^h1LfX0RFx?N@-U+}zwYn;jte zmhB;WJQ@uTRp`2tCr>6xS*?RL+FR@@sk{mi5Nue1Xm{A5F}&(LxL=mnio_e^PBQM! zLl2XJaZ|)jt#5hU)RZtGHmcnR&r1j5o$~Kr>twsl6&Vrn5>Gy4OCQ86O>}AkL7~{@ zzJKHd9yU7ef6NGkcEhGk*1#xN-;5O$71d#P&Y^UTokn!X&wuZyhx$VaS_|G>bNoTl+YFvb@_!2Z(c9AstO*T$< zTQ%L5jB7AzXfIy82op8W{sKfihqNqUOsaF|&P{y%TJ!E56ZEZ(Bn4IF-G! z-3;)qTNGTxWcQN?K|aWUU$M=uOt|qd(dwJE~+-VnLa9fMNDoNU=L#-rG zSe~>kBN&pll+rAZl9G}JhYzv#1I1}&5#CiAb@*?Zu|Y?xR6^4 zt`ZN*&j0wd-!SrZLoDye+~;=%k00+ruOh$>wuJz^QCd#<5bzTj85y_XT1xb}>ekjn z&T=gna_--H>1J?pbn@tY!s`zZi&9=UKR})0GUhDGYb^WZ~7A*kG z?|r4~A*chN*ETe09zM+U7rhJu7CfY5kY%)0MaBH7M|ecG;)-e&FXl7k)uA#PQhJ={ zD%%&ZJR7tLpScMJ0K~ZX_)8$7g8)N5t1C+NF&vj5`i$Uz(A_c$I0J^NLml*Ppp6my zk+9XCJ(I6k`XWk*66-h7>;|?CY0%czwgRcq)ziCp>C$@e{&1k4A#C(lZ{OBI=OrU? zaB!5I{mLqGLVELOuQI{1868JMbG_)?zM=nw=_WE^Zpp2q~n#JF$ z{he9+*i_&Y`~8~SgBlBwF~_(d$US~L0JQQ;l#QaGh={)UOgpHCd5Iet`eRs5&gaU2 z0MJSoXx-agPLAR#2&J#r$J#o>Y$HFgE5Qu&}AjA5^ZX{!(z6kzw&ej}4Tg)=Zy9^@8? zfY^c)6BDyjUWL#8`lW-}-gEUg(+XsMc11Tn5Rxzm4E$nZG$2#;FbF~# z2+{#uq~Wm*H4BzaVAeyx(D>pes!)xE;)mv?GR>)~(Jx=_PQTa(vlLz>9oh(KHw0>I z>7n7_U_j>)7+lp_FH1egrL1cy2s8)n=n&)|MR&ZPSZjs9A2c#+@CMTCvqO&d_SyUI z)Ya7yT1vHu0eBS}-9{4O)DpG;h6(%#RrF7ZYh8&E6O{y3-9*D4M8zUPVS_G#W~gzv z4JeUth~Sfk19o8wgo4a#9y~}7gs%-bfM9hr95NdKAH!e+qFubtw}>UI!*Og70+32U z=OEU3%a$z*zvl;LKDbt&bFvjGdg^jfrDd*nR{A7LMS?>;kAhDyV=G$jNkiT zh%Nj^y*YQp9&H$BZ8-4Je z9<1ud!xa+Jfk!(uHWmtU7^!yQ?!9{iXLb1S*we$K0FGTiiC_k!D1=(LvNf32V88$n zGN;fhC}?QJ`Uk1vaGP+t!pPQd+RYzF@ldj#g^O#Y|F$oRmvGkH2VY(^Ivn_7n=g_^ zF8$>@WX%YDUPVR4ZK_`tXzeBg48|8$&c%ce}g3OMvff+h5VNsH%f!i>DHFFOrGPI3*!#>x|G8&p+ zfg?Ko>)Y|RWXmXT6XvZ)Dc8)* zOvYDK>=yhLCNT4B;eA;~1o=N78pEZ;h)hFkdpnTuCMaOw92ZfA)xEu2P^v6~%;+DM z4h~fCcMap)P>oHgs+^a%$OS_?LMJOY^G*=j;0Sgad>Q6%#-(1L8HIv!YTH{|gWwUF zaU3K<0*w-pTI-mY;@1VHUcP+UFwb16=m&;8hRxw`Dv$^uzg>tF_X8$r=U`{2@En?w zk(M^}lf0q34PPoRP@I|?_nlWY$8drdkWmXp|7?17?YqruRPzHyftPI0Jx8bHijfR) zgRlXce7JCB{gH}gq9b2LI8mG`}gmMgqa~8*J4|RQeK222`=%#+5gwfOjQ(%DAA6Pm$(Bf zo}lZ{?~=K|D(Pt~_qY2U8&G8;$KjE1r-s56`LHzI7`KXoh;% zbWpi+7p|Z#l$uS8`aQOHIN^gZazc}e+@=TjK*?u@9SlwzggSn3G@p)C;yF%7c<`s+++caXZ$FVD zOq+-{xU%vQ`{-(XyugLlO?#~C*=hn*qhJkR9N=@Ub41jN^D(1g zq+pzGhv3S-UOzW0N^0s$(b3VpbQ(}1U;|av)oFiALIWMd4I_F`G_9=azr;+ydXuL1 zN&qmXgd>f`u=)d41Otu4WM!E#;4@?xg$|~M6crU6dP4O|H2v)IFK0u>4D=nsoXXFa zgax%V#?iqc!@4et08)gMa;w!3z9Se;-VLs{_I8nJCBohp(1|(3>Dphe1PHr2v&g`} z5{@*^FzPyKryU&v1eedeySEidmc#c9_Mq3nsEZ-=%dw~ZEG#UcD3YQn1zb$+xdDHn zE#SC3#v%#XUGIQ>JG~=FFTuPYyL?n`>n;=RpPw3_=j2SDm+<0*Kr3=IF9uPgJE-OC zqxn1)i=Q}&FcS>0>H@18#1q4Sr${73E9tRT#2`f5z633iNq~45+iTepxeSGw3E2vM zj+A{XAE>Pj&?upE=PD})uBte6<IiN;F1TCDzdCp&c+b`vTXXt1c`pSp&9LlYi^(HZ^~7o&|A-DqQ2$q@)NyQIM7q zba9xM;n_J4p^~9#bf$hF^wWnX;z_q|@goEZP>iOd=svswRj2*X5(DLdNdWR~QT;IQ z2@?i!9X;8+L&60>+}$5OJnTFffajHGUdrD~OB&6U$c20u*I?j_XPf@3><3y8s3D7g z7y>imX3v}`F=3!=Q||W#exk!&8|N5p#Dirp(OB>Mvj|T{LiwDoE(Jy_!KTxdmhayf z-Nr9+1`VX+mcmzbn9D4+H8q;L5~IjU)S!nHfG@QzREJVaJ9SH;O$#V0vV(%wVTrT` zE|d6FRJb7G+IJUhWn^NCOGw!MMLp+`u-mW6V^BFY=T`k;IT>El0Y;IKkk}~UHU0DF z5zcq$?8(W=_QhKn5&}T9SXgCqj75f~0;l#8PxS*^@hJs3v*7-94~{&;iFk-RN zkX?ggV`HZmuvS)pA4FgayHk7oWRXKcuU0P+{5${Cf%Gy%J38vi83u*~5j1n4<&6GA zBtjxJ&NDyuv`i9C9g)HSup>woei_*-(ZZAgN5)slC38Q1XhK3YM6wQen;aWO5xd7= z|Na@MJ=?Zz8^j2*?|qt6P$-8|1`SLbI>uypMj!#~w5+$uHElHA+l5J6$AXRX~hu|y|oUGjU2j;0{-`c{AjX|>f z{*HG9BFJbUX>>fAE}N>T!Shcy7J2OZ!xq4Y34&(m+AtEV z>(BzA4p+fUhQ3orh`VTCgmH?ZM3N0a5lm-qjwtUKfF{IGuZdQ&W5g!h{CD{rG z&`QQ11|VrR;2;vZMzZpVhN!^GQ2#x5_~Q6l$Lf)}83@vs(HW!lUyO~8@+m8ub@1wB z837ZdquWv-&;zwcye|T*aHfiF6MKsCS$&@rSkrjCNd9buyoh~w z2VjTrselgn67-2xX}TpxO;A^(oLX=IzW7F@P6k@0A3>}FmDfd=e^7Es*a;m5o(jDs zBwQq(_5fi4m-sBr?En}gQfKRg1_}QVXdBMd;iNkeQBfLDz6hDYP}LlwypOLh5k$`L zM9;G!EZ~_2-p*!M{JxSB#Ws<97y<<3aA2|_5*|p0+_=FC@NDVgLXaC=I)NGhld3ly zfXCx9P(cex1nLk=@2OsF3E1N~@+YrxK4CH(TD%`N3I5Fvs6u`lcB2)B1-q>MW-e~- z^h3g}8QRnxa3A2u5*`NVF%g_10_C}rhxD=z2}^lTni3TWPk1-D5!vXsEgaI;8wflG z58{wl)9lzAX)qN)`Ap2+ItKX{4HyD{Fn5b3loC2F#UR(p-zIb2Ao>(p5|t1x+7_S` z8-nn+1xFFs=-s{jgf1pbHjBLg74T2`sv(gXZeEyy;9*XSVi)eaMrWjPbeP#B_(TW00#afH{nuTSfOOV%Fex8^3drkqA8Q+zn6M;d0ZO1WJ?8 z%Oj&9AOU|yds~AEYDgqz;%Gv~2zJnAJlk{_k`Ci^6coGh-dd5ML(F~8&;Av@9@%eT z(29A~mIUV9%YL=1L@w|?6@#JP?A+(W@UL-847>ZkD+&k*AXrXOCIoetZ13K^MZLq@ zdte5+O?FGG05O4oR+mIZQWB}yDRRDi$s+&~N$fk)`)?n5b3ZdP^9DN^-*wgH))_+Z zFqMA@hLgc9sHJti*wLf$+IEK#xQ~+4Glb6roY7gI`{+>zt0(3`luM%MQ98m?y^J>g zb!43&;?)^O(QhpK-7!_fObd3)dP8uNwQt>RpGQYRHU=Ij1GX0t4zyZ9N-+CGnLKes z1Q|6xkh~ICyo1BfaXjxUK(O%sx$YlKt4Aa-`cotjSUiSx#<{8f>Th6rd^jcoFQa<- z3lIlF9G8s_8+`?ON79QH4M$KvhO`^D95Yp@S@0_IEhXoFiU2t+DQRy1>*xl5M&QfQ zS8HrY(v6Ou>h9@@tiKc-EamuKtj!dY3@W-k1OZU$TBcQ zlDzX4c%2~|saXlTw_1W0VR}5(8k}h%5~%u~J-xlwP;(W1XYac8BM_2_tA>HOc}Yeb zgb?dU=m$LK#+$kC-o1MV&;{OdL17``8gGW_jo|>Dk=&*}hJh#FScJ&10xm|Sf|g4H zkqr(HuOkt9q14$g#=!r?0G4y;*!)enr^t~Fa;cGUzMP?u!3HY8+2 z+sMFgzX~MYh@2kLKX6l5(XxqH9n3_uY*0=)$a&;Y3&Ennk1l30|9vkJQ4H}QNygg{ z@V0Vxz6|C-K&|Y<_vr|GgUB8u5`#TX;9R5^B(8U~16KaAcF~DUKIEJOaFliJ=R%M? zV+Jx76S^s*3;He|pr)}hH3{*`qh4P+^kkpezTGl3wdlXU_nGr@Y>Okf6XYO}1~w82 z4Xz$y0%4dz`$GnN2xV_q`JIkm76vX7@{(K1F_7}#=TJp>3EBJ-UO zp1lj6G=hA*KQrP={fy-B9^~T}?`AGvFkQ!<$lqiEp$|zl1%tHNLUn>{k;N z7@eq^muTT+Bmm?ZL|2GJ*H&-Dkcs3v_9Ss$04ayN@ALmXA*m)woW#n0MFf>6wqI-M z=~=Y!;~=7a@HN8Wd`bO+M29mh>3A^JNkCXn&%A4`3=AX%#m3U3ns97^27$CH&`qC7 zL|PC4Z9~4Cs6R{uEd(w9mK5#JoEgH65Jb@X9-*a}7Lag=jiK&7_5)%W59&Sw!+1^g zpK$I2RoA_PNHp;>{JcHB5ty!{9&zcv;6z$fBLhFb$WGLNCeVW)gk=;6@Nc<Cs*ViyHxse^Ge6{#TCg3tO(kLktP!}xXCSaX#_#v=t=}GqQ?$IK3Lxhe%{RnOa zv12ztG9jMh{2vh|L4uqhiG)po=S_4J2v9XpKnUWA*&MC!`PYguFtjCJ{If!akkmqO zzv+K0MgS|M6Euko1BJS}I)b&D$PeS#2lMx2IY-vjrLU4uw=-~^L|_6l2Idb;7Djj6!(zgk+v#G89FQ}q9w;UwU|l84*)zlST^$Z^t+0hu2X z#BOvUG7@-R!-)wktt^Uofw@@x?P%ZDze@%Q5ZK(%mxRAdVSuRrdm>m13FV=wseJkG zpFVBEnKSFZ-aOmw@qgY4YnZUZ;L1P0im1Sw)&Du=`9J+~*d%fHck2fRcrmO0l!Q^v zQ}(|>4Zn!WU%xNz)}1?d0{{Kp3EpB4|Ahhg`O{QHC;azid{62>M}q%nUoP0X3kXbC zFdi-0h}bk-$o!%X|9Ke>SrX^csF*o8C_!b5-0ke_{_A|HvCT2@f36UJiX8sW5nB8; zLnf3cp?_Z@hr9nA>-*<%t%pz#fX1SDAd8xK05~2Ift^LoFY9qg1^Zz3+lFwUywf4s^6#Txv-zf9Xhj4|D}>T=dM^s~~G82?(YjOvEJt|P~t zxbw|_FIp7Me-2d>@BY86tJMFl2j1O+@i;=Aa~YO~ZdN;LSB>S*=;X^WHnY^Ug`#gA z0afSED|-Ql4F7-olPIw_{(ez$`~B|~{C`~-FOHIEgMpDqFg_CWHFF%4q)ARF3InQ}|UP?|r zPH;%Pj7$E9B8>RoKHRVAO*|l@k*O*An+EsfSJ%4BeoPC$@mm|2YSdwTU#sP9Tg`KW z|B0}8NaP@uOYQ4?@dQ2Rhdh40W{w{V^d1xyb@Ci{E*gKjzd%Ae`9XGWu#~1&2*2?L z+U2Ir6k92+-pCvqpKnkUkS;uZaPY_Lx+DIdZrXisJdOXf_y<CEX2Iup%NRHrivC=IpK_F24UizjMw(((s=v#lN-paM88!GJIy}@cjFx@W1TePEmVN96-hG#h(ggLq9%_H?X>;p$`}H<=w7{9pM1IeI_;@X!HpZUE<#vRH)K$)v2TmEP{*>>u$&1vA9;@B4dQo{rw;>n^A|tUv68!|DZb2b2dF ztJC)K*fyO|(Ix7{+unw6IKggaj-9eTIMiGtWgGuTjlX}`$47`FrMYFFoqt@<T$%yXlC7Mjze*C+aGktjDJBw|mQq~^3^Yp4BbY1^!5-ly_ zQ0dq2TXpw-&X!>;O;cbjvaAYeboCXN{Of(44Fsi4@+(I)gvG66-yHT zMCZGo;q!&RtD#x0e|N3zkL%a$B?~#kZhQ)tvS~O#wSz3}h9=#Cl+kOP(_F;u-!_+E zgGY6@q9PUc9`Y1`F@wR>5-%;Kyru>X1Y2HQ?dki@BOqTWglFQk-ci}I#i7eFQn$rgE+g|PNgVTNoczj7zx>)+>drq9-MgS<|$&+i4w{T{( zdux}og9G6ktxT=0N|MuYS>mk9^~6Na7S(y;JP{+I+HIEw`M&K!}tmM}(n zTI+6C#Kd#^qUY2=@ zA-Zm7&SpFVd8Z_)jVeO1<#%F1pmfK(i(Pru7aI@aDAx}8Ck6 z5KKr&fH`b*>{ugc|Khu})mk{?rC;;PkUyXNZWV?&#E=NRIt}M(TU6~5CKK1;U9q^WG z5WQTlHlADDZ;g|2u#00xM@Q*6_vzXe3h^~I(Jb%iId(OkdnM(O)2mu_aU~P!UR{vG$d`1C#x?S1sVEyQ)XW8l!(+%pY44G{aJx+pKw_e1dJ>*a8I9s`i{dZsYg$o zcpDoqC-C6m!_v`f`<@4^>#?5cQxAPyCa|-s&~aN*pFv9=!%L=$4Lzqt$5VrY*UkR; zah$8=)hj-Ho(8ARHs>sCdyO9o9G)gNCR0>~`#oMlH|i<%6e|Acoji4KCqI8MkO1;5 z_0S0#TUy2uAXYus?`x+JD_?@E1j8%yW9R3;cKf6I<<8@ov}0Qhl3s03IEHG(Xh>r%CzS64Y-?;0=h}JI~Ke1f@41iPC~1Af#}pF`u8#Y&iYc z=X;LRv!MHTkE{CY1V6s#w-mEr-xZ;opOq@WKb%$^P})PYV@K+3e!EN?^=pQ)!5U83o(EGV!5GrMSCD5 za0t!xl*sFrAdcm}gbHu3g1%008QD{7Ncd+v3HyzMAAvAZ+NF`f*KV&=oZ z>J0#Z?=R;2S!0t81p8bR2rGEZgMpWKfHsqvA?{?&so4vH$!}Bd+uTKdwH7_x(%+}NPIcuSbJXWI1W;Km)&L%&{^;=N2>c$ z)o$!*MBGlOC^In3j3e}3{-b#pj+doD#WplD(m>$d=S$9g(DSd~hkQ+xdYw*`i9B9U zRP>T}>;`IGv9#YCKAa)OjcV+hv0?SKMSAp6rvB z)H>Tsb91)gT!Olf$pddSlKo;wk&$KpfRvRxkN=vzK30E1C4r$qW^C_W^6`X*J&IsL6h4Q zRAu?{oA>(ZR!#b|{+Tn9*?D=k5aErDjR(EUIFLqwzBT)?i%Z!n0&bQEQn82+-h>Ur zX70A9)iG7cGYmS+VKcKYqV%ee&e6Pg2n$=nk43NQpWniJVVVq?&>7XWOY~}Y9zTvl zq#FlXnh`c2J9s)2jw14QJsg!$pPm#RZwl;gN;NX8N^-`3#d)P3QkK0^+P%}05m%?$ z@lo^Yl}`XAju)Pt_EPH!sEJdVZC=*93?hj9HqZ3T{j`8v@RcjK7i(#4D+%WfNJ4MG zXZpNVs9fB?gm>K9#l6#(X8z+jUYFuzsHulcO?A!8wlFZLK2l8stVmC~$uZmE{W)ZI z^RZLD65QMPX1+@(4NPsvP9GPQl)Mj9sVuxL7*D=YC~X6ulG07cp0i6|^F80nbrs-5 z4ACKByyDDo`m<*@h^_G{*>(1-#o8@MR+U|Sm|B5r%L7Wjv)gdGWE(7u#5Z-26H1r9 zJjA*UR#Quk4o6G=QoWBp4LNdo^>MSV?0KKU}gttsGf?NhQDS=;)ZngD8Y1 zTCw^wA6j~fZP&y6d^=dqE5C{>Pb2DND#nW5sfk^lou5CB3mv%>PFYCeBI7>0#R33o1En_qxf{XQeK#4hLYpEDE@ z5h1l~=!7s^f~W60c#&LKLc-;TBW{cMm=2ecp-r#fya5v6-@ZMo#^G%xdQ~~Tb7h#V za*%UiK_}e&Fq%jqV#Huxz6(R)yT+ELDwZV>1w@v&xQA`N@QAT75rRO1SM&7gto)~1 ziBC+G$EkhUu7~5Wjs7%AFr4cTsL!2i+Sx*UenQ2M+fxgyBT(dJ+=sQ)mL>#vd`B6u zd)~mkz^Tk})FWZWc64?w>WAERh6<0k#=4*nE@hXon-^I_U@m!f9y#ick3hj;9d;#; zZRi}oR2A@%VQ{_BAbM~P2@X8T+`H#=#Rxv}=Qd|dw^QokRjlmI()3GleNB8-6h5qX zqFyQO*|k@zdAoeFUAV5@g+qZ>Zze8$TBg;IFG#Pk=>6tB;!)~WYTnV+rH~u}ypRc$ zNdQ?+&JQbFpvqO$yWyk&1m(w;Rlz~yYrm%g@1txHU5kV1HrIQ+*kvr?=*gkV)vuMn zfoGB%g!%bzKfH4%2xl{JbXxDih4KMUVr(LI!GSb3nk&6&aK>Sd%c(8r3rl}T^ESWN z9^2ZKgaB;FjzJ}*(ZO0h2?s9`RPSpXw8m^^fj08?`+m?e7`a$ALko)*cs1-&mLw9o zhb>OSB)$E_y2}>Hr`P!bKSg2e*Pb}{v2J^!d3%QbrTOQ$kv(tX%yLbD*5OT%P30?_ zpG+N)x%=7Vpi-{oqt9S2WI05y1Xb5{?W(W;k~?e{>Tsvs>{&~I@{^|~7)T0zZtICz z0^HUm)@&(7M2Gt?{Me7c4bB4z^!_#r4GqP?FPxyT&#PQLUpa1UVls;bOT%G%(Ba@P z0c0SCi@HvZ+)H*Del41B%;e1w$KwR&73aCp{Dy&mL|H)GatBw;K)*4C5A$0L(R*7D65 z)%n2A^cC5Y;Zm34RH$u_pB!XrvTad~k4V{RY_jjJ!t9N=GVE%S$(=V>9>q+2X*a{+ z2*PBf;gpZTFd3LZuf`Y-YqNLqF3){rLKG{!HsVZX<@lrH#aHmy+ObvOIC4&#VfrRo z1l&aJKgJs52 z^LyHdeEX9MtZAQL+V(d$p%6Rq{5nF_#fm45@gWtLIf=@{2i{RWoFBuCdrd7_z8g<8 z@#Og(Oo9f}e*M7b7pp9ZaRey0q-qClWqjla25aMyqbE9_D|F%_?>}8&`?QzqUTamv zd}^&zBW*>z36N8>l3BG_`*E*RHb37}$Q)}=$SQjN8ZC)vkiCxiv_8#_`7d;ky~Z=S zV}Y|8El4>H-9FbpbZwu+8F!`$k5aU5I|K!|-=iVl%^VvaU(Qv(4JiOIW`&q+Pk;1( zG-oM%Tmd@uO6rK4BH@V8($boHG;T+-F*|zi;!Tn$|A$AMZ#0vn7QIt8_3jz2{k-eo zA1<_y?WB)Yv<;dXm!ew`TrrO}Z8m9~^S;`ZgZ7RC*LUwFFIL(Ym#NoWTb!)Mwi z+wNA{i<%MQnV;`2i|4Y~dt}~!_~_ASW~>@?w9l3kJn*qoSf(7bb8b{rU4-cB#H$IE)bwyItC$Lpas{*D(V@ zra?-P0APYbM7|NVB@S&O)xO^Y~D ze^bu!#?&J8p00r}fEq3)))`NqUcb1W+yGVyQhbN%o9Uw5NlkWAYJ2;%jLaW+_RaE>`$_t$_S*9;FL| zm*^v6W9OG|cR6C)BQX_)9M7gny>=IL=!REp^vSBbbb|4rUQucBPoAjEpRgP7JZ)F( zAH&~n^JjHEJf7QyYG84H%<$4Fx_S8w#N|yR*yM>vgskNHyazqcoH?_yvMTO*T8w&> zmdyA25d{ZKWP2&?5FW%MzkQD*BIYcC;I3U`BxV!rrn-mu6|Zrw{twoeYu|Sa(NAQ_ zPs5rt_Z>@6`PFSh>{~<=Fn$(4DOBpgA}Vy0N8iF(1Dug1R0>2ko01ji0myJj=fKI6 zJZmfC>h7iHZ}lDar>gX@Z3b7J>9&cTd1JAPW8*h)9AJ#vfn?$N?!Y^aDvG(J+08mJ zAgpvs6(}#|L6jK7CNp%lq@ zYiUqvX{n?ll(e)|(k_+O?>ODx&+qa5{&D|tdsEl-dY$KU9LMoIo(~t5(Svx+)f%CE zStYb<*Bxv39^T@UyU+|zDlelY?D1g~7*oy#o`K?1H-1m);oa@Mdp%Ky+Yi5Lp6HD} zGJ!fQ2wn#rp7U2rGOT5|XYpHGM>@j-tMdoILoSRsmEa)qxKF1TJ%MEprOj)3k(7aL zsL(Kq8;IFYq80(S{Q_5Ts%2+s-bt|eAH<|1+ z1)8*alQmk&ry!0XRXc`w94w0_CnoF?yp~)#+(yX@)O{9>^1vPqUT5;5+}igmatkKo zGTi*ncqtv-)2R3475b+t#QhqNnf5A_f-X2ABvt{m<3l;v6SOtK7$ZDN(_fB%vyw;b z2b5@xAqCGHY0nZ|gA*5jTeRVy;@NvWL>xV3Pb9Ii(GM@Abl8FPwNU zUhH)QI_qIad$=0f0^^tsUk_vsnR%pm{+j354MBR3Bdcx*8@KCIqbASb=8}tgKv$Qf z+B%gR^V36GP+UP&V}Y8VGz#ARwXe#%gaBxPXU-KnWBJNp4uF;#rXtp>%o>MqMB*@z z?2n>Q>dioscQgfA9eu5w%?~wXjyF==OwY#O)b!4^$;T-RAIuZyb*oN~%DyLKQo4dw zMrb_d+xyMA&BMny5b!Ngagw))qR0jh$9TuR=g*)2fa1n{cB~7f=&Nrh&z?M4DH@?v zW)`8`l-hO@YEW)fPbC6Kd+L~Y;Jozq^}+nYe=b{t6L}KYXpduo2L_r)JL+%;^AKe2 zw|_pVub+i(=c(?4U(EUjo2##V|1D8nvuWO-h7q+nD9Mt%E{wa)-|U3J5U901&WPN{PPZiaYma49lDs6@hxfAdC>0+88pNUh}*VFToj7ftcu!)sL+2an`;2a9tnyEK-GO2>DM{%{_IW8T zG#C(Q1Y~$E4y9#$>9TPiaRGEU*o0D#BV~l2mdrEE7iHKNeMIFE`Xd}`j2iry_r98e zIvroyfC5E|e|eYXYquXEsJ4moFlkKQ-`*yvZ{o0V^!>-FsWBb%oW5NKpYDDQ{B)|L zJLLOugA`OWZlgubr&d*pOG$-emNeUO*Z>Lk473{*#1mv-+foKDXj?jEkIS3-@a^sxTcF^gAt)aE1Q+)yq4kgC|%!YC`xH zO4c*3>l`_G*dY+O$FFz1*1eGJxeuM~?1phkA4;?ZpgY~L=c*cXFwG`DgKIS(M?ncn z(1_^!_upekG$<_Ws%;UH5urH7;yM>@l8ymNU=tZ`qWU6@6@j23Z#8epx`=7z5Qvt6 zPF16(xl1ZMMd;p@NpFu6e?(=@4@CjdqVug|gyVtpP^V?&j>qT*6Qhh%PrvFpK{Ehr zSwsx+aE^(8n5Sw*Ys&NIfv7w`!K(zo@9&v-bjpTai`Pjla;<1uAEhX8D}I$2Z=|@x z`z+G_SXHJ|GbFMWZVC-=OPj{sB`|RoT50nt2-Ja0l;OxD?RI)GqOUt{mVC|4XBw-F*4eO=nvdie012%G6O1 zw{TX$P8npH_A_pID{hF6CV7vhp3A?57?GaW9sE+~ipu|>pUC_7H_CkJfd3vUtj(J@ z`wQ>CIeWhJ^s!@Q&-~*e82(QSkmz|3?F&+$q~j|GvC&}dXL7SH`T3<>au+Pz(z~8< zI-OM+D$w}{kxFLvtvx~j((1VZyy~et0%1WxCV1mYV{cdD)B@)Mw* zJCk+cK=u%tXF~!Bi8G2XdKlW#V^vO0f z*E*OTOAzrCY{Y|)8F|3mFY z&_TQW74r7dl9H8OHb((tKqlMyi`cb3V%V+#%`}<}-oT)sF3n(f=(#2v9O%zVDDc&KS)B112j=;*w z5p97vTD=Xrt!t6#;4b=A-mt-Mn=vKp)}5> z9A@&XanS+aU?0u;u0c0ZlQ z$pnG|q`BEChG$jnDtZlHPi@e5pumczpQID zcj&hdk*4smgxN+_x-pIB3xd}!2C{j32wus>^A`ZJ=NsQZBw{ZIHS0=gw1j#2H&h6~zwza;YkbxZD;kE36#ulbAzMbbqkp=_GiA6Hv zn*Q4jm!R8yiogUtw8QBPUMh9&oQH=pk_V2a?ZT{W=QyZAQl{c3%>Fu6^1a&yZJ5$b zn?2y5r%cnFD-SYQ$!i~A z;tow_G?Mn=v9H$P5D|H3L#x18yD|i?Bs_gE&+fG8#ib_67$<-1iL^%$a6_kZZB@2< z+QB@yblb(n^RaOw?#^+9wk}jKKzy!OyRlOhjE3=J5piMx zr4xh?v_6HTB?y>TS$^7`i~~bB2s9+GR;z8%jUQnCzBs%wH9h^^abFPkWZR-eZw6~ngrk^5Y)aldTx@R#? z{U({m9%MRco6-4wX=(y4P!6ijBbuv&<0;6&o$~S|5A8q&1bo@oFgq#97ws0BT)I4n z7YN`?N>C65hwjRh=DHEq36pno2}bA6eZZHaH1yV>nemA5K+pOVm=^z%n`LR&*wjX* zT~Uo2`9I$Mde)%Uh_&&1u_$KQ{&i%4w9$xA61=-XW4%SD&pd4~<%({sA4mOXR;?5< zRr0o&33B29%C!N2;^lDz1O)6>xCbz1hv$N`(Y>51L9cj3)M)%k7KZWG2`E7oz=8>h zYn<`G(L+dP$Jjx~Xm4C#pdUF}1Z6^6nR8r0m^N}9YbA5XszoDaS6F8tA``j;U+Epb zY@D(@SOate@3$tpt0N1bgc?`=X_rqdYT=rZgw#w(%7d3toV&qpp!L+7%tw#tfOjC= z$!=7YuC>91Gj#4^xK=!8UXmy+tiYGR5%nnNX{f$T>HXZ99h#x1GjHiCuH*A;s1aSy z`#N$j8UIdZWcujLFSM80F6)M>a)`v8Z?~nQVq!Fbgh2|V=v_t~i~=G*hYF_-K@Qz` zGTw-?+aF}`_g>30UY~)bk}edyV0fwV5AxQ(ezolh+YLMZpSgOMF(-?ehl`kC&1=5~ ziIPWi0!FY$i)I91>@=q`AN*xG`jb|^QVpzmri!+7>1j9Wc8Yc3wI znU?m|2LUU)&3X?)dD@$OE)dK)Ptu)mb$&Trl+Uj#pDl?5pxl}L$=`BD$%dZ&NX*+`SdzS7SJ2%R`H)O(-}q-C(v1t=T20{2p(rJ$|hYjwE=1c!XyITL{prBr*#km zs39dij4a%5Nfkn1`lWuarGP2{Hx@GRlZlZ75a@_3j{yR{c(EOiot#mBF2f%{SpFMT z#AB(nlFz>Ub-R&u6wc3NgxA6iWUI+L8SYP}*M8soX8f;9wcoZ2YTfN7rsbRt&mcz$ zoTBV={A~Wy)iI1dT1SeL2(u;_04P*lva;&MX(FQ-7>f)+nhyoIV2m;t%^ltsSzJPZ zH#Cg@sv_4RNwP)9SwA>I*=KD+qgKh{u=N}yC#3+YD+x^tv|9_DKPEcb01a@}zvG!5V zSlrUT;aVCRvY@pIn1t*BBWUOBEhB;+Q&g{|$wWBzW0%3F+{ym@{sc43s1%>yiVhY1 z=?>I@onE{+A^2(JS`|;HSwGlYWC2A~4f0bSY6ij$bpn6oQF4}PHguB&wL^+TREHT0 zQ_oZ{q7#5vHS+dtVp4fm+0F6o12uJ{zK*=a${WDGFWwMb z@W$D*L%7;Gq<05xdTz<$Rh$Zf&^ZHdBV{!qgAjK9cui#%wQ2gAJSJLk`zgZgco}M{yk?`H4aQI`tu6O+_o~J^Xc5a3~UE%TMhi* zNG*#qPlQe&qB`x1q6H9xi+7kIU7Ml0*{9=t{khGSV;{_q42Id%PlL{M(0D;L3%K3% z=I8w@WW0L4@)RrDB%JQ!2>7|h(~@{jRSjNC_AnA^(BL}n*M7|cc%b`yYo|GiRLS&R zcw0D^rP7|`%%m{C7<>LE0ue&{Q`{r3g&siyYhnLX4Edp`ZxF~qmn!uXyaOBh9=%K* z5&IzYkV+Eg{&q}^-SXm0?lg=5MJ&LYS^%0t|DK5KjwKU=%^8WK*-e%^C%zm%(q)$~ z!_9&qPnzJQq!pd@&`=VNV;ufe#1sI`4oYYU8#=ESWwd39ArHLqdhQeb?!XC;9%ljq7zE3^e%E<_&|aPe%g?cVgjA@%e>8P?P$+-1c>3H+j^ zzErj?b~Ptupq+$J()BhPDl7$1C(yVty;1=M;HAwTJ|H8VG-u=~@>=-!#)zlq`2wks zFZc=11mDk+lf zNI?x#skh|hl+-nq%E>yS=fco#8MIv3zI8{MeI95r^IN<1h^fg3}Ac0VtgQN=r-SF~0;43dWK1_PE;G~k_K>zQ5Ks4onHV=UDm@8LlpR_iTU*)K z^N#m2WM4Pq8xX36Vzl_{l!x+GWP{RC&zZfrn_!0P-t+RSxZmBH;OvWtM_73J_=Pcf z$3*flvkqCH;WmRX2Y^EW%t0I~TGythv2EKD-M^*WYMr5J3LDUW!1!!zy76h>mC3cV zC0Ek}YhNBr>Hjgjr+9rkR8G_LXplE_&c562tD-6^8)fY=bT-ZPFuwoGdH?GtdZs6D zGqG_>KrVDHwCk-8kDN>IB@t-NzF)*EF3xAnZ5^KP2N^oTr#~8XZ}493Shy_TgZ4 z&ec0D6#paxajnC}OYBSf{&2iPR!*eIzzPQ`p}8rSTOe*Nx;;kGjvWR@7j~|at}&5! zbG+-Yg6*j{+d7|LK8bZcH>|RDT>>0eWMpXQ9M;Sc>aZHfeD#_)$G-+eAKFpgQ3qbW z`v~l6fhNH=bJx5D6AHgJUWV10RzCrk@njRqPUgFUi1I;fItt+zrGWb_9Dm1)Ti}T} z5O6&W$`T|vfKC?)8amq9IqE|40p1N91a}#?gkger0#FF4YRE@VdUhaS4cd!q*T#a_ zM=ykw9Jsg6s%+#4*m<8aD&x9|YO~Gawm$~GWFpl%1T{@DN3Xl1!FjE1pN>`9h0WgT zw{ZP;U{TP(h{?ZhjW?A3xs8dO+x-kO<;l^$H%PncQ$2N%gXk3S5aD`X(QWb1aS&iv zUAV{8CoMDwbc8rBX}$_F1lIsIfqn?ztIiDqMqnb<7c^A$&|iL^G53Xblt1)topEkJ zZrK;BYiCrh%uVPTJ=kd3T(wGmwru_AI_k<*$Fzk5_!w|88s}X~W&_q}N^mlWJ`jtu z%358>UEyVzT+N$(MEBiWP!>|1`B{n!gAqMVg`Dn z`S)pvm5?BY|g+{M*N@1)2i;b?69x0727H1B3TaE*MOu5o=vwXcI;H zaem(netnJ!Vh?%ZGMFZ1IdysIcU2;v-F*fTA8uL4#sNeq#RFIV`d>dbDm`~jzNL4= z!b~cG6_TSbS};n=-M`1I&{8TLHQy2usI7H;kBIV3s0^SXA~-Ty&Ua1=KE}WMU8y@x{%f=j?2x-L^7L@7dcx+}%~0jAc5k9a$scQ=4$2Z#gW&ZL zq>@?}V%4?l^4hOPo7f*` z*7{Pdmm~k&6xwo#e}@3Ri*B>tVp;NWXWrbWksHS1`Rr`Ezq~ks{}9ee zOF}uWynVqgBEpX`iz#m0O61rb7_cT%5QuP(xVR`lKSFrHg|PtD3n)m?EObhR0fZjM zV0ktwb)3c;{kNVE1uyrLp#bf@d-xLCb9@Bk{nu#{ zq46nlOB8WS%t-D8)eQ30r^V_gx_knkN|wHD_Yvi@vhdv_@b$O|3FLG zu(WCB^Ahd#CS`tWdCfoUTe*Q({`a>6M;kx^{l2VUWg&)T8mA7O|0pEV$iV{XLVNp- z$b>;aM^9hAd|5e;8Bv)_jmf$$b?8>cp@=ljZ9Q=CU^hOt1vud1TPLN9XGI7YPAGoR zrV>WQ@qo<&3P%9R*8!m-J2p^-^?)oN^WO+CiLi}`81SXi1}3igIP=W{rWzJ98AWqq1f!eM#wGdwoO9zB+@$p0Jdkc}4a|Bp@QE&gdOAo4_I zzg(lc<*cxlY!9L{F(al<+Cu9Qcw zeHSX#PT1@qV_KR7Oz@3A<{cbf1Eg(rJ979iGpGDZ;UAZE@Kx<~1ErVLB1XBk)bYsP zIxG(z#E)gex}a!>#*?Nx7{CcB`*t|)0RsuG(HfjBetQli2W%4;e+)fGAt`dzUSNuu?K$KGkV^hPQU~~@pg6T4G;>~0_+G%foF%iB z(l0|_8<)`(eVkZbc=Ns9vurRPy#>ta<^lqr10mLd6OMtgP zYXsj&iW(}M{Jxm8L}cmIex2)x@-#b`exaY6=G z6&c|Hmu8>cgxLw5baDpAaRzf@wjA@Fh-uz{|DRcCw6Onn@LN;8o@sk+Xe%IufDuzR z;CRl6s)`C?CkQAWQlzo35sVFsVQbHRq*Me&vmaYAB0~^^PlaL4{NZo=?;gUCOJ9y} zrfH>mU^Ff+1zM!8M%uw|{{|2u`fQvzrTu zCbB1VW6&3D+5W5=8~7rqBx}8|Uw4Kd{f9%`Gdee+j`kRSy~FCX zWN|S*DwEdTvpTz?CKQ-s9Irk)d1(zknoS2sl&-}O`@)N|k}*;nZ#&$i}<@b8f^(Ca zPO&;f19at4hFj(29L^ldo-33NX%QgT1a;|;*pdZ8+D`K?gUNdb+uB*EcB*5Gr=G|g zl|VoQad|fI>kf~Z^AxhuqSV-O<{O>L!q|RP8cPwRXaGA3P-5U=Kd-E5sW0zth1rr* z4g`isOX8qLB~*2d{eHT(G^QZlLC5f;J-O{}VwgvSK6g{()D&8=e-YICixoT34Upn{Am>9j4@F!B z2>ZPfeEG=RMEX|)9NL3`B~A7iKiConwM;_K+kIQ^4R7Ct5a0Ny@S^4o*?ZO52HLr) zEUx#-Eg8lwao5XR)YuudL0;~s{^2#;dJ5o$E^QqRuD1E>f6>CJ)VuyXQB44~JLfk1 z%6+DwbDDqm#;`V<^0h?IOQv>)*X)1{9HJMrh{sS;B_D{^9+%66xaUlXr!uyzb!S!y zJVsqTa2*aUXI@+JERQZPs(MaDC+1uVZTVj^5O5*MAbH;ZlQVyGkk!21jyjipM`}fM zP#8?PVwT$OwM=e|>8lqd+Ts_Z`r`OVra$qEtErD*MI}|%p!z}gH?4D73+SIPwjilN zp~12^t5V_Q$&&>CL@{QEVvn&s%`~@_aQGl86<}0o-TbYvK+AU;lBdpN+T`4c#A`e> zm%FFW=0bN+)3uMx&jzeLOiL~x6U?V@2{}S@9^|@Q61#j3PxmV4&tUD}jvfg!JFZ{9 zPS_D7e-w55Pn@W|ZV25c@YiLs;u6l{{vZke7D3ZP%_#b_kvtttom~3!4d~_`_gNV* zoM{`4IL`>{1pSPBOa^`5=!?HFOFqk#|L{}rNj!~sIfP4k^U-Io|4$20W?gtK*QwI7 z{>(orL!G<8KDiYUT#t}X&S=yy?eyy=H7L-J6`%7?($?)(cAce79hTM<_!t$-SNPtZ&y^%&_;Lsk~MhwFX^pR97aVJ6&|A{u#Qhs^=w(o zVdg!l`4{e=0=7QJXRWFy_wx_SNrW;CxhDnPmg*~d^nO!zL4b%bP=EO=sY6&MOoo_U zIRjSikSibJdb%W1)CBnP(8-e=kS$KlE1UokkFIXp_U+IVr5l5XlB_2%Bv4`SUp`3w zvmOs1@nYlRYW^v(&HTe28~S#pI5Gz;>~x>rto$j{b}E%lDN%eK-`g`?ZiWf{OJdy7 zOj_4saP$?dxA8?y#%?+GCX9huH=gd)p%=t0z)$i6N7VL^76s%2B5y;|0g6t*JM<_} zJ*@hHS^(|+(c{PW1L1*;LzK7_O=BACFr-(&B`8Sw(LkqFm*3riGT2f6~| z6L&^64#OtM;}3x}wwR;`0t~DF^zBtYoukz?$B;(Pzk3?KfBnAWX_aV@ZJ9yb6DmUIzaS)xAc$0U;e9zl1 z@zy?%bk3M~Rle|sW#?snC9lfC427u4&n+SA9_-27^EoqZ)9lt8mA;Zr3WLJFR5|n1 z=fO7v;|tistLL=Xwki8kMO}IaYYPJda}MuWEIYxO{TWyQfw3n4v|2-~;0~YfBar7w zmn`gnWKR`cF$IIpcCA-)KY@FLpeXd~WrB#Ik&-V+z=RwTgc8*)RKo7VSBwZjiaDLh zrnA=LZ&$7-gbRqvCdB3b1UL7v&+wkChVfB8l#?#_xA3BjA1#E83d;zqR<2k2$WZd2 z;hu~?i+21Tm?Pbk?DdFRsg=!9jA{1B$L*s1bOGg)yS98Za#K(+-s#%pg|Vy*S&{yb z?Al8%3Sp7g7_KPZ*HO6d!et6i;Msr>whF_8w{j0prhl+ca4f&uW7 z-H)A#BfV=?e7|0D>So#a(0eQQcyo5ztG!2(oyPT7=pJBNY)<$*$aJ*vv+|Xq!h(ae z-ZqZsCtj8JGHDyUG^nYS_h-?Rz{d}y#}+-TPP!bT3a8L^<(-!{Ll5XJ;5lnzbk8?C z?Q~|6lf2nFlrELsEwUo#-}&|}XFK(eAHL%`t^%dP%r{XU50F zTa>uotH;#YCATSW$m2_+w+gA*ls3I%}b66Hlh z@tus@m^390ZK*0=MU7gf3V&a#K>4piUq6a+J4R%GvzwZ=o}b7nQ5yRR?(n0=J+xpW zRw0K$)kcq3$zl^a{8YW8*(U+$@!fIX$@*i7IR8c%Brr} z__#pmOtFF@4~>{rhdEi_e&q_;XN{8Ea#9|v85OXi0r@fWOeudiJ6y+(8Rhzcl zeR!jOX$7D|sq!qRc zHM8EF1{aK$zVfmis#cHLAAN@NU2RGA^5!k?-IQwccTpKuiH5El-Z+?=(eA&3o;=hX zI0~&et{=l5KBe)@?6JN-RYUyBBc^Xi|?(iZbe~znXp7Ai&S0LD*=0WR1iy1%4w&^@q3-tq3x*x!Yt!3 zaMXRSFZOv#%6{Y*NFbW~mdL&-lnP;Ji<+~PwDNh3z1O@@skKsgyjnx1tlGOG!ezp} z$@oY86_I@Gm;3&GYps?1*=0VAN9Qx{;x?A3_tAU0D?xD}^(${+pf5-5WxXFZIpeN| z0yn7sH4URp8{Z{g|FyIwn*G+dls7Pt^L*PVotj}>af~w>P|mL1yL&JeT8fFXmylGz zM3jyKEpZUac4AqNfu!>zpF29jQU7%zpAFl&VCm{BrP#C`=iwSn^wY%I4=cf7lv!#e zZw7C1kOqv102B(4@|6_u`lL{6ns07uA|sXA`5_tyvZBJ2s_m~`_GMxi``EN{Vo&Y5 zOGg9}yH-aj?cBH)%ShlA+uGM45FJGaZ|r9?!J)b)GU~KVUG>y8ESita7Bwa&@?y`K zglf*Ihb_4O_knXcI4E%8cB#;lb`L83VEO)`bCJ3ec+lm*NyBhOawcpPhd!tb zDuc+_*Z?dML&O1M>W`jZ7`}D2eVDseM{!)d^!EygBw-bm4%hSc_Htyu6gwB!<42E5 z4Wv@i)7PPrCTljKk)e?K53&2aV;F+gW7r}9cK6kMJpQFtm>2xKkkHBzr>>YQGX#&_ zAt%=^uwJ3QqcyPr(&L-bv|hRgFU)#}=pO4nEjL`Mv@@Ww|IV}bTSzQ@`)?O*wF4hv1Yyp!;*@oDs#enXFXG=5_EE%~K9mF7|`Gi-F zwzvy&hL6`3pUr>G@MNIC#BhbsE?4jlGbZwJwlIJdm5}%_DIMy2*Z9|~2&QduyX#F{ z5)Soh(K2;{81fy%D983B-KZa2(x=gUZtm5pE>Sq983MLae5=-yv?b1D6s~rvO=*gnrN`9Rdp%aO z9b(ZA6dRD`6JD-B7}*+}FrTtbDrV`D&0|(-RM{DQ+V}EzL&1;qQ^-DYA~n{)X@b~_ zEE>47@();~9wakO{2>wqk_u5qAb0OhP5F8meb)~x;Us=LUoP*$KO+91#EX+c*7L+! z7xPd)f~#S{&c<3_=kP8fPC65t)If zfy>XgzqvH&sUlgCedATzG-aJ4X1&D@THD7Qr5$*Znsw1?;1F%cpc&ONhHJNduiAZU zrX24Jn9XszWJCrPZE41*`QjVN%ltZH)|N^{h4+Ms7_wnzQ_yP(_Nyb9Gr z7iH=GFh716>BEu{^Trgvb%4*$d8n=FyiV+2S9;nP{|9*5D&Ge0_^_;IEnI%PLoTjv z7AjLh;}Is8FhrF9lv4FmDv5#xtlAm`IbYppHw@DLj0Xl)pW+UWw6hi2Z0vHSa^vzD z{8BYRagnrdxYvHGsfq(ahD&{tZOYv$IzbXg{Dpmm{JD5|65Pal&q_r3e)})no z=Zn{-@}-7%g`F1#UVdYp4%NB!J@#4M#4_JC$jlhXQv*s8JA~~}us;Zd(2*WnaZUHA z>tZxS4a)%|YIkj1yK`gMdQDn7{`h@4p8A&HKg+ItWzyawpDcIE^X`D)eb#sW$`tQ@3 zNW8zP*MCpC&L>Z&A_h>GRqN}uFo-FI%vlE7Y^i2=DT`t!2g-IZl71~#*z+;*$+AZ9 zDm;N;QJ^f<@Zkf^-2CbPeaJ3LFRE_x?B>UMe|`JY?tgGCauV1Y3C>8ASRIrLiFlQJ zn$UDVsfSoyFxwU*A~+|e=A)&~!gi=lxiM%n3!PH7P1S~*l=_mdOui0MDaZ1c<^OA* zOpDQ`#y<~_>hUqAtygZ05|9aN8oq}GasNJS=Mad}iVvx1NoHKcB!`d;}>rp|_NkS&!38_@5&;88&s`|Biu=j@c4Bx)2LzppVw)$ zFF|bLB0<%*F{?&>Ak`FJAhQ;4g7I=|lqE_CPU5dXa>0uOwDC9&vqR=CQvwx!LJ< z6*{im*7}BqMhj`>2ai@qnFb~~jF|11_9!S+8hWqd%xd4-xJZFHGoQz-R;X zkT(s|9Ov_By(M^yqvS2-Z45rG-=>hYre04cc88Xz$i&7!%%T%(=+t9ZjjZC1jBt+) z->gnknOL;bu(fzl&U$30-pobrM&2}i#pYMHLAi9gK*xmhK}SI~1e+R)Hxyng?w1Fg ze-R$Fvi}s1xhOABwVGn#MSQThOOyvV;iJ2JXjJz3=Y6;h&EroQgp8f?SX6%EHLevHs zTIS-9W1ZsHt}nw<~?DD6L@5P~Y<$LrQ>`*A>lb)mkHHiqVcRPhLHBY8dRV*G?BeEwHt z{wWmM$~ZgUIbWPDq-|~b;MX|E!?#@tM{x;Yh2f3vmMk6sbKVZ`XF`R|9FR*FNUfS=Rxy zz|Rg|Q^<0IX9w&5#hjdU_CGtV{e=}DjCQ`j!5Sk=0#J!mLHhU-gO9Kg6U0h-bu^Jv zuF6EteeU|#lSJf8icMlcKvt>{64vkLM2d1$M%vPEIm`F8^Tm475JfPhW=E}U?=vdi zfCYk>{`*$wj&e0ixgNN(ABJ4v)lZCnP~oj~ZF>qgGxXEtFya;tHcjl(26R9M9?23x zp!Bg<<7vIi&Z{&!%29t=$Y>iK9hTfMqRW~*|3`m?8f&tR+2FDMTKn-tFy65|EPpzm zGwl1@ZN1I!2uf-k_ecf_r zvyo_}p-f|KaB=bbOLKOyfzs$8x}atGc=;84Cg5Py3pklbg2@`uoSYoPh?9nf;edcq z1*&ykrH;V=nH84n;PLVkS5W7e`r`NL8QVLEJZWt@HlgfvKXu6Am`&F}+uhBy1BY%4 zWamm(#u#okkaP-RrMjHdyt&AuLoW2a`pvl$QJog|IN1)Vj4GQdG~`UJjfz8Zxb0C7 zkMs~yB7=P>MZ=mJOYWeE$O`av#K?pUCs7)uq5)Vz9?OZBcKpO8?(CLOs8>uNjL(DB zIRbzWx!P#6j(yn^)SL8QO1aMRQtl!zC&%>t_utp3+ZY2kV?wwooF=43;+t#eW6{eT?DosTe!9Nk+$RO6OOfC z<5TP(AJDt)_##l`sKJZh(ywVWy4p{kCaJdC$*Wh;a~rF1X8o1O?;CCxjhWO@OYpRB zpVQ%PY)?O5Kma!4?0ANswD9l=z@z_hllK~S!*NrL;Q~fTek>0m>p+DzZK^o6fAkU1 zEMlw%E+h>9voN3vqIx_;H+8xFDBPB3Oo{Ldr8_8wXXaI}lc)3I^wBGMNb_-IM1tM@1%7 zP^c@!Yme)s-xMjDX0weI(pD|D3_d`kMXGRbU>ojCkKl-+_42msTKH-Yo_-wSjfGpJ|Z80#V zh*t7CF?OJF_AD<{ovyzRUAcCRFsx&EU}kX5lV2H$m`Q9ZHZKSI@*#GyX#EYL#B1xZ zB8KG{6u(t`fBeyEv?uydJnRfuFDkjWV3ap&jxEaQf+U_SNAv!u);JW^uvckn0@)iC~DsU*;g1wX^A$D z6sp&*rDp`tHkE|3B-b3}-|x3t?nBnv+GrrLc>ezIAY#G=SI5jA1pGl*3qwdKLUxeJ zGwv`=FxqR-l;a> zK*4Zr(H=jF>`TE7CkYUF4GTJ!F=6x4zF!wV|HP?NkFkBMxqlb_)lj;U$QoT1Ep1P$ z!jV&Lm$K@Q`UQ&FTv}Rk$~{Hf5H)f7;59lm+2AOzxmaO~8^9ms=n2t30I|rPM98Q6bg63&?Ko967V9a3$4ABu3_Ye#wmJ8N zIpc8ZAv%Gaqxy zY9k1l(^y(SJnUXI2L>+0+SMnE$?W;={D5tniiJ$x>!c&B$@a=chD)+wB-N{PL~aG% zmRy{Ww%}7qEiJu~V4WmjWuonby;f!T3b2yBfFX=xtpj&ql!*1$>>w`EA~e5!1_cks z#~T(T+qUNj-_l?`RYmAnA_f?kb|r($D=CjiI+K>c*|Xzq_6;VO@Bl_phP54OUDkj+h<^%ZCIoT)iLqtD6mL*)@CpiX z!s)vWU1}YhjhS}M?-0rNfbEEd$?2`M0*pgc z!pG7N>IrcCYler1VL!9vgViU58$)@t9F8g2>KR|^oVsVRgXfok;OAn!_4jVLZPoRA z+Hm%7ETp=Su`Mh{4w7wJ|AOICfr%pCxK;iUoS0SW1WOuDB${@gu4hkKZi^xQM zLaLEGj1#*34h{(or6EttV(B-`(>|GJL?Xrm)ndeMA2LvM18fX)In_>Z>oN9*_M@93 zFDtT9^`w2BQ7B%28Na)D#!xYwujcmRY^lIa2aBB>#{sZB#;z4ufK(6^X=%ZQ9DO)_ z03OSV`M4sv@g#ARIORgPgClWpX zG9fJy=n!Jfi^Gyxc6|py-LfE$H_h;mGdHN)LFEul@ZBz>irf=>|^=E1~h%gzCw{(V&Be`MJ8O#a@94+ z_chV(gdUr`8?v?)!VF7f2vDWO?e}47DhtjQSp`Tsc$6IEI*Eykmy?O>i?192ZDM*j z6P^qJ>qM}J6c~#d&ak2TxCQ~qpi{5J=Eo4?k@4m)F9lxAL5u><6o@gLVN4ep-sH
ZwfSn8^d){kOyKf{yTn8pi}E{J-6B>-d8~y$ef9uElmOEtKmd zRS_5h7!-Pt3V7I@g&9z4tu4ruOiYlIH;>_IO-L>P#1j<_U=#`^{mrlK5C90E7~Xf` z!Y0Z?AmEr&twahN!*m#NFl;h=jZKpvKAyjL0R;V6Vgm*<*+JpJh{rYdYah+CrqnoD zW*3Kl^cI(k+QmDJ-+Bwg-m5c*JQ7Y#+{Aly|K3U|!@_@Pjj*sV>ySE0De~O;&`vo;r5S7u#I~VzDU+o%|a1byL{xjrz$a+$1n-t$-3Y znc2j$9wwBySZ+v00g!&ka!YtQ=$${mHoL{Ds3>hr7&!y$7St>)#XM)b7)c?3+X5#6 z@zh)x3!jMCu#-#Luma<+p=A9z0zJMSv3|kct#FuuKz~3?QHt6dcVV_>KTbA0a>UD1 z)O+`bpQvJ`S5h!%IjFdcdHoi0_n>7z3PYE()7yz5c zk!Ra}+QOTUP^H9=4M*c?T`NY+lklv4g&dgdg96W0+W5uy08=V5{4T35(66gX^=khH zPZD2Pu<)ZlA?JxyB;@~s$S)xyqG6Y$sM*ITNO62$ z$M6Cb%)d|>5>gI>0=M5Q@=@#~Nf=-Qq zM9{>+5}qE3p1hWlg+6KQx*|iQBsCIy8uI!`1^l?MkPpn$_1Le_x$dhe^)xvmb&fj+ z*wMoOskok&uPbCskHgu9QPLK+!%N7qkFtp(^=;4u|yc@lwVFfvAR)r%su*zqwBMyR7VN zqU@p^yF+~T$P~Byyf0}#n%b`sFyd&+SKv9Q)bi@85L5Z_e6V@U4`PHB6jBLcVni6( z$oot5CR_s;xGKMARsjUyJXyE9-Lr$nehv_m8DE>huh>@Bh0v5o+Co!ovcRZ+rDWKQ zv?*M;c=61stTVdd1l56m+M0}#B}Mvu-Yr&|uvI4O*YU0dwptdO%f}HTpx*v|$d*>1 zg!(t-^VE*kt8k8bpm#Hkv3YF zrax5vX(s1tt2|MkpN3%-fy@?AgF#cW7x>l)PzpVv_wTa=LkAoh8CR(`06JoSG6{oP zMXnFkk@bvyn0V_C(G)e7*I|Bao&TglQ%3!u6M3k(xp=4=R#vI)tzhXnQ0d8URgfe= z3bNshD~3P3$i{;|yu!<(!Hv8rMaG+?r3Wy;Nv;L5EnyMK=sZr@ED~i@LIQyS(U<%{ zVP|>uD(N0Eqs4fm)EnAt)FCfwGXcKbBoG53b9Y<6HZ(kW(jOJm1^~3>VY@p3y!gL- zDTCM4g^25?pW^@Vzurn9rPPfZFH81aAMe(%YJbClUCeC6WiO*pibi?($ucrNzO5%9 zgnnVYQ~1#gGQXDG-zS`wlY^FX{EZe*SfdDsh0Z4WGTo>VTyJs*V8!T=Ux-iz_db5S z`N)KXj?MJ>c)m7JS-7}8cYJ^zxMaS4l+6B-l7Q4~B*o$OY`+%3Or29m58O1dH|7EM ztNvMTzV6?rBrCd7`$+8aY%ecxXp*#0BEf?0$I*-;lrpPGNl(+uaan{UX8eP&$`3Xk07H+!ie+ZXzW}kp9m9)H;k3tm051ll~c10v)hgqjH;VSb9d2#tGEe& zFxB^s$5*J`Y4F%wRqqu5+?Whsg{$)BoFQ-->{0JRv;7t^+BNl1jIaM$4VV0D{2nsQNJ4PC5ZRu-sJ}30MHI5{(N=6~wTQL*qm|f-$roP}uEVih}-!mNjq^z$h*Oz zQS|fkQ%m`}J^eS=|Bm4?2|&JT#={~{i#0SgO@RyG6J@q+YKcKAB-%!3&ESyeUvohn zbTug$vD33aSO;GOZU+R)z)+!f__KYzXLxx2bUI^R;W8Zja7ujYhG1C{-y!=HIsZyi zs$m8f{UaS7$+I*4P8K4!*vhHDg3-En>5AQYAbC=U>8Je-w2Gpr>B+wDh=@_U^DxRF zAPjMJv2;*AkDN_1ARtp&3`QEQ4+vZl^;ldaBzVQ%lUt9iuFV(t@qI2qnPvZK3hIZC z7IP_tur>kEBPF?m>;b?h9)p`579vn}I}@v~@L?YILEY|SU?R(G{1o@dL>;4Y_y>cU zUvqUSKk=tjp9Ao-#`o-U^Q{KPTGM~MrdHhRF@}J&r6j! zkBbnP1y}$&B_2qF_hZZigR=feK+pdX{Kgopl&?B|>Qn$ig8*oS{U{|vXYgEJx=1t- zz%a>_31*PilUoh*hW5u(l2NAT(!)(o=~20SHc%OOZi;TQH5Td9KF60}kjI*G!zIM` z(ErF2Us?7{B4b)Y^KOdjhXDMfC4~Z@2R};#odBuj!oEuY!#R5Ns5gwfXZCbp%%v;sEP(N$rY3_9DbB-uGj#MM*;=g0 ziBk?E+*>-o1AM`#bpQaIE_f4)fmZ~R5rO4Qcq2yZBc5+vf3gZ0@zr!^;S-lV1j~6N zuM1CGrkkRr^`$|#%f6go*S+6+N0?y;0CvVJVx~|@2O^k4)4Bqs&T3}na$qwsJ{^O# zugKoLyueci?Gj;;Aqc`?BvX_KqHu!AvkwCTYliXaS;&ktvB>~)->d)!J`PQjZ|&mZ z5^F;Q38yg&@vq+kQ3v46#~|${N+I-a(C1mn@iF070sntsN>3~lBEvnH+7 zAH`$g3w!SFpkiSVbn<#zH^Y4A9V(oc_;m!jD!piih&fqy!v#Y%`g9v}{ta$?!3mho?TIjP;iJiv@*DE;z5aTPk!%U*=wXL86d0vwO>o;fqHLus~gd2$cun zCT7F^k+2E5kC#Nm6l0@a%e=T1S~$oAxakChDmCc)hp6gv@b-hjcs(fW|*TVr_OR>Fv{ ztH170@(G%Tuv0zXDp4uBXEkpOr5UzM-1|ttOyn2hpD_kQAtr$J&e%JV)*91^VvYDD z%%aH(MWKO_7xp)XTj+XvdO!@{0H#e4Dgw8o7_mG*E)PprbWJH$ix4ZVKr4yPjRTdQ z2I4TeH~>-sMv;R;0Mozj0#IV%T;u^CrEYwmYb;_H%?L=Bp}?4Ve;{AqM#QXcB3pv4Wt?C`>j*K%(A1JM|p zUijo=+jZdnp3NdzK{##~DrNivs~Z$f^WV6ri4)}8rTx;@T%@mNYmsb{cj=a)B_ba} zxquztn~2O29&qGO@GGHT+UMrBmsBc5q=nnO5AllNL5S$2I)79pfA7#9rx8AK)V_br94yd`pW zH7Im}aOd;z6(WDZ!drVit;AfvdM;k+XVp~efFi9=kP&s!>Rjz(q^GC7FHj8I*%_KyQyWwVx4YD!A| zReshhTcj=AmFQCrrX_MuPN4?c2U>(mUa_uopsJLV%6Uf`3;{!%oJ4q7N4{R}5yWd- z4(n__h6EuG*%Z*%?pTXo9Y0H2TF;_~3%QHllp6~q%t7J^S3eb#+jv`Mdb%gKZz&)! zSyPr;tZC&|LX$dU23FsTRw6pSuz5ib=*atFXcAd4Iet50$X8vBXQaxD8JMq z6-nef?o8S*E&d{X?N1@g=ZP{6rV}TC!ikhHG6ka0ssZ_}+F;TU9tbpMza2+5VF~Sy zZDgf3!G+K+J-q+<8Rd3I(c62aq{zbN#~Im3bJI;Zv!!L6d>v}sul3fX?3=K|QkFiAO^~lI#DewZA}gEv<7apbcpd`#g;*naRbA^XH`odGRuybUcE#@Qo^5G1m{6 zCHA5e^%>(iY9Nw+_xpDnl2Jq$_yWpiD0v^{KYEl2z^!W6S~Z(~UpeZnIJT6DuN=6PJcT9)F+_0k1}DG_>EVb?Y2%}LS)N&C%&po?$=YGAhrfdt67Y8Nvt z&&CCBHA>+_rxm#|*Sl-scDN<-@yxW&h$uWb0D}`3KiIOqXy`b7Yf-9>0RVx>7ZCJu zZl5vTRo2lPe5^cA5f?P;(jVVWkkToJ%Ge)9s1@_D)&6BYt@R3}ZYIzrWgp7f`f^{L zp|@QtNo%(q%QQ--Eu~J`0y}0G?{k;GX13mICM5O^d5Hdh9)bxE@%gKif&LiC9G(>t{YHc;rHrnDuB6n4{4h=)cb z>Uva&ICp&Fz|h@Ltih@9&)1@#^}lbBK6!sz{3B{6yfuxsRPgR;EjmYHHYXc4XBiWU zb3s8H;eoLpIlwZt?x9?MV+`k*OXW;Kem%y(Ifo`K*3cJV>+w!2JxSQJZQIY%V}}mW zAzexG%4^tVGym~+&Q!IuDL)ExQhL9ra9`-=sCnOk<7(?p3V6RQ(l>~QGNA=8W@#c5jUk+_(l zkM-Rz*nd)yy%&thv@D+96$i?_gqD`9UEdDz_sY=^AHI%VCDkses~eSfM?&R-NFx1; zgF&q84(mXWq?$p3kG14VU$2$OsVWZNoj2qV_L?1a+$8x9PSNM%0F*4mvjUKBzKgoq zf(s+~rZ`4=QeW;c25_IGC?}WJE*m#w7ifWajwRbO)^LtuTaHMn&7aSY7+65)4}?uR zu`rfVekq^xY)1z8fUPX+FuyDQNIbs-a2wGVcGJirFE**|`JMI&|?ff}K z{k^_zdclo+}3U$VQpqPONglBt*+)?XR>W#Yh4 zz?vl-Cr;Ut@NEohl(zOShp92=EQ;*{eSKJ@}P+Vbf%tK9fkr&iTGt-*egLEWNOKe$LB;^+nBp+neu|ZFm;s zGBW+taC7Yycr8^)dVyi_>9wqCY79R2_@KwVMkY}3rR3%we#Oc4tY+py%!2p#B<6Y* z5vRCj?QOQ$Z6@awrI%@N%V#BVjZyV}m2rj#f=OKWGOB(eG7H zg}MIH)f$7e)+`9xuDAGD*X!5bAGokEX7G@Ma2-XX)9Gk~cGuNCS~6mk=s4ix!@b{j z*e7juy_7HFi{^v>#23Gc+Z#ABMw~s@hsHAeU_vaFq-TIMt7wd2+@`IxYWvq0kn5HH z9b>??Q=Ur6$L0n@4mX;AbRM=Ge;+oW9^zPOASz3J-~9)Esjigu;8(0TuDF|Q_as(~ zeDvAvfU~lWF`!PslS_ScR7k{@bz_(Quq)lm@k>06M#N$J0tB9{-Fo}1f@!@!@eynM z{)505BlV&it35YtkTrM?ncj^gSq>2*g?-|y(C3&?OxVI3d2*fls=3B zrF@i<@T)>P)m<42($bpe+mcVQb1GQKSzC9F5f>KO-c#i!hGW}vLgfnFvhBm^=?L4c zb33unRJ~LH)e>2zjChR|g7Cm-l+fAFVk--ebC#1%FH+9OH}mPkqh&t#KJ(h+TuJKD zj+DUVWEDPq^+0KQm*H~2{ZoYvm)q9(h=;tGy@REAo{id&z24y`=TiX^tg^WHfXR8f7X zD8vVjIG-9kqeq)z9=?VJ?FBwQPoB3*$Gb1@Z5-Sl1gslZ*E})` zKmB}`;WON@U|-20;P%$Av43v1Im?FtXQ;;(96u4%Y$KgKuxxD8d(}cu?D?j=4J~6K z)UbmUQl^Kao2qu*8kn1cu~X>j<%Y%HY+0Rgkg?ef=U@;1>4oKgs!oN=m-`wmH?E+H zQv2k6?xH~hDj|+BD&=`ve3K9GxURSPSlt@z48U&o+E}JjvqhryTQB}T^u_r{;ud?T z+FYr)sPLf@2Ute@xfS@h$1j(iZjtJn$lCcI7r<96!N)YhXHT6NP8w_6fUjI$;ln^m zZcNBK0;+uKl->1+smp;}bZM&aF1@xSZR6ldu5IL5ZKad7;fuEVCJxtZiBXy78#uKI zpK?S*cPE=zG~F7UJ9@?dhhg+ths#r4-*BSbn4GE=fqbmTW$49L2K{ke=xF%YCoJ5C zq1DUK-*_&yP;%xpaoMb@#v*Mx;3_iEQ$LUc;uMvHZ>Kibqvff1W8}jXmF~1j^5DVb zJ|ZP1wC)^Z0LA@K&;<~GPMrZF)*ZEnUd0+Xkx>5>6EoZV#kXwQXSZGYXMgs~ zW*LWuyBG^$Z+vTPp10l&O=OMBj<;D3{fCZbH$mu`JaA79Sq^T>oc!{W8LpqOk$OhQ zGjWTxySq(>@n<+9&-ka1SurLDhyLlGWa4;(^444tNo+wGpIrHNE48IWpPw_5yTICb z4QaZ$P~wW>GzrpF5ALUtrAOHMwI*O0%&46n@U1(6O?_IdpZ@dg8*$HgS7*FvD37?I zsQBoHMn8Ol&Khlz#HV-ZZ?53xzJL1*4Y{f5i%MAw)?K*;zOF_=$nY+_8TFu?Bh5rA zk{qL~*J^0j06V-@y>;&xUML;G24zIA#yr&B$u!9OxYwS=0Il0I?(C~P%7`Ixos ztr1m=Z7dS+_^i!5>C1~#P5luo&OpBl{TnX=Vp-puC+6Xxvn9u;h(Gl_Sx6~2bv!yg z-DCTMPdV7gKpSn%p9qKl&sDi?}k&nu7#6B{MxH~wEBSvC|Lt=Sl>IJ^n(C$`A zW~6C5>RUhfadGv@28$UNoja27`qe9Kq}fTl!)h0~GLv|?HBnEgWnK6Gd%~!tMQ$K6 zX_xoR_9=SEhU38X4%n13A&MoWHc#Y^S$KEuZe+wO%?LcA3e`iK=Z(v@5U$VawWskk z7Hs9sxI7<|1}wR2+_6_uZr>3TbfE8(&vq_#e!AU152rc2I!#t`lei8o&;MY@f5MO| zL;zAJ_bmr8;@pQ60F+F4K~4BMyGL&zR4J2c9es3vtH{HmUxRQh=tQQrjFDvi8ox=r}0^;EOX^9|<|H?Hl{@}~B3 z=u1)j))S1!uH$($Ri5IS%2l%^-gs*D@)D1pq&Jd#ExTf7%ibB6L+B3(Y#DPtf#n_C zw6sC?SZQ%Pq7u=VB6;uGr?79)vsVqz{=GD(uYJk$jrs7Zt_^&wOvEhs=OG{Aa^v*<&r&dRmpaAMd)I0ALVwIl$HAm-GfYzZBh)s`hi_*zQdD zx$IZ-p87nVU(9pcDpy0IUu|SqyyWHnJ zK+{q`d9DomD1kV$e7eL~bejA5D-9sIlmA(<>i=BJKF3Dw_OGPyUbE(XX}3!n|I+PD z=NnpCE4!K2Ydu3UkGwQQ?`28hIa$mst_mJ$^p#aTtY)@+6bu}ltJrqC1fEb*3lcg~ zQ??O?UB$!2GE(G~sO{#k9W8Xk4Tuv6-*t4MY>c}xfcEIw6Jv)CChsKYMe0OC$4X~s z?z*kV=J#`=RD@rYy@pYxal2m!2pLSKRQt4YoAO>Nz;X<_@rLEew$LqGp?bk{skv=8 zW;pi+E}p@&jZeLM6ZB@|8L=*^F3XgMk}2gM$BUlcI5l*b`XVz;uJ8p@5+AZU4-PN5 ztlqkOCZzOrc&*NUInVFbSsD#9NWVEQCd=|I%{sw*{ag#iicPd+OqGi%cRGrWoX!@& z6R>Yq6}sSb3vBw*RiW7SKY@8~5-2?U`F_s|(xtYDXv4!jlkbEbrffc|{rJvkP~fFO ziKjziPNVw<{t2`@Xz|k1)1BL|FFymloGVkn%*%sj7uGBBS12<^Z9%a)cgI?5!J$YEUjD1q zeTrV}O+7WWs2~<_IQRkgHxVO4I(*>~>q!+$*hiV?hN zQ(|z|K`f#B0|(^2C=Z(2Cbl+iqMJC3XXUUHW0@Mddiv!3m9&aG?XdpSyClPX9=bkD z3RKf==zt8O6#X>}-6)|Xr^`_r`ubAB6ru`grTZeL+bmP2zB=suv|f@t14l^>ZIT+b z>=Wg*5SL>60&;4dj#ZATp#_9^1MZdwO0kh|A36OsIVeADlaeA~n}6^W-P5ag1#ffW z{Vc4#+3w!$%~=y0BaoBZg2@pN@ZIfVs<|$&8&}P^G_zn(g->0Vo!#KR%?28Sy6NXf z_%0lD92Xk-X!Ts1-@jkLX#E0qC+OivYFQ25qjxUsSh$PT;ECXJ3Vf7Ik|nb#=%B-_ z3u#m{Q5GgV2Be8miy}Qo;GuvaLYO0%<3*$bZr#2;@A6KIGo*--ol7?Ma~ZSW%nOrX zh4Y{ysL)9;RVWwfj<9HKc}ElLi= z^#P>2PayyXJukj%jPNcAfrE60ffz^X5QJxi0eH`$))hbfQbBdy-9(37L)H=lch4v9 zOMKVb{QQ!Z%3VRllpsI9fSp|Yt#AgFw$Ampj-2@Z(TV|yWU1cs`|B1=MZ}_?!Aa82 z=7K5c0rmMUX!SvPP;$#^LK-GyInMGpVO9)>R-e?>82DE2(RiUdMKPsMEUrSQ!CvRh=1Qih2TE;|J=-VFzKo6) zTzqr!yOh-G(tP{p=kH$`Gc!D^PT=g^pzdt?TY0|Sa0NatKiU-cC4hm2E+*1$LZN`+ zPQ*J1)&8r4C~1;|y2lNWd+2Rs^%>3P{TM9s z6x76=`A`QVXHJy?aN63KXo4 zBjciwiirA!~W24KKCxkZ|XJ3o8nIEy<{bKb#X(&V5r;henyv)Vpb(jbPA)FNSxLgvU=ngsP{T^)gt^}0}Y zp_TbMxKoYm4~2MTL$5vGsO#VSoR!1lai4xiErVu4VsC#SgY_HJ$AP-3(gHuA;Q-X! ze$;p<@O9o$Ab1T9t4lP**vY&YLQ2D*{iw&>x_vc9iw7kW&>=}Z7ihhUqH&AiFzI3TvLe*{x#oPgu98Q990{4mkkrxGXVAcIofhd3^!q@b9 zEFC8r7|31idNq&QXFmAsP=HTv93m84umLcXJ`knn9i$~6{aU17;NkJ>+bEAP!i>zH z27lS^7sq;C-5mN8UuV_r7ZO=5w#!;%%nTzFB-)s}>B@n~7QNB-@h@pctC8B<`9(+OI+aCK--D9xhKQN z*;dJj6O+A@&=hF^DbXDDGwx{p6SW<Pbs`5t88pDL|k>SZX`W0HRKn+*dC!qFcz-j1x`t(WE zZqP^6X~NaH+9Kz*w}1P-OWL37f1a)Qd8e0iW8~%FxXHh|CApTVcf0>c^5`~y)Zvn} zdW@adw{H?=7BauzWn0lB7~RB~_BI3KXSFWgaTsZnu3lQ4++m!#-4PeC2q^6F7G@kqV z!W@Vy5Rh=PKbcxS_-^>N1m# zYi$-Xd*O{OQQC}uzmX&$i2A-dogf5bA0^LB09T%QTxfYVYWaL z{;4IH(gh#BF+-us%gf6F9Xb#Z!gLYZa&*B~<{owZ{RlMJ9ISp8SaSx$UN>)cWSrpw zvttC;y05=~2UHdLZXSivzQ;jvbB#W|*|ku~brsj#+(f^38jY6Z#V9#(lgQs|RbB=- zF2!8@ZhrUA>;_xWn1=fLR8n3*1&myig|mOIYAOK0eZk=LnzzUZRV@0M3dQ?xAe)(#j=2GOVdT#&Apj? zt)_*Gr#Tzxq%6{Rv2H)n^+LfRf8uud04~#c^lc3}h%(U8C6TGEW3Dtrv!E-f;j6Pvsy`*{US4->|qpq>fg;u(;C zo)kDdzg*IjiZMZ_AW+nXZxM#&_Mk)ZeW6((83y^SyHFO6>@5VcqT7|ZDHQWwbo8oq zXJ18O&GN(J9I!=`&@!Hh71n)xDH2I)VGQDi6l&&^Cwgdxo*vyWFohmUWeCPHAG>B_ z`lI*W(d$36A6xfif?{NarWk`1eR%C+$1yE^x7iL&g=Cln-=_K_7}GM{ zp8pE$O}s_og9i_oyV9DQnL@p*^Rx*E!{7rRN+%ac$vD9`vibL1GUjb_j1~};f zvlgtG79eRNay|=;*D$#Nei)2R)%f`Am%ojlY|L@_HN1Wu+GE{?%hC~~G)ue>b|-}0 zsdfe=;l;1A;?vZ+m>+|4#Czz_8GPUU+D{m)`~bg|g!4m8MbpJleu6R&36o-rx15)I z$~A+FOdGfLfOb6kv8>SjXJMQLST3`RmnAp0pR;-XIVIxb#`k^BY4dyTaxovjn!UhG zp|li{QH@cco@z^~)%U!cfgiIg(?eA?^U`Z~@cGAp(SqKNXk5E?ZOE2F@ti-1HQ&fA zR5w|MKBbH}v$4O@tJkdI;pVQpq9wlvE|`*v7D zVFr6viOCWF^J(*FTgp95?nwbp zDIIKeUT*Hwc6J3oBH5%*tKmxFCHb6@IVu%_P|M`XT8L6-K$9;M9XMgLB^*vkYR&#^xDC&2 zgF}P7krak_^&4XkW-w>X0yO^MkER+pxb0cm`|@bI0T6g#6lnQ*i_3?Bo<14)B11dR zz(9uwF#UbL@tstqNcGQqHtD8~DmXZIb{~u6+}P3eyhJGCO8@f`sqH7}+S8Cp_!?!E z+X^(14Mo@bPc~nn==b)C^AC;Fr{kf4poO#X7U>I6QBs!{`~g}(dxtXqk# z<{GdEN6ap^G`v&$sRfGJ?Fo|Wq}5Ml$d`_v6Fpwt@+u%Dj*3EQx?8>Z8=RkR;YI)P zs})f5HB7UG$T$ELJHQ?M)6Hk`*wnn}+JS)!*nK5@@EOoA7sJ!QvwwhDrDzD{&Wr8@ zm4*BmF-0Y8BM-(~Z!XmE$p*|SRk+EMCr{$sZ!fM|K`~E3Q(*>fzrcA+;1RZHV|N7R znf?eKv-;5yI45~L_-R~t@@rb(78;*rie`D3g-9kZ``&t>8Mhv#km_)U7t#%~T za9|UAUCe;TY*gIb^CJLqT2R~-LU4UFMg{t}r*N&t0OAK(_u z`18jV7cOq1_~wqB>-))RhOWA8?=p24?RiC(2u`phvo=%7ngGMd?f%&Wy0Ns zi5&(t+!6q06sM86Hllg~P=*YLG1~u2aJ_Qn<@0a5hxvsGki9<(o?GrQ16Uqr%NXJ| zG9RjeS7$@xC*V6zQfoT;*}!kH0_)%42EvyXQS<;<;Nm5pHiI5K?X~>4x(KT8{&tms z`BC8PA>mbPX-;3bkl2~4pG8gNBn@!NY%NWo2u8mOry07#ACr`p&UA^R^%hFvJT34~vN43aL*w+1#LO?bzo?Eb#|*K+%;x1GJ0hu|q39^OFI)uCPZ+*|B3f zoUy6jH}=pN*qKHCz8H0FK9Z3p^lQyfYoQqi>O@$<3BcCxgYUYp+(=MBKpAg?y?_76 z=q3SNa2gvK_e&7$scn-YD&{nyf$4f-oKxne!qL756pCU^neA?P?M z#qFSJ`mCSWHey|n9raD0;B7`oSY~7M2+ZVoxSa;SAQElPqh*7P>Kyv=B@_Ehc&P6m zU(UkKGJ?-HaIEC8z{uo+?n{UxW_7QLAo|8^tVD#@^1*t75hQ?l@#@tWa=99C$pB}L z7|hy(jGShD?wQm7aRH*5$g(HlQ;RJJ1j`PoU_8O+UmpQ?h{LEJ_LE6^nO}2{Gug0u zcY%`YLey_^+g|)0%TL$$^lnHDsvoi5@ot z*26?Lwwuo+)*8{BvRO@`Xb=g6oO2~A#M6-$0c4TCBFbt*vtS$Tt{fVi8|ms+8@%$nS}gR7qmr&Y(&~0Up|MJ0Ufnf!N9A~ zjK~tI#6Gm69~kNsDL66O&bf$t)DQiw(7%jk0A{4b*qvnL@gC?kA1e-dHR3P|XaLH2 zNcW{&8W*_<`VsOa!85qPdBnvH!7jd!{x!bc#`_t~-Pd82+D}!xPvqqrXI+e9q?s7s zs(M>qGU99bBc+Ht)k<_;oZm4Ox()tI$ZdXxS(C05JrlE#0a4p@hppeZpr8O~PZ%BG z)x*Cg`|@BhZC8@-X0$d);F*vj;4JLUMq(3A=rXxeXK`VT;&8tuKmq`Y#5%;I0bT)1 zvG-j4aCya!f|V4{R4cd}I}sKHX>9~kBhq!m#^Nd?0WN%OD)OJu9UvP50d@RJxVEuF zAZ*VNA_-ks17U}+EzHN4aQhZP2OH~ZiQC9vnBW4mx&Zt=X?PO@!z2C^J4TK(NnjvT zd-xK`kwrZFCjtWTf0wzftI5?huMp!rTAm~L-RdnB&F1~$Z02vcV2>VGBv>kX&LguNl;JMi6rG^6(&J zd6FFh!;VlzP7(ifF}4CGn>Dj-eR6iT5*nV6E%ES=LpG;}#J*?|t>QH;$4SQtJ<$|- zNOn2pp`d{2!sM>Xn?3+MM63hNkI0i){&_;ILvWw!UBL{?yMn4$=u}hXC1>&eO^yi1 zRh2&kHdN!$Bg7$7K^Q({(K>9lDxdQ759_v`Nd^8P{$Ev|3Wod7*uM5&nc^ zuaGW#mo<2Z*|7Lo5MGT}Oza$rY~Mu<3=9MTh7bXvh$MrbgtL>(B0;JhDT>>V|L(uZ zFN3EdA&pj6vBLe&r%1k$M2P2~t(2eOoFG0)#3o+b<393A7G1ge07=n+KFDXyLK9Hi z!n_3i9|Xsu8@Vz$)tS5`^X;_f9q;o09#Y=K@_Vm2tFWtK2t4Rd;i;L!33>8AXt!=v z?;b8VOkz`*dY02U zwdqhI(ovr5Wa-ImQH)R)x8C(xHZhOCPf%lw+Bl>RoQZ}}j;xMM=m)!) zUh`u-a0(1ym%>f8pzVtUp7?p)#lj54)y(4VPejq+uy+;`ASVNlP5VFrJsr}a zdfVn(~B1^aT(7~ z1$t!nElxOvtq29?u>#5zMGimq+Q52p!9dQfd^9s_83T#C>p>~cU)v@bDt^G%?Vc-E zJ!!B(>4*fH%wkuwNlJj^Az%m{kSQpJP9ZT%;u}Z^jxyy}l#Bm-2F_f5OCD9j5I~gh z;e(7HyE$TMLT(GTIvPa^ZdMJy%T=2!yjofCPwaIsE#;%NsW^e7kVP<7UA3KdzsNL# z=h5xezr9K9Pt3$zXgM;4RkLimgU;s;L{bf}DiFjE4_l#g7uR!irXM;yP8^5S3_dB3 z2$kl&4u;#!gjMiN6c7-m$vGoyb#}#D1Bo$9n;8Pk3+^Xjhi-onx({r!H!}dOVs!He!f$}XEZ0And|8A6PTruWTyFadHQcZ8+fhgDFNXPrAaj?F zQ$r3na#2DP)#CZME;|rK0u9IWJ`2KlS)`VMYUGiN$~xAYs=Py5R+djlNaJv{G6|nh zfe?a}06twI82Sja6W*V+{(ZId760G{_$|b6y&*cEc8EY@_E%rD%c0*tEI18re|dMh3)7Yp^l+8LJ>f_aQ)2R$>2V&8 z!K+U(^WhF94(M%KAg5}q-cT9BAriLDIjMfB2ShJ|<{2q(hNV=pk3e1oEiIZD%}vk5 zltF^_*bUUwZ8*VP_4>J1z=i;vrhF=T&M2fcm7F`vP^hme6 zjObrOS2vR+)$vB8Iz)0mNyGm-4bS<(k@W|!Ug}@IIRb^fx9Z2v{T%DEaZ?uL*l+iV z@$<{FRT9d=ht?{~KJAwSSeS)u6AA#Qf=y5zpr>b-Cy7#bXcue6hJ0k&vG(S(NemDB zrVh+WFb#Z4yZHr}0x2N_mUSR7&&yXumI%^}UsI#4)ls2YG?=s)us$5k0P$EG>wj0Y zg;9_n2Rrp{npEfV1zj%H<*g%7HB`p#bo+K%iM*Rh^x&}KAv1&bjJwutDy@!KEoXCJ z#Z9*03J=-%^D>UZt|Uw#=}8>rEH^!b{-*Y`|Kugjp=)QrL7W~cEslU&%Ng_D$_kPm z0k=v5O{wXxx9}k1f>u@zE_`{YF=e(MLlJ`fiO-+=DO`5;Ztm>~tLcdfKb}17K2B@UEgyZ zLQ5LB$)-n*?7qBaB17*-Ta)e((ExdX6Dv|4&6xs)We=w&QDzD1r~ zQ5X`F0dPNhW8JRwW7rH*wj#An$g+?igMH*jv$!^5f8sm=U^FiV5#>zIudB?cKv<>`$2Flz;cYZhXJkXzB1kPscM`J`OO3_Tc}Zg!_bzo3?5fQ&fFS0Q(vcl+Ai|5l9u+Da%cgN*6SmGg%mj^@;yi-duRdt}nnH(fw2oMzf z3w0Ph{C7k{ux_vh&<^aZOfQ$W>Epi^L;1T?q<>=fbcgoD-@O}qx{|9Ze^1z6ySA$zQ@5o1W3GUe3HM|LpW6 zPD-(L%)(hovuMAaA7%>AwTS%hzhhQ%ud0ZvSKO!nH+z#88x?zLKG%PrX?~oQt;d5L z_~tdgE#JPQ`|k%uCpOZSG9H|)nNJG(=mU%v1+3K#n7dXZ$^)-+-v{urA%q* zpPvl@l$rJyH-0&^V@K2OXSDx*ltfbLL(@Dc4^I30e+leIK7aX7yqc7Z!sL@POP>RF z$X3&8{GXq+ezd!Xm6<)#TWSmQpFOfmm(vTik>x+0GH>(ZD#Yd7W_Iu0?-g~BAXj76 ze+`b}8DS8-O(63_@oJgVJ4g!epFhekL7PS_Ff&mheEmH_mT0Mdqw{HR*Eje|2!Y%|KlID zm-I9Wn@y;jvRoDV2_B2YhBKw3JjzRsQQ2n9*E#WOO%Dz!D(=3fQNLru4*Uf(?K9f= zSW}M&*@I!KJSRj|0&@#|x2y+cV7Iq|n^~EPPLW=6qLj-wmGrYhupE3L{3>R+$Xjp=_GOKEv9?(Ho)!aa4A zIkV-FO=LN?0EM4}i*=Je+Z@u2O&i!3(h)Umr55-H8 zftjDl{}c7*yxQZplWT-W8mjhlbj0SQ+_klHy`OO8%8?EWERN@wr?w*JBF9Sj#F|sR zjISPL3HFm=s%105J;ksy>swl1en!!Hug7YN=fz2NFE&j^Q|8EYS;Mp0b0yJPeR7{3S&rs2-_G7fAUie+|1KDuixstc^4BLJi{p_%(zph=7-CLnT|KkIV z>9HM6U$o*G;)khpxzaRwcCV)LlM8;E+OEmucq!Fd<8@<(?!i_rssVggc3P;`_!2L^ zLHDResj&2hNybFlw}R@E?-^6(Jmfoz>L=ijM*qrNU_=@jmfVblY zd}`ro9ghdO@1|6FDx>`QKOtOgv_Ej-5$p>4I@J4$T7K#nCQ=@Z`~R(KYo73NTI#0kF#O$%*3<5 z-KUPv?%MbsY!iD^?U!DwIV$ge($9R7n{-((GdW{J7=sP%%k-~l5mtZY;#g?!Xg2OX zm(4%GCUH;JW_8YeNhS~ZpxFyoie8KqxH_!gK(lIiq-jl+*cSE=VyYJPGQCDCD4rhP zR@*G+t-3o-ymXk{DzIy^a6_$5deQc6yfONWFbt-G%0uVgMwoVt-By3~vd?_}vjuf& zxv^hojrK54gW21=#;??y42^PLMN-UtzaI7K99Am-BBiW8{pp@~yU1VVp+EMquN~b4 zuj6l3)$OW&efs{PFMKwnJCR=mK10LtxU5%`Z**k3c^`9^%AN5Shlfnec(0dK&QHgr zS|}9jhMrZnZknqz^Xx609eGm{ zCNXw4{spUum)5Jltq#&XVd{nS%Eo@-CT5m@`^&}SF6%tGeQe}yHM@wykFH_!`mg1I zNCGjdIX&p5YvHL-1<4jbyFy)v&K5uT|{5C=cvBok@>*;jI#SW zf6g{**Kf%va!inU{zsCeUc9wM5J;>o{orJ8eI z73d0G^$jVKEoVijLIeu8(rR-jTmSTvu?z7`SFd29;@6 zy)ifLIlZUeASilk>dS}%%h&7exqc=5i3*4D6q^eEny6j%EcKrH;_dPyh9PbFMIP&hKK`u! z1wvP7#-*@WTW0d^dJ_gzea-yh0w4RUf91S%3X?@sti5mRNUus4}4 zG3uYTx$kkF{A_jWHi^RR=^JTVrpF&~J7)E@_za|~1lOCZw|%=PpnoxI+NY*t zTgnHz(6f!d1{L&^gfBc&vpCT_S*M_V+wE~`hoc@|)$eF%WEkD~RWhZCvK`rTX$M4Q z*t|l*Cc-QR?mH^qzSJz!aiN3**L*Fs!UV12gLNg18P0r|4SpojblvUlg~c_8yqdO` z?K{YKRpVCCGc*0V*aScRJ6m6Af0$h@aYza~Bcr7dRj?SAMs@GZfX8*O=j@yff9|9d z%$>*;(XZdlz-ybjRK>pCOl#(mjYXDfY+a<1=|#o2))}84VLiGFuAI$&aYNzKPnEWr zD0j-FsFYfe`n>gIZNRvaX9f zFWln7FJi7ls_)z7p^S%7c7RGQ)zz;`4M*dZM=+4XkzX(o!rro zoY(ph`_;(xkR8AOdz4R^Q`R!2L}nbb$8&8-?cyv{zk`iSEl)J~0pR(ZZ|2wpO`u7zJfoeDa9m7{SD~&#l zj$g@7Dm~3Lc9<0&hKJ>F%JJ=ad$nJTWoUMK`pd6czxkKp)f%-+X(!-;B>WwPf}>s2Z6aSkQ~k6*4XgFEO;;say{|ES*2|l{-hAfF#HrvUseshSXMkRX zs9$}e5pdh15)y9OHR@h#K65={Z_H7hk_9+F&)Y5SF4tD?U>N;XHJUu0w!`%>Meq>N zLHE^^x`K|SV9%*-;vB`tGI??|OW$wZLpQ{_vg(r+iTy zO4aO>)Qy{4OxktQ{e~}EjRfc(GqXuwQf5@SqcHj%n291No^)!}_~iZMt+Jm#zF=IH z-6!(ScIEO5C7Pz44gL13iBCdaAaGQ{iXp9WT5Tt_^4fLg9JMCq6bkhanGwMLMdNK& zKCrsr$@Cg!&PbPqg@cX9j>+8JED+AYcjk)fMw=r8b)CW5XDki2^0jdK6^JVB#s;2b zTot}HjKxk!VJBbG(gH765N$k%rDeU9#G(;cd)};U)t-)TF@74Vj0LWljZMC#CXw0OIQko@oDpu(3 zanEi&)g4wfdWOLu*G@BlUq?$vcu8BWxb)?&_yewH-akU9C=z_8>QYyd1}6odS!kb3 z&{vPQqZ7Azxc>C$-Y51Ob8#P;v%EBtq`w?m@aJQL$kof*kM$3BeYyDZZ^pLf>A0}0 zM^Y&&{%4B$j$-5g9NvC__ldEAV)pyhhT~k0ZGE4W zKaJ`chQMB%{3HV`%dgM(m3WjA`%cD8p||xH}B5UKVEQ z?k-qcZgM{K_4djGqC6GVA2R!?q}!9*c|0$A_fzR6lj~-?;TNv%7}myl-6!&KMOjaq zjf`*U(3%hn7Cm|}tc;^&@s=8`zSXw(+}tNdxPCqV-6u-2)XEw*e!Bzp*O*sO z^bBDCD-xQ|tN)l1t*DfAfx$6`@+*5%D(BEpLHlrE1uC7NgPheZTN0jqd>wc zt67HgEW^^VukL)dxiRkE8(+)TRBScAiBvW2WPAKp?%!qck~=alQ^#g-aIeR?{p|K8 z-!dIm;m{U6SeMEt`cxp$t?5*gp=RNCALFqL0tO866_*~#hgb(zT$8z+(ByIVr(EId zOPAK@vf#`*Urk%{<|~zA%ZJBX!$R8!teXCKUh(watrI#??JLdj*fqLuXN=_SjTxUd z;41`PGDWv}#3ak_GvV&fkuu31Slemx=#te5b@7*L>mP|ap1<~&^x^2f*CF52X}33vu4ozmZtTB} zOYFr!Nl*kk(-rylz+dBci8Tg{gC~ zS3;>a992(F+s2WAv_j^M37>N(mD{|>dWCW}mDZbV^hQDrwy6?p=Cn+6<>veZtJu98992aJKIeE zsvr8M;L)5|voNeFp8GcQo7m!$S|@+akKf;`;L;becyr1$PemEx#_7XZusX&+#^?{k0jG%%6!g7o{s$F@K;~iIb_zl zyeEDmtMX&jiQ48w>Cypf)$^_RP>q9|l-h%v+lsF#DLbi(gqC%x_H8xJ-g+g?0l7JI zl^TX$9~@Ga4xdbQNq01^a=Y86EuP2dk)pDA^kIUZzWd9_2X!yC;=OTqhS$alHd>0) zy30EXW(-aBwdvk4@i?!h75_fGfNzma{NA?85TS*urwF-Ny~~{AFR$i@!ZX4i4gwRK zSt1n{vPTzHfB&{n@Xf9s0g4s!vw3lBA){caOzQ3JPTQtrtD@A(pp0*EueFr~Za>OF zzuDx=&VFu=6I+nBTYiFVwN>vEX9g%-`x5>EB#A4(>AfS4greWSIjx}})v`J${kj=n zLTbf!JVoP{OQ8q&^-^!B`7*_&cdPE}{1A6Z`&q&_1-GMWYQ;~EJn{(7*64F*d&bFe zT&Qv)`V@AFIdUX?uE69&(c;Y)QBK#3l(yef0gyO$G3rb8REC{hN#pd{%Z>Ir!0|dM zeLl8>mzs9zXWgI(vHh2b5zxW7Jo<&POOr?5JrWV9|EvCE-Qmt`JkSrbx6>bPe{B^$ zvHSC#6M5gd?=k3a9^%HgMy<{JsdCE zc`S49`o>Dd;&pfUrc||3dYi-P;!_6L=xmNMq?%9pj`NCbe*LSt>)1-{K>t<|k#zFpX^$1mYdz8kD$lTeh=5$G?ln$2nPk6ClH{MtrEn})RJ2OI^Kb534*1RT4$U4Ay{i<54>(yGE|z&bpt zE!O+%i_?;us%1^IUM)1=TEMe&CD1jd$>G16mT$2+|GeNji*8-*wllp(>I}S{4>$te zb4~qZfBEa|wRRH)gPW%9`Mw9Z)DftqG<42F_xG_Yc;;`Kd+Z{2YNe^)n|z=ZDJ|K1uWR-jG#5PfR~%Qo2h>??!f_ zjE!&pde5tS7S^-?hmKZk1QnW-^hHJozIbi)6uQ_7ciO;=xHihO_=3 zEUW!xv=`{R6Ays9IeKk57r1Jk3;6UZz;~Cu+cMxVDlkMcJY}SVr_@-vZ9YC*^CvI@ z0@BTvu0IiUWbS2=yIVItXIijP06sOru$C)zD^Lv>IIKoYKrn34LX5T3N8;0eM)h}p U_Rcu`(-GtxPgg&ebxsLQ0P+aEpa1{> literal 0 HcmV?d00001 diff --git a/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp new file mode 100644 index 0000000000..4bfc69ed14 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/acceleration_filter.cpp @@ -0,0 +1,349 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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. + *********************************************************************/ + +#include +#include +#include + +// Disable -Wold-style-cast because all _THROTTLE macros trigger this +#pragma GCC diagnostic ignored "-Wold-style-cast" + +namespace online_signal_smoothing +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.core.acceleration_limited_plugin"); +} + +// The threshold below which any velocity or position difference is considered zero (rad and rad/s). +constexpr double COMMAND_DIFFERENCE_THRESHOLD = 1E-4; +// The scaling parameter alpha between the current point and commanded point must be less than 1.0 +constexpr double ALPHA_UPPER_BOUND = 1.0; +// The scaling parameter alpha must also be greater than 0.0 +constexpr double ALPHA_LOWER_BOUND = 0.0; + +/** \brief Wrapper struct to make memory management easier for using osqp's C sparse_matrix types */ +struct CSCWrapper +{ + /// row indices, size nzmax starting from 0 + std::vector row_indices; + /// column pointers (size n+1); col indices (size nzmax) + std::vector column_pointers; + /// holds the non-zero values in Compressed Sparse Column (CSC) form + std::vector elements; + /// osqp C sparse_matrix type + csc csc_sparse_matrix; + + CSCWrapper(Eigen::SparseMatrix& M) + { + M.makeCompressed(); + + csc_sparse_matrix.n = M.cols(); + csc_sparse_matrix.m = M.rows(); + row_indices.assign(M.innerSize(), 0); + csc_sparse_matrix.i = row_indices.data(); + column_pointers.assign(M.outerSize() + 1, 0); + csc_sparse_matrix.p = column_pointers.data(); + csc_sparse_matrix.nzmax = M.nonZeros(); + csc_sparse_matrix.nz = -1; + elements.assign(M.nonZeros(), 0.0); + csc_sparse_matrix.x = elements.data(); + + update(M); + } + + /// Update the the data point to by sparse_matrix without reallocating memory + void update(Eigen::SparseMatrix& M) + { + for (size_t ind = 0; ind < row_indices.size(); ++ind) + { + row_indices[ind] = M.innerIndexPtr()[ind]; + } + + for (size_t ind = 0; ind < column_pointers.size(); ++ind) + { + column_pointers[ind] = M.outerIndexPtr()[ind]; + } + for (size_t ind = 0; ind < elements.size(); ++ind) + { + elements[ind] = M.data().at(ind); + } + } +}; + +MOVEIT_STRUCT_FORWARD(OSQPDataWrapper); + +/** \brief Wrapper struct to make memory management easier for using osqp's C API */ +struct OSQPDataWrapper +{ + OSQPDataWrapper(Eigen::SparseMatrix& objective_sparse, Eigen::SparseMatrix& constraints_sparse) + : P{ objective_sparse }, A{ constraints_sparse } + { + data.n = objective_sparse.rows(); + data.m = constraints_sparse.rows(); + data.P = &P.csc_sparse_matrix; + q = Eigen::VectorXd::Zero(objective_sparse.rows()); + data.q = q.data(); + data.A = &A.csc_sparse_matrix; + l = Eigen::VectorXd::Zero(constraints_sparse.rows()); + data.l = l.data(); + u = Eigen::VectorXd::Zero(constraints_sparse.rows()); + data.u = u.data(); + } + + /// Update the constraint matrix A without reallocating memory + void updateA(OSQPWorkspace* work, Eigen::SparseMatrix& constraints_sparse) + { + constraints_sparse.makeCompressed(); + A.update(constraints_sparse); + osqp_update_A(work, A.elements.data(), OSQP_NULL, A.elements.size()); + } + + CSCWrapper P; + CSCWrapper A; + Eigen::VectorXd q; + Eigen::VectorXd l; + Eigen::VectorXd u; + OSQPData data{}; +}; + +bool AccelerationLimitedPlugin::initialize(rclcpp::Node::SharedPtr node, moveit::core::RobotModelConstPtr robot_model, + size_t num_joints) +{ + // copy inputs into member variables + node_ = node; + num_joints_ = num_joints; + robot_model_ = robot_model; + cur_acceleration_ = Eigen::VectorXd::Zero(num_joints); + + // get node parameters and store in member variables + auto param_listener = online_signal_smoothing::ParamListener(node_); + params_ = param_listener.get_params(); + + // get robot acceleration limits and store in member variables + auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name); + auto joint_bounds = joint_model_group->getActiveJointModelsBounds(); + min_acceleration_limits_ = Eigen::VectorXd::Zero(num_joints); + max_acceleration_limits_ = Eigen::VectorXd::Zero(num_joints); + size_t ind = 0; + for (const auto& joint_bound : joint_bounds) + { + for (const auto& variable_bound : *joint_bound) + { + if (variable_bound.acceleration_bounded_) + { + min_acceleration_limits_[ind] = variable_bound.min_acceleration_; + max_acceleration_limits_[ind] = variable_bound.max_acceleration_; + } + else + { + RCLCPP_ERROR(getLogger(), "The robot must have acceleration joint limits specified for all joints to " + "use AccelerationLimitedPlugin."); + return false; + } + } + ind++; + } + + // setup osqp optimization problem + Eigen::SparseMatrix objective_sparse(1, 1); + objective_sparse.insert(0, 0) = 1.0; + size_t num_constraints = num_joints + 1; + constraints_sparse_ = Eigen::SparseMatrix(num_constraints, 1); + for (size_t i = 0; i < num_constraints - 1; ++i) + { + constraints_sparse_.insert(i, 0) = 0; + } + constraints_sparse_.insert(num_constraints - 1, 0) = 0; + osqp_set_default_settings(&osqp_settings_); + osqp_settings_.warm_start = 0; + osqp_settings_.verbose = 0; + osqp_data_ = std::make_shared(objective_sparse, constraints_sparse_); + osqp_data_->q[0] = 0; + + if (osqp_setup(&osqp_workspace_, &osqp_data_->data, &osqp_settings_) != 0) + { + osqp_settings_.verbose = 1; + // call setup again with verbose enables to trigger error message printing + osqp_setup(&osqp_workspace_, &osqp_data_->data, &osqp_settings_); + RCLCPP_ERROR(getLogger(), "Failed to initialize osqp problem."); + return false; + } + + return true; +} + +double jointLimitAccelerationScalingFactor(const Eigen::VectorXd& accelerations, + const moveit::core::JointBoundsVector& joint_bounds) +{ + double min_scaling_factor = 1.0; + + // Now get the scaling factor from joint limits. + size_t idx = 0; + for (const auto& joint_bound : joint_bounds) + { + for (const auto& variable_bound : *joint_bound) + { + const auto& target_accel = accelerations(idx); + if (variable_bound.acceleration_bounded_ && target_accel != 0.0) + { + // Find the ratio of clamped acceleration to original acceleration + const auto bounded_vel = + std::clamp(target_accel, variable_bound.min_acceleration_, variable_bound.max_acceleration_); + double joint_scaling_factor = bounded_vel / target_accel; + min_scaling_factor = std::min(min_scaling_factor, joint_scaling_factor); + } + ++idx; + } + } + + return min_scaling_factor; +} + +inline bool updateData(const OSQPDataWrapperPtr& data, OSQPWorkspace* work, + Eigen::SparseMatrix& constraints_sparse, const Eigen::VectorXd& lower_bound, + const Eigen::VectorXd& upper_bound) +{ + data->updateA(work, constraints_sparse); + size_t num_constraints = constraints_sparse.rows(); + data->u.block(0, 0, num_constraints - 1, 1) = upper_bound; + data->l.block(0, 0, num_constraints - 1, 1) = lower_bound; + data->u[num_constraints - 1] = ALPHA_UPPER_BOUND; + data->l[num_constraints - 1] = ALPHA_LOWER_BOUND; + return 0 == osqp_update_bounds(work, data->l.data(), data->u.data()); +} + +bool AccelerationLimitedPlugin::doSmoothing(Eigen::VectorXd& positions, Eigen::VectorXd& velocities, + Eigen::VectorXd& /* unused */) +{ + const size_t num_positions = velocities.size(); + if (num_positions != num_joints_) + { + RCLCPP_ERROR_THROTTLE( + getLogger(), *node_->get_clock(), 1000, + "The length of the joint positions parameter is not equal to the number of joints, expected %zu got %zu.", + num_joints_, num_positions); + return false; + } + else if (last_positions_.size() != positions.size()) + { + RCLCPP_ERROR_THROTTLE(getLogger(), *node_->get_clock(), 1000, + "The length of the last joint positions not equal to the current, expected %zu got %zu. Make " + "sure the reset was called.", + last_positions_.size(), positions.size()); + return false; + } + + // formulate a quadratic program to find the best new reference point subject to the robot's acceleration limits + // p_c: robot's current position + // v_c: robot's current velocity + // p_t: robot's target position + // acc: acceleration to be applied + // p_n: next position + // dt: time step + // p_n_hat: parameterize solution to be along the line from p_c to p_t + // p_n_hat = p_t*alpha + p_c*(1-alpha) + // define constraints + // p_c + v_c*dt + acc_min*dt^2 < p_n_hat < p_c + v_c*dt + acc_max*dt^2 + // p_c + v_c*dt -p_t + acc_min*dt^2 < (p_c-p_t)alpha < p_c + v_c*dt -p_t + acc_max*dt^2 + // 0 < alpha < 1 + // define optimization + // opt ||alpha|| + // s.t. constraints + // p_n = p_t*alpha + p_c*(1-alpha) + + double& update_period = params_.update_period; + size_t num_constraints = num_joints_ + 1; + positions_offset_ = last_positions_ - positions; + velocities_offset_ = last_velocities_ - velocities; + for (size_t i = 0; i < num_constraints - 1; ++i) + { + constraints_sparse_.coeffRef(i, 0) = positions_offset_[i]; + } + constraints_sparse_.coeffRef(num_constraints - 1, 0) = 1; + Eigen::VectorXd vel_point = last_positions_ + last_velocities_ * update_period; + Eigen::VectorXd upper_bound = vel_point - positions + max_acceleration_limits_ * (update_period * update_period); + Eigen::VectorXd lower_bound = vel_point - positions + min_acceleration_limits_ * (update_period * update_period); + if (!updateData(osqp_data_, osqp_workspace_, constraints_sparse_, lower_bound, upper_bound)) + { + RCLCPP_ERROR_THROTTLE(getLogger(), *node_->get_clock(), 1000, + "failed to set osqp_update_bounds. Make sure the robot's acceleration limits are valid"); + return false; + } + + if (positions_offset_.norm() < COMMAND_DIFFERENCE_THRESHOLD && + velocities_offset_.norm() < COMMAND_DIFFERENCE_THRESHOLD) + { + positions = last_positions_; + velocities = last_velocities_; + } + else if (osqp_solve(osqp_workspace_) == 0 && + osqp_workspace_->solution->x[0] >= ALPHA_LOWER_BOUND - osqp_settings_.eps_abs && + osqp_workspace_->solution->x[0] <= ALPHA_UPPER_BOUND + osqp_settings_.eps_abs) + { + double alpha = osqp_workspace_->solution->x[0]; + positions = alpha * last_positions_ + (1.0 - alpha) * positions.eval(); + velocities = (positions - last_positions_) / update_period; + } + else + { + auto joint_model_group = robot_model_->getJointModelGroup(params_.planning_group_name); + auto joint_bounds = joint_model_group->getActiveJointModelsBounds(); + cur_acceleration_ = -(last_velocities_) / update_period; + cur_acceleration_ *= jointLimitAccelerationScalingFactor(cur_acceleration_, joint_bounds); + velocities = last_velocities_ + cur_acceleration_ * update_period; + positions = last_positions_ + velocities * update_period; + } + + last_velocities_ = velocities; + last_positions_ = positions; + + return true; +} + +bool AccelerationLimitedPlugin::reset(const Eigen::VectorXd& positions, const Eigen::VectorXd& velocities, + const Eigen::VectorXd& /* unused */) +{ + last_velocities_ = velocities; + last_positions_ = positions; + cur_acceleration_ = Eigen::VectorXd::Zero(num_joints_); + + return true; +} + +} // namespace online_signal_smoothing + +#include + +PLUGINLIB_EXPORT_CLASS(online_signal_smoothing::AccelerationLimitedPlugin, online_signal_smoothing::SmoothingBaseClass) diff --git a/moveit_core/online_signal_smoothing/src/acceleration_filter_parameters.yaml b/moveit_core/online_signal_smoothing/src/acceleration_filter_parameters.yaml new file mode 100644 index 0000000000..92575e5059 --- /dev/null +++ b/moveit_core/online_signal_smoothing/src/acceleration_filter_parameters.yaml @@ -0,0 +1,16 @@ +online_signal_smoothing: + update_period: { + type: double, + description: "The expected time in seconds between calls to `doSmoothing` method.", + read_only: true, + validation: { + gt<>: 0.0 + } + } + planning_group_name: { + type: string, + read_only: true, + description: "The name of the MoveIt planning group of the robot \ + This parameter does not have a default value and \ + must be passed to the node during launch time." + } diff --git a/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp b/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp new file mode 100644 index 0000000000..aeff731f2a --- /dev/null +++ b/moveit_core/online_signal_smoothing/test/test_acceleration_filter.cpp @@ -0,0 +1,207 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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. + *********************************************************************/ + +#include +#include +#include + +constexpr std::string_view PLANNING_GROUP_NAME = "panda_arm"; +constexpr size_t PANDA_NUM_JOINTS = 7u; +constexpr std::string_view ROBOT_MODEL = "panda"; + +class AccelerationFilterTest : public testing::Test +{ +protected: + void SetUp() override + { + robot_model_ = moveit::core::loadTestingRobotModel(ROBOT_MODEL.data()); + }; + + void setLimits(std::optional acceleration_limits) + { + const std::vector joint_models = robot_model_->getJointModels(); + auto joint_model_group = robot_model_->getJointModelGroup(PLANNING_GROUP_NAME.data()); + size_t ind = 0; + for (auto& joint_model : joint_models) + { + if (!joint_model_group->hasJointModel(joint_model->getName())) + { + continue; + } + std::vector joint_bounds_msg(joint_model->getVariableBoundsMsg()); + for (auto& joint_bound : joint_bounds_msg) + { + joint_bound.has_acceleration_limits = acceleration_limits.has_value(); + if (joint_bound.has_acceleration_limits) + { + joint_bound.max_acceleration = acceleration_limits.value()[ind]; + } + } + joint_model->setVariableBounds(joint_bounds_msg); + ind++; + } + } + +protected: + moveit::core::RobotModelPtr robot_model_; +}; + +TEST_F(AccelerationFilterTest, FilterInitialize) +{ + online_signal_smoothing::AccelerationLimitedPlugin filter; + rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest"); + + // fail because the update_period parameter is not set + EXPECT_THROW(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS), + rclcpp::exceptions::ParameterUninitializedException); + + node = std::make_shared("AccelerationFilterTest"); + node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data()); + node->declare_parameter("update_period", 0.01); + + // fail because the number of joints is wrong + EXPECT_FALSE(filter.initialize(node, robot_model_, 3u)); + + // fail because the robot does not have acceleration limits + setLimits({}); + EXPECT_FALSE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + + // succeed with acceleration limits + Eigen::VectorXd acceleration_limits = 1.2 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS); + setLimits(acceleration_limits); + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); +} + +TEST_F(AccelerationFilterTest, FilterDoSmooth) +{ + online_signal_smoothing::AccelerationLimitedPlugin filter; + + rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest"); + node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data()); + const double update_period = 1.0; + node->declare_parameter("update_period", update_period); + Eigen::VectorXd acceleration_limits = 1.2 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS); + setLimits(acceleration_limits); + + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + + // fail when called with the wrong number of joints + Eigen::VectorXd position = Eigen::VectorXd::Zero(5); + Eigen::VectorXd velocity = Eigen::VectorXd::Zero(5); + Eigen::VectorXd acceleration = Eigen::VectorXd::Zero(5); + EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration)); + + // fail because reset was not called + position = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + velocity = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + acceleration = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration)); + + // succeed + filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), + Eigen::VectorXd::Zero(PANDA_NUM_JOINTS)); + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + + // succeed: apply acceleration limits + filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), + Eigen::VectorXd::Zero(PANDA_NUM_JOINTS)); + position.array() = 3.0; + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + EXPECT_TRUE((position.array() - update_period * update_period * acceleration_limits.array()).matrix().norm() < 1E-3); + + // succeed: apply acceleration limits + position.array() = 0.9; + filter.reset(position * 0, velocity * 0, acceleration * 0); + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + EXPECT_TRUE((position.array() - 0.9).matrix().norm() < 1E-3); + + // succeed: slow down + velocity = 10 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS); + filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), velocity, Eigen::VectorXd::Zero(PANDA_NUM_JOINTS)); + position.array() = 0.01; + Eigen::VectorXd expected_offset = + velocity.array() * update_period - update_period * update_period * acceleration_limits.array(); + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + EXPECT_TRUE((velocity * update_period - expected_offset).norm() < 1E-3); +} + +TEST_F(AccelerationFilterTest, FilterBadAccelerationConfig) +{ + online_signal_smoothing::AccelerationLimitedPlugin filter; + + rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest"); + node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data()); + const double update_period = 0.1; + node->declare_parameter("update_period", update_period); + Eigen::VectorXd acceleration_limits = -1.0 * Eigen::VectorXd::Ones(PANDA_NUM_JOINTS); + setLimits(acceleration_limits); + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + Eigen::VectorXd position = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + Eigen::VectorXd velocity = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + Eigen::VectorXd acceleration = Eigen::VectorXd::Zero(PANDA_NUM_JOINTS); + EXPECT_TRUE(filter.reset(position, velocity, acceleration)); + EXPECT_FALSE(filter.doSmoothing(position, velocity, acceleration)); +} + +TEST_F(AccelerationFilterTest, FilterDoSmoothRandomized) +{ + online_signal_smoothing::AccelerationLimitedPlugin filter; + rclcpp::Node::SharedPtr node = std::make_shared("AccelerationFilterTest"); + const double update_period = 0.1; + node->declare_parameter("planning_group_name", PLANNING_GROUP_NAME.data()); + node->declare_parameter("update_period", update_period); + Eigen::VectorXd acceleration_limits = 1.2 * (1.0 + Eigen::VectorXd::Random(PANDA_NUM_JOINTS).array()); + setLimits(acceleration_limits); + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + + for (size_t iter = 0; iter < 64; ++iter) + { + acceleration_limits = 1.2 * (1.0 + Eigen::VectorXd::Random(PANDA_NUM_JOINTS).array()); + setLimits(acceleration_limits); + EXPECT_TRUE(filter.initialize(node, robot_model_, PANDA_NUM_JOINTS)); + filter.reset(Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), Eigen::VectorXd::Zero(PANDA_NUM_JOINTS), + Eigen::VectorXd::Zero(PANDA_NUM_JOINTS)); + Eigen::VectorXd position = Eigen::VectorXd::Random(PANDA_NUM_JOINTS); + Eigen::VectorXd velocity = Eigen::VectorXd::Random(PANDA_NUM_JOINTS); + Eigen::VectorXd acceleration = Eigen::VectorXd::Random(PANDA_NUM_JOINTS); + EXPECT_TRUE(filter.doSmoothing(position, velocity, acceleration)); + } +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + rclcpp::init(argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_core/package.xml b/moveit_core/package.xml index 2b22cc7cab..76dd4b5596 100644 --- a/moveit_core/package.xml +++ b/moveit_core/package.xml @@ -2,7 +2,7 @@ moveit_core - 2.9.0 + 2.10.0 Core libraries used by MoveIt Henning Kayser @@ -13,8 +13,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan Sachin Chitta @@ -31,8 +31,8 @@ boost bullet common_interfaces - eigen_stl_containers eigen + eigen_stl_containers generate_parameter_library geometric_shapes geometry_msgs @@ -41,8 +41,15 @@ libfcl-dev moveit_common moveit_msgs - octomap_msgs + + octomap_msgs + osqp_vendor pluginlib random_numbers rclcpp @@ -52,14 +59,14 @@ shape_msgs srdfdom std_msgs + tf2 tf2_eigen tf2_geometry_msgs tf2_kdl - tf2 trajectory_msgs urdf - urdfdom_headers urdfdom + urdfdom_headers visualization_msgs python3-sphinx-rtd-theme @@ -76,9 +83,6 @@ rclpy rcl_interfaces - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_core/planning_interface/CMakeLists.txt b/moveit_core/planning_interface/CMakeLists.txt index 46e7ff7789..338765e2f9 100644 --- a/moveit_core/planning_interface/CMakeLists.txt +++ b/moveit_core/planning_interface/CMakeLists.txt @@ -1,23 +1,14 @@ -add_library(moveit_planning_interface SHARED - src/planning_interface.cpp - src/planning_response.cpp -) -target_include_directories(moveit_planning_interface PUBLIC - $ - $ -) -set_target_properties(moveit_planning_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_planning_interface - moveit_msgs - urdf - urdfdom - urdfdom_headers -) -target_link_libraries(moveit_planning_interface - moveit_robot_trajectory - moveit_robot_state - moveit_planning_scene - moveit_utils -) +add_library(moveit_planning_interface SHARED src/planning_interface.cpp + src/planning_response.cpp) +target_include_directories( + moveit_planning_interface + PUBLIC $ + $) +set_target_properties(moveit_planning_interface + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_planning_interface moveit_msgs urdf urdfdom + urdfdom_headers) +target_link_libraries(moveit_planning_interface moveit_robot_trajectory + moveit_robot_state moveit_planning_scene moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/planning_interface/src/planning_interface.cpp b/moveit_core/planning_interface/src/planning_interface.cpp index 7e37b942b8..67ec7fe7d5 100644 --- a/moveit_core/planning_interface/src/planning_interface.cpp +++ b/moveit_core/planning_interface/src/planning_interface.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("planning_interface"); + return moveit::getLogger("moveit.core.planning_interface"); } } // namespace diff --git a/moveit_core/planning_scene/CMakeLists.txt b/moveit_core/planning_scene/CMakeLists.txt index 9eb48ce44e..debb3e5b65 100644 --- a/moveit_core/planning_scene/CMakeLists.txt +++ b/moveit_core/planning_scene/CMakeLists.txt @@ -1,25 +1,26 @@ -add_library(moveit_planning_scene SHARED - src/planning_scene.cpp -) -target_include_directories(moveit_planning_scene PUBLIC - $ - $ -) +add_library(moveit_planning_scene SHARED src/planning_scene.cpp) +target_include_directories( + moveit_planning_scene + PUBLIC $ + $) include(GenerateExportHeader) generate_export_header(moveit_planning_scene) -target_include_directories(moveit_planning_scene PUBLIC $) -#TODO: Fix the versioning -set_target_properties(moveit_planning_scene PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_planning_scene +target_include_directories( + moveit_planning_scene PUBLIC $) +# TODO: Fix the versioning +set_target_properties(moveit_planning_scene + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_planning_scene Boost rclcpp urdfdom urdfdom_headers octomap_msgs - OCTOMAP -) + octomap) -target_link_libraries(moveit_planning_scene +target_link_libraries( + moveit_planning_scene moveit_robot_model moveit_robot_state moveit_exceptions @@ -29,32 +30,34 @@ target_link_libraries(moveit_planning_scene moveit_kinematic_constraints moveit_robot_trajectory moveit_trajectory_processing - moveit_utils -) + moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_export.h DESTINATION include/moveit_core) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_export.h + DESTINATION include/moveit_core) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) if(UNIX OR APPLE) - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_BINARY_DIR}/../utils:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection") + set(APPEND_LIBRARY_DIRS + "${CMAKE_CURRENT_BINARY_DIR}:${CMAKE_CURRENT_BINARY_DIR}/../utils:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection_fcl:${CMAKE_CURRENT_BINARY_DIR}/../collision_detection" + ) endif() ament_add_gtest(test_planning_scene test/test_planning_scene.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - ament_target_dependencies(test_planning_scene - geometric_shapes - srdfdom - ) - target_link_libraries(test_planning_scene moveit_test_utils moveit_planning_scene) + APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}") + ament_target_dependencies(test_planning_scene geometric_shapes srdfdom) + target_link_libraries(test_planning_scene moveit_test_utils + moveit_planning_scene) ament_add_gtest(test_collision_objects test/test_collision_objects.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - target_link_libraries(test_collision_objects moveit_test_utils moveit_planning_scene) + APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}") + target_link_libraries(test_collision_objects moveit_test_utils + moveit_planning_scene) ament_add_gtest(test_multi_threaded test/test_multi_threaded.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - target_link_libraries(test_multi_threaded moveit_test_utils moveit_planning_scene) + APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}") + target_link_libraries(test_multi_threaded moveit_test_utils + moveit_planning_scene) endif() diff --git a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h index 155b703420..3d71a117e7 100644 --- a/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h +++ b/moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h @@ -56,6 +56,7 @@ #include #include #include +#include #include #include @@ -159,7 +160,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro const moveit::core::RobotState& getCurrentState() const { // if we have an updated state, return it; otherwise, return the parent one - return robot_state_ ? *robot_state_ : parent_->getCurrentState(); + return robot_state_.has_value() ? robot_state_.value() : parent_->getCurrentState(); } /** \brief Get the state at which the robot is assumed to be. */ moveit::core::RobotState& getCurrentStateNonConst(); @@ -176,15 +177,15 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro const std::string& getPlanningFrame() const { // if we have an updated set of transforms, return it; otherwise, return the parent one - return scene_transforms_ ? scene_transforms_->getTargetFrame() : parent_->getPlanningFrame(); + return scene_transforms_.has_value() ? scene_transforms_.value()->getTargetFrame() : parent_->getPlanningFrame(); } /** \brief Get the set of fixed transforms from known frames to the planning frame */ const moveit::core::Transforms& getTransforms() const { - if (scene_transforms_ || !parent_) + if (scene_transforms_.has_value() || !parent_) { - return *scene_transforms_; + return *scene_transforms_.value(); } // if this planning scene is a child of another, and doesn't have its own custom transforms @@ -290,7 +291,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro /** \brief Get the allowed collision matrix */ const collision_detection::AllowedCollisionMatrix& getAllowedCollisionMatrix() const { - return acm_ ? *acm_ : parent_->getAllowedCollisionMatrix(); + return acm_.has_value() ? acm_.value() : parent_->getAllowedCollisionMatrix(); } /** \brief Get the allowed collision matrix */ @@ -1011,14 +1012,14 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro moveit::core::RobotModelConstPtr robot_model_; // Never null (may point to same model as parent) - moveit::core::RobotStatePtr robot_state_; // if nullptr use parent's + std::optional robot_state_; // if there is no value use parent's // Called when changes are made to attached bodies moveit::core::AttachedBodyCallback current_state_attached_body_callback_; // This variable is not necessarily used by child planning scenes // This Transforms class is actually a SceneTransforms class - moveit::core::TransformsPtr scene_transforms_; // if nullptr use parent's + std::optional scene_transforms_; // if there is no value use parent's collision_detection::WorldPtr world_; // never nullptr, never shared with parent/child collision_detection::WorldConstPtr world_const_; // copy of world_ @@ -1028,7 +1029,7 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro CollisionDetectorPtr collision_detector_; // Never nullptr. - collision_detection::AllowedCollisionMatrixPtr acm_; // if nullptr use parent's + std::optional acm_; // if there is no value use parent's StateFeasibilityFn state_feasibility_; MotionFeasibilityFn motion_feasibility_; @@ -1038,6 +1039,6 @@ class MOVEIT_PLANNING_SCENE_EXPORT PlanningScene : public std::enable_shared_fro std::unique_ptr original_object_colors_; // a map of object types - std::unique_ptr object_types_; + std::optional object_types_; }; } // namespace planning_scene diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp index d75e0707f2..908a053df8 100644 --- a/moveit_core/planning_scene/src/planning_scene.cpp +++ b/moveit_core/planning_scene/src/planning_scene.cpp @@ -59,7 +59,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("planning_scene"); + return moveit::getLogger("moveit.core.planning_scene"); } } // namespace @@ -189,13 +189,13 @@ void PlanningScene::initialize() { name_ = DEFAULT_SCENE_NAME; - scene_transforms_ = std::make_shared(this); + scene_transforms_.emplace(std::make_shared(this)); - robot_state_ = std::make_shared(robot_model_); - robot_state_->setToDefaultValues(); - robot_state_->update(); + robot_state_.emplace(moveit::core::RobotState(robot_model_)); + robot_state_.value().setToDefaultValues(); + robot_state_.value().update(); - acm_ = std::make_shared(*getRobotModel()->getSRDF()); + acm_.emplace(collision_detection::AllowedCollisionMatrix(*getRobotModel()->getSRDF())); allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create()); } @@ -336,15 +336,15 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene) if (!parent_) return; - if (scene_transforms_) - scene->getTransformsNonConst().setAllTransforms(scene_transforms_->getAllTransforms()); + if (scene_transforms_.has_value()) + scene->getTransformsNonConst().setAllTransforms(scene_transforms_.value()->getAllTransforms()); - if (robot_state_) + if (robot_state_.has_value()) { - scene->getCurrentStateNonConst() = *robot_state_; + scene->getCurrentStateNonConst() = robot_state_.value(); // push colors and types for attached objects std::vector attached_objs; - robot_state_->getAttachedBodies(attached_objs); + robot_state_.value().getAttachedBodies(attached_objs); for (const moveit::core::AttachedBody* attached_obj : attached_objs) { if (hasObjectType(attached_obj->getName())) @@ -354,8 +354,8 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene) } } - if (acm_) - scene->getAllowedCollisionMatrixNonConst() = *acm_; + if (acm_.has_value()) + scene->getAllowedCollisionMatrixNonConst() = acm_.value(); collision_detection::CollisionEnvPtr active_cenv = scene->getCollisionEnvNonConst(); active_cenv->setLinkPadding(collision_detector_->cenv_->getLinkPadding()); @@ -526,13 +526,13 @@ const collision_detection::CollisionEnvPtr& PlanningScene::getCollisionEnvNonCon moveit::core::RobotState& PlanningScene::getCurrentStateNonConst() { - if (!robot_state_) + if (!robot_state_.has_value()) { - robot_state_ = std::make_shared(parent_->getCurrentState()); - robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); + robot_state_.emplace(moveit::core::RobotState(parent_->getCurrentState())); + robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } - robot_state_->update(); - return *robot_state_; + robot_state_.value().update(); + return robot_state_.value(); } moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_msgs::msg::RobotState& update) const @@ -545,8 +545,8 @@ moveit::core::RobotStatePtr PlanningScene::getCurrentStateUpdated(const moveit_m void PlanningScene::setAttachedBodyUpdateCallback(const moveit::core::AttachedBodyCallback& callback) { current_state_attached_body_callback_ = callback; - if (robot_state_) - robot_state_->setAttachedBodyUpdateCallback(callback); + if (robot_state_.has_value()) + robot_state_.value().setAttachedBodyUpdateCallback(callback); } void PlanningScene::setCollisionObjectUpdateCallback(const collision_detection::World::ObserverCallbackFn& callback) @@ -560,9 +560,9 @@ void PlanningScene::setCollisionObjectUpdateCallback(const collision_detection:: collision_detection::AllowedCollisionMatrix& PlanningScene::getAllowedCollisionMatrixNonConst() { - if (!acm_) - acm_ = std::make_shared(parent_->getAllowedCollisionMatrix()); - return *acm_; + if (!acm_.has_value()) + acm_.emplace(collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix())); + return acm_.value(); } void PlanningScene::setAllowedCollisionMatrix(const collision_detection::AllowedCollisionMatrix& acm) @@ -581,14 +581,14 @@ moveit::core::Transforms& PlanningScene::getTransformsNonConst() { // Trigger an update of the robot transforms getCurrentStateNonConst().update(); - if (!scene_transforms_) + if (!scene_transforms_.has_value()) { // The only case when there are no transforms is if this planning scene has a parent. When a non-const version of // the planning scene is requested, a copy of the parent's transforms is forced - scene_transforms_ = std::make_shared(this); - scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms()); + scene_transforms_.emplace(std::make_shared(this)); + scene_transforms_.value()->setAllTransforms(parent_->getTransforms().getAllTransforms()); } - return *scene_transforms_; + return *scene_transforms_.value(); } void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& scene_msg) const @@ -597,18 +597,18 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce scene_msg.robot_model_name = getRobotModel()->getName(); scene_msg.is_diff = true; - if (scene_transforms_) + if (scene_transforms_.has_value()) { - scene_transforms_->copyTransforms(scene_msg.fixed_frame_transforms); + scene_transforms_.value()->copyTransforms(scene_msg.fixed_frame_transforms); } else { scene_msg.fixed_frame_transforms.clear(); } - if (robot_state_) + if (robot_state_.has_value()) { - moveit::core::robotStateToRobotStateMsg(*robot_state_, scene_msg.robot_state); + moveit::core::robotStateToRobotStateMsg(robot_state_.value(), scene_msg.robot_state); } else { @@ -616,9 +616,9 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce } scene_msg.robot_state.is_diff = true; - if (acm_) + if (acm_.has_value()) { - acm_->getMessage(scene_msg.allowed_collision_matrix); + acm_.value().getMessage(scene_msg.allowed_collision_matrix); } else { @@ -1118,15 +1118,15 @@ void PlanningScene::setCurrentState(const moveit_msgs::msg::RobotState& state) if (parent_) { - if (!robot_state_) + if (!robot_state_.has_value()) { - robot_state_ = std::make_shared(parent_->getCurrentState()); - robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); + robot_state_.emplace(moveit::core::RobotState(parent_->getCurrentState())); + robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } - moveit::core::robotStateMsgToRobotState(getTransforms(), state_no_attached, *robot_state_); + moveit::core::robotStateMsgToRobotState(getTransforms(), state_no_attached, robot_state_.value()); } else - moveit::core::robotStateMsgToRobotState(*scene_transforms_, state_no_attached, *robot_state_); + moveit::core::robotStateMsgToRobotState(*scene_transforms_.value(), state_no_attached, robot_state_.value()); for (std::size_t i = 0; i < state.attached_collision_objects.size(); ++i) { @@ -1153,20 +1153,20 @@ void PlanningScene::decoupleParent() return; // This child planning scene did not have its own copy of frame transforms - if (!scene_transforms_) + if (!scene_transforms_.has_value()) { - scene_transforms_ = std::make_shared(this); - scene_transforms_->setAllTransforms(parent_->getTransforms().getAllTransforms()); + scene_transforms_.emplace(std::make_shared(this)); + scene_transforms_.value()->setAllTransforms(parent_->getTransforms().getAllTransforms()); } - if (!robot_state_) + if (!robot_state_.has_value()) { - robot_state_ = std::make_shared(parent_->getCurrentState()); - robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); + robot_state_.emplace(moveit::core::RobotState(parent_->getCurrentState())); + robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } - if (!acm_) - acm_ = std::make_shared(parent_->getAllowedCollisionMatrix()); + if (!acm_.has_value()) + acm_.emplace(collision_detection::AllowedCollisionMatrix(parent_->getAllowedCollisionMatrix())); world_diff_.reset(); @@ -1187,11 +1187,11 @@ void PlanningScene::decoupleParent() } } - if (!object_types_) + if (!object_types_.has_value()) { ObjectTypeMap kc; parent_->getKnownObjectTypes(kc); - object_types_ = std::make_unique(kc); + object_types_.emplace(ObjectTypeMap(kc)); } else { @@ -1199,8 +1199,8 @@ void PlanningScene::decoupleParent() parent_->getKnownObjectTypes(kc); for (ObjectTypeMap::const_iterator it = kc.begin(); it != kc.end(); ++it) { - if (object_types_->find(it->first) == object_types_->end()) - (*object_types_)[it->first] = it->second; + if (object_types_.value().find(it->first) == object_types_.value().end()) + (object_types_.value())[it->first] = it->second; } } @@ -1225,9 +1225,9 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen // if the list is empty, then nothing has been set if (!scene_msg.fixed_frame_transforms.empty()) { - if (!scene_transforms_) - scene_transforms_ = std::make_shared(this); - scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms); + if (!scene_transforms_.has_value()) + scene_transforms_.emplace(std::make_shared(this)); + scene_transforms_.value()->setTransforms(scene_msg.fixed_frame_transforms); } // if at least some joints have been specified, we set them @@ -1237,7 +1237,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen // if at least some links are mentioned in the allowed collision matrix, then we have an update if (!scene_msg.allowed_collision_matrix.entry_names.empty()) - acm_ = std::make_shared(scene_msg.allowed_collision_matrix); + acm_.emplace(collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix)); if (!scene_msg.link_padding.empty() || !scene_msg.link_scale.empty()) { @@ -1275,9 +1275,9 @@ bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::msg::PlanningScene& s decoupleParent(); object_types_.reset(); - scene_transforms_->setTransforms(scene_msg.fixed_frame_transforms); + scene_transforms_.value()->setTransforms(scene_msg.fixed_frame_transforms); setCurrentState(scene_msg.robot_state); - acm_ = std::make_shared(scene_msg.allowed_collision_matrix); + acm_.emplace(collision_detection::AllowedCollisionMatrix(scene_msg.allowed_collision_matrix)); collision_detector_->cenv_->setPadding(scene_msg.link_padding); collision_detector_->cenv_->setScale(scene_msg.link_scale); object_colors_ = std::make_unique(); @@ -1442,12 +1442,12 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At return false; } - if (!robot_state_) // there must be a parent in this case + if (!robot_state_.has_value()) // there must be a parent in this case { - robot_state_ = std::make_shared(parent_->getCurrentState()); - robot_state_->setAttachedBodyUpdateCallback(current_state_attached_body_callback_); + robot_state_.emplace(moveit::core::RobotState(parent_->getCurrentState())); + robot_state_.value().setAttachedBodyUpdateCallback(current_state_attached_body_callback_); } - robot_state_->update(); + robot_state_.value().update(); // The ADD/REMOVE operations follow this order: // STEP 1: Get info about the object from either the message or the world/RobotState @@ -1478,7 +1478,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At RCLCPP_DEBUG(getLogger(), "Attaching world object '%s' to link '%s'", object.object.id.c_str(), object.link_name.c_str()); - object_pose_in_link = robot_state_->getGlobalLinkTransform(link_model).inverse() * obj_in_world->pose_; + object_pose_in_link = robot_state_.value().getGlobalLinkTransform(link_model).inverse() * obj_in_world->pose_; shapes = obj_in_world->shapes_; shape_poses = obj_in_world->shape_poses_; subframe_poses = obj_in_world->subframe_poses_; @@ -1499,7 +1499,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At return false; const Eigen::Isometry3d world_to_header_frame = getFrameTransform(object.object.header.frame_id); const Eigen::Isometry3d link_to_header_frame = - robot_state_->getGlobalLinkTransform(link_model).inverse() * world_to_header_frame; + robot_state_.value().getGlobalLinkTransform(link_model).inverse() * world_to_header_frame; object_pose_in_link = link_to_header_frame * header_frame_to_object_pose; Eigen::Isometry3d subframe_pose; @@ -1540,23 +1540,23 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At // STEP 3: Attach the object to the robot if (object.object.operation == moveit_msgs::msg::CollisionObject::ADD || - !robot_state_->hasAttachedBody(object.object.id)) + !robot_state_.value().hasAttachedBody(object.object.id)) { - if (robot_state_->clearAttachedBody(object.object.id)) + if (robot_state_.value().clearAttachedBody(object.object.id)) { RCLCPP_DEBUG(getLogger(), "The robot state already had an object named '%s' attached to link '%s'. " "The object was replaced.", object.object.id.c_str(), object.link_name.c_str()); } - robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, object.touch_links, - object.link_name, object.detach_posture, subframe_poses); + robot_state_.value().attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, object.touch_links, + object.link_name, object.detach_posture, subframe_poses); RCLCPP_DEBUG(getLogger(), "Attached object '%s' to link '%s'", object.object.id.c_str(), object.link_name.c_str()); } else // APPEND: augment to existing attached object { - const moveit::core::AttachedBody* ab = robot_state_->getAttachedBody(object.object.id); + const moveit::core::AttachedBody* ab = robot_state_.value().getAttachedBody(object.object.id); // Allow overriding the body's pose if provided, otherwise keep the old one if (moveit::core::isEmpty(object.object.pose)) @@ -1572,9 +1572,9 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At touch_links.insert(std::make_move_iterator(object.touch_links.begin()), std::make_move_iterator(object.touch_links.end())); - robot_state_->clearAttachedBody(object.object.id); - robot_state_->attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, touch_links, - object.link_name, detach_posture, subframe_poses); + robot_state_.value().clearAttachedBody(object.object.id); + robot_state_.value().attachBody(object.object.id, object_pose_in_link, shapes, shape_poses, touch_links, + object.link_name, detach_posture, subframe_poses); RCLCPP_DEBUG(getLogger(), "Appended things to object '%s' attached to link '%s'", object.object.id.c_str(), object.link_name.c_str()); } @@ -1595,16 +1595,16 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At object.link_name.empty() ? nullptr : getRobotModel()->getLinkModel(object.link_name); if (link_model) { // if we have a link model specified, only fetch bodies attached to this link - robot_state_->getAttachedBodies(attached_bodies, link_model); + robot_state_.value().getAttachedBodies(attached_bodies, link_model); } else { - robot_state_->getAttachedBodies(attached_bodies); + robot_state_.value().getAttachedBodies(attached_bodies); } } else // A specific object id will be removed. { - const moveit::core::AttachedBody* body = robot_state_->getAttachedBody(object.object.id); + const moveit::core::AttachedBody* body = robot_state_.value().getAttachedBody(object.object.id); if (body) { if (!object.link_name.empty() && (body->getAttachedLinkName() != object.link_name)) @@ -1648,7 +1648,7 @@ bool PlanningScene::processAttachedCollisionObjectMsg(const moveit_msgs::msg::At name.c_str(), object.link_name.c_str()); } - robot_state_->clearAttachedBody(name); + robot_state_.value().clearAttachedBody(name); } if (!attached_bodies.empty() || object.object.id.empty()) return true; @@ -1923,9 +1923,9 @@ bool PlanningScene::knowsFrameTransform(const moveit::core::RobotState& state, c bool PlanningScene::hasObjectType(const std::string& object_id) const { - if (object_types_) + if (object_types_.has_value()) { - if (object_types_->find(object_id) != object_types_->end()) + if (object_types_.value().find(object_id) != object_types_.value().end()) return true; } if (parent_) @@ -1935,10 +1935,10 @@ bool PlanningScene::hasObjectType(const std::string& object_id) const const object_recognition_msgs::msg::ObjectType& PlanningScene::getObjectType(const std::string& object_id) const { - if (object_types_) + if (object_types_.has_value()) { - ObjectTypeMap::const_iterator it = object_types_->find(object_id); - if (it != object_types_->end()) + ObjectTypeMap::const_iterator it = object_types_.value().find(object_id); + if (it != object_types_.value().end()) return it->second; } if (parent_) @@ -1949,15 +1949,15 @@ const object_recognition_msgs::msg::ObjectType& PlanningScene::getObjectType(con void PlanningScene::setObjectType(const std::string& object_id, const object_recognition_msgs::msg::ObjectType& type) { - if (!object_types_) - object_types_ = std::make_unique(); - (*object_types_)[object_id] = type; + if (!object_types_.has_value()) + object_types_.emplace(ObjectTypeMap()); + (object_types_.value())[object_id] = type; } void PlanningScene::removeObjectType(const std::string& object_id) { - if (object_types_) - object_types_->erase(object_id); + if (object_types_.has_value()) + object_types_.value().erase(object_id); } void PlanningScene::getKnownObjectTypes(ObjectTypeMap& kc) const @@ -1965,10 +1965,10 @@ void PlanningScene::getKnownObjectTypes(ObjectTypeMap& kc) const kc.clear(); if (parent_) parent_->getKnownObjectTypes(kc); - if (object_types_) + if (object_types_.has_value()) { - for (ObjectTypeMap::const_iterator it = object_types_->begin(); it != object_types_->end(); ++it) - kc[it->first] = it->second; + for (const auto& it : object_types_.value()) + kc[it.first] = it.second; } } diff --git a/moveit_core/robot_model/CMakeLists.txt b/moveit_core/robot_model/CMakeLists.txt index 0d11decc6f..1a1c59fb49 100644 --- a/moveit_core/robot_model/CMakeLists.txt +++ b/moveit_core/robot_model/CMakeLists.txt @@ -1,4 +1,5 @@ -add_library(moveit_robot_model SHARED +add_library( + moveit_robot_model SHARED src/aabb.cpp src/fixed_joint_model.cpp src/floating_joint_model.cpp @@ -8,16 +9,27 @@ add_library(moveit_robot_model SHARED src/planar_joint_model.cpp src/prismatic_joint_model.cpp src/revolute_joint_model.cpp - src/robot_model.cpp -) -target_include_directories(moveit_robot_model PUBLIC - $ - $ # Work around cyclic dependency between this and kinematics base - $ # Ditto but for finding the export header - $ -) -set_target_properties(moveit_robot_model PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_robot_model + src/robot_model.cpp) +target_include_directories( + moveit_robot_model + PUBLIC + $ + $ # Work + # around + # cyclic + # dependency + # between + # this and + # kinematics + # base + $ # Ditto but for + # finding the + # export header + $) +set_target_properties(moveit_robot_model + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_robot_model angles moveit_msgs Eigen3 @@ -25,20 +37,14 @@ ament_target_dependencies(moveit_robot_model urdf urdfdom_headers srdfdom - visualization_msgs -) -target_link_libraries(moveit_robot_model - moveit_exceptions - moveit_macros - moveit_utils -) + visualization_msgs) +target_link_libraries(moveit_robot_model moveit_exceptions moveit_macros + moveit_utils) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_robot_model test/test.cpp) - ament_target_dependencies(test_robot_model - rclcpp - ) + ament_target_dependencies(test_robot_model rclcpp) target_link_libraries(test_robot_model moveit_test_utils moveit_robot_model) endif() diff --git a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h index 65112176a4..c63112fde4 100644 --- a/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h +++ b/moveit_core/robot_model/include/moveit/robot_model/joint_model_group.h @@ -406,7 +406,7 @@ class JointModelGroup void interpolate(const double* from, const double* to, double t, double* state) const; /** \brief Get the number of variables that describe this joint group. This includes variables necessary for mimic - joints, so will always be >= the number of items returned by getActiveVariableNames() */ + joints, so will always be >= getActiveVariableCount() */ unsigned int getVariableCount() const { return variable_count_; @@ -586,6 +586,21 @@ class JointModelGroup bool computeJointVariableIndices(const std::vector& joint_names, std::vector& joint_bijection) const; + /** + * @brief Get the lower and upper position limits of all active variables in the group. + * + * @return std::pair Containing the lower and upper joint limits for all active variables. + */ + [[nodiscard]] std::pair getLowerAndUpperLimits() const; + + /** + * @brief Gets the pair of maximum joint velocities/accelerations for a given group. Asserts that the group contains + * only single-variable joints, + * @details In case of asymmetric velocity or acceleration limits, this function will return the most limiting component. + * @return std::pair Containing the velocity and acceleration limits + */ + [[nodiscard]] std::pair getMaxVelocitiesAndAccelerationBounds() const; + protected: /** \brief Update the variable values for the state of a group with respect to the mimic joints. This only updates mimic joints that have the parent in this group. If there is a joint mimicking one that is outside the group, diff --git a/moveit_core/robot_model/src/floating_joint_model.cpp b/moveit_core/robot_model/src/floating_joint_model.cpp index 94fb8d2f6e..776ed5efd9 100644 --- a/moveit_core/robot_model/src/floating_joint_model.cpp +++ b/moveit_core/robot_model/src/floating_joint_model.cpp @@ -54,7 +54,7 @@ constexpr size_t STATE_SPACE_DIMENSION = 7; rclcpp::Logger getLogger() { - return moveit::getLogger("floating_joint_model"); + return moveit::getLogger("moveit.core.floating_joint_model"); } } // namespace diff --git a/moveit_core/robot_model/src/joint_model_group.cpp b/moveit_core/robot_model/src/joint_model_group.cpp index dc9bfb39a5..2776bb0c9d 100644 --- a/moveit_core/robot_model/src/joint_model_group.cpp +++ b/moveit_core/robot_model/src/joint_model_group.cpp @@ -54,7 +54,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("joint_model_group"); + return moveit::getLogger("moveit.core.joint_model_group"); } // check if a parent or ancestor of joint is included in this group @@ -229,6 +229,14 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode { link_model_map_[link_model->getName()] = link_model; link_model_name_vector_.push_back(link_model->getName()); + // if this is the first link of the group with a valid parent and includes geometry (for example `base_link`) it should included + if (link_model_with_geometry_vector_.empty() && link_model->getParentLinkModel() && + !link_model->getParentLinkModel()->getShapes().empty()) + { + link_model_with_geometry_vector_.push_back(link_model->getParentLinkModel()); + link_model_with_geometry_name_vector_.push_back(link_model->getParentLinkModel()->getName()); + } + // all child links with collision geometry should also be included if (!link_model->getShapes().empty()) { link_model_with_geometry_vector_.push_back(link_model); @@ -823,5 +831,56 @@ bool JointModelGroup::isValidVelocityMove(const double* from_joint_pose, const d return true; } + +std::pair JointModelGroup::getLowerAndUpperLimits() const +{ + // Get the group joints lower/upper position limits. + Eigen::VectorXd lower_limits(active_variable_count_); + Eigen::VectorXd upper_limits(active_variable_count_); + int variable_index = 0; + for (const moveit::core::JointModel::Bounds* joint_bounds : active_joint_models_bounds_) + { + for (const moveit::core::VariableBounds& variable_bounds : *joint_bounds) + { + lower_limits[variable_index] = variable_bounds.min_position_; + upper_limits[variable_index] = variable_bounds.max_position_; + variable_index++; + } + } + return { lower_limits, upper_limits }; +} + +std::pair JointModelGroup::getMaxVelocitiesAndAccelerationBounds() const +{ + Eigen::VectorXd max_joint_velocities = Eigen::VectorXd::Constant(active_variable_count_, 0.0); + Eigen::VectorXd max_joint_accelerations = Eigen::VectorXd::Constant(active_variable_count_, 0.0); + // Check if variable count matches number of joint model bounds + if (active_joint_models_bounds_.size() != active_variable_count_) + { + // TODO(sjahr) Support multiple variables + RCLCPP_ERROR(getLogger(), "Number of active joint models does not match number of active joint model bounds. " + "Returning bound vectors with zeros"); + return { max_joint_velocities, max_joint_accelerations }; + } + // Check if the joint group contains multi-dof joints + for (const auto& bound : active_joint_models_bounds_) + { + if (bound->size() != 1) + { + RCLCPP_ERROR(getLogger(), "Multi-dof joints are currently not supported by " + "getMaxVelocitiesAndAccelerationBounds(). Returning bound vectors with zeros."); + return { max_joint_velocities, max_joint_accelerations }; + } + } + // Populate max_joint_velocity and acceleration vectors + for (std::size_t i = 0; i < active_joint_models_bounds_.size(); ++i) + { + max_joint_velocities[i] = std::min(-active_joint_models_bounds_[i]->at(0).min_velocity_, + active_joint_models_bounds_[i]->at(0).max_velocity_); + max_joint_accelerations[i] = std::min(-active_joint_models_bounds_[i]->at(0).min_acceleration_, + active_joint_models_bounds_[i]->at(0).max_acceleration_); + } + return { max_joint_velocities, max_joint_accelerations }; +} } // end of namespace core } // end of namespace moveit diff --git a/moveit_core/robot_model/src/prismatic_joint_model.cpp b/moveit_core/robot_model/src/prismatic_joint_model.cpp index e229910d2f..81e9128053 100644 --- a/moveit_core/robot_model/src/prismatic_joint_model.cpp +++ b/moveit_core/robot_model/src/prismatic_joint_model.cpp @@ -79,7 +79,7 @@ void PrismaticJointModel::getVariableDefaultPositions(double* values, const Boun bool PrismaticJointModel::satisfiesPositionBounds(const double* values, const Bounds& bounds, double margin) const { - return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin); + return values[0] >= bounds[0].min_position_ - margin && values[0] <= bounds[0].max_position_ + margin; } void PrismaticJointModel::getVariableRandomPositions(random_numbers::RandomNumberGenerator& rng, double* values, diff --git a/moveit_core/robot_model/src/revolute_joint_model.cpp b/moveit_core/robot_model/src/revolute_joint_model.cpp index 1b80b4e2e7..524632159c 100644 --- a/moveit_core/robot_model/src/revolute_joint_model.cpp +++ b/moveit_core/robot_model/src/revolute_joint_model.cpp @@ -189,7 +189,7 @@ bool RevoluteJointModel::satisfiesPositionBounds(const double* values, const Bou } else { - return !(values[0] < bounds[0].min_position_ - margin || values[0] > bounds[0].max_position_ + margin); + return values[0] >= bounds[0].min_position_ - margin && values[0] <= bounds[0].max_position_ + margin; } } diff --git a/moveit_core/robot_model/src/robot_model.cpp b/moveit_core/robot_model/src/robot_model.cpp index 7c5536c117..f65ff86a11 100644 --- a/moveit_core/robot_model/src/robot_model.cpp +++ b/moveit_core/robot_model/src/robot_model.cpp @@ -54,7 +54,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("robot_model"); + return moveit::getLogger("moveit.core.robot_model"); } } // namespace diff --git a/moveit_core/robot_state/CMakeLists.txt b/moveit_core/robot_state/CMakeLists.txt index 6448b21296..fad8b36830 100644 --- a/moveit_core/robot_state/CMakeLists.txt +++ b/moveit_core/robot_state/CMakeLists.txt @@ -1,26 +1,16 @@ -add_library(moveit_robot_state SHARED - src/attached_body.cpp - src/conversions.cpp - src/robot_state.cpp - src/cartesian_interpolator.cpp -) -target_include_directories(moveit_robot_state PUBLIC - $ - $ -) -set_target_properties(moveit_robot_state PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) -ament_target_dependencies(moveit_robot_state - urdf - tf2_geometry_msgs - geometric_shapes - urdfdom_headers - Boost -) -target_link_libraries(moveit_robot_state - moveit_robot_model - moveit_kinematics_base - moveit_transforms -) +add_library( + moveit_robot_state SHARED src/attached_body.cpp src/conversions.cpp + src/robot_state.cpp src/cartesian_interpolator.cpp) +target_include_directories( + moveit_robot_state + PUBLIC $ + $) +set_target_properties(moveit_robot_state PROPERTIES VERSION + ${${PROJECT_NAME}_VERSION}) +ament_target_dependencies(moveit_robot_state urdf tf2_geometry_msgs + geometric_shapes urdfdom_headers Boost) +target_link_libraries(moveit_robot_state moveit_robot_model + moveit_kinematics_base moveit_transforms) install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -37,51 +27,32 @@ if(BUILD_TESTING) if(WIN32) # TODO add windows paths else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") + set(APPEND_LIBRARY_DIRS + "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../utils") endif() ament_add_gtest(test_robot_state test/robot_state_test.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") + APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}") - target_link_libraries(test_robot_state - moveit_test_utils - moveit_utils - moveit_exceptions - moveit_robot_state - ) + target_link_libraries(test_robot_state moveit_test_utils moveit_utils + moveit_exceptions moveit_robot_state) ament_add_gtest(test_robot_state_complex test/test_kinematic_complex.cpp) - target_link_libraries(test_robot_state_complex - moveit_test_utils - moveit_utils - moveit_exceptions - moveit_robot_state - ) + target_link_libraries(test_robot_state_complex moveit_test_utils moveit_utils + moveit_exceptions moveit_robot_state) ament_add_gtest(test_aabb test/test_aabb.cpp) - target_link_libraries(test_aabb - moveit_test_utils - moveit_utils - moveit_exceptions - moveit_robot_state - ) - - ament_add_gtest(test_cartesian_interpolator test/test_cartesian_interpolator.cpp) - target_link_libraries(test_cartesian_interpolator - moveit_test_utils - moveit_robot_state - moveit_kinematics_base - ) - - ament_add_google_benchmark( - robot_state_benchmark - test/robot_state_benchmark.cpp) - ament_target_dependencies(robot_state_benchmark - kdl_parser - ) - target_link_libraries(robot_state_benchmark - moveit_robot_model - moveit_test_utils - moveit_robot_state - ) + target_link_libraries(test_aabb moveit_test_utils moveit_utils + moveit_exceptions moveit_robot_state) + + ament_add_gtest(test_cartesian_interpolator + test/test_cartesian_interpolator.cpp) + target_link_libraries(test_cartesian_interpolator moveit_test_utils + moveit_robot_state moveit_kinematics_base) + + ament_add_google_benchmark(robot_state_benchmark + test/robot_state_benchmark.cpp) + ament_target_dependencies(robot_state_benchmark kdl_parser) + target_link_libraries(robot_state_benchmark moveit_robot_model + moveit_test_utils moveit_robot_state) endif() diff --git a/moveit_core/robot_state/src/cartesian_interpolator.cpp b/moveit_core/robot_state/src/cartesian_interpolator.cpp index 5a9f7fbe31..02707c3481 100644 --- a/moveit_core/robot_state/src/cartesian_interpolator.cpp +++ b/moveit_core/robot_state/src/cartesian_interpolator.cpp @@ -57,7 +57,7 @@ namespace rclcpp::Logger getLogger() { - return moveit::getLogger("cartesian_interpolator"); + return moveit::getLogger("moveit.core.cartesian_interpolator"); } std::optional hasRelativeJointSpaceJump(const std::vector& waypoints, diff --git a/moveit_core/robot_state/src/conversions.cpp b/moveit_core/robot_state/src/conversions.cpp index 50b588e16b..89538525b5 100644 --- a/moveit_core/robot_state/src/conversions.cpp +++ b/moveit_core/robot_state/src/conversions.cpp @@ -56,7 +56,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("conversions"); + return moveit::getLogger("moveit.core.conversions"); } bool jointStateToRobotStateImpl(const sensor_msgs::msg::JointState& joint_state, RobotState& state) diff --git a/moveit_core/robot_state/src/robot_state.cpp b/moveit_core/robot_state/src/robot_state.cpp index 5f7bf96010..b16373da04 100644 --- a/moveit_core/robot_state/src/robot_state.cpp +++ b/moveit_core/robot_state/src/robot_state.cpp @@ -59,7 +59,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("robot_state"); + return moveit::getLogger("moveit.core.robot_state"); } } // namespace diff --git a/moveit_core/robot_state/test/robot_state_benchmark.cpp b/moveit_core/robot_state/test/robot_state_benchmark.cpp index 8a5e8e46a3..06c40df745 100644 --- a/moveit_core/robot_state/test/robot_state_benchmark.cpp +++ b/moveit_core/robot_state/test/robot_state_benchmark.cpp @@ -38,21 +38,16 @@ // To run this benchmark, 'cd' to the build/moveit_core/robot_state directory and directly run the binary. #include +#include #include -#include +#include #include #include #include -#include // Robot and planning group for benchmarks. constexpr char PANDA_TEST_ROBOT[] = "panda"; constexpr char PANDA_TEST_GROUP[] = "panda_arm"; -constexpr char PR2_TEST_ROBOT[] = "pr2"; -constexpr char PR2_TIP_LINK[] = "r_wrist_roll_link"; - -// Number of iterations to use in matrix multiplication / inversion benchmarks. -constexpr int MATRIX_OPS_N_ITERATIONS = 1e7; namespace { @@ -66,267 +61,228 @@ Eigen::Isometry3d createTestIsometry() } // namespace // Benchmark time to multiply an Eigen::Affine3d with an Eigen::Matrix4d. -static void multiplyAffineTimesMatrix(benchmark::State& st) +// The NoAlias versions just use Eigen's .noalias() modifier, allowing to write the result of matrix multiplication +// directly into the result matrix instead of using an intermediate temporary (which is the default). +static void multiplyAffineTimesMatrixNoAlias(benchmark::State& st) { - int n_iters = st.range(0); Eigen::Isometry3d isometry = createTestIsometry(); + Eigen::Affine3d result; for (auto _ : st) { - for (int i = 0; i < n_iters; ++i) - { - Eigen::Affine3d result; - benchmark::DoNotOptimize(result = isometry.affine() * isometry.matrix()); - benchmark::ClobberMemory(); - } + benchmark::DoNotOptimize(result.affine().noalias() = isometry.affine() * isometry.matrix()); + benchmark::ClobberMemory(); } } // Benchmark time to multiply an Eigen::Matrix4d with an Eigen::Matrix4d. -static void multiplyMatrixTimesMatrix(benchmark::State& st) +static void multiplyMatrixTimesMatrixNoAlias(benchmark::State& st) { - int n_iters = st.range(0); Eigen::Isometry3d isometry = createTestIsometry(); + Eigen::Isometry3d result; for (auto _ : st) { - for (int i = 0; i < n_iters; ++i) - { - Eigen::Matrix4d result; - benchmark::DoNotOptimize(result = isometry.matrix() * isometry.matrix()); - benchmark::ClobberMemory(); - } + benchmark::DoNotOptimize(result.matrix().noalias() = isometry.matrix() * isometry.matrix()); + benchmark::ClobberMemory(); } } +// Benchmark time to multiply an Eigen::Isometry3d with an Eigen::Isometry3d. +static void multiplyIsometryTimesIsometryNoAlias(benchmark::State& st) +{ + Eigen::Isometry3d isometry = createTestIsometry(); + Eigen::Isometry3d result; + for (auto _ : st) + { + benchmark::DoNotOptimize(result.affine().noalias() = isometry.affine() * isometry.matrix()); + benchmark::ClobberMemory(); + } +} + +// Benchmark time to multiply an Eigen::Matrix4d with an Eigen::Matrix4d. +static void multiplyMatrixTimesMatrix(benchmark::State& st) +{ + Eigen::Isometry3d isometry = createTestIsometry(); + Eigen::Isometry3d result; + for (auto _ : st) + { + benchmark::DoNotOptimize(result.matrix() = isometry.matrix() * isometry.matrix()); + benchmark::ClobberMemory(); + } +} // Benchmark time to multiply an Eigen::Isometry3d with an Eigen::Isometry3d. static void multiplyIsometryTimesIsometry(benchmark::State& st) { - int n_iters = st.range(0); Eigen::Isometry3d isometry = createTestIsometry(); + Eigen::Isometry3d result; for (auto _ : st) { - for (int i = 0; i < n_iters; ++i) - { - Eigen::Isometry3d result; - benchmark::DoNotOptimize(result = isometry * isometry); - benchmark::ClobberMemory(); - } + benchmark::DoNotOptimize(result = isometry * isometry); + benchmark::ClobberMemory(); } } // Benchmark time to invert an Eigen::Isometry3d. static void inverseIsometry3d(benchmark::State& st) { - int n_iters = st.range(0); Eigen::Isometry3d isometry = createTestIsometry(); + Eigen::Isometry3d result; for (auto _ : st) { - for (int i = 0; i < n_iters; ++i) - { - Eigen::Isometry3d result; - benchmark::DoNotOptimize(result = isometry.inverse()); - benchmark::ClobberMemory(); - } + benchmark::DoNotOptimize(result = isometry.inverse()); + benchmark::ClobberMemory(); } } // Benchmark time to invert an Eigen::Affine3d(Eigen::Isometry). static void inverseAffineIsometry(benchmark::State& st) { - int n_iters = st.range(0); - Eigen::Isometry3d isometry = createTestIsometry(); - Eigen::Affine3d affine; - affine.matrix() = isometry.matrix(); - + Eigen::Affine3d affine = createTestIsometry(); + Eigen::Affine3d result; for (auto _ : st) { - for (int i = 0; i < n_iters; ++i) - { - Eigen::Affine3d result; - benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry).affine()); - benchmark::ClobberMemory(); - } + benchmark::DoNotOptimize(result = affine.inverse(Eigen::Isometry)); + benchmark::ClobberMemory(); } } // Benchmark time to invert an Eigen::Affine3d. static void inverseAffine(benchmark::State& st) { - int n_iters = st.range(0); - Eigen::Isometry3d isometry = createTestIsometry(); - Eigen::Affine3d affine; - affine.matrix() = isometry.matrix(); - + Eigen::Affine3d affine = createTestIsometry(); + Eigen::Affine3d result; for (auto _ : st) { - for (int i = 0; i < n_iters; ++i) - { - Eigen::Affine3d result; - benchmark::DoNotOptimize(result = affine.inverse().affine()); - benchmark::ClobberMemory(); - } + benchmark::DoNotOptimize(result = affine.inverse()); + benchmark::ClobberMemory(); } } // Benchmark time to invert an Eigen::Matrix4d. static void inverseMatrix4d(benchmark::State& st) { - int n_iters = st.range(0); - Eigen::Isometry3d isometry = createTestIsometry(); - Eigen::Affine3d affine; - affine.matrix() = isometry.matrix(); - + Eigen::Affine3d affine = createTestIsometry(); + Eigen::Affine3d result; for (auto _ : st) { - for (int i = 0; i < n_iters; ++i) - { - Eigen::Affine3d result; - benchmark::DoNotOptimize(result = affine.matrix().inverse()); - benchmark::ClobberMemory(); - } + benchmark::DoNotOptimize(result = affine.matrix().inverse()); + benchmark::ClobberMemory(); } } -// Benchmark time to construct a RobotState given a RobotModel. -static void robotStateConstruct(benchmark::State& st) +struct RobotStateBenchmark : ::benchmark::Fixture { - int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); - - // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) + void SetUp(const ::benchmark::State& /*state*/) override { - st.SkipWithError("The planning group doesn't exist."); - return; + std::ignore = rcutils_logging_set_logger_level("moveit_robot_model.robot_model", RCUTILS_LOG_SEVERITY_WARN); + robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); } - for (auto _ : st) + std::vector constructStates(size_t num) { - for (int i = 0; i < n_states; i++) - { - std::unique_ptr robot_state; - benchmark::DoNotOptimize(robot_state = std::make_unique(robot_model)); - benchmark::ClobberMemory(); - } + std::vector states; + states.reserve(num); + for (size_t i = 0; i < num; i++) + states.emplace_back(robot_model); + return states; + } + std::vector constructStates(size_t num, const moveit::core::RobotState& state) + { + std::vector states; + states.reserve(num); + for (size_t i = 0; i < num; i++) + states.emplace_back(state); + return states; } -} - -// Benchmark time to copy a RobotState. -static void robotStateCopy(benchmark::State& st) -{ - int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); - // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) + std::vector randomPermutation(size_t num) { - st.SkipWithError("The planning group doesn't exist."); - return; + std::vector result(num); + std::iota(result.begin(), result.end(), 0); + + std::mt19937 generator; + std::shuffle(result.begin(), result.end(), generator); + return result; } - // Robot state. - moveit::core::RobotState robot_state(robot_model); - robot_state.setToDefaultValues(); + moveit::core::RobotModelPtr robot_model; +}; +// Benchmark time to construct RobotStates +BENCHMARK_DEFINE_F(RobotStateBenchmark, construct)(benchmark::State& st) +{ for (auto _ : st) { - for (int i = 0; i < n_states; i++) - { - std::unique_ptr robot_state_copy; - benchmark::DoNotOptimize(robot_state_copy = std::make_unique(robot_state)); - benchmark::ClobberMemory(); - } + auto states = constructStates(st.range(0)); + benchmark::DoNotOptimize(states); + benchmark::ClobberMemory(); } } -// Benchmark time to call `setToRandomPositions` and `update` on a RobotState. -static void robotStateUpdate(benchmark::State& st) +// Benchmark time to copy-construct a RobotState. +BENCHMARK_DEFINE_F(RobotStateBenchmark, copyConstruct)(benchmark::State& st) { - int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT); moveit::core::RobotState state(robot_model); + state.setToDefaultValues(); + state.update(); for (auto _ : st) { - for (int i = 0; i < n_states; ++i) - { - state.setToRandomPositions(); - state.update(); - benchmark::ClobberMemory(); - } + auto states = constructStates(st.range(0), state); + benchmark::DoNotOptimize(states); + benchmark::ClobberMemory(); } } -// Benchmark time to call `setToRandomPositions` and `getGlobalLinkTransform` on a RobotState. -static void robotStateForwardKinematics(benchmark::State& st) +// Benchmark time to call `setToRandomPositions` and `update` on RobotState. +BENCHMARK_DEFINE_F(RobotStateBenchmark, update)(benchmark::State& st) { - int n_states = st.range(0); - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PR2_TEST_ROBOT); - moveit::core::RobotState state(robot_model); - + auto states = constructStates(st.range(0)); + auto permutation = randomPermutation(states.size()); for (auto _ : st) { - for (int i = 0; i < n_states; ++i) + for (auto i : permutation) // process states in random order to challenge the cache { - state.setToRandomPositions(); - Eigen::Isometry3d transform; - benchmark::DoNotOptimize(transform = state.getGlobalLinkTransform(robot_model->getLinkModel(PR2_TIP_LINK))); - benchmark::ClobberMemory(); + states[i].setToRandomPositions(); + states[i].update(); } } } // Benchmark time to compute the Jacobian, using MoveIt's `getJacobian` function. -static void moveItJacobian(benchmark::State& st) +BENCHMARK_DEFINE_F(RobotStateBenchmark, jacobianMoveIt)(benchmark::State& st) { - // Load a test robot model. - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); - - // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(PANDA_TEST_GROUP); + if (!jmg) { st.SkipWithError("The planning group doesn't exist."); return; } - // Robot state. - moveit::core::RobotState kinematic_state(robot_model); - const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(PANDA_TEST_GROUP); - - // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint - // configurations. + // Manually seeded RandomNumberGenerator for deterministic results random_numbers::RandomNumberGenerator rng(0); for (auto _ : st) { // Time only the jacobian computation, not the forward kinematics. st.PauseTiming(); - kinematic_state.setToRandomPositions(jmg, rng); - kinematic_state.updateLinkTransforms(); + state.setToRandomPositions(jmg, rng); + state.updateLinkTransforms(); st.ResumeTiming(); - kinematic_state.getJacobian(jmg); + state.getJacobian(jmg); } } // Benchmark time to compute the Jacobian using KDL. -static void kdlJacobian(benchmark::State& st) +BENCHMARK_DEFINE_F(RobotStateBenchmark, jacobianKDL)(benchmark::State& st) { - const moveit::core::RobotModelPtr& robot_model = moveit::core::loadTestingRobotModel(PANDA_TEST_ROBOT); - - // Make sure the group exists, otherwise exit early with an error. - if (!robot_model->hasJointModelGroup(PANDA_TEST_GROUP)) + moveit::core::RobotState state(robot_model); + const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(PANDA_TEST_GROUP); + if (!jmg) { st.SkipWithError("The planning group doesn't exist."); return; } - - // Robot state. - moveit::core::RobotState kinematic_state(robot_model); - const moveit::core::JointModelGroup* jmg = kinematic_state.getJointModelGroup(PANDA_TEST_GROUP); - - // Provide our own random number generator to setToRandomPositions to get a deterministic sequence of joint - // configurations. - random_numbers::RandomNumberGenerator rng(0); - KDL::Tree kdl_tree; if (!kdl_parser::treeFromUrdfModel(*robot_model->getURDF(), kdl_tree)) { @@ -334,44 +290,45 @@ static void kdlJacobian(benchmark::State& st) return; } - KDL::Chain kdl_chain; - if (!kdl_tree.getChain(jmg->getJointModels().front()->getParentLinkModel()->getName(), - jmg->getLinkModelNames().back(), kdl_chain)) - { - st.SkipWithError("Can't create KDL Chain."); - return; - } + KDL::TreeJntToJacSolver jacobian_solver(kdl_tree); + KDL::Jacobian jacobian(kdl_tree.getNrOfJoints()); + KDL::JntArray kdl_q(kdl_tree.getNrOfJoints()); + const std::string tip_link = jmg->getLinkModelNames().back(); - KDL::ChainJntToJacSolver jacobian_solver(kdl_chain); + // Manually seeded RandomNumberGenerator for deterministic results + random_numbers::RandomNumberGenerator rng(0); for (auto _ : st) { // Time only the jacobian computation, not the forward kinematics. st.PauseTiming(); - kinematic_state.setToRandomPositions(jmg, rng); - kinematic_state.updateLinkTransforms(); - KDL::Jacobian jacobian(kdl_chain.getNrOfJoints()); - KDL::JntArray kdl_q; - kdl_q.resize(kdl_chain.getNrOfJoints()); - kinematic_state.copyJointGroupPositions(jmg, &kdl_q.data[0]); + state.setToRandomPositions(jmg, rng); + state.copyJointGroupPositions(jmg, &kdl_q.data[0]); st.ResumeTiming(); - jacobian_solver.JntToJac(kdl_q, jacobian); + jacobian_solver.JntToJac(kdl_q, jacobian, tip_link); } } -BENCHMARK(multiplyAffineTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(multiplyMatrixTimesMatrix)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(multiplyIsometryTimesIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); - -BENCHMARK(inverseIsometry3d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(inverseAffineIsometry)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(inverseAffine)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); -BENCHMARK(inverseMatrix4d)->Arg(MATRIX_OPS_N_ITERATIONS)->Unit(benchmark::kMillisecond); - -BENCHMARK(robotStateConstruct)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); -BENCHMARK(robotStateCopy)->RangeMultiplier(10)->Range(100, 10000)->Unit(benchmark::kMillisecond); -BENCHMARK(robotStateUpdate)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond); -BENCHMARK(robotStateForwardKinematics)->RangeMultiplier(10)->Range(10, 1000)->Unit(benchmark::kMillisecond); - -BENCHMARK(moveItJacobian); -BENCHMARK(kdlJacobian); +BENCHMARK(multiplyAffineTimesMatrixNoAlias); +BENCHMARK(multiplyMatrixTimesMatrixNoAlias); +BENCHMARK(multiplyIsometryTimesIsometryNoAlias); +BENCHMARK(multiplyMatrixTimesMatrix); +BENCHMARK(multiplyIsometryTimesIsometry); + +BENCHMARK(inverseIsometry3d); +BENCHMARK(inverseAffineIsometry); +BENCHMARK(inverseAffine); +BENCHMARK(inverseMatrix4d); + +BENCHMARK_REGISTER_F(RobotStateBenchmark, construct) + ->RangeMultiplier(10) + ->Range(100, 10000) + ->Unit(benchmark::kMillisecond); +BENCHMARK_REGISTER_F(RobotStateBenchmark, copyConstruct) + ->RangeMultiplier(10) + ->Range(100, 10000) + ->Unit(benchmark::kMillisecond); +BENCHMARK_REGISTER_F(RobotStateBenchmark, update)->RangeMultiplier(10)->Range(10, 10000)->Unit(benchmark::kMillisecond); + +BENCHMARK_REGISTER_F(RobotStateBenchmark, jacobianMoveIt); +BENCHMARK_REGISTER_F(RobotStateBenchmark, jacobianKDL); diff --git a/moveit_core/robot_state/test/robot_state_test.cpp b/moveit_core/robot_state/test/robot_state_test.cpp index 8fdce99e73..cafb5bf01f 100644 --- a/moveit_core/robot_state/test/robot_state_test.cpp +++ b/moveit_core/robot_state/test/robot_state_test.cpp @@ -95,7 +95,7 @@ void checkJacobian(moveit::core::RobotState& state, const moveit::core::JointMod EXPECT_NEAR(angle, 0.0, 1e-05) << "Angle between Cartesian velocity and Cartesian displacement larger than expected. " "Angle: " << angle << ". displacement: " << displacement.transpose() - << ". Cartesian velocity: " << cartesian_velocity.head<3>().transpose() << std::endl; + << ". Cartesian velocity: " << cartesian_velocity.head<3>().transpose() << '\n'; } } // namespace diff --git a/moveit_core/robot_trajectory/CMakeLists.txt b/moveit_core/robot_trajectory/CMakeLists.txt index 5dcff268a8..c4b26ebb82 100644 --- a/moveit_core/robot_trajectory/CMakeLists.txt +++ b/moveit_core/robot_trajectory/CMakeLists.txt @@ -1,24 +1,20 @@ add_library(moveit_robot_trajectory SHARED src/robot_trajectory.cpp) -target_include_directories(moveit_robot_trajectory PUBLIC - $ - $ -) -set_target_properties(moveit_robot_trajectory PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) -ament_target_dependencies(moveit_robot_trajectory - rclcpp - urdfdom - urdfdom_headers -) +target_include_directories( + moveit_robot_trajectory + PUBLIC $ + $) +set_target_properties(moveit_robot_trajectory + PROPERTIES VERSION ${${PROJECT_NAME}_VERSION}) +ament_target_dependencies(moveit_robot_trajectory rclcpp urdfdom + urdfdom_headers) -target_link_libraries(moveit_robot_trajectory - moveit_robot_model - moveit_robot_state - moveit_utils -) +target_link_libraries(moveit_robot_trajectory moveit_robot_model + moveit_robot_state moveit_utils) install(DIRECTORY include/ DESTINATION include/moveit_core) if(BUILD_TESTING) ament_add_gtest(test_robot_trajectory test/test_robot_trajectory.cpp) - target_link_libraries(test_robot_trajectory moveit_test_utils moveit_robot_trajectory) + target_link_libraries(test_robot_trajectory moveit_test_utils + moveit_robot_trajectory) endif() diff --git a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h index 35533d173c..1f77946234 100644 --- a/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h +++ b/moveit_core/robot_trajectory/include/moveit/robot_trajectory/robot_trajectory.h @@ -242,7 +242,7 @@ class RobotTrajectory RobotTrajectory& append(const RobotTrajectory& source, double dt, size_t start_index = 0, size_t end_index = std::numeric_limits::max()); - void swap(robot_trajectory::RobotTrajectory& other); + void swap(robot_trajectory::RobotTrajectory& other) noexcept; RobotTrajectory& clear() { @@ -410,4 +410,13 @@ std::ostream& operator<<(std::ostream& out, const RobotTrajectory& trajectory); /// or nullopt if it is not possible to calculate the density [[nodiscard]] std::optional waypointDensity(const RobotTrajectory& trajectory); +/// \brief Converts a RobotTrajectory to a JointTrajectory message +// \param[in] trajectory Given robot trajectory +// \param[in] include_mdof_joints Treat Multi-DOF variables as joints, e.g. position/x position/y position/theta +// \param[in] joint_filter Exclude joints with the provided names +// \return JointTrajectory message including all waypoints +// or nullopt if the provided RobotTrajectory or RobotModel is empty +[[nodiscard]] std::optional +toJointTrajectory(const RobotTrajectory& trajectory, bool include_mdof_joints = false, + const std::vector& joint_filter = {}); } // namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/src/robot_trajectory.cpp b/moveit_core/robot_trajectory/src/robot_trajectory.cpp index ee072f9929..8e0eed38be 100644 --- a/moveit_core/robot_trajectory/src/robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/src/robot_trajectory.cpp @@ -52,7 +52,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("robot_trajectory"); + return moveit::getLogger("moveit.core.robot_trajectory"); } } // namespace @@ -121,7 +121,7 @@ double RobotTrajectory::getAverageSegmentDuration() const return getDuration() / static_cast(duration_from_previous_.size()); } -void RobotTrajectory::swap(RobotTrajectory& other) +void RobotTrajectory::swap(RobotTrajectory& other) noexcept { robot_model_.swap(other.robot_model_); std::swap(group_, other.group_); @@ -373,29 +373,36 @@ void RobotTrajectory::getRobotTrajectoryMsg(moveit_msgs::msg::RobotTrajectory& t { const std::vector names = mdof[j]->getVariableNames(); const double* velocities = waypoints_[i]->getJointVelocities(mdof[j]); + const double* accelerations = waypoints_[i]->getJointAccelerations(mdof[j]); geometry_msgs::msg::Twist point_velocity; + geometry_msgs::msg::Twist point_acceleration; for (std::size_t k = 0; k < names.size(); ++k) { if (names[k].find("/x") != std::string::npos) { point_velocity.linear.x = velocities[k]; + point_acceleration.linear.x = accelerations[k]; } else if (names[k].find("/y") != std::string::npos) { point_velocity.linear.y = velocities[k]; + point_acceleration.linear.y = accelerations[k]; } else if (names[k].find("/z") != std::string::npos) { point_velocity.linear.z = velocities[k]; + point_acceleration.linear.z = accelerations[k]; } else if (names[k].find("/theta") != std::string::npos) { point_velocity.angular.z = velocities[k]; + point_acceleration.angular.z = accelerations[k]; } } trajectory.multi_dof_joint_trajectory.points[i].velocities.push_back(point_velocity); + trajectory.multi_dof_joint_trajectory.points[i].accelerations.push_back(point_acceleration); } } if (duration_from_previous_.size() > i) @@ -707,4 +714,129 @@ std::optional waypointDensity(const RobotTrajectory& trajectory) return std::nullopt; } +std::optional toJointTrajectory(const RobotTrajectory& trajectory, + bool include_mdof_joints, + const std::vector& joint_filter) +{ + const auto group = trajectory.getGroup(); + const auto& robot_model = trajectory.getRobotModel(); + const std::vector& jnts = + group ? group->getActiveJointModels() : robot_model->getActiveJointModels(); + + if (trajectory.empty() || jnts.empty()) + return std::nullopt; + + trajectory_msgs::msg::JointTrajectory joint_trajectory; + std::vector onedof; + std::vector mdof; + + for (const moveit::core::JointModel* active_joint : jnts) + { + // only consider joints listed in joint_filter + if (!joint_filter.empty() && + std::find(joint_filter.begin(), joint_filter.end(), active_joint->getName()) == joint_filter.end()) + continue; + + if (active_joint->getVariableCount() == 1) + { + onedof.push_back(active_joint); + } + else if (include_mdof_joints) + { + mdof.push_back(active_joint); + } + } + + for (const auto& joint : onedof) + { + joint_trajectory.joint_names.push_back(joint->getName()); + } + for (const auto& joint : mdof) + { + for (const auto& name : joint->getVariableNames()) + { + joint_trajectory.joint_names.push_back(name); + } + } + + if (!onedof.empty() || !mdof.empty()) + { + joint_trajectory.header.frame_id = robot_model->getModelFrame(); + joint_trajectory.header.stamp = rclcpp::Time(0, 0, RCL_ROS_TIME); + joint_trajectory.points.resize(trajectory.getWayPointCount()); + } + + static const auto ZERO_DURATION = rclcpp::Duration::from_seconds(0); + double total_time = 0.0; + for (std::size_t i = 0; i < trajectory.getWayPointCount(); ++i) + { + total_time += trajectory.getWayPointDurationFromPrevious(i); + joint_trajectory.points[i].time_from_start = rclcpp::Duration::from_seconds(total_time); + const auto& waypoint = trajectory.getWayPoint(i); + + if (!onedof.empty()) + { + joint_trajectory.points[i].positions.resize(onedof.size()); + joint_trajectory.points[i].velocities.reserve(onedof.size()); + + for (std::size_t j = 0; j < onedof.size(); ++j) + { + joint_trajectory.points[i].positions[j] = waypoint.getVariablePosition(onedof[j]->getFirstVariableIndex()); + // if we have velocities/accelerations/effort, copy those too + if (waypoint.hasVelocities()) + { + joint_trajectory.points[i].velocities.push_back( + waypoint.getVariableVelocity(onedof[j]->getFirstVariableIndex())); + } + if (waypoint.hasAccelerations()) + { + joint_trajectory.points[i].accelerations.push_back( + waypoint.getVariableAcceleration(onedof[j]->getFirstVariableIndex())); + } + if (waypoint.hasEffort()) + { + joint_trajectory.points[i].effort.push_back(waypoint.getVariableEffort(onedof[j]->getFirstVariableIndex())); + } + } + // clear velocities if we have an incomplete specification + if (joint_trajectory.points[i].velocities.size() != onedof.size()) + joint_trajectory.points[i].velocities.clear(); + // clear accelerations if we have an incomplete specification + if (joint_trajectory.points[i].accelerations.size() != onedof.size()) + joint_trajectory.points[i].accelerations.clear(); + // clear effort if we have an incomplete specification + if (joint_trajectory.points[i].effort.size() != onedof.size()) + joint_trajectory.points[i].effort.clear(); + } + + if (!mdof.empty()) + { + for (const auto joint : mdof) + { + // Add variable placeholders + const std::vector names = joint->getVariableNames(); + joint_trajectory.points[i].positions.reserve(joint_trajectory.points[i].positions.size() + names.size()); + + joint_trajectory.points[i].velocities.reserve(joint_trajectory.points[i].velocities.size() + names.size()); + + for (const auto& name : names) + { + joint_trajectory.points[i].positions.push_back(waypoint.getVariablePosition(name)); + + if (waypoint.hasVelocities()) + { + joint_trajectory.points[i].velocities.push_back(waypoint.getVariableVelocity(name)); + } + if (waypoint.hasAccelerations()) + { + joint_trajectory.points[i].accelerations.push_back(waypoint.getVariableAcceleration(name)); + } + } + } + } + } + + return joint_trajectory; +} + } // end of namespace robot_trajectory diff --git a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp index 4a22a813ce..8ffdcb1a8a 100644 --- a/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp +++ b/moveit_core/robot_trajectory/test/test_robot_trajectory.cpp @@ -634,6 +634,38 @@ TEST_F(OneRobot, UnwindFromState) } } +TEST_F(OneRobot, MultiDofTrajectoryToJointStates) +{ + // GIVEN a RobotTrajectory with two waypoints of a robot model that has a multi-dof base joint + robot_trajectory::RobotTrajectory trajectory(robot_model_); + trajectory.addSuffixWayPoint(robot_state_, 0.01 /* dt */); + trajectory.addSuffixWayPoint(robot_state_, 0.01 /* dt */); + + // WHEN converting the RobotTrajectory to a JointTrajectory message, including mdof variables + auto maybe_trajectory_msg = toJointTrajectory(trajectory, true /* include_mdof_joints */); + + // WHEN the optional trajectory result is valid (always assumed) + ASSERT_TRUE(maybe_trajectory_msg.has_value()); + + const auto traj = maybe_trajectory_msg.value(); + const auto& joint_names = traj.joint_names; + + size_t joint_variable_count = 0u; + for (const auto& active_joint : robot_model_->getActiveJointModels()) + { + joint_variable_count += active_joint->getVariableCount(); + } + + // THEN all joints names should include the base joint variables + EXPECT_EQ(joint_names.size(), joint_variable_count); + EXPECT_TRUE(std::find(joint_names.begin(), joint_names.end(), "base_joint/x") != joint_names.end()); + // THEN the size of the trajectory should equal the input size + ASSERT_EQ(traj.points.size(), 2u); + // THEN all positions size should equal the variable size + EXPECT_EQ(traj.points.at(0).positions.size(), joint_variable_count); + EXPECT_EQ(traj.points.at(1).positions.size(), joint_variable_count); +} + int main(int argc, char** argv) { testing::InitGoogleTest(&argc, argv); diff --git a/moveit_core/trajectory_processing/CMakeLists.txt b/moveit_core/trajectory_processing/CMakeLists.txt index 50c1ac9ddf..bbbe8d1f2c 100644 --- a/moveit_core/trajectory_processing/CMakeLists.txt +++ b/moveit_core/trajectory_processing/CMakeLists.txt @@ -1,27 +1,24 @@ -add_library(moveit_trajectory_processing SHARED - src/ruckig_traj_smoothing.cpp - src/trajectory_tools.cpp - src/time_optimal_trajectory_generation.cpp -) -target_include_directories(moveit_trajectory_processing PUBLIC - $ - $ -) -set_target_properties(moveit_trajectory_processing PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_trajectory_processing +add_library( + moveit_trajectory_processing SHARED + src/ruckig_traj_smoothing.cpp src/trajectory_tools.cpp + src/time_optimal_trajectory_generation.cpp) +target_include_directories( + moveit_trajectory_processing + PUBLIC $ + $) +set_target_properties(moveit_trajectory_processing + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_trajectory_processing rclcpp rmw_implementation urdf urdfdom urdfdom_headers visualization_msgs - Boost -) -target_link_libraries(moveit_trajectory_processing - moveit_robot_state - moveit_robot_trajectory - ruckig::ruckig -) + Boost) +target_link_libraries(moveit_trajectory_processing moveit_robot_state + moveit_robot_trajectory ruckig::ruckig) install(DIRECTORY include/ DESTINATION include/moveit_core) @@ -30,29 +27,27 @@ if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(benchmark REQUIRED) if(WIN32) - # TODO add windows paths - # set(append_library_dirs "$;$") + # TODO add windows paths set(APPEND_LIBRARY_DIRS + # "$;$") else() - set(append_library_dirs "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_trajectory;${CMAKE_CURRENT_BINARY_DIR}/../utils") + set(APPEND_LIBRARY_DIRS + "${CMAKE_CURRENT_BINARY_DIR};${CMAKE_CURRENT_BINARY_DIR}/../robot_trajectory;${CMAKE_CURRENT_BINARY_DIR}/../utils" + ) endif() - ament_add_gtest(test_time_optimal_trajectory_generation test/test_time_optimal_trajectory_generation.cpp) - target_link_libraries(test_time_optimal_trajectory_generation moveit_test_utils moveit_trajectory_processing) + ament_add_gtest(test_time_optimal_trajectory_generation + test/test_time_optimal_trajectory_generation.cpp) + target_link_libraries(test_time_optimal_trajectory_generation + moveit_test_utils moveit_trajectory_processing) - ament_add_gtest(test_ruckig_traj_smoothing test/test_ruckig_traj_smoothing.cpp) - target_link_libraries(test_ruckig_traj_smoothing - moveit_trajectory_processing - moveit_test_utils - ) + ament_add_gtest(test_ruckig_traj_smoothing + test/test_ruckig_traj_smoothing.cpp) + target_link_libraries(test_ruckig_traj_smoothing moveit_trajectory_processing + moveit_test_utils) - ament_add_google_benchmark( - robot_trajectory_benchmark - test/robot_trajectory_benchmark.cpp) - target_link_libraries(robot_trajectory_benchmark - moveit_robot_model - moveit_test_utils - moveit_robot_state - moveit_robot_trajectory - moveit_trajectory_processing - ) + ament_add_google_benchmark(robot_trajectory_benchmark + test/robot_trajectory_benchmark.cpp) + target_link_libraries( + robot_trajectory_benchmark moveit_robot_model moveit_test_utils + moveit_robot_state moveit_robot_trajectory moveit_trajectory_processing) endif() diff --git a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h index 2e8d600cb3..876f3af06c 100644 --- a/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h +++ b/moveit_core/trajectory_processing/include/moveit/trajectory_processing/trajectory_tools.h @@ -38,6 +38,7 @@ #include #include +#include namespace trajectory_processing { @@ -79,4 +80,11 @@ bool applyTOTGTimeParameterization(robot_trajectory::RobotTrajectory& trajectory bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double velocity_scaling_factor, double acceleration_scaling_factor, bool mitigate_overshoot = false, double overshoot_threshold = 0.01); + +/** + * @brief Converts a `trajectory_processing::Trajectory` into a `JointTrajectory` message with a given sampling rate. + */ +[[nodiscard]] trajectory_msgs::msg::JointTrajectory +createTrajectoryMessage(const std::vector& joint_names, + const trajectory_processing::Trajectory& trajectory, const int sampling_rate); } // namespace trajectory_processing diff --git a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp index 31a72d3265..84c27c8c22 100644 --- a/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp +++ b/moveit_core/trajectory_processing/src/ruckig_traj_smoothing.cpp @@ -57,7 +57,7 @@ constexpr double OVERSHOOT_CHECK_PERIOD = 0.01; // sec rclcpp::Logger getLogger() { - return moveit::getLogger("ruckig_traj_smoothing"); + return moveit::getLogger("moveit.core.ruckig_traj_smoothing"); } } // namespace diff --git a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp index d38eae1784..ad9225b34a 100644 --- a/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp +++ b/moveit_core/trajectory_processing/src/time_optimal_trajectory_generation.cpp @@ -56,7 +56,7 @@ constexpr double DEFAULT_SCALING_FACTOR = 1.0; rclcpp::Logger getLogger() { - return moveit::getLogger("time_optimal_trajectory_generation"); + return moveit::getLogger("moveit.core.time_optimal_trajectory_generation"); } } // namespace @@ -586,7 +586,7 @@ bool Trajectory::integrateForward(std::list& trajectory, double if (next_discontinuity != switching_points.end() && path_pos > next_discontinuity->first) { // Avoid having a TrajectoryStep with path_pos near a switching point which will cause an almost identical - // TrajectoryStep get added in the next run (https://github.com/ros-planning/moveit/issues/1665) + // TrajectoryStep get added in the next run (https://github.com/moveit/moveit/issues/1665) if (path_pos - next_discontinuity->first < EPS) { continue; diff --git a/moveit_core/trajectory_processing/src/trajectory_tools.cpp b/moveit_core/trajectory_processing/src/trajectory_tools.cpp index 6ce5a412e6..b15f0ca9a3 100644 --- a/moveit_core/trajectory_processing/src/trajectory_tools.cpp +++ b/moveit_core/trajectory_processing/src/trajectory_tools.cpp @@ -37,8 +37,20 @@ #include #include #include + +#include +#include namespace trajectory_processing { + +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.trajectory_processing.trajectory_tools"); +} +} // namespace + bool isTrajectoryEmpty(const moveit_msgs::msg::RobotTrajectory& trajectory) { return trajectory.joint_trajectory.points.empty() && trajectory.multi_dof_joint_trajectory.points.empty(); @@ -62,4 +74,37 @@ bool applyRuckigSmoothing(robot_trajectory::RobotTrajectory& trajectory, double return time_param.applySmoothing(trajectory, velocity_scaling_factor, acceleration_scaling_factor, mitigate_overshoot, overshoot_threshold); } + +trajectory_msgs::msg::JointTrajectory createTrajectoryMessage(const std::vector& joint_names, + const trajectory_processing::Trajectory& trajectory, + const int sampling_rate) +{ + trajectory_msgs::msg::JointTrajectory trajectory_msg; + if (sampling_rate <= 0) + { + RCLCPP_ERROR(getLogger(), "Cannot sample trajectory with sampling rate <= 0. Returning empty trajectory."); + return trajectory_msg; + } + trajectory_msg.joint_names = joint_names; + const double time_step = 1.0 / static_cast(sampling_rate); + const int n_samples = static_cast(trajectory.getDuration() / time_step) + 1; + trajectory_msg.points.reserve(n_samples); + for (int sample = 0; sample < n_samples; ++sample) + { + const double t = sample * time_step; + trajectory_msgs::msg::JointTrajectoryPoint point; + auto position = trajectory.getPosition(t); + auto velocity = trajectory.getVelocity(t); + auto acceleration = trajectory.getAcceleration(t); + for (std::size_t i = 0; i < joint_names.size(); i++) + { + point.positions.push_back(position[i]); + point.velocities.push_back(velocity[i]); + point.accelerations.push_back(acceleration[i]); + } + point.time_from_start = rclcpp::Duration(std::chrono::duration(t)); + trajectory_msg.points.push_back(std::move(point)); + } + return trajectory_msg; +} } // namespace trajectory_processing diff --git a/moveit_core/transforms/CMakeLists.txt b/moveit_core/transforms/CMakeLists.txt index 7ed4f0debf..5880e5795a 100644 --- a/moveit_core/transforms/CMakeLists.txt +++ b/moveit_core/transforms/CMakeLists.txt @@ -1,19 +1,20 @@ add_library(moveit_transforms SHARED src/transforms.cpp) -target_include_directories(moveit_transforms PUBLIC - $ - $ -) +target_include_directories( + moveit_transforms + PUBLIC $ + $) target_link_libraries(moveit_transforms moveit_macros moveit_utils) -set_target_properties(moveit_transforms PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_transforms +set_target_properties(moveit_transforms PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_transforms geometric_shapes tf2_eigen rclcpp rmw_implementation urdfdom urdfdom_headers - Boost -) + Boost) install(DIRECTORY include/ DESTINATION include/moveit_core) diff --git a/moveit_core/transforms/src/transforms.cpp b/moveit_core/transforms/src/transforms.cpp index c2a2345301..14a57b2c6e 100644 --- a/moveit_core/transforms/src/transforms.cpp +++ b/moveit_core/transforms/src/transforms.cpp @@ -50,7 +50,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("transforms"); + return moveit::getLogger("moveit.core.transforms"); } } // namespace diff --git a/moveit_core/utils/CMakeLists.txt b/moveit_core/utils/CMakeLists.txt index 2abc76218e..ca44183fa0 100644 --- a/moveit_core/utils/CMakeLists.txt +++ b/moveit_core/utils/CMakeLists.txt @@ -1,28 +1,25 @@ -add_library(moveit_utils SHARED - src/lexical_casts.cpp - src/message_checks.cpp - src/rclcpp_utils.cpp - src/logger.cpp -) -target_include_directories(moveit_utils PUBLIC - $ - $ -) +add_library(moveit_utils SHARED src/lexical_casts.cpp src/message_checks.cpp + src/rclcpp_utils.cpp src/logger.cpp) +target_include_directories( + moveit_utils PUBLIC $ + $) ament_target_dependencies(moveit_utils Boost moveit_msgs rclcpp fmt) target_link_libraries(moveit_utils rsl::rsl) -set_target_properties(moveit_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(moveit_utils PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}") install(DIRECTORY include/ DESTINATION include/moveit_core) - find_package(ament_index_cpp REQUIRED) add_library(moveit_test_utils SHARED src/robot_model_test_utils.cpp) -target_include_directories(moveit_test_utils PUBLIC - $ - $ -) -target_link_libraries(moveit_test_utils moveit_robot_model moveit_kinematics_base rsl::rsl) -ament_target_dependencies(moveit_test_utils +target_include_directories( + moveit_test_utils + PUBLIC $ + $) +target_link_libraries(moveit_test_utils moveit_robot_model + moveit_kinematics_base rsl::rsl) +ament_target_dependencies( + moveit_test_utils ament_index_cpp Boost geometry_msgs @@ -32,10 +29,10 @@ ament_target_dependencies(moveit_test_utils urdfdom urdfdom_headers rclcpp - fmt -) -set_target_properties(moveit_test_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + fmt) +set_target_properties(moveit_test_utils PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}") if(BUILD_TESTING) - add_subdirectory(test) + add_subdirectory(test) endif() diff --git a/moveit_core/utils/src/robot_model_test_utils.cpp b/moveit_core/utils/src/robot_model_test_utils.cpp index f53ba4912a..5c417f0c71 100644 --- a/moveit_core/utils/src/robot_model_test_utils.cpp +++ b/moveit_core/utils/src/robot_model_test_utils.cpp @@ -56,7 +56,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("robot_model_test_utils"); + return moveit::getLogger("moveit.core.robot_model_test_utils"); } } // namespace diff --git a/moveit_core/utils/test/CMakeLists.txt b/moveit_core/utils/test/CMakeLists.txt index 4006b8b755..30737b7e72 100644 --- a/moveit_core/utils/test/CMakeLists.txt +++ b/moveit_core/utils/test/CMakeLists.txt @@ -3,21 +3,13 @@ add_executable(logger_dut logger_dut.cpp) target_link_libraries(logger_dut rclcpp::rclcpp moveit_utils) # Install is needed to for launchtest to execute -install( - TARGETS - logger_dut - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS logger_dut DESTINATION lib/${PROJECT_NAME}) find_package(launch_testing_ament_cmake) -# These tests do not work on Humble as /rosout logging from child loggers -# does not work. -# ROS_DISTRO is not defined on rolling -if((NOT DEFINED ENV{ROS_DISTRO}) OR - (NOT $ENV{ROS_DISTRO} STREQUAL "humble")) - add_launch_test(rosout_publish_test.py - TARGET test-node_logging - ARGS "dut:=logger_dut" - ) +# These tests do not work on Humble as /rosout logging from child loggers does +# not work. ROS_DISTRO is not defined on rolling +if((NOT DEFINED ENV{ROS_DISTRO}) OR (NOT $ENV{ROS_DISTRO} STREQUAL "humble")) + add_launch_test(rosout_publish_test.py TARGET test-node_logging ARGS + "dut:=logger_dut") endif() diff --git a/moveit_core/utils/test/logger_dut.cpp b/moveit_core/utils/test/logger_dut.cpp index 59e2c29540..2cd19d8dd4 100644 --- a/moveit_core/utils/test/logger_dut.cpp +++ b/moveit_core/utils/test/logger_dut.cpp @@ -47,8 +47,9 @@ int main(int argc, char** argv) moveit::setNodeLoggerName(node->get_name()); // A node logger, should be in the file output and rosout - auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), - [&] { RCLCPP_INFO(moveit::getLogger("child"), "hello from node!"); }); + auto wall_timer = node->create_wall_timer(std::chrono::milliseconds(100), [&] { + RCLCPP_INFO(moveit::getLogger("moveit.core.child"), "hello from node!"); + }); rclcpp::spin(node); } diff --git a/moveit_core/version/CMakeLists.txt b/moveit_core/version/CMakeLists.txt index 749ebb50c6..c67eec7170 100644 --- a/moveit_core/version/CMakeLists.txt +++ b/moveit_core/version/CMakeLists.txt @@ -1,31 +1,38 @@ # Generate and install version.h -if(NOT "$ENV{USER}" STREQUAL "buildfarm") # Don't define version postfix on buildfarm - set(MOVEIT_VERSION_EXTRA "devel" CACHE STRING "version string postfix") +if(NOT "$ENV{USER}" STREQUAL "buildfarm") # Don't define version postfix on + # buildfarm + set(MOVEIT_VERSION_EXTRA + "devel" + CACHE STRING "version string postfix") endif() -message(STATUS " *** Building MoveIt ${moveit_core_VERSION} ${MOVEIT_VERSION_EXTRA} ***") +message( + STATUS + " *** Building MoveIt ${moveit_core_VERSION} ${MOVEIT_VERSION_EXTRA} ***") # https://stackoverflow.com/questions/13920072/how-to-always-run-command-when-building-regardless-of-any-dependency add_custom_command( - OUTPUT ${CMAKE_BINARY_DIR}/include/moveit/version.h always_rebuild - COMMAND ${CMAKE_COMMAND} - -DVERSION_FILE_PATH="${CMAKE_BINARY_DIR}/include" - -DMOVEIT_VERSION="${moveit_core_VERSION}" - -DMOVEIT_VERSION_EXTRA="${MOVEIT_VERSION_EXTRA}" - -P ${CMAKE_CURRENT_SOURCE_DIR}/version.cmake - WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR} -) + OUTPUT ${CMAKE_BINARY_DIR}/include/moveit/version.h always_rebuild + COMMENT "Generate version header" + COMMAND + ${CMAKE_COMMAND} -DVERSION_FILE_PATH="${CMAKE_BINARY_DIR}/include" + -DMOVEIT_VERSION="${moveit_core_VERSION}" + -DMOVEIT_VERSION_EXTRA="${MOVEIT_VERSION_EXTRA}" -P + ${CMAKE_CURRENT_SOURCE_DIR}/version.cmake + WORKING_DIRECTORY ${CMAKE_CURRENT_SOURCE_DIR}) -add_custom_target(version_h - DEPENDS always_rebuild - COMMENT "Generating version.h" -) +add_custom_target( + version_h + DEPENDS always_rebuild + COMMENT "Generating version.h") add_executable(moveit_version version.cpp) add_dependencies(moveit_version version_h) -target_include_directories(moveit_version PRIVATE ${CMAKE_BINARY_DIR}/include/moveit_core) +target_include_directories(moveit_version + PRIVATE ${CMAKE_BINARY_DIR}/include/moveit_core) -install(FILES "${CMAKE_BINARY_DIR}/include/moveit_core/moveit/version.h" DESTINATION include/moveit_core/moveit) +install(FILES "${CMAKE_BINARY_DIR}/include/moveit_core/moveit/version.h" + DESTINATION include/moveit_core/moveit) install(TARGETS moveit_version RUNTIME DESTINATION bin) diff --git a/moveit_core/version/version.cmake b/moveit_core/version/version.cmake index 789f6babb9..60c4723703 100644 --- a/moveit_core/version/version.cmake +++ b/moveit_core/version/version.cmake @@ -1,39 +1,38 @@ # Retrieve (active) branch name execute_process( - COMMAND git rev-parse --abbrev-ref HEAD - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - OUTPUT_VARIABLE MOVEIT_GIT_NAME - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_QUIET -) + COMMAND git rev-parse --abbrev-ref HEAD + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + OUTPUT_VARIABLE MOVEIT_GIT_NAME + OUTPUT_STRIP_TRAILING_WHITESPACE ERROR_QUIET) if("${MOVEIT_GIT_NAME}" STREQUAL "HEAD") - # Retrieve any associated name (tag or branch) - execute_process( - COMMAND git describe --contains --all HEAD - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - OUTPUT_VARIABLE MOVEIT_GIT_NAME - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_QUIET - ) + # Retrieve any associated name (tag or branch) + execute_process( + COMMAND git describe --contains --all HEAD + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + OUTPUT_VARIABLE MOVEIT_GIT_NAME OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_QUIET) endif() # Retrieve (short) commit hash execute_process( - COMMAND git rev-parse --short HEAD - WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} - OUTPUT_VARIABLE MOVEIT_GIT_COMMIT_HASH - OUTPUT_STRIP_TRAILING_WHITESPACE - ERROR_QUIET -) + COMMAND git rev-parse --short HEAD + WORKING_DIRECTORY ${CMAKE_SOURCE_DIR} + OUTPUT_VARIABLE MOVEIT_GIT_COMMIT_HASH OUTPUT_STRIP_TRAILING_WHITESPACE + ERROR_QUIET) -string(REGEX REPLACE "^([0-9]+)\\..*" "\\1" MOVEIT_VERSION_MAJOR "${moveit_core_VERSION}") -string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_MINOR "${moveit_core_VERSION}") -string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_PATCH "${moveit_core_VERSION}") -set(MOVEIT_VERSION "${MOVEIT_VERSION_MAJOR}.${MOVEIT_VERSION_MINOR}.${MOVEIT_VERSION_PATCH}") +string(REGEX REPLACE "^([0-9]+)\\..*" "\\1" MOVEIT_VERSION_MAJOR + "${moveit_core_VERSION}") +string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_MINOR + "${moveit_core_VERSION}") +string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+).*" "\\1" MOVEIT_VERSION_PATCH + "${moveit_core_VERSION}") +set(MOVEIT_VERSION + "${MOVEIT_VERSION_MAJOR}.${MOVEIT_VERSION_MINOR}.${MOVEIT_VERSION_PATCH}") if(NOT "${MOVEIT_VERSION_EXTRA}" STREQUAL "") - string(APPEND MOVEIT_VERSION "-${MOVEIT_VERSION_EXTRA}") + string(APPEND MOVEIT_VERSION "-${MOVEIT_VERSION_EXTRA}") endif() -configure_file("version.h.in" "${VERSION_FILE_PATH}/moveit_core/moveit/version.h") +configure_file("version.h.in" + "${VERSION_FILE_PATH}/moveit_core/moveit/version.h") diff --git a/moveit_core/version/version.cpp b/moveit_core/version/version.cpp index 533b482cda..498c61cd4c 100644 --- a/moveit_core/version/version.cpp +++ b/moveit_core/version/version.cpp @@ -47,6 +47,6 @@ int main(int /*argc*/, char** /*argv*/) if (strlen(MOVEIT_GIT_NAME)) std::cout << " (" << MOVEIT_GIT_NAME << ")"; } - std::cout << std::endl; + std::cout << '\n'; return 0; } diff --git a/moveit_kinematics/CHANGELOG.rst b/moveit_kinematics/CHANGELOG.rst index 91fe92c643..78f9552a4a 100644 --- a/moveit_kinematics/CHANGELOG.rst +++ b/moveit_kinematics/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package moveit_kinematics ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* At least on humble, error is: 'robot_description_kinematics.arm.min_joint_config_distance' has invalid type: expected [double] got [integer]. (`#2865 `_) +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CI: Fix building of ikfast plugins (`#2791 `_) + Ignore missing authentication for Indigo packages using --force-yes. +* Unify log names (`#2720 `_) +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Henning Kayser, Robert Haschke, Sebastian Jahr, Tyler Weaver, s-trinh + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_kinematics/CMakeLists.txt b/moveit_kinematics/CMakeLists.txt index 06615d6140..21b03dfb14 100644 --- a/moveit_kinematics/CMakeLists.txt +++ b/moveit_kinematics/CMakeLists.txt @@ -24,39 +24,38 @@ find_package(tf2_kdl REQUIRED) include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DIRS - kdl_kinematics_plugin/include - srv_kinematics_plugin/include - cached_ik_kinematics_plugin/include -) + kdl_kinematics_plugin/include srv_kinematics_plugin/include + cached_ik_kinematics_plugin/include) set(THIS_PACKAGE_LIBRARIES - cached_ik_kinematics_parameters - moveit_cached_ik_kinematics_base - moveit_cached_ik_kinematics_plugin - kdl_kinematics_parameters - moveit_kdl_kinematics_plugin - srv_kinematics_parameters - moveit_srv_kinematics_plugin -) + cached_ik_kinematics_parameters + moveit_cached_ik_kinematics_base + moveit_cached_ik_kinematics_plugin + kdl_kinematics_parameters + moveit_kdl_kinematics_plugin + srv_kinematics_parameters + moveit_srv_kinematics_plugin) set(THIS_PACKAGE_INCLUDE_DEPENDS - Eigen3 - class_loader - generate_parameter_library - kdl_parser - moveit_core - moveit_msgs - moveit_ros_planning - orocos_kdl_vendor - pluginlib - random_numbers - rclcpp - tf2_kdl -) - -pluginlib_export_plugin_description_file(moveit_core kdl_kinematics_plugin_description.xml) -pluginlib_export_plugin_description_file(moveit_core srv_kinematics_plugin_description.xml) -pluginlib_export_plugin_description_file(moveit_core cached_ik_kinematics_plugin_description.xml) + Eigen3 + class_loader + generate_parameter_library + kdl_parser + moveit_core + moveit_msgs + moveit_ros_planning + orocos_kdl_vendor + pluginlib + random_numbers + rclcpp + tf2_kdl) + +pluginlib_export_plugin_description_file(moveit_core + kdl_kinematics_plugin_description.xml) +pluginlib_export_plugin_description_file(moveit_core + srv_kinematics_plugin_description.xml) +pluginlib_export_plugin_description_file( + moveit_core cached_ik_kinematics_plugin_description.xml) include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) @@ -72,25 +71,10 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_kinematics -) + INCLUDES + DESTINATION include/moveit_kinematics) ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_pep257_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_xmllint_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() -endif() - ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt index f8240f409c..f281644753 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/cached_ik_kinematics_plugin/CMakeLists.txt @@ -3,61 +3,65 @@ find_package(trac_ik_kinematics_plugin QUIET) find_package(ur_kinematics QUIET) add_library(moveit_cached_ik_kinematics_base SHARED src/ik_cache.cpp) -set_target_properties(moveit_cached_ik_kinematics_base PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_cached_ik_kinematics_base PUBLIC - rclcpp - moveit_core - moveit_msgs -) +set_target_properties(moveit_cached_ik_kinematics_base + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_cached_ik_kinematics_base PUBLIC rclcpp + moveit_core moveit_msgs) if(trac_ik_kinematics_plugin_FOUND) - include_directories(${trac_ik_kinematics_plugin_INCLUDE_DIRS}) + include_directories(${trac_ik_kinematics_plugin_INCLUDE_DIRS}) endif() generate_parameter_library( cached_ik_kinematics_parameters # cmake target name for the parameter library - include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_parameters.yaml # path to input yaml file + include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_parameters.yaml # path to + # input yaml + # file ) -target_link_libraries(moveit_cached_ik_kinematics_base PUBLIC - cached_ik_kinematics_parameters - moveit_kdl_kinematics_plugin) +target_link_libraries( + moveit_cached_ik_kinematics_base PUBLIC cached_ik_kinematics_parameters + moveit_kdl_kinematics_plugin) -add_library(moveit_cached_ik_kinematics_plugin SHARED src/cached_ik_kinematics_plugin.cpp) -set_target_properties(moveit_cached_ik_kinematics_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_cached_ik_kinematics_plugin PUBLIC - rclcpp - moveit_core - moveit_msgs - rsl -) -target_link_libraries(moveit_cached_ik_kinematics_plugin PRIVATE - cached_ik_kinematics_parameters - moveit_kdl_kinematics_plugin - moveit_srv_kinematics_plugin - moveit_cached_ik_kinematics_base) +add_library(moveit_cached_ik_kinematics_plugin SHARED + src/cached_ik_kinematics_plugin.cpp) +set_target_properties(moveit_cached_ik_kinematics_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_cached_ik_kinematics_plugin PUBLIC rclcpp + moveit_core moveit_msgs rsl) +target_link_libraries( + moveit_cached_ik_kinematics_plugin + PRIVATE cached_ik_kinematics_parameters moveit_kdl_kinematics_plugin + moveit_srv_kinematics_plugin moveit_cached_ik_kinematics_base) if(trac_ik_kinematics_plugin_FOUND) - target_link_libraries(moveit_cached_ik_kinematics_plugin PRIVATE ${trac_ik_kinematics_plugin_LIBRARIES}) - set_target_properties(moveit_cached_ik_kinematics_plugin PROPERTIES COMPILE_DEFINITIONS "CACHED_IK_KINEMATICS_TRAC_IK") + target_link_libraries(moveit_cached_ik_kinematics_plugin + PRIVATE ${trac_ik_kinematics_plugin_LIBRARIES}) + set_target_properties( + moveit_cached_ik_kinematics_plugin + PROPERTIES COMPILE_DEFINITIONS "CACHED_IK_KINEMATICS_TRAC_IK") endif() # This is just for testing purposes; the arms from Universal Robots have # analytic solvers, so caching just adds extra overhead. if(ur_kinematics_FOUND) - include_directories(${ur_kinematics_INCLUDE_DIRS}) - foreach(UR 3 5 10) - add_library(moveit_cached_ur${UR}_kinematics_plugin SHARED src/cached_ur_kinematics_plugin.cpp) - set_target_properties(moveit_cached_ur${UR}_kinematics_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - find_library(ur${UR}_pluginlib ur${UR}_moveit_plugin PATHS ${ur_kinematics_LIBRARY_DIRS}) - target_link_libraries(moveit_cached_ur${UR}_kinematics_plugin - moveit_cached_ik_kinematics_base - ${ur${UR}_pluginlib}) - install(TARGETS moveit_cached_ur${UR}_kinematics_plugin - EXPORT moveit_cached_ur${UR}_kinematics_plugin - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin) - endforeach() + include_directories(${ur_kinematics_INCLUDE_DIRS}) + foreach(ur_version 3 5 10) + add_library(moveit_cached_ur${ur_version}_kinematics_plugin SHARED + src/cached_ur_kinematics_plugin.cpp) + set_target_properties(moveit_cached_ur${ur_version}_kinematics_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + find_library(ur${ur_version}_pluginlib ur${ur_version}_moveit_plugin + PATHS ${ur_kinematics_LIBRARY_DIRS}) + target_link_libraries( + moveit_cached_ur${ur_version}_kinematics_plugin + moveit_cached_ik_kinematics_base ${ur${ur_version}_pluginlib}) + install( + TARGETS moveit_cached_ur${ur_version}_kinematics_plugin + EXPORT moveit_cached_ur${ur_version}_kinematics_plugin + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin) + endforeach() endif() install(DIRECTORY include/ DESTINATION include/moveit_kinematics) diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/README.md b/moveit_kinematics/cached_ik_kinematics_plugin/README.md index d935fc6869..3d7f7c3b05 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/README.md +++ b/moveit_kinematics/cached_ik_kinematics_plugin/README.md @@ -17,8 +17,8 @@ to this: kinematics_solver: cached_ik_kinematics_plugin/CachedKDLKinematicsPlugin # optional parameters for caching: max_cache_size: 10000 - min_pose_distance: 1 - min_joint_config_distance: 4 + min_pose_distance: 1.0 + min_joint_config_distance: 4.0 The cache size can be controlled with an absolute cap (`max_cache_size`) or with a distance threshold on the end effector pose (`min_pose_distance`) or robot joint state (`min_joint_config_distance`). Normally, the cache files are saved to the current working directory (which is usually `${HOME}/.ros`, not the directory where you ran `roslaunch`), in a subdirectory for each robot. Possible values for `kinematics_solver` are: diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h index b7a70b6086..0ba16b4142 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h +++ b/moveit_kinematics/cached_ik_kinematics_plugin/include/moveit/cached_ik_kinematics_plugin/cached_ik_kinematics_plugin.h @@ -280,7 +280,7 @@ class CachedIKKinematicsPlugin : public KinematicsPlugin { if (tip_frames.size() != 1) { - RCLCPP_ERROR(moveit::getLogger("cached_ik_kinematics_plugin"), + RCLCPP_ERROR(moveit::getLogger("moveit.core.cached_ik_kinematics_plugin"), "This solver does not support multiple tip frames"); return false; } diff --git a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp index 91653cd348..65d2ad4c0c 100644 --- a/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp +++ b/moveit_kinematics/cached_ik_kinematics_plugin/src/ik_cache.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("dynamics_solver"); + return moveit::getLogger("moveit.core.dynamics_solver"); } } // namespace IKCache::IKCache() diff --git a/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt index 63e1f0d5a5..44c836e763 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/CMakeLists.txt @@ -1,21 +1,13 @@ set(MOVEIT_LIB_NAME moveit_ikfast_kinematics_plugin) -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## install( - PROGRAMS - scripts/auto_create_ikfast_moveit_plugin.sh - scripts/create_ikfast_moveit_plugin.py - scripts/round_collada_numbers.py - DESTINATION - lib/${PROJECT_NAME} -) + PROGRAMS scripts/auto_create_ikfast_moveit_plugin.sh + scripts/create_ikfast_moveit_plugin.py + scripts/round_collada_numbers.py DESTINATION lib/${PROJECT_NAME}) -install( - DIRECTORY - templates - DESTINATION - share/${PROJECT_NAME}/ikfast_kinematics_plugin -) +install(DIRECTORY templates + DESTINATION share/${PROJECT_NAME}/ikfast_kinematics_plugin) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/README.md b/moveit_kinematics/ikfast_kinematics_plugin/README.md index 6d82d0a417..ad46636df0 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/README.md +++ b/moveit_kinematics/ikfast_kinematics_plugin/README.md @@ -7,4 +7,4 @@ Generates a IKFast kinematics plugin for MoveIt using [OpenRave](http://openrave Tested on ROS Hydro and greater with Catkin using OpenRave 0.8 with a 6dof and 7dof robot arm manipulator. Does not work with greater than 7dof. -[Documentation on docs.ros.org](https://ros-planning.github.io/moveit_tutorials/doc/ikfast/ikfast_tutorial.html#creating-the-ikfast-moveit-plugin) +[Documentation on docs.ros.org](https://moveit.github.io/moveit_tutorials/doc/ikfast/ikfast_tutorial.html#creating-the-ikfast-moveit-plugin) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh index 59649a2031..82efd73c8b 100755 --- a/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh +++ b/moveit_kinematics/ikfast_kinematics_plugin/scripts/auto_create_ikfast_moveit_plugin.sh @@ -120,7 +120,7 @@ FROM personalrobotics/ros-openrave RUN apt-key adv --keyserver 'hkp://keyserver.ubuntu.com:80' --recv-key 4B63CF8FDE49746E98FA01DDAD19BAB3CBF125EA && \ apt-key del 421C365BD9FF1F717815A3895523BAEEB01FA116 && \ apt-get update && \ - apt-get install -y --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \ + apt-get install -y --force-yes --no-install-recommends python-pip build-essential liblapack-dev ros-indigo-collada-urdf && \ apt-get clean && rm -rf /var/lib/apt/lists/* # enforce a specific version of sympy, which is known to work with OpenRave RUN pip install git+https://github.com/sympy/sympy.git@sympy-0.7.1 diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt index 3efe2673f2..c77cb1ff06 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/CMakeLists.txt @@ -24,30 +24,32 @@ generate_parameter_library( ) set(IKFAST_LIBRARY_NAME _LIBRARY_NAME_) -add_library(${IKFAST_LIBRARY_NAME} SHARED src/_ROBOT_NAME___GROUP_NAME__ikfast_moveit_plugin.cpp) -ament_target_dependencies(${IKFAST_LIBRARY_NAME} +add_library(${IKFAST_LIBRARY_NAME} SHARED + src/_ROBOT_NAME___GROUP_NAME__ikfast_moveit_plugin.cpp) +ament_target_dependencies( + ${IKFAST_LIBRARY_NAME} rclcpp moveit_core pluginlib tf2_kdl orocos_kdl tf2_eigen - LAPACK -) + LAPACK) # suppress warnings about unused variables in OpenRave's solver code -target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable -Wno-unused-parameter) +target_compile_options(${IKFAST_LIBRARY_NAME} PRIVATE -Wno-unused-variable + -Wno-unused-parameter) -target_link_libraries(${IKFAST_LIBRARY_NAME} - ikfast_kinematics_parameters -) +target_link_libraries(${IKFAST_LIBRARY_NAME} ikfast_kinematics_parameters) -install(TARGETS ${IKFAST_LIBRARY_NAME} ikfast_kinematics_parameters +install( + TARGETS ${IKFAST_LIBRARY_NAME} ikfast_kinematics_parameters EXPORT ${PROJECT_NAME}Targets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin) -pluginlib_export_plugin_description_file(moveit_core _ROBOT_NAME___GROUP_NAME__moveit_ikfast_plugin_description.xml) +pluginlib_export_plugin_description_file( + moveit_core _ROBOT_NAME___GROUP_NAME__moveit_ikfast_plugin_description.xml) ament_export_targets(${PROJECT_NAME}Targets HAS_LIBRARY_TARGET) ament_export_dependencies(moveit_core) diff --git a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp index 14c2d776dd..fdba5f1f5b 100644 --- a/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp +++ b/moveit_kinematics/ikfast_kinematics_plugin/templates/ikfast61_moveit_plugin_template.cpp @@ -67,7 +67,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("_ROBOT_NAME___GROUP_NAME__ikfast_solver"); + return moveit::getLogger("moveit.core._ROBOT_NAME___GROUP_NAME__ikfast_solver"); } } // namespace @@ -808,7 +808,10 @@ bool IKFastKinematicsPlugin::getPositionFK(const std::vector& link_ return false; } +#pragma clang diagnostic push +#pragma clang diagnostic ignored "-Wvla-cxx-extension" IkReal angles[num_joints_]; +#pragma clang diagnostic pop for (unsigned char i = 0; i < num_joints_; ++i) angles[i] = joint_angles[i]; diff --git a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt index 0e9ca42a34..edf5b93990 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/kdl_kinematics_plugin/CMakeLists.txt @@ -4,10 +4,10 @@ generate_parameter_library( ) add_library(moveit_kdl_kinematics_plugin SHARED - src/kdl_kinematics_plugin.cpp - src/chainiksolver_vel_mimic_svd.cpp) + src/kdl_kinematics_plugin.cpp src/chainiksolver_vel_mimic_svd.cpp) -ament_target_dependencies(moveit_kdl_kinematics_plugin +ament_target_dependencies( + moveit_kdl_kinematics_plugin rclcpp random_numbers pluginlib @@ -16,17 +16,16 @@ ament_target_dependencies(moveit_kdl_kinematics_plugin orocos_kdl kdl_parser tf2_kdl - EIGEN3 -) + EIGEN3) -target_link_libraries(moveit_kdl_kinematics_plugin - kdl_kinematics_parameters -) +target_link_libraries(moveit_kdl_kinematics_plugin kdl_kinematics_parameters) # prevent pluginlib from using boost -target_compile_definitions(moveit_kdl_kinematics_plugin PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(moveit_kdl_kinematics_plugin PRIVATE "MOVEIT_KDL_KINEMATICS_PLUGIN_BUILDING_DLL") +target_compile_definitions(moveit_kdl_kinematics_plugin + PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +# Causes the visibility macros to use dllexport rather than dllimport, which is +# appropriate when building the dll but not consuming it. +target_compile_definitions(moveit_kdl_kinematics_plugin + PRIVATE "MOVEIT_KDL_KINEMATICS_PLUGIN_BUILDING_DLL") install(DIRECTORY include/ DESTINATION include/moveit_kinematics) diff --git a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp index fd34b8a8a9..54d3f0aa4f 100644 --- a/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp +++ b/moveit_kinematics/kdl_kinematics_plugin/src/kdl_kinematics_plugin.cpp @@ -52,7 +52,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("kdl_kinematics_plugin"); + return moveit::getLogger("moveit.kinematics.kdl_kinematics_plugin"); } } // namespace diff --git a/moveit_kinematics/package.xml b/moveit_kinematics/package.xml index 3455450c52..317aef4fe9 100644 --- a/moveit_kinematics/package.xml +++ b/moveit_kinematics/package.xml @@ -2,7 +2,7 @@ moveit_kinematics - 2.9.0 + 2.10.0 Package for all inverse kinematics solvers in MoveIt Henning Kayser @@ -14,8 +14,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2 - https://github.com/ros-planning/moveit2/issues + https://github.com/moveit/moveit2 + https://github.com/moveit/moveit2/issues Dave Coleman Ioan Sucan @@ -46,8 +46,6 @@ moveit_resources_panda_moveit_config ament_cmake_gtest - ament_lint_auto - ament_lint_common ros_testing moveit_configs_utils launch_param_builder diff --git a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt index fa90c53a7d..6614010e33 100644 --- a/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt +++ b/moveit_kinematics/srv_kinematics_plugin/CMakeLists.txt @@ -4,16 +4,12 @@ generate_parameter_library( ) add_library(moveit_srv_kinematics_plugin SHARED src/srv_kinematics_plugin.cpp) -set_target_properties(moveit_srv_kinematics_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(moveit_srv_kinematics_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_srv_kinematics_plugin - rclcpp - moveit_core - moveit_msgs -) +ament_target_dependencies(moveit_srv_kinematics_plugin rclcpp moveit_core + moveit_msgs) -target_link_libraries(moveit_srv_kinematics_plugin - srv_kinematics_parameters -) +target_link_libraries(moveit_srv_kinematics_plugin srv_kinematics_parameters) install(DIRECTORY include/ DESTINATION include/moveit_kinematics) diff --git a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp index e2707e64ce..9538cf080c 100644 --- a/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp +++ b/moveit_kinematics/srv_kinematics_plugin/src/srv_kinematics_plugin.cpp @@ -54,7 +54,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("srv_kinematics_plugin"); + return moveit::getLogger("moveit.kinematics.srv_kinematics_plugin"); } } // namespace diff --git a/moveit_kinematics/test/CMakeLists.txt b/moveit_kinematics/test/CMakeLists.txt index 2057c0e654..2da6c7aa69 100644 --- a/moveit_kinematics/test/CMakeLists.txt +++ b/moveit_kinematics/test/CMakeLists.txt @@ -8,43 +8,44 @@ if(BUILD_TESTING) find_package(moveit_ros_planning REQUIRED) ament_add_gtest_executable(test_kinematics_plugin test_kinematics_plugin.cpp) - ament_target_dependencies(test_kinematics_plugin moveit_ros_planning pluginlib) + ament_target_dependencies(test_kinematics_plugin moveit_ros_planning + pluginlib) target_link_libraries(test_kinematics_plugin moveit_kdl_kinematics_plugin) if(NOT CMAKE_CXX_COMPILER_ID STREQUAL "MSVC") - target_compile_options(test_kinematics_plugin PRIVATE -Wno-deprecated-declarations) + target_compile_options(test_kinematics_plugin + PRIVATE -Wno-deprecated-declarations) endif() # KDL testing set(ARGS ARGS ik_plugin:=kdl_kinematics_plugin/KDLKinematicsPlugin) - add_ros_test(launch/fanuc-kdl-singular.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - add_ros_test(launch/fanuc-kdl.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - add_ros_test(launch/panda-kdl-singular.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - add_ros_test(launch/panda-kdl.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - # Run ikfast tests only if the corresponding packages were built - # TODO (vatanaksoytezer): Enable ikfast tests - # find_package(fanuc_ikfast_plugin QUIET) - # if(fanuc_ikfast_plugin_FOUND) - # add_ros_test(launch/fanuc-ikfast.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # add_ros_test(launch/fanuc-ikfast-singular.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # endif() - - # find_package(panda_ikfast_plugin QUIET) - # if(panda_ikfast_plugin_FOUND) - # add_ros_test(launch/panda-ikfast.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # add_ros_test(launch/panda-ikfast-singular.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # endif() + add_ros_test(launch/fanuc-kdl-singular.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + add_ros_test(launch/fanuc-kdl.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + add_ros_test(launch/panda-kdl-singular.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + add_ros_test(launch/panda-kdl.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + # Run ikfast tests only if the corresponding packages were built TODO + # (vatanaksoytezer): Enable ikfast tests find_package(fanuc_ikfast_plugin + # QUIET) if(fanuc_ikfast_plugin_FOUND) + # add_ros_test(launch/fanuc-ikfast.test.py ARGS + # "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + # add_ros_test(launch/fanuc-ikfast-singular.test.py ARGS + # "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() + + # find_package(panda_ikfast_plugin QUIET) if(panda_ikfast_plugin_FOUND) + # add_ros_test(launch/panda-ikfast.test.py ARGS + # "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + # add_ros_test(launch/panda-ikfast-singular.test.py ARGS + # "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() # Benchmarking program for cached_ik_kinematics add_executable(benchmark_ik benchmark_ik.cpp) - ament_target_dependencies( - benchmark_ik - rclcpp - moveit_core - moveit_ros_planning - Boost - ) + ament_target_dependencies(benchmark_ik rclcpp moveit_core moveit_ros_planning + Boost) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(DIRECTORY launch DESTINATION share/${PROJECT_NAME}) diff --git a/moveit_kinematics/test/test_ikfast_plugins.sh b/moveit_kinematics/test/test_ikfast_plugins.sh index 96e63e2396..c435c46921 100755 --- a/moveit_kinematics/test/test_ikfast_plugins.sh +++ b/moveit_kinematics/test/test_ikfast_plugins.sh @@ -11,7 +11,7 @@ sudo apt-get -qq update sudo apt-get -qq install python3-lxml python3-yaml # Clone moveit_resources for URDFs. They are not available before running docker. -git clone -q --depth=1 -b ros2 https://github.com/ros-planning/moveit_resources /tmp/resources +git clone -q --depth=1 -b ros2 https://github.com/moveit/moveit_resources /tmp/resources fanuc=/tmp/resources/fanuc_description/urdf/fanuc.urdf panda=/tmp/resources/panda_description/urdf/panda.urdf diff --git a/moveit_kinematics/test/test_kinematics_plugin.cpp b/moveit_kinematics/test/test_kinematics_plugin.cpp index cd1d1785bf..87f870550c 100644 --- a/moveit_kinematics/test/test_kinematics_plugin.cpp +++ b/moveit_kinematics/test/test_kinematics_plugin.cpp @@ -58,7 +58,7 @@ rclcpp::Logger getLogger() { - return moveit::getLogger("test_kinematics_plugin"); + return moveit::getLogger("moveit.kinematics.test_kinematics_plugin"); } const std::string ROBOT_DESCRIPTION_PARAM = "robot_description"; const double DEFAULT_SEARCH_DISCRETIZATION = 0.01f; diff --git a/moveit_planners/chomp/README.md b/moveit_planners/chomp/README.md index a4d37b32e9..0be4fddc1f 100644 --- a/moveit_planners/chomp/README.md +++ b/moveit_planners/chomp/README.md @@ -1,3 +1,3 @@ ## CHOMP Motion Planner -See [Chomp Interface Tutorial](https://ros-planning.github.io/moveit_tutorials/doc/chomp_planner/chomp_planner_tutorial.html) +See [Chomp Interface Tutorial](https://moveit.github.io/moveit_tutorials/doc/chomp_planner/chomp_planner_tutorial.html) diff --git a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst index 4b9ac61f7a..9b0a690d08 100644 --- a/moveit_planners/chomp/chomp_interface/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_interface/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package chomp_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_planners/chomp/chomp_interface/CMakeLists.txt b/moveit_planners/chomp/chomp_interface/CMakeLists.txt index 448aade247..3a2c114617 100644 --- a/moveit_planners/chomp/chomp_interface/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_interface/CMakeLists.txt @@ -13,65 +13,37 @@ find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(rsl REQUIRED) -set(THIS_PACKAGE_INCLUDE_DEPENDS - moveit_core - chomp_motion_planner - moveit_core - pluginlib - rclcpp - rsl -) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_core chomp_motion_planner moveit_core + pluginlib rclcpp rsl) -include_directories( - include -) +include_directories(include) -add_library(moveit_chomp_interface SHARED - src/chomp_interface.cpp - src/chomp_planning_context.cpp) -set_target_properties(moveit_chomp_interface PROPERTIES VERSION "${moveit_planners_chomp_VERSION}") -ament_target_dependencies(moveit_chomp_interface ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_library(moveit_chomp_interface SHARED src/chomp_interface.cpp + src/chomp_planning_context.cpp) +set_target_properties(moveit_chomp_interface + PROPERTIES VERSION "${moveit_planners_chomp_VERSION}") +ament_target_dependencies(moveit_chomp_interface + ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(moveit_chomp_planner_plugin SHARED src/chomp_plugin.cpp) -ament_target_dependencies(moveit_chomp_planner_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(moveit_chomp_planner_plugin - moveit_chomp_interface -) +ament_target_dependencies(moveit_chomp_planner_plugin + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(moveit_chomp_planner_plugin moveit_chomp_interface) install( TARGETS moveit_chomp_interface moveit_chomp_planner_plugin EXPORT moveit_planners_chompTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) ament_export_targets(moveit_planners_chompTargets HAS_LIBRARY_TARGET) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -pluginlib_export_plugin_description_file(moveit_core chomp_interface_plugin_description.xml) +pluginlib_export_plugin_description_file(moveit_core + chomp_interface_plugin_description.xml) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() -endif() - -# TODO: Enable testing () -#if(CATKIN_ENABLE_TESTING) - #set(CHOMP_TEST_DEPS moveit_ros_planning_interface) -#else() - #set(CHOMP_TEST_DEPS) -#endif() +# TODO: Enable testing () if(CATKIN_ENABLE_TESTING) set(CHOMP_TEST_DEPS +# moveit_ros_planning_interface) else() set(CHOMP_TEST_DEPS) endif() ament_package() diff --git a/moveit_planners/chomp/chomp_interface/package.xml b/moveit_planners/chomp/chomp_interface/package.xml index 2e44c3c871..8f55fadf2d 100644 --- a/moveit_planners/chomp/chomp_interface/package.xml +++ b/moveit_planners/chomp/chomp_interface/package.xml @@ -2,7 +2,7 @@ moveit_planners_chomp - 2.9.0 + 2.10.0 The interface for using CHOMP within MoveIt Chittaranjan Srinivas Swaminathan @@ -21,9 +21,6 @@ pluginlib rclcpp - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp index 4592138113..f7cd00ad37 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_interface.cpp @@ -43,7 +43,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("chomp_optimizer"); + return moveit::getLogger("moveit.planners.chomp"); } } // namespace diff --git a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp index 0bc97f92b2..997bf4a964 100644 --- a/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp +++ b/moveit_planners/chomp/chomp_interface/src/chomp_plugin.cpp @@ -48,7 +48,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("chomp_optimizer"); + return moveit::getLogger("moveit.planners.chomp.planner_manager"); } } // namespace diff --git a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst index 83f7ba3263..6591863976 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst +++ b/moveit_planners/chomp/chomp_motion_planner/CHANGELOG.rst @@ -2,6 +2,13 @@ Changelog for package chomp_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt index f4827f5f98..b6407f0e3f 100644 --- a/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt +++ b/moveit_planners/chomp/chomp_motion_planner/CMakeLists.txt @@ -12,50 +12,27 @@ find_package(rclcpp REQUIRED) find_package(rsl REQUIRED) find_package(trajectory_msgs REQUIRED) -set(THIS_PACKAGE_INCLUDE_DEPENDS - moveit_core - rclcpp - rsl - trajectory_msgs - visualization_msgs -) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_core rclcpp rsl trajectory_msgs + visualization_msgs) -include_directories( - include -) +include_directories(include) -add_library(chomp_motion_planner SHARED - src/chomp_cost.cpp - src/chomp_parameters.cpp - src/chomp_trajectory.cpp - src/chomp_optimizer.cpp - src/chomp_planner.cpp -) -set_target_properties(chomp_motion_planner PROPERTIES VERSION "${chomp_motion_planner_VERSION}") +add_library( + chomp_motion_planner SHARED + src/chomp_cost.cpp src/chomp_parameters.cpp src/chomp_trajectory.cpp + src/chomp_optimizer.cpp src/chomp_planner.cpp) +set_target_properties(chomp_motion_planner + PROPERTIES VERSION "${chomp_motion_planner_VERSION}") ament_target_dependencies(chomp_motion_planner ${THIS_PACKAGE_INCLUDE_DEPENDS}) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() -endif() - install( TARGETS chomp_motion_planner EXPORT chomp_motion_plannerTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/chomp_motion_planner -) + INCLUDES + DESTINATION include/chomp_motion_planner) install(DIRECTORY include/ DESTINATION include/chomp_motion_planner) ament_export_targets(chomp_motion_plannerTargets HAS_LIBRARY_TARGET) diff --git a/moveit_planners/chomp/chomp_motion_planner/package.xml b/moveit_planners/chomp/chomp_motion_planner/package.xml index 6415b10f83..0db9ec3005 100644 --- a/moveit_planners/chomp/chomp_motion_planner/package.xml +++ b/moveit_planners/chomp/chomp_motion_planner/package.xml @@ -2,7 +2,7 @@ chomp_motion_planner - 2.9.0 + 2.10.0 chomp_motion_planner Chittaranjan Srinivas Swaminathan @@ -22,9 +22,6 @@ rsl trajectory_msgs - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp index 4e8a4d9e5a..d1fc2c2091 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_optimizer.cpp @@ -54,7 +54,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("chomp_optimizer"); + return moveit::getLogger("moveit.planners.chomp.optimizer"); } } // namespace diff --git a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp index 34c603054e..9b78d26394 100644 --- a/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp +++ b/moveit_planners/chomp/chomp_motion_planner/src/chomp_planner.cpp @@ -51,7 +51,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("chomp_planner"); + return moveit::getLogger("moveit.planners.chomp.planner"); } } // namespace diff --git a/moveit_planners/moveit_planners/CHANGELOG.rst b/moveit_planners/moveit_planners/CHANGELOG.rst index 27901f8ac6..9154400d7c 100644 --- a/moveit_planners/moveit_planners/CHANGELOG.rst +++ b/moveit_planners/moveit_planners/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_planners ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit_planners/moveit_planners/CMakeLists.txt b/moveit_planners/moveit_planners/CMakeLists.txt index 9b8f81ada0..324f77b89d 100644 --- a/moveit_planners/moveit_planners/CMakeLists.txt +++ b/moveit_planners/moveit_planners/CMakeLists.txt @@ -2,9 +2,4 @@ cmake_minimum_required(VERSION 3.22) project(moveit_planners) find_package(ament_cmake REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/moveit_planners/moveit_planners/package.xml b/moveit_planners/moveit_planners/package.xml index 3470f91bd9..a1355c8bfe 100644 --- a/moveit_planners/moveit_planners/package.xml +++ b/moveit_planners/moveit_planners/package.xml @@ -2,7 +2,7 @@ moveit_planners - 2.9.0 + 2.10.0 Meta package that installs all available planners for MoveIt Henning Kayser Tyler Weaver @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan Sachin Chitta @@ -27,9 +27,6 @@ moveit_planners_stomp pilz_industrial_motion_planner - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_planners/ompl/CHANGELOG.rst b/moveit_planners/ompl/CHANGELOG.rst index 8db3baef40..7bdd3f4e2b 100644 --- a/moveit_planners/ompl/CHANGELOG.rst +++ b/moveit_planners/ompl/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package moveit_planners_ompl ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* Do not overwrite the error code with OMPL interface (`#2725 `_) + In case of failure, set the error code to the one returned by the + planning pipeline's `solve` method rather than overwriting it with + `PLANNING_FAILED`. +* Set `planner_id` in reponses with OMPL interface (`#2724 `_) + This avoids a warning `PlanningPipeline::generatePlan()`. + Co-authored-by: Gaël Écorchard +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Gaël Écorchard, Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Handle unsupported position constraints in OMPL (`#2417 `_) diff --git a/moveit_planners/ompl/CMakeLists.txt b/moveit_planners/ompl/CMakeLists.txt index b6e1bb4d10..91329a1e8d 100644 --- a/moveit_planners/ompl/CMakeLists.txt +++ b/moveit_planners/ompl/CMakeLists.txt @@ -5,7 +5,14 @@ project(moveit_planners_ompl LANGUAGES CXX) find_package(moveit_common REQUIRED) moveit_package() -find_package(Boost REQUIRED system filesystem date_time thread serialization) +find_package( + Boost + REQUIRED + system + filesystem + date_time + thread + serialization) find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) find_package(moveit_ros_planning REQUIRED) @@ -16,38 +23,26 @@ find_package(tf2_ros REQUIRED) find_package(ompl REQUIRED) if(CMAKE_CXX_COMPILER_ID MATCHES "(GNU|Clang)") - add_compile_options(-Wno-overloaded-virtual) + add_compile_options(-Wno-overloaded-virtual) endif() include_directories(ompl_interface/include) -include_directories(SYSTEM - ${Boost_INCLUDE_DIRS} - ${OMPL_INCLUDE_DIRS}) +include_directories(SYSTEM ${Boost_INCLUDE_DIRS} ${OMPL_INCLUDE_DIRS}) add_subdirectory(ompl_interface) -install(TARGETS moveit_ompl_interface moveit_ompl_planner_plugin +install( + TARGETS moveit_ompl_interface moveit_ompl_planner_plugin EXPORT moveit_planners_omplTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_planners_ompl -) + INCLUDES + DESTINATION include/moveit_planners_ompl) ament_export_targets(moveit_planners_omplTargets HAS_LIBRARY_TARGET) ament_export_dependencies(moveit_core ompl) -pluginlib_export_plugin_description_file(moveit_core ompl_interface_plugin_description.xml) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_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() -endif() +pluginlib_export_plugin_description_file(moveit_core + ompl_interface_plugin_description.xml) ament_package() diff --git a/moveit_planners/ompl/ompl_interface/CMakeLists.txt b/moveit_planners/ompl/ompl_interface/CMakeLists.txt index 933e19b6e1..90fe140801 100644 --- a/moveit_planners/ompl/ompl_interface/CMakeLists.txt +++ b/moveit_planners/ompl/ompl_interface/CMakeLists.txt @@ -1,4 +1,5 @@ -add_library(moveit_ompl_interface SHARED +add_library( + moveit_ompl_interface SHARED src/ompl_interface.cpp src/planning_context_manager.cpp src/model_based_planning_context.cpp @@ -17,9 +18,9 @@ add_library(moveit_ompl_interface SHARED src/detail/goal_union.cpp src/detail/constraints_library.cpp src/detail/constrained_sampler.cpp - src/detail/constrained_goal_sampler.cpp -) -set_target_properties(moveit_ompl_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + src/detail/constrained_goal_sampler.cpp) +set_target_properties(moveit_ompl_interface + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") find_package(OpenMP REQUIRED) @@ -28,7 +29,8 @@ if(APPLE) target_link_directories(moveit_ompl_interface PUBLIC ${OMPL_LIBRARY_DIRS}) endif() -ament_target_dependencies(moveit_ompl_interface +ament_target_dependencies( + moveit_ompl_interface moveit_core moveit_msgs moveit_ros_planning @@ -37,32 +39,37 @@ ament_target_dependencies(moveit_ompl_interface tf2_eigen tf2_ros OMPL - Boost -) -set_target_properties(moveit_ompl_interface PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -set_target_properties(moveit_ompl_interface PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + Boost) +set_target_properties( + moveit_ompl_interface PROPERTIES COMPILE_FLAGS + "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set_target_properties(moveit_ompl_interface PROPERTIES LINK_FLAGS + "${OpenMP_CXX_FLAGS}") -add_executable(moveit_generate_state_database scripts/generate_state_database.cpp) +add_executable(moveit_generate_state_database + scripts/generate_state_database.cpp) target_link_libraries(moveit_generate_state_database moveit_ompl_interface) -set_target_properties(moveit_generate_state_database PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") -set_target_properties(moveit_generate_state_database PROPERTIES OUTPUT_NAME "generate_state_database") +set_target_properties(moveit_generate_state_database + PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") +set_target_properties(moveit_generate_state_database + PROPERTIES OUTPUT_NAME "generate_state_database") add_library(moveit_ompl_planner_plugin SHARED src/ompl_planner_manager.cpp) -set_target_properties(moveit_ompl_planner_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_ompl_planner_plugin +set_target_properties(moveit_ompl_planner_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_ompl_planner_plugin moveit_core moveit_ros_planning rclcpp pluginlib tf2_ros OMPL - Boost -) + Boost) target_link_libraries(moveit_ompl_planner_plugin moveit_ompl_interface) install(TARGETS moveit_generate_state_database - RUNTIME DESTINATION lib/${PROJECT_NAME} -) + RUNTIME DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/moveit_planners) if(BUILD_TESTING) @@ -72,36 +79,52 @@ if(BUILD_TESTING) ament_add_gtest(test_state_space test/test_state_space.cpp) ament_target_dependencies(test_state_space moveit_core OMPL Boost Eigen3) target_link_libraries(test_state_space moveit_ompl_interface) - set_target_properties(test_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + set_target_properties(test_state_space PROPERTIES LINK_FLAGS + "${OpenMP_CXX_FLAGS}") - ament_add_gtest(test_state_validity_checker test/test_state_validity_checker.cpp) - ament_target_dependencies(test_state_validity_checker moveit_core OMPL Boost Eigen3) + ament_add_gtest(test_state_validity_checker + test/test_state_validity_checker.cpp) + ament_target_dependencies(test_state_validity_checker moveit_core OMPL Boost + Eigen3) target_link_libraries(test_state_validity_checker moveit_ompl_interface) - set_target_properties(test_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + set_target_properties(test_state_validity_checker + PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") - ament_add_gtest(test_planning_context_manager test/test_planning_context_manager.cpp) - ament_target_dependencies(test_planning_context_manager moveit_core tf2_eigen OMPL Boost Eigen3) + ament_add_gtest(test_planning_context_manager + test/test_planning_context_manager.cpp) + ament_target_dependencies(test_planning_context_manager moveit_core tf2_eigen + OMPL Boost Eigen3) target_link_libraries(test_planning_context_manager moveit_ompl_interface) - # Disabling flaky test - # TODO (vatanaksoytezer): Uncomment once this is fixed + # Disabling flaky test TODO (vatanaksoytezer): Uncomment once this is fixed # ament_add_gtest(test_ompl_constraints test/test_ompl_constraints.cpp) - # ament_target_dependencies(test_ompl_constraints moveit_core OMPL Boost Eigen3) - # target_link_libraries(test_ompl_constraints moveit_ompl_interface) + # ament_target_dependencies(test_ompl_constraints moveit_core OMPL Boost + # Eigen3) target_link_libraries(test_ompl_constraints moveit_ompl_interface) - ament_add_gtest(test_constrained_planning_state_space test/test_constrained_planning_state_space.cpp) - ament_target_dependencies(test_constrained_planning_state_space moveit_core OMPL Boost Eigen3) - target_link_libraries(test_constrained_planning_state_space moveit_ompl_interface) - set_target_properties(test_constrained_planning_state_space PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + ament_add_gtest(test_constrained_planning_state_space + test/test_constrained_planning_state_space.cpp) + ament_target_dependencies(test_constrained_planning_state_space moveit_core + OMPL Boost Eigen3) + target_link_libraries(test_constrained_planning_state_space + moveit_ompl_interface) + set_target_properties(test_constrained_planning_state_space + PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") - ament_add_gtest(test_constrained_state_validity_checker test/test_constrained_state_validity_checker.cpp) - ament_target_dependencies(test_constrained_state_validity_checker moveit_core OMPL Boost Eigen3) - target_link_libraries(test_constrained_state_validity_checker moveit_ompl_interface) - set_target_properties(test_constrained_state_validity_checker PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + ament_add_gtest(test_constrained_state_validity_checker + test/test_constrained_state_validity_checker.cpp) + ament_target_dependencies(test_constrained_state_validity_checker moveit_core + OMPL Boost Eigen3) + target_link_libraries(test_constrained_state_validity_checker + moveit_ompl_interface) + set_target_properties(test_constrained_state_validity_checker + PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") - ament_add_gtest(test_threadsafe_state_storage test/test_threadsafe_state_storage.cpp) - ament_target_dependencies(test_threadsafe_state_storage moveit_core OMPL Boost Eigen3) + ament_add_gtest(test_threadsafe_state_storage + test/test_threadsafe_state_storage.cpp) + ament_target_dependencies(test_threadsafe_state_storage moveit_core OMPL + Boost Eigen3) target_link_libraries(test_threadsafe_state_storage moveit_ompl_interface) - set_target_properties(test_threadsafe_state_storage PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") + set_target_properties(test_threadsafe_state_storage + PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") endif() diff --git a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h index 5dfafa7f2c..a3fead6927 100644 --- a/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h +++ b/moveit_planners/ompl/ompl_interface/include/moveit/ompl_interface/detail/state_validity_checker.h @@ -115,7 +115,7 @@ class StateValidityChecker : public ompl::base::StateValidityChecker * * We still check the path constraints, because not all states sampled by the constrained state space * satisfy the constraints unfortunately. This is a complicated issue. For more details see: - * https://github.com/ros-planning/moveit/issues/2092#issuecomment-669911722. + * https://github.com/moveit/moveit/issues/2092#issuecomment-669911722. **/ class ConstrainedPlanningStateValidityChecker : public StateValidityChecker { diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp index 4d226366dd..84814ac7c0 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constrained_goal_sampler.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_constrained_goal_sampler"); + return moveit::getLogger("moveit.planners.ompl.constrained_goal_sampler"); } } // namespace diff --git a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp index 20c1d97d7a..35a94544a7 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/constraints_library.cpp @@ -50,7 +50,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_constraints_library"); + return moveit::getLogger("moveit.planners.ompl.constraints_library"); } template diff --git a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp index 9bf8f42fa1..a6bb82bde0 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/ompl_constraints.cpp @@ -48,7 +48,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_constraints"); + return moveit::getLogger("moveit.planners.ompl.constraints"); } } // namespace diff --git a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp index b828fac874..ae626d0cb5 100644 --- a/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/src/detail/state_validity_checker.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_state_validity_checker"); + return moveit::getLogger("moveit.planners.ompl.state_validity_checker"); } } // namespace diff --git a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp index 4529975595..dca38f7da8 100644 --- a/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp +++ b/moveit_planners/ompl/ompl_interface/src/model_based_planning_context.cpp @@ -74,7 +74,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_model_based_planning_context"); + return moveit::getLogger("moveit.planners.ompl.model_based_planning_context"); } } // namespace @@ -771,6 +771,7 @@ void ModelBasedPlanningContext::postSolve() void ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& res) { + res.planner_id = request_.planner_id; res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts); if (res.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { @@ -800,12 +801,11 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanResponse& re void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResponse& res) { - moveit_msgs::msg::MoveItErrorCodes moveit_result = - solve(request_.allowed_planning_time, request_.num_planning_attempts); - if (moveit_result.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + res.planner_id = request_.planner_id; + res.error_code = solve(request_.allowed_planning_time, request_.num_planning_attempts); + if (res.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) { RCLCPP_INFO(getLogger(), "Unable to solve the planning problem"); - res.error_code.val = moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED; return; } @@ -843,7 +843,6 @@ void ModelBasedPlanningContext::solve(planning_interface::MotionPlanDetailedResp RCLCPP_DEBUG(getLogger(), "%s: Returning successful solution with %lu states", getName().c_str(), getOMPLSimpleSetup()->getSolutionPath().getStateCount()); - res.error_code.val = moveit_result.val; } const moveit_msgs::msg::MoveItErrorCodes ModelBasedPlanningContext::solve(double timeout, unsigned int count) diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp index 1291b791f0..cc34c91f85 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_interface.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_interface"); + return moveit::getLogger("moveit.planners.ompl.interface"); } } // namespace diff --git a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp index 9e69e99d72..7daef874b5 100644 --- a/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/ompl_planner_manager.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_planner_manager"); + return moveit::getLogger("moveit.planners.ompl.planner_manager"); } } // namespace diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp index 361c580239..4482e781d4 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/model_based_state_space.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_model_based_state_space"); + return moveit::getLogger("moveit.planners.ompl.model_based_state_space"); } } // namespace diff --git a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp index 58bb0468c6..1325a13638 100644 --- a/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/src/parameterization/work_space/pose_model_state_space.cpp @@ -46,7 +46,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_pose_model_state_space"); + return moveit::getLogger("moveit.planners.ompl.pose_model_state_space"); } } // namespace diff --git a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp index b6424fcd7c..bedd813c48 100644 --- a/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/src/planning_context_manager.cpp @@ -86,7 +86,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ompl_planning_context_manager"); + return moveit::getLogger("moveit.planners.ompl.planning_context_manager"); } } // namespace diff --git a/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp index 7f96dacf0b..b411d7b630 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_constrained_planning_state_space.cpp @@ -61,7 +61,7 @@ rclcpp::Logger getLogger() { - return moveit::getLogger("test_constrained_planning_state_space"); + return moveit::getLogger("moveit.planners.ompl.test_constrained_planning_state_space"); } /** \brief Dummy constraint for testing, always satisfied. We need this to create and OMPL ConstrainedStateSpace. **/ diff --git a/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp index a37943c042..bdb5676551 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_constrained_state_validity_checker.cpp @@ -58,7 +58,7 @@ constexpr bool VERBOSE = false; rclcpp::Logger getLogger() { - return moveit::getLogger("test_constrained_state_validity_checker"); + return moveit::getLogger("moveit.planners.ompl.test_constrained_state_validity_checker"); } /** \brief Pretty print std:vectors **/ diff --git a/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp b/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp index 1042c69a5c..aa06ac75d1 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_ompl_constraints.cpp @@ -65,7 +65,7 @@ rclcpp::Logger getLogger() { - return moveit::getLogger("test_ompl_constraints"); + return moveit::getLogger("moveit.planners.ompl.test_constraints"); } /** \brief Number of times to run a test that uses randomly generated input. **/ @@ -252,7 +252,7 @@ class TestOMPLConstraints : public ompl_interface_testing::LoadTestRobot, public // But these issues do not prevent us to use the ConstrainedPlanningStateSpace! :) // The jacobian test is expected to fail because of the discontinuous constraint derivative. // In addition not all samples returned from the state sampler will be valid. - // For more details: https://github.com/ros-planning/moveit/issues/2092#issuecomment-669911722 + // For more details: https://github.com/moveit/moveit/issues/2092#issuecomment-669911722 try { constrained_state_space->sanityChecks(); diff --git a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp index 87a0d7b275..16ecc3fc63 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_planning_context_manager.cpp @@ -46,7 +46,7 @@ * TODO(jeroendm) I also tried something similar with position constraints, but get a segmentation fault * that occurs in the 'geometric_shapes' package, in the method 'useDimensions' in 'bodies.h'. * git permalink: - * https://github.com/ros-planning/geometric_shapes/blob/df0478870b8592ce789ee1919f3124058c4327d7/include/geometric_shapes/bodies.h#L196 + * https://github.com/moveit/geometric_shapes/blob/df0478870b8592ce789ee1919f3124058c4327d7/include/geometric_shapes/bodies.h#L196 * **/ diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp index cd62ba78f3..72568adc20 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_space.cpp @@ -50,7 +50,7 @@ rclcpp::Logger getLogger() { - return moveit::getLogger("test_state_space"); + return moveit::getLogger("moveit.planners.ompl.test_state_space"); } constexpr double EPSILON = std::numeric_limits::epsilon(); diff --git a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp index 2a774e6e7b..dbf5a69749 100644 --- a/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp +++ b/moveit_planners/ompl/ompl_interface/test/test_state_validity_checker.cpp @@ -68,7 +68,7 @@ constexpr bool VERBOSE = false; rclcpp::Logger getLogger() { - return moveit::getLogger("test_state_validity_checker"); + return moveit::getLogger("moveit.planners.ompl.test_state_validity_checker"); } /** \brief Pretty print std:vectors **/ diff --git a/moveit_planners/ompl/package.xml b/moveit_planners/ompl/package.xml index 7183f4ca07..0e9e7733e3 100644 --- a/moveit_planners/ompl/package.xml +++ b/moveit_planners/ompl/package.xml @@ -2,7 +2,7 @@ moveit_planners_ompl - 2.9.0 + 2.10.0 MoveIt interface to OMPL Henning Kayser Tyler Weaver @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan @@ -37,9 +37,6 @@ eigen3_cmake_module ament_cmake_gtest - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst index f5f8bb1b56..7fa4535832 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package pilz_industrial_motion_planner ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Apply clang-tidy fixes +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* Add parameter api integration test (`#2662 `_) +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt index 47dc33a727..e8420aec32 100644 --- a/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/CMakeLists.txt @@ -16,7 +16,6 @@ find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) find_package(moveit_ros_move_group REQUIRED) find_package(moveit_ros_planning REQUIRED) -find_package(moveit_ros_planning_interface REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) find_package(tf2 REQUIRED) @@ -29,196 +28,150 @@ find_package(tf2_ros REQUIRED) find_package(orocos_kdl REQUIRED) find_package(Boost REQUIRED COMPONENTS) -# TODO(henning): Remove/Port -# if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) -# find_package(code_coverage REQUIRED) -# append_coverage_compiler_flags() -# endif() -# -# ################ -# ## Clang tidy ## -# ################ -# if(CATKIN_ENABLE_CLANG_TIDY) -# find_program( -# CLANG_TIDY_EXE -# NAMES "clang-tidy" -# DOC "Path to clang-tidy executable" -# ) -# if(NOT CLANG_TIDY_EXE) -# message(FATAL_ERROR "clang-tidy not found.") -# else() -# message(STATUS "clang-tidy found: ${CLANG_TIDY_EXE}") -# set(CMAKE_CXX_CLANG_TIDY "${CLANG_TIDY_EXE}") -# endif() -# endif() - -########### -## Build ## -########### -include_directories( - include -) +# ############################################################################## +# Build ## +# ############################################################################## +include_directories(include) set(THIS_PACKAGE_INCLUDE_DEPENDS - Eigen3 - eigen3_cmake_module - # joint_limits_interface - generate_parameter_library - geometry_msgs - moveit_core - moveit_msgs - moveit_ros_move_group - moveit_ros_planning - moveit_ros_planning_interface - pluginlib - rclcpp - tf2 - tf2_eigen - tf2_eigen_kdl - tf2_geometry_msgs - tf2_kdl - tf2_ros - orocos_kdl - Boost -) - - -############### -## Libraries ## -############### + Eigen3 + eigen3_cmake_module + # joint_limits_interface + generate_parameter_library + geometry_msgs + moveit_core + moveit_msgs + moveit_ros_move_group + moveit_ros_planning + pluginlib + rclcpp + tf2 + tf2_eigen + tf2_eigen_kdl + tf2_geometry_msgs + tf2_kdl + tf2_ros + orocos_kdl + Boost) + +# ############################################################################## +# Libraries ## +# ############################################################################## generate_parameter_library( cartesian_limits_parameters # cmake target name for the parameter library src/cartesian_limits_parameters.yaml # path to input yaml file ) -add_library(planning_context_loader_base SHARED - src/planning_context_loader.cpp -) +add_library(planning_context_loader_base SHARED src/planning_context_loader.cpp) target_link_libraries(planning_context_loader_base cartesian_limits_parameters) -ament_target_dependencies(planning_context_loader_base ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(planning_context_loader_base + ${THIS_PACKAGE_INCLUDE_DEPENDS}) -add_library(joint_limits_common SHARED - src/joint_limits_aggregator.cpp - src/joint_limits_container.cpp - src/joint_limits_validator.cpp - src/limits_container.cpp -) +add_library( + joint_limits_common SHARED + src/joint_limits_aggregator.cpp src/joint_limits_container.cpp + src/joint_limits_validator.cpp src/limits_container.cpp) target_link_libraries(joint_limits_common cartesian_limits_parameters) ament_target_dependencies(joint_limits_common ${THIS_PACKAGE_INCLUDE_DEPENDS}) -add_library(trajectory_generation_common SHARED - src/trajectory_functions.cpp - src/trajectory_generator.cpp - src/trajectory_blender_transition_window.cpp -) +add_library( + trajectory_generation_common SHARED + src/trajectory_functions.cpp src/trajectory_generator.cpp + src/trajectory_blender_transition_window.cpp) target_link_libraries(trajectory_generation_common cartesian_limits_parameters) -ament_target_dependencies(trajectory_generation_common ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(trajectory_generation_common + ${THIS_PACKAGE_INCLUDE_DEPENDS}) -add_library(command_list_manager SHARED - src/command_list_manager.cpp - src/plan_components_builder.cpp -) -target_link_libraries(command_list_manager trajectory_generation_common joint_limits_common) +add_library(command_list_manager SHARED src/command_list_manager.cpp + src/plan_components_builder.cpp) +target_link_libraries(command_list_manager trajectory_generation_common + joint_limits_common) ament_target_dependencies(command_list_manager ${THIS_PACKAGE_INCLUDE_DEPENDS}) -############# -## Plugins ## -############# +# ############################################################################## +# Plugins ## +# ############################################################################## add_library(pilz_industrial_motion_planner SHARED - src/pilz_industrial_motion_planner.cpp -) -ament_target_dependencies(pilz_industrial_motion_planner ${THIS_PACKAGE_INCLUDE_DEPENDS}) + src/pilz_industrial_motion_planner.cpp) +ament_target_dependencies(pilz_industrial_motion_planner + ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(pilz_industrial_motion_planner - planning_context_loader_base - joint_limits_common) - -add_library(planning_context_loader_ptp SHARED - src/planning_context_loader_ptp.cpp - src/trajectory_generator_ptp.cpp - src/velocity_profile_atrap.cpp -) -ament_target_dependencies(planning_context_loader_ptp ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(planning_context_loader_ptp planning_context_loader_base joint_limits_common trajectory_generation_common) - -add_library(planning_context_loader_lin SHARED - src/planning_context_loader_lin.cpp - src/trajectory_generator_lin.cpp - src/velocity_profile_atrap.cpp -) -ament_target_dependencies(planning_context_loader_lin ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(planning_context_loader_lin planning_context_loader_base joint_limits_common trajectory_generation_common) - -add_library(planning_context_loader_circ SHARED - src/planning_context_loader_circ.cpp - src/trajectory_generator_circ.cpp - src/path_circle_generator.cpp -) -ament_target_dependencies(planning_context_loader_circ ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(planning_context_loader_circ planning_context_loader_base joint_limits_common trajectory_generation_common) - - -add_library(sequence_capability SHARED - src/move_group_sequence_action.cpp - src/move_group_sequence_service.cpp -) + planning_context_loader_base joint_limits_common) + +add_library( + planning_context_loader_ptp SHARED + src/planning_context_loader_ptp.cpp src/trajectory_generator_ptp.cpp + src/velocity_profile_atrap.cpp) +ament_target_dependencies(planning_context_loader_ptp + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(planning_context_loader_ptp planning_context_loader_base + joint_limits_common trajectory_generation_common) + +add_library( + planning_context_loader_lin SHARED + src/planning_context_loader_lin.cpp src/trajectory_generator_lin.cpp + src/velocity_profile_atrap.cpp) +ament_target_dependencies(planning_context_loader_lin + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(planning_context_loader_lin planning_context_loader_base + joint_limits_common trajectory_generation_common) + +add_library( + planning_context_loader_circ SHARED + src/planning_context_loader_circ.cpp src/trajectory_generator_circ.cpp + src/path_circle_generator.cpp) +ament_target_dependencies(planning_context_loader_circ + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(planning_context_loader_circ planning_context_loader_base + joint_limits_common trajectory_generation_common) + +add_library(sequence_capability SHARED src/move_group_sequence_action.cpp + src/move_group_sequence_service.cpp) ament_target_dependencies(sequence_capability ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(sequence_capability joint_limits_common command_list_manager trajectory_generation_common) - -############# -## Install ## -############# -pluginlib_export_plugin_description_file(moveit_core plugins/pilz_industrial_motion_planner_plugin_description.xml) -pluginlib_export_plugin_description_file(moveit_ros_move_group plugins/sequence_capability_plugin_description.xml) -pluginlib_export_plugin_description_file(pilz_industrial_motion_planner plugins/planning_context_plugin_description.xml) - -## Mark libraries for installation +target_link_libraries(sequence_capability joint_limits_common + command_list_manager trajectory_generation_common) + +# ############################################################################## +# Install ## +# ############################################################################## +pluginlib_export_plugin_description_file( + moveit_core plugins/pilz_industrial_motion_planner_plugin_description.xml) +pluginlib_export_plugin_description_file( + moveit_ros_move_group plugins/sequence_capability_plugin_description.xml) +pluginlib_export_plugin_description_file( + pilz_industrial_motion_planner + plugins/planning_context_plugin_description.xml) + +# Mark libraries for installation install( - TARGETS - pilz_industrial_motion_planner - cartesian_limits_parameters - joint_limits_common - planning_context_loader_base - planning_context_loader_ptp - planning_context_loader_lin - planning_context_loader_circ - command_list_manager - sequence_capability - trajectory_generation_common + TARGETS pilz_industrial_motion_planner + cartesian_limits_parameters + joint_limits_common + planning_context_loader_base + planning_context_loader_ptp + planning_context_loader_lin + planning_context_loader_circ + command_list_manager + sequence_capability + trajectory_generation_common EXPORT pilz_industrial_motion_plannerTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/pilz_industrial_motion_planner -) + INCLUDES + DESTINATION include/pilz_industrial_motion_planner) -install( - DIRECTORY include/ - DESTINATION include/pilz_industrial_motion_planner -) +install(DIRECTORY include/ DESTINATION include/pilz_industrial_motion_planner) -ament_export_include_directories( - include/pilz_industrial_motion_planner -) +ament_export_include_directories(include/pilz_industrial_motion_planner) ament_export_targets(pilz_industrial_motion_plannerTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} orocos_kdl_vendor) if(BUILD_TESTING) # Include Tests add_subdirectory(test) - - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() endif() ament_package() diff --git a/moveit_planners/pilz_industrial_motion_planner/package.xml b/moveit_planners/pilz_industrial_motion_planner/package.xml index 23ad17aee0..8d3689d254 100644 --- a/moveit_planners/pilz_industrial_motion_planner/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner/package.xml @@ -2,7 +2,7 @@ pilz_industrial_motion_planner - 2.9.0 + 2.10.0 MoveIt plugin to generate industrial trajectories PTP, LIN, CIRC and sequences thereof. Christian Henkel @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 ament_cmake eigen3_cmake_module @@ -24,7 +24,6 @@ --> generate_parameter_library geometry_msgs - moveit_ros_planning_interface moveit_msgs moveit_core moveit_ros_planning @@ -52,8 +51,6 @@ ament_cmake_gmock ament_cmake_gtest - ament_lint_auto - ament_lint_common ros_testing diff --git a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp index 9443116576..be11aa84da 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/command_list_manager.cpp @@ -57,7 +57,7 @@ namespace const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_command_list_manager"); + return moveit::getLogger("moveit.planners.pilz.command_list_manager"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp index be1ead4996..e0bec40209 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_aggregator.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_joint_limits_aggregator"); + return moveit::getLogger("moveit.planners.pilz.joint_limits_aggregator"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp index bcde37c223..55127d0334 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/joint_limits_container.cpp @@ -46,7 +46,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_joint_limits_container"); + return moveit::getLogger("moveit.planners.pilz.joint_limits_container"); } } // namespace bool JointLimitsContainer::addLimit(const std::string& joint_name, @@ -118,26 +118,26 @@ std::map::const_iterator JointLimitsContainer::end() co bool JointLimitsContainer::verifyPositionLimit(const std::string& joint_name, double joint_position) const { - return (!(hasLimit(joint_name) && getLimit(joint_name).has_position_limits && - (joint_position < getLimit(joint_name).min_position || joint_position > getLimit(joint_name).max_position))); + return !hasLimit(joint_name) || !getLimit(joint_name).has_position_limits || + (joint_position >= getLimit(joint_name).min_position && joint_position <= getLimit(joint_name).max_position); } bool JointLimitsContainer::verifyVelocityLimit(const std::string& joint_name, double joint_velocity) const { - return (!(hasLimit(joint_name) && getLimit(joint_name).has_velocity_limits && - fabs(joint_velocity) > getLimit(joint_name).max_velocity)); + return !hasLimit(joint_name) || !getLimit(joint_name).has_velocity_limits || + fabs(joint_velocity) <= getLimit(joint_name).max_velocity; } bool JointLimitsContainer::verifyAccelerationLimit(const std::string& joint_name, double joint_acceleration) const { - return (!(hasLimit(joint_name) && getLimit(joint_name).has_acceleration_limits && - fabs(joint_acceleration) > getLimit(joint_name).max_acceleration)); + return !hasLimit(joint_name) || !getLimit(joint_name).has_acceleration_limits || + fabs(joint_acceleration) <= getLimit(joint_name).max_acceleration; } bool JointLimitsContainer::verifyDecelerationLimit(const std::string& joint_name, double joint_acceleration) const { - return (!(hasLimit(joint_name) && getLimit(joint_name).has_deceleration_limits && - fabs(joint_acceleration) > -1.0 * getLimit(joint_name).max_deceleration)); + return !hasLimit(joint_name) || !getLimit(joint_name).has_deceleration_limits || + fabs(joint_acceleration) <= -1.0 * getLimit(joint_name).max_deceleration; } void JointLimitsContainer::updateCommonLimit(const JointLimit& joint_limit, JointLimit& common_limit) diff --git a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp index 537e487e36..b99fc6c1fb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/limits_container.cpp @@ -42,7 +42,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_limits_container"); + return moveit::getLogger("moveit.planners.pilz.limits_container"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp index c78fcf2be0..a8e04ab132 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_action.cpp @@ -58,7 +58,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_move_group_sequence_action"); + return moveit::getLogger("moveit.planners.pilz.move_group_sequence_action"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp index 0004b40cfb..8233f0b67e 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/move_group_sequence_service.cpp @@ -51,7 +51,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_move_group_sequence_service"); + return moveit::getLogger("moveit.planners.pilz.move_group_sequence_service"); } } // namespace MoveGroupSequenceService::MoveGroupSequenceService() : MoveGroupCapability("SequenceService") diff --git a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp index f57118636c..0bde5aa46f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/pilz_industrial_motion_planner.cpp @@ -57,7 +57,7 @@ namespace const std::string PARAM_NAMESPACE_LIMITS = "robot_description_planning"; rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_industrial_motion_planner"); + return moveit::getLogger("moveit.planners.pilz.command_planner"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp index ac72891536..7052088e2f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_circ.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_planning_context_loader_circ"); + return moveit::getLogger("moveit.planners.pilz.planning_context_loader.circ"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp index 0e0c11b859..7deb454f1d 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_lin.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_planning_context_loader_lin"); + return moveit::getLogger("moveit.planners.pilz.planning_context_loader.lin"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp index 21d11e1b95..429ec42657 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/planning_context_loader_ptp.cpp @@ -43,7 +43,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_planning_context_loader_ptp"); + return moveit::getLogger("moveit.planners.pilz.planning_context_loader.ptp"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp index e3a14310bb..a2998e6ecb 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp @@ -45,7 +45,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_trajectory_blender_transition_window"); + return moveit::getLogger("moveit.planners.pilz.trajectory_blender_transition_window"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp index 494acabaea..dfeeec3f55 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator.cpp @@ -52,7 +52,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_trajectory_generator"); + return moveit::getLogger("moveit.planners.pilz.trajectory_generator"); } } // namespace diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp index 61e4cb0142..c512e21578 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_circ.cpp @@ -56,7 +56,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_trajectory_generator_circ"); + return moveit::getLogger("moveit.planners.pilz.trajectory_generator.circ"); } } // namespace TrajectoryGeneratorCIRC::TrajectoryGeneratorCIRC(const moveit::core::RobotModelConstPtr& robot_model, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp index 9c9b251ecd..07b222f915 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp @@ -57,7 +57,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_trajectory_generator_lin"); + return moveit::getLogger("moveit.planners.pilz.trajectory_generator.lin"); } } // namespace TrajectoryGeneratorLIN::TrajectoryGeneratorLIN(const moveit::core::RobotModelConstPtr& robot_model, diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp index 75145e902f..ddea11c25a 100644 --- a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_ptp.cpp @@ -51,7 +51,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("pilz_trajectory_generator_ptp"); + return moveit::getLogger("moveit.planners.pilz.trajectory_generator.ptp"); } } // namespace TrajectoryGeneratorPTP::TrajectoryGeneratorPTP(const moveit::core::RobotModelConstPtr& robot_model, diff --git a/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt index 79c8e0d015..df01a02d4f 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/test/CMakeLists.txt @@ -8,19 +8,14 @@ find_package(Boost REQUIRED thread) find_package(${PROJECT_NAME}_testutils REQUIRED) # Add test directory as include directory -include_directories( - ${CMAKE_CURRENT_SOURCE_DIR} -) +include_directories(${CMAKE_CURRENT_SOURCE_DIR}) # pilz_industrial_motion_testhelpers -add_library(${PROJECT_NAME}_test_utils - test_utils.cpp -) -ament_target_dependencies(${PROJECT_NAME}_test_utils ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(${PROJECT_NAME}_test_utils - joint_limits_common - trajectory_generation_common -) +add_library(${PROJECT_NAME}_test_utils test_utils.cpp) +ament_target_dependencies(${PROJECT_NAME}_test_utils + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(${PROJECT_NAME}_test_utils joint_limits_common + trajectory_generation_common) # Unit tests add_subdirectory(unit_tests) diff --git a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/CMakeLists.txt index e69de29bb2..932b79829c 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/test/integration_tests/CMakeLists.txt @@ -0,0 +1 @@ +# Empty file diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp index 844fe72de7..5dcfad0cde 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.cpp @@ -301,9 +301,9 @@ bool testutils::checkSLERP(const Eigen::Isometry3d& start_pose, const Eigen::Iso // parallel rotation axis // it is possible the axis opposite is // if the angle is zero, axis is arbitrary - if (!(((start_goal_aa.axis() - start_wp_aa.axis()).norm() < fabs(rot_axis_norm_tolerance)) || - ((start_goal_aa.axis() + start_wp_aa.axis()).norm() < fabs(rot_axis_norm_tolerance)) || - (fabs(start_wp_aa.angle()) < fabs(rot_angle_tolerance)))) + if (((start_goal_aa.axis() - start_wp_aa.axis()).norm() >= fabs(rot_axis_norm_tolerance)) && + ((start_goal_aa.axis() + start_wp_aa.axis()).norm() >= fabs(rot_axis_norm_tolerance)) && + (fabs(start_wp_aa.angle()) >= fabs(rot_angle_tolerance))) { std::cout << "Rotational linearity is violated. \n" << '\n' diff --git a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h index 625f70fd23..758c5ecada 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h +++ b/moveit_planners/pilz_industrial_motion_planner/test/test_utils.h @@ -477,7 +477,7 @@ checkCartesianRotationalPath(const robot_trajectory::RobotTrajectoryConstPtr& tr inline bool isMonotonouslyDecreasing(const std::vector& vec, double tol) { return std::is_sorted(vec.begin(), vec.end(), [tol](double a, double b) { - return !(std::abs(a - b) < tol || a < b); // true -> a is ordered before b -> list is not sorted + return std::abs(a - b) >= tol && a >= b; // true -> a is ordered before b -> list is not sorted }); } diff --git a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt index 1178f540b1..2c46012d19 100644 --- a/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner/test/unit_tests/CMakeLists.txt @@ -1,167 +1,156 @@ set(UNIT_TEST_SOURCEFILES - unittest_pilz_industrial_motion_planner_direct - unittest_velocity_profile_atrap - unittest_trajectory_generator - unittest_trajectory_functions - unittest_trajectory_blender_transition_window - unittest_trajectory_generator_common - unittest_trajectory_generator_circ - unittest_trajectory_generator_lin - unittest_trajectory_generator_ptp - unittest_pilz_industrial_motion_planner - unittest_joint_limit - unittest_joint_limits_aggregator - unittest_joint_limits_container - unittest_joint_limits_validator - unittest_planning_context_loaders - unittest_planning_context - unittest_get_solver_tip_frame -) + unittest_pilz_industrial_motion_planner_direct + unittest_velocity_profile_atrap + unittest_trajectory_generator + unittest_trajectory_functions + unittest_trajectory_blender_transition_window + unittest_trajectory_generator_common + unittest_trajectory_generator_circ + unittest_trajectory_generator_lin + unittest_trajectory_generator_ptp + unittest_pilz_industrial_motion_planner + unittest_joint_limit + unittest_joint_limits_aggregator + unittest_joint_limits_container + unittest_joint_limits_validator + unittest_planning_context_loaders + unittest_planning_context + unittest_get_solver_tip_frame) # Direct Command Planner Unit Test ament_add_gtest(unittest_pilz_industrial_motion_planner_direct - src/unittest_pilz_industrial_motion_planner_direct.cpp) + src/unittest_pilz_industrial_motion_planner_direct.cpp) target_link_libraries(unittest_pilz_industrial_motion_planner_direct - ${PROJECT_NAME} - planning_context_loader_ptp -) + ${PROJECT_NAME} planning_context_loader_ptp) # Asymmetric Trapezoidal Velocity Profile Unit Test -ament_add_gtest(unittest_velocity_profile_atrap - src/unittest_velocity_profile_atrap.cpp +ament_add_gtest( + unittest_velocity_profile_atrap src/unittest_velocity_profile_atrap.cpp ${CMAKE_SOURCE_DIR}/src/velocity_profile_atrap.cpp) target_link_libraries(unittest_velocity_profile_atrap ${PROJECT_NAME}) # Trajectory Generator Unit Test -ament_add_gtest(unittest_trajectory_generator - src/unittest_trajectory_generator.cpp +ament_add_gtest( + unittest_trajectory_generator src/unittest_trajectory_generator.cpp ${CMAKE_SOURCE_DIR}/src/trajectory_generator.cpp) target_link_libraries(unittest_trajectory_generator ${PROJECT_NAME}) # Trajectory Functions Unit Test ament_add_gtest_executable(unittest_trajectory_functions - src/unittest_trajectory_functions.cpp -) + src/unittest_trajectory_functions.cpp) ament_target_dependencies(unittest_trajectory_functions Boost) -target_link_libraries(unittest_trajectory_functions - ${PROJECT_NAME}_test_utils - trajectory_generation_common - joint_limits_common -) -add_ros_test(launch/unittest_trajectory_functions.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") +target_link_libraries(unittest_trajectory_functions ${PROJECT_NAME}_test_utils + trajectory_generation_common joint_limits_common) +add_ros_test(launch/unittest_trajectory_functions.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # unittest for trajectory blender transition window -ament_add_gtest_executable(unittest_trajectory_blender_transition_window +ament_add_gtest_executable( + unittest_trajectory_blender_transition_window src/unittest_trajectory_blender_transition_window.cpp - ${CMAKE_SOURCE_DIR}/src/trajectory_blender_transition_window.cpp -) -ament_target_dependencies(unittest_trajectory_blender_transition_window pilz_industrial_motion_planner_testutils) -target_link_libraries(unittest_trajectory_blender_transition_window - ${PROJECT_NAME}_test_utils - trajectory_generation_common - planning_context_loader_lin -) -add_ros_test(launch/unittest_trajectory_blender_transition_window.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + ${CMAKE_SOURCE_DIR}/src/trajectory_blender_transition_window.cpp) +ament_target_dependencies(unittest_trajectory_blender_transition_window + pilz_industrial_motion_planner_testutils) +target_link_libraries( + unittest_trajectory_blender_transition_window ${PROJECT_NAME}_test_utils + trajectory_generation_common planning_context_loader_lin) +add_ros_test(launch/unittest_trajectory_blender_transition_window.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # trajectory generator Unit Test ament_add_gtest_executable(unittest_trajectory_generator_common - src/unittest_trajectory_generator_common.cpp -) -target_link_libraries(unittest_trajectory_generator_common - ${PROJECT_NAME}_test_utils - trajectory_generation_common - planning_context_loader_lin - planning_context_loader_ptp - planning_context_loader_circ -) -add_ros_test(launch/unittest_trajectory_generator_common.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + src/unittest_trajectory_generator_common.cpp) +target_link_libraries( + unittest_trajectory_generator_common ${PROJECT_NAME}_test_utils + trajectory_generation_common planning_context_loader_lin + planning_context_loader_ptp planning_context_loader_circ) +add_ros_test(launch/unittest_trajectory_generator_common.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # trajectory generator circ Unit Test ament_add_gtest_executable(unittest_trajectory_generator_circ - src/unittest_trajectory_generator_circ.cpp -) -ament_target_dependencies(unittest_trajectory_generator_circ pilz_industrial_motion_planner_testutils) + src/unittest_trajectory_generator_circ.cpp) +ament_target_dependencies(unittest_trajectory_generator_circ + pilz_industrial_motion_planner_testutils) target_link_libraries(unittest_trajectory_generator_circ - ${PROJECT_NAME}_test_utils - planning_context_loader_circ -) -add_ros_test(launch/unittest_trajectory_generator_circ.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + ${PROJECT_NAME}_test_utils planning_context_loader_circ) +add_ros_test(launch/unittest_trajectory_generator_circ.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # trajectory generator lin Unit Test ament_add_gtest_executable(unittest_trajectory_generator_lin - src/unittest_trajectory_generator_lin.cpp -) -ament_target_dependencies(unittest_trajectory_generator_lin pilz_industrial_motion_planner_testutils) + src/unittest_trajectory_generator_lin.cpp) +ament_target_dependencies(unittest_trajectory_generator_lin + pilz_industrial_motion_planner_testutils) target_link_libraries(unittest_trajectory_generator_lin - ${PROJECT_NAME}_test_utils - planning_context_loader_lin -) -add_ros_test(launch/unittest_trajectory_generator_lin.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + ${PROJECT_NAME}_test_utils planning_context_loader_lin) +add_ros_test(launch/unittest_trajectory_generator_lin.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # trajectory generator ptp Unit Test ament_add_gtest_executable(unittest_trajectory_generator_ptp - src/unittest_trajectory_generator_ptp.cpp -) -target_link_libraries(unittest_trajectory_generator_ptp - ${PROJECT_NAME}_test_utils - trajectory_generation_common - planning_context_loader_ptp -) -add_ros_test(launch/unittest_trajectory_generator_ptp.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + src/unittest_trajectory_generator_ptp.cpp) +target_link_libraries( + unittest_trajectory_generator_ptp ${PROJECT_NAME}_test_utils + trajectory_generation_common planning_context_loader_ptp) +add_ros_test(launch/unittest_trajectory_generator_ptp.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # Command Planner Unit Test -ament_add_gtest_executable(unittest_pilz_industrial_motion_planner src/unittest_pilz_industrial_motion_planner.cpp) +ament_add_gtest_executable(unittest_pilz_industrial_motion_planner + src/unittest_pilz_industrial_motion_planner.cpp) target_link_libraries(unittest_pilz_industrial_motion_planner ${PROJECT_NAME}) -add_ros_test(launch/unittest_pilz_industrial_motion_planner.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") +add_ros_test(launch/unittest_pilz_industrial_motion_planner.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # JointLimits Unit Test ament_add_gtest_executable(unittest_joint_limit src/unittest_joint_limit.cpp) target_link_libraries(unittest_joint_limit joint_limits_common) -add_ros_test(launch/unittest_joint_limit.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") +add_ros_test(launch/unittest_joint_limit.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # JointLimitsAggregator Unit Test -ament_add_gtest_executable(unittest_joint_limits_aggregator src/unittest_joint_limits_aggregator.cpp) +ament_add_gtest_executable(unittest_joint_limits_aggregator + src/unittest_joint_limits_aggregator.cpp) target_link_libraries(unittest_joint_limits_aggregator joint_limits_common) -add_ros_test(launch/unittest_joint_limits_aggregator.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") +add_ros_test(launch/unittest_joint_limits_aggregator.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # JointLimitsContainer Unit Test ament_add_gtest(unittest_joint_limits_container - src/unittest_joint_limits_container.cpp) + src/unittest_joint_limits_container.cpp) target_link_libraries(unittest_joint_limits_container joint_limits_common) # JointLimitsValidator Unit Test ament_add_gtest(unittest_joint_limits_validator - src/unittest_joint_limits_validator.cpp) + src/unittest_joint_limits_validator.cpp) target_link_libraries(unittest_joint_limits_validator joint_limits_common) # PlanningContextLoaderPTP Unit Test ament_add_gtest_executable(unittest_planning_context_loaders - src/unittest_planning_context_loaders.cpp -) -target_link_libraries(unittest_planning_context_loaders - ${PROJECT_NAME} - ${PROJECT_NAME}_test_utils -) -add_ros_test(launch/unittest_planning_context_loaders.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + src/unittest_planning_context_loaders.cpp) +target_link_libraries(unittest_planning_context_loaders ${PROJECT_NAME} + ${PROJECT_NAME}_test_utils) +add_ros_test(launch/unittest_planning_context_loaders.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # PlanningContext Unit Test (Typed test) ament_add_gtest_executable(unittest_planning_context - src/unittest_planning_context.cpp -) -target_link_libraries(unittest_planning_context - ${PROJECT_NAME} - ${PROJECT_NAME}_test_utils - planning_context_loader_lin - planning_context_loader_ptp - planning_context_loader_circ -) -add_ros_test(launch/unittest_planning_context.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + src/unittest_planning_context.cpp) +target_link_libraries( + unittest_planning_context ${PROJECT_NAME} ${PROJECT_NAME}_test_utils + planning_context_loader_lin planning_context_loader_ptp + planning_context_loader_circ) +add_ros_test(launch/unittest_planning_context.test.py ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") # GetTipFrames Unit Test -ament_add_gmock(unittest_get_solver_tip_frame src/unittest_get_solver_tip_frame.cpp) +ament_add_gmock(unittest_get_solver_tip_frame + src/unittest_get_solver_tip_frame.cpp) target_link_libraries(unittest_get_solver_tip_frame ${PROJECT_NAME}) -install(TARGETS ${UNIT_TEST_SOURCEFILES} +install( + TARGETS ${UNIT_TEST_SOURCEFILES} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/${PROJECT_NAME}) diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst index 3fcb672b95..8c79e3e119 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package pilz_industrial_motion_planner_testutils ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt index a59bc99564..1026df76f3 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/CMakeLists.txt @@ -14,37 +14,34 @@ find_package(eigen3_cmake_module REQUIRED) find_package(Eigen3 REQUIRED) find_package(Boost REQUIRED COMPONENTS) -########### -## Build ## -########### +# ############################################################################## +# Build ## +# ############################################################################## -## Specify additional locations of header files -## Your package locations should be listed before other locations +# Specify additional locations of header files Your package locations should be +# listed before other locations include_directories(include) -## Declare a C++ library -add_library(pilz_industrial_motion_planner_testutils SHARED - src/cartesianconfiguration.cpp - src/jointconfiguration.cpp - src/robotconfiguration.cpp - src/sequence.cpp - src/xml_testdata_loader.cpp -) +# Declare a C++ library +add_library( + pilz_industrial_motion_planner_testutils SHARED + src/cartesianconfiguration.cpp src/jointconfiguration.cpp + src/robotconfiguration.cpp src/sequence.cpp src/xml_testdata_loader.cpp) -## Specify libraries to link a library or executable target against -ament_target_dependencies(pilz_industrial_motion_planner_testutils +# Specify libraries to link a library or executable target against +ament_target_dependencies( + pilz_industrial_motion_planner_testutils Boost Eigen3 eigen3_cmake_module rclcpp moveit_core moveit_msgs - tf2_eigen -) + tf2_eigen) -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## install( TARGETS pilz_industrial_motion_planner_testutils @@ -52,26 +49,14 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/pilz_industrial_motion_planner_testutils -) + INCLUDES + DESTINATION include/pilz_industrial_motion_planner_testutils) -install(DIRECTORY include/ DESTINATION include/pilz_industrial_motion_planner_testutils) +install(DIRECTORY include/ + DESTINATION include/pilz_industrial_motion_planner_testutils) -ament_export_targets(pilz_industrial_motion_planner_testutilsTargets HAS_LIBRARY_TARGET) +ament_export_targets(pilz_industrial_motion_planner_testutilsTargets + HAS_LIBRARY_TARGET) ament_export_dependencies(rclcpp moveit_core moveit_msgs tf2_eigen) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml index f62103ad5e..51c65c0758 100644 --- a/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml +++ b/moveit_planners/pilz_industrial_motion_planner_testutils/package.xml @@ -2,7 +2,7 @@ pilz_industrial_motion_planner_testutils - 2.9.0 + 2.10.0 Helper scripts and functionality to test industrial motion generation Christian Henkel @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 ament_cmake eigen3_cmake_module @@ -29,9 +29,6 @@ moveit_commander --> - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_planners/stomp/CHANGELOG.rst b/moveit_planners/stomp/CHANGELOG.rst index bc931841ec..832d2956a0 100644 --- a/moveit_planners/stomp/CHANGELOG.rst +++ b/moveit_planners/stomp/CHANGELOG.rst @@ -2,6 +2,32 @@ Changelog for package moveit_planners_stomp ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* missing destination path (`#2668 `_) +* Fix penalty-based cost function in STOMP (`#2625 `_) + * Fix penalty-based cost function in STOMP + This adds several test cases for STOMP's noise generation and cost + functions, and provides the following fixes: + * out-of-bounds vector access when tail states of trajectory are invalid + * smoothed costs overriding values of previous invalid groups + * missing validity check of last state in trajectory + * inability to disable cost function interpolation steps + * total cost of trajectory not summing up to sum of state penalties + * bug in Gaussian producing infinite values with invalid start states + * Improve documentation + --------- +* Contributors: Henning Kayser, Robert Haschke, Sarvajith Adyanthaya, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_planners/stomp/CMakeLists.txt b/moveit_planners/stomp/CMakeLists.txt index 941da3dd2e..955f227d87 100644 --- a/moveit_planners/stomp/CMakeLists.txt +++ b/moveit_planners/stomp/CMakeLists.txt @@ -20,43 +20,30 @@ generate_parameter_library(stomp_moveit_parameters res/stomp_moveit.yaml) include_directories(include) # Planner Plugin -add_library(stomp_moveit_plugin SHARED - src/stomp_moveit_planner_plugin.cpp - src/stomp_moveit_planning_context.cpp -) -ament_target_dependencies(stomp_moveit_plugin - moveit_core - std_msgs - tf2_eigen - visualization_msgs - rsl -) -target_link_libraries(stomp_moveit_plugin stomp::stomp stomp_moveit_parameters) - -pluginlib_export_plugin_description_file(moveit_core stomp_moveit_plugin_description.xml) - -install(TARGETS stomp_moveit_plugin stomp_moveit_parameters +add_library(stomp_moveit_plugin SHARED src/stomp_moveit_planner_plugin.cpp + src/stomp_moveit_planning_context.cpp) +ament_target_dependencies(stomp_moveit_plugin moveit_core std_msgs tf2_eigen + visualization_msgs) +target_link_libraries(stomp_moveit_plugin stomp::stomp stomp_moveit_parameters + rsl::rsl) + +pluginlib_export_plugin_description_file(moveit_core + stomp_moveit_plugin_description.xml) + +install( + TARGETS stomp_moveit_plugin stomp_moveit_parameters EXPORT moveit_planners_stompTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include -) + INCLUDES + DESTINATION include/moveit_planners_stomp) ament_export_targets(moveit_planners_stompTargets HAS_LIBRARY_TARGET) ament_export_dependencies(moveit_core stomp generate_parameter_library) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_lint_cmake_FOUND TRUE) - - ament_lint_auto_find_test_dependencies() + add_subdirectory(test) endif() ament_package() diff --git a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp index 4d1dd2993a..705f796cba 100644 --- a/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp +++ b/moveit_planners/stomp/include/stomp_moveit/cost_functions.hpp @@ -60,72 +60,90 @@ constexpr double COL_CHECK_DISTANCE = 0.05; constexpr double CONSTRAINT_CHECK_DISTANCE = 0.05; /** - * Creates a cost function from a binary robot state validation function. - * This is used for computing smooth cost profiles for binary state conditions like collision checks and constraints. + * Creates a cost function from a robot state validation function. + * This is used for computing smooth cost profiles for waypoint state conditions like collision checks and constraints. * The validator function is applied for all states in the validated path while also considering interpolated states. * If a waypoint or an interpolated state is invalid, a local penalty is being applied to the path. * Penalty costs are being smoothed out using a Gaussian so that valid neighboring states (near collisions) are * optimized as well. + * This implementation does not support cost thresholds, non-zero local costs will render the trajectory as invalid. * - * @param state_validator_fn The validator function that tests for binary conditions - * @param interpolation_step_size The L2 norm distance step used for interpolation + * @param state_validator_fn The validator function that produces local costs for all waypoints + * @param interpolation_step_size The L2 norm distance step used for interpolation (disabled when set to 0.0) * * @return Cost function that computes smooth costs for binary validity conditions */ CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator_fn, double interpolation_step_size) { CostFn cost_fn = [=](const Eigen::MatrixXd& values, Eigen::VectorXd& costs, bool& validity) { + // Assume zero cost and valid trajectory from the start costs.setZero(values.cols()); - validity = true; - std::vector> invalid_windows; - bool in_invalid_window = false; // Iterate over sample waypoint pairs and check for validity in each segment. // If an invalid state is found, weighted penalty costs are applied to both waypoints. // Subsequent invalid states are assumed to have the same cause, so we are keeping track // of "invalid windows" which are used for smoothing out the costs per violation cause // with a gaussian, penalizing neighboring valid states as well. - for (int timestep = 0; timestep < values.cols() - 1; ++timestep) + // Invalid windows are represented as pairs of start and end timesteps. + std::vector> invalid_windows; + bool in_invalid_window = false; + for (int timestep = 0; timestep < values.cols(); ++timestep) { + // Get state at current timestep and check for validity + // The penalty of the validation function is added to the cost of the current timestep + // A state is rendered invalid if a cost results from the current validity check or if a penalty is carried over + // from the previous iteration. Eigen::VectorXd current = values.col(timestep); - Eigen::VectorXd next = values.col(timestep + 1); - const double segment_distance = (next - current).norm(); - double interpolation_fraction = 0.0; - const double interpolation_step = std::min(0.5, interpolation_step_size / segment_distance); - bool found_invalid_state = false; - double penalty = 0.0; - while (!found_invalid_state && interpolation_fraction < 1.0) + costs(timestep) += state_validator_fn(current); + bool found_invalid_state = costs(timestep) > 0.0; + + // If state is valid, interpolate towards the next waypoint if there is one + bool continue_interpolation = + !found_invalid_state && timestep < (values.cols() - 1) && interpolation_step_size > 0.0; + if (continue_interpolation) { - Eigen::VectorXd sample_vec = (1 - interpolation_fraction) * current + interpolation_fraction * next; + Eigen::VectorXd next = values.col(timestep + 1); + // Interpolate waypoints at least once, even if interpolation_step_size exceeds the waypoint distance + const double interpolation_step = std::min(0.5, interpolation_step_size / (next - current).norm()); + for (double interpolation_fraction = interpolation_step; interpolation_fraction < 1.0; + interpolation_fraction += interpolation_step) + { + Eigen::VectorXd sample_vec = (1 - interpolation_fraction) * current + interpolation_fraction * next; - penalty = state_validator_fn(sample_vec); - found_invalid_state = penalty > 0.0; - interpolation_fraction += interpolation_step; + double penalty = state_validator_fn(sample_vec); + found_invalid_state = penalty > 0.0; + if (found_invalid_state) + { + // Apply weighted penalties -> This trajectory is definitely invalid + costs(timestep) += (1 - interpolation_fraction) * penalty; + costs(timestep + 1) += interpolation_fraction * penalty; + break; + } + } } + // Track groups of invalid states as "invalid windows" for subsequent smoothing if (found_invalid_state) { - // Apply weighted penalties -> This trajectory is definitely invalid - costs(timestep) = (1 - interpolation_fraction) * penalty; - costs(timestep + 1) = interpolation_fraction * penalty; + // Mark solution as invalid validity = false; - // Open new invalid window when this is the first detected invalid state in a group + // OPEN new invalid window when this is the first detected invalid state in a group if (!in_invalid_window) { - invalid_windows.emplace_back(timestep, values.cols()); + // new windows only include a single timestep as start and end state + invalid_windows.emplace_back(timestep, timestep); in_invalid_window = true; } + + // Update end of invalid window with the current invalid timestep + invalid_windows.back().second = timestep; } else { - // Close current invalid window if the current state is valid - if (in_invalid_window) - { - invalid_windows.back().second = timestep - 1; - in_invalid_window = false; - } + // CLOSE current invalid window if the current state is valid + in_invalid_window = false; } } @@ -134,18 +152,31 @@ CostFn getCostFunctionFromStateValidator(const StateValidatorFn& state_validator // before and after the violation are penalized as well. for (const auto& [start, end] : invalid_windows) { + // Total cost of invalid states + // We are smoothing the exact same total cost over a wider neighborhood + const double window_cost = costs(Eigen::seq(start, end)).sum(); + + // window size defines 2 sigma of gaussian smoothing kernel + // which equals 68.2% of overall cost and about 25% of width const double window_size = static_cast(end - start) + 1; - const double sigma = window_size / 5.0; + const double sigma = std::max(1.0, 0.5 * window_size); const double mu = 0.5 * (start + end); + // Iterate over waypoints in the range of +/-sigma (neighborhood) - // and add a weighted and continuous cost value for each waypoint - // based on a Gaussian distribution. - for (auto j = std::max(0l, start - static_cast(sigma)); - j <= std::min(values.cols() - 1, end + static_cast(sigma)); ++j) + // and add a discrete cost value for each waypoint based on a Gaussian + // distribution. + const long kernel_start = mu - static_cast(sigma) * 4; + const long kernel_end = mu + static_cast(sigma) * 4; + const long bounded_kernel_start = std::max(0l, kernel_start); + const long bounded_kernel_end = std::min(values.cols() - 1, kernel_end); + for (auto j = bounded_kernel_start; j <= bounded_kernel_end; ++j) { - costs(j) += - std::exp(-std::pow(j - mu, 2) / (2 * std::pow(sigma, 2))) / (sigma * std::sqrt(2 * mu)) * window_size; + costs(j) = std::exp(-std::pow(j - mu, 2) / (2 * std::pow(sigma, 2))) / (sigma * std::sqrt(2 * M_PI)); } + + // Normalize values to original total window cost + const double cost_sum = costs(Eigen::seq(bounded_kernel_start, bounded_kernel_end)).sum(); + costs(Eigen::seq(bounded_kernel_start, bounded_kernel_end)) *= window_cost / cost_sum; } return true; diff --git a/moveit_planners/stomp/package.xml b/moveit_planners/stomp/package.xml index 48a9be02ca..4a69a95eb2 100644 --- a/moveit_planners/stomp/package.xml +++ b/moveit_planners/stomp/package.xml @@ -2,13 +2,13 @@ moveit_planners_stomp - 2.9.0 + 2.10.0 STOMP Motion Planner for MoveIt Henning Kayser BSD-3-Clause - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Henning Kayser @@ -23,9 +23,6 @@ visualization_msgs rsl - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp index 2c4ffdbda9..b7f588ea21 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planner_plugin.cpp @@ -51,7 +51,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("stomp_moveit"); + return moveit::getLogger("moveit.planners.stomp.planner_manager"); } } // namespace diff --git a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp index f5895ed0d3..de5d43a6ff 100644 --- a/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp +++ b/moveit_planners/stomp/src/stomp_moveit_planning_context.cpp @@ -59,7 +59,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("stomp_moveit"); + return moveit::getLogger("moveit.planners.stomp.planning_context"); } } // namespace diff --git a/moveit_planners/stomp/test/CMakeLists.txt b/moveit_planners/stomp/test/CMakeLists.txt new file mode 100644 index 0000000000..e09602fc6f --- /dev/null +++ b/moveit_planners/stomp/test/CMakeLists.txt @@ -0,0 +1,8 @@ +find_package(ament_cmake_gtest REQUIRED) +ament_add_gtest(test_noise_generator test_noise_generator.cpp) +ament_target_dependencies(test_noise_generator tf2_eigen) +target_link_libraries(test_noise_generator rsl::rsl) + +ament_add_gtest(test_cost_functions test_cost_functions.cpp) +ament_target_dependencies(test_cost_functions moveit_core) +target_link_libraries(test_cost_functions rsl::rsl) diff --git a/moveit_planners/stomp/test/test_cost_functions.cpp b/moveit_planners/stomp/test/test_cost_functions.cpp new file mode 100644 index 0000000000..f31fae3aa5 --- /dev/null +++ b/moveit_planners/stomp/test/test_cost_functions.cpp @@ -0,0 +1,117 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, 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. + *********************************************************************/ + +/** @file + * @author Henning Kayser + */ + +#include +#include + +constexpr size_t TIMESTEPS = 100; +constexpr size_t VARIABLES = 6; +constexpr double PENALTY = 1.0; + +TEST(NoiseGeneratorTest, testGetCostFunctionAllValidStates) +{ + // GIVEN a cost function with a state validator that only returns valid costs of 0.0 + auto state_validator_fn = [](const Eigen::VectorXd& /* state_positions */) { return 0.0; }; + auto cost_fn = + stomp_moveit::costs::getCostFunctionFromStateValidator(state_validator_fn, 0.1 /* interpolation_step_size */); + + // GIVEN a trajectory with TIMESTEPS states, with waypoints interpolating from 0.0 to 1.0 joint values + Eigen::MatrixXd values = Eigen::MatrixXd::Zero(VARIABLES, TIMESTEPS); + const int last_timestep = values.cols() - 1; + for (int timestep = 0; timestep <= last_timestep; ++timestep) + { + values.col(timestep).fill(static_cast(timestep) / last_timestep); + } + + // WHEN the cost function is applied to the trajectory + Eigen::VectorXd costs(TIMESTEPS); + bool validity; + ASSERT_TRUE(cost_fn(values, costs, validity)); + + // THEN the trajectory must be valid and have zero costs + EXPECT_TRUE(validity); + EXPECT_EQ(costs.sum(), 0.0); +} + +TEST(NoiseGeneratorTest, testGetCostFunctionInvalidStates) +{ + // GIVEN a cost function with a simulated state validator that tags selected timesteps as invalid. + // The state validation function is called once per timestep since interpolation is disabled. + // This assumption is confirmed as boundary assumption after calling the solver. + static const std::set INVALID_TIMESTEPS( + { 0, 10, 11, 12, 25, 26, 27, 46, 63, 64, 65, 66, 67, 68, 69, 97, 98, 99 }); + size_t timestep_counter = 0; + auto state_validator_fn = [&](const Eigen::VectorXd& /* state_positions */) { + return PENALTY * INVALID_TIMESTEPS.count(timestep_counter++); + }; + auto cost_fn = + stomp_moveit::costs::getCostFunctionFromStateValidator(state_validator_fn, 0.0 /* interpolation disabled */); + + // GIVEN a trajectory with TIMESTEPS states, with waypoints interpolating from 0.0 to 1.0 joint values + Eigen::MatrixXd values = Eigen::MatrixXd::Zero(VARIABLES, TIMESTEPS); + const int last_timestep = values.cols() - 1; + for (int timestep = 0; timestep <= last_timestep; ++timestep) + { + values.col(timestep).fill(static_cast(timestep) / last_timestep); + } + + // WHEN the cost function is applied to the trajectory + Eigen::VectorXd costs(TIMESTEPS); + bool validity; + ASSERT_TRUE(cost_fn(values, costs, validity)); + + // THEN the following boundary assumptions about cost function outputs, costs and validity need to be met + EXPECT_FALSE(validity); // invalid states must result in an invalid trajectory + EXPECT_EQ(timestep_counter, 100u); // 100 timesteps checked without interpolation + EXPECT_LE(costs.maxCoeff(), PENALTY); // the highest cost must not be higher than the configured penalty + EXPECT_GE(costs.minCoeff(), 0.0); // no negative cost values should be computed + + // THEN the total cost must equal the sum of penalties produced by all invalid timesteps + EXPECT_DOUBLE_EQ(costs.sum(), PENALTY * INVALID_TIMESTEPS.size()); + + // THEN invalid timesteps must account for the majority of the total cost. + // We expect that invalid windows cover at least 2*sigma (=68.1%) of each cost distribution. + const std::vector invalid_timesteps_vec(INVALID_TIMESTEPS.begin(), INVALID_TIMESTEPS.end()); + EXPECT_GE(costs(invalid_timesteps_vec).sum(), 0.681 * PENALTY); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h b/moveit_planners/stomp/test/test_noise_generator.cpp similarity index 54% rename from moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h rename to moveit_planners/stomp/test/test_noise_generator.cpp index f658e6a440..bf47b966d6 100644 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/roscpp_initializer.h +++ b/moveit_planners/stomp/test/test_noise_generator.cpp @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2012, Willow Garage, Inc. + * Copyright (c) 2023, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -14,7 +14,7 @@ * 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 Willow Garage nor the names of its + * * 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. * @@ -32,43 +32,45 @@ * POSSIBILITY OF SUCH DAMAGE. *********************************************************************/ -/* Author: Ioan Sucan */ +/** @file + * @author Henning Kayser + */ -#pragma once +#include +#include -#include -#include +constexpr size_t TIMESTEPS = 100; +constexpr size_t VARIABLES = 6; +static const std::vector STDDEV(VARIABLES, 0.2); +static const Eigen::MatrixXd VALUES = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 1.0); +static const Eigen::MatrixXd NOISY_VALUES = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 0.0); +static const Eigen::MatrixXd NOISE = Eigen::MatrixXd::Constant(VARIABLES, TIMESTEPS, 0.0); -namespace moveit +TEST(NoiseGeneratorTest, testStartEndUnchanged) { -/** \brief Tools for creating python bindings for MoveIt */ -namespace py_bindings_tools -{ -/** \brief The constructor of this class ensures that ros::init() has - been called. Thread safety and multiple initialization is - properly handled. When the process terminates, ros::shutdown() is - also called, if needed. */ -class ROScppInitializer -{ -public: - ROScppInitializer(); - ROScppInitializer(boost::python::list& argv); - ROScppInitializer(const std::string& node_name, boost::python::list& argv); -}; + auto noise_gen = stomp_moveit::noise::getNormalDistributionGenerator(TIMESTEPS, STDDEV); -/** \brief This function can be used to specify the ROS command line arguments for the internal ROScpp instance; - Usually this function would also be exposed in the py module that uses ROScppInitializer. */ -void roscpp_set_arguments(const std::string& node_name, boost::python::list& argv); + auto noise = NOISE; + auto noisy_values = NOISY_VALUES; -/** \brief Initialize ROScpp with specified command line args */ -void roscpp_init(const std::string& node_name, boost::python::list& argv); + noise_gen(VALUES, noisy_values, noise); -/** \brief Initialize ROScpp with specified command line args */ -void roscpp_init(boost::python::list& argv); + // Test that noise creates output unlike the input + EXPECT_NE(noise, NOISE); + EXPECT_NE(noisy_values, NOISY_VALUES); + EXPECT_NE(VALUES, noisy_values); -/** \brief Initialize ROScpp with default command line args */ -void roscpp_init(); + // Test that the dimensions of the output matches the input + EXPECT_EQ(noise.size(), NOISE.size()); + EXPECT_EQ(noisy_values.size(), NOISY_VALUES.size()); -void roscpp_shutdown(); -} // namespace py_bindings_tools -} // namespace moveit + // Test that start and end columns (=states) are not modified + EXPECT_EQ(VALUES.col(0), noisy_values.col(0)); + EXPECT_EQ(VALUES.col(TIMESTEPS - 1), noisy_values.col(TIMESTEPS - 1)); +} + +int main(int argc, char** argv) +{ + testing::InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst index d0ded08330..f37ad6ba94 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_resources_prbt_ikfast_manipulator_plugin ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Do not pass and return simple types by const ref (`#2453 `_) diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt index c970a6771b..cc451f3d65 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/CMakeLists.txt @@ -12,7 +12,7 @@ endif() # enable aligned new in gcc7+ if(CMAKE_COMPILER_IS_GNUCXX) - if (CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 7.0) + if(CMAKE_CXX_COMPILER_VERSION VERSION_GREATER 7.0) set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -faligned-new") endif() endif() @@ -30,41 +30,48 @@ find_package(generate_parameter_library REQUIRED) include_directories(include) generate_parameter_library( - prbt_ikfast_kinematics_parameters # cmake target name for the parameter library + prbt_ikfast_kinematics_parameters # cmake target name for the parameter + # library src/prbt_ikfast_kinematics_parameters.yaml # path to input yaml file ) add_library(prbt_manipulator_moveit_ikfast_plugin SHARED - src/prbt_manipulator_ikfast_moveit_plugin.cpp) + src/prbt_manipulator_ikfast_moveit_plugin.cpp) if(NOT WIN32) # suppress warnings about unused variables in OpenRave's solver code - target_compile_options(prbt_manipulator_moveit_ikfast_plugin PRIVATE -Wno-unused-variable) + target_compile_options(prbt_manipulator_moveit_ikfast_plugin + PRIVATE -Wno-unused-variable) endif() -ament_target_dependencies(prbt_manipulator_moveit_ikfast_plugin +ament_target_dependencies( + prbt_manipulator_moveit_ikfast_plugin moveit_core pluginlib rclcpp tf2_kdl tf2_eigen tf2_eigen_kdl - tf2_geometry_msgs -) + tf2_geometry_msgs) -target_link_libraries(prbt_manipulator_moveit_ikfast_plugin prbt_ikfast_kinematics_parameters) +target_link_libraries(prbt_manipulator_moveit_ikfast_plugin + prbt_ikfast_kinematics_parameters) -pluginlib_export_plugin_description_file(moveit_core prbt_manipulator_moveit_ikfast_plugin_description.xml) +pluginlib_export_plugin_description_file( + moveit_core prbt_manipulator_moveit_ikfast_plugin_description.xml) install( - TARGETS prbt_manipulator_moveit_ikfast_plugin prbt_ikfast_kinematics_parameters + TARGETS prbt_manipulator_moveit_ikfast_plugin + prbt_ikfast_kinematics_parameters EXPORT moveit_resources_prbt_ikfast_manipulator_pluginTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_resources_prbt_ikfast_manipulator_plugin -) + INCLUDES + DESTINATION include/moveit_resources_prbt_ikfast_manipulator_plugin) -ament_export_targets(moveit_resources_prbt_ikfast_manipulator_pluginTargets HAS_LIBRARY_TARGET) -ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} generate_parameter_library) +ament_export_targets(moveit_resources_prbt_ikfast_manipulator_pluginTargets + HAS_LIBRARY_TARGET) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} + generate_parameter_library) ament_package() diff --git a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/package.xml b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/package.xml index 55cfbedf10..e912b2f7b3 100644 --- a/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/package.xml +++ b/moveit_planners/test_configs/prbt_ikfast_manipulator_plugin/package.xml @@ -1,7 +1,7 @@ moveit_resources_prbt_ikfast_manipulator_plugin - 2.9.0 + 2.10.0 The prbt_ikfast_manipulator_plugin package Alexander Gutenkunst Christian Henkel @@ -10,8 +10,8 @@ Apache 2.0 ament_cmake http://moveit.ros.org - https://github.com/ros-planning/moveit-resources/issues - https://github.com/ros-planning/moveit-resources + https://github.com/moveit/moveit-resources/issues + https://github.com/moveit/moveit-resources tf2_geometry_msgs diff --git a/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst b/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst index 8141afdc5a..187e15861e 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_moveit_config/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_resources_prbt_moveit_config ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Update ros2_control usage (`#2620 `_) diff --git a/moveit_planners/test_configs/prbt_moveit_config/CMakeLists.txt b/moveit_planners/test_configs/prbt_moveit_config/CMakeLists.txt index 0378606d21..4e3314324b 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/CMakeLists.txt +++ b/moveit_planners/test_configs/prbt_moveit_config/CMakeLists.txt @@ -3,7 +3,9 @@ project(moveit_resources_prbt_moveit_config) find_package(ament_cmake REQUIRED) -install(DIRECTORY launch DESTINATION share/moveit_resources_prbt_moveit_config +install( + DIRECTORY launch + DESTINATION share/moveit_resources_prbt_moveit_config PATTERN "setup_assistant.launch" EXCLUDE) install(DIRECTORY config DESTINATION share/moveit_resources_prbt_moveit_config) diff --git a/moveit_planners/test_configs/prbt_moveit_config/package.xml b/moveit_planners/test_configs/prbt_moveit_config/package.xml index 848c17573d..453d451405 100644 --- a/moveit_planners/test_configs/prbt_moveit_config/package.xml +++ b/moveit_planners/test_configs/prbt_moveit_config/package.xml @@ -1,6 +1,6 @@ moveit_resources_prbt_moveit_config - 2.9.0 + 2.10.0

MoveIt Resources for testing: Pilz PRBT 6 @@ -17,8 +17,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit-resources/issues - https://github.com/ros-planning/moveit-resources + https://github.com/moveit/moveit-resources/issues + https://github.com/moveit/moveit-resources ament_cmake diff --git a/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst b/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst index 2ef2500719..1084e487cf 100644 --- a/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_pg70_support/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_resources_prbt_pg70_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit_planners/test_configs/prbt_pg70_support/CMakeLists.txt b/moveit_planners/test_configs/prbt_pg70_support/CMakeLists.txt index 9724504f19..1a8120af86 100644 --- a/moveit_planners/test_configs/prbt_pg70_support/CMakeLists.txt +++ b/moveit_planners/test_configs/prbt_pg70_support/CMakeLists.txt @@ -4,9 +4,9 @@ project(moveit_resources_prbt_pg70_support) find_package(ament_cmake REQUIRED) ament_package() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## install(DIRECTORY config DESTINATION share/moveit_resources_prbt_pg70_support) install(DIRECTORY meshes DESTINATION share/moveit_resources_prbt_pg70_support) install(DIRECTORY urdf DESTINATION share/moveit_resources_prbt_pg70_support) diff --git a/moveit_planners/test_configs/prbt_pg70_support/package.xml b/moveit_planners/test_configs/prbt_pg70_support/package.xml index 4bbdd0a22e..6f9e82aa2c 100644 --- a/moveit_planners/test_configs/prbt_pg70_support/package.xml +++ b/moveit_planners/test_configs/prbt_pg70_support/package.xml @@ -1,7 +1,7 @@ moveit_resources_prbt_pg70_support - 2.9.0 + 2.10.0 PRBT support for Schunk pg70 gripper. Alexander Gutenkunst @@ -11,8 +11,8 @@ Apache 2.0 http://moveit.ros.org - https://github.com/ros-planning/moveit-resources/issues - https://github.com/ros-planning/moveit-resources + https://github.com/moveit/moveit-resources/issues + https://github.com/moveit/moveit-resources ament_cmake diff --git a/moveit_planners/test_configs/prbt_support/CHANGELOG.rst b/moveit_planners/test_configs/prbt_support/CHANGELOG.rst index 40a72cfa3a..92ee4c29fb 100644 --- a/moveit_planners/test_configs/prbt_support/CHANGELOG.rst +++ b/moveit_planners/test_configs/prbt_support/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package prbt_support ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Update ros2_control usage (`#2620 `_) diff --git a/moveit_planners/test_configs/prbt_support/CMakeLists.txt b/moveit_planners/test_configs/prbt_support/CMakeLists.txt index 28f4f15676..f95395e474 100644 --- a/moveit_planners/test_configs/prbt_support/CMakeLists.txt +++ b/moveit_planners/test_configs/prbt_support/CMakeLists.txt @@ -4,11 +4,11 @@ project(moveit_resources_prbt_support) find_package(ament_cmake REQUIRED) ament_package() -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## -## Install URDF and meshes +# Install URDF and meshes install(DIRECTORY urdf DESTINATION share/moveit_resources_prbt_support) install(DIRECTORY meshes DESTINATION share/moveit_resources_prbt_support) install(DIRECTORY config DESTINATION share/moveit_resources_prbt_support) diff --git a/moveit_planners/test_configs/prbt_support/package.xml b/moveit_planners/test_configs/prbt_support/package.xml index e5d0a49d44..8cea882e64 100644 --- a/moveit_planners/test_configs/prbt_support/package.xml +++ b/moveit_planners/test_configs/prbt_support/package.xml @@ -1,6 +1,6 @@ moveit_resources_prbt_support - 2.9.0 + 2.10.0 Mechanical, kinematic and visual description of the Pilz light weight arm PRBT. @@ -12,8 +12,8 @@ Apache 2.0 http://moveit.ros.org - https://github.com/ros-planning/moveit-resources/issues - https://github.com/ros-planning/moveit-resources + https://github.com/moveit/moveit-resources/issues + https://github.com/moveit/moveit-resources ament_cmake diff --git a/moveit_plugins/moveit_plugins/CHANGELOG.rst b/moveit_plugins/moveit_plugins/CHANGELOG.rst index fc665eda48..673ff39ab6 100644 --- a/moveit_plugins/moveit_plugins/CHANGELOG.rst +++ b/moveit_plugins/moveit_plugins/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit_plugins/moveit_plugins/CMakeLists.txt b/moveit_plugins/moveit_plugins/CMakeLists.txt index 0bc40f0936..fcb9a6622e 100644 --- a/moveit_plugins/moveit_plugins/CMakeLists.txt +++ b/moveit_plugins/moveit_plugins/CMakeLists.txt @@ -2,9 +2,4 @@ cmake_minimum_required(VERSION 3.22) project(moveit_plugins) find_package(ament_cmake REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/moveit_plugins/moveit_plugins/package.xml b/moveit_plugins/moveit_plugins/package.xml index 142f3065d2..15027970ed 100644 --- a/moveit_plugins/moveit_plugins/package.xml +++ b/moveit_plugins/moveit_plugins/package.xml @@ -2,7 +2,7 @@ moveit_plugins - 2.9.0 + 2.10.0 Metapackage for MoveIt plugins. Henning Kayser @@ -24,9 +24,6 @@ moveit_ros_control_interface --> - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst index ce620f18f7..db67aaada4 100644 --- a/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst +++ b/moveit_plugins/moveit_ros_control_interface/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package moveit_ros_control_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Revert "Simplify controller manager namespacing (`#2210 `_)" + This reverts commit 55df0bccd5e884649780b4ceeee80891e563b57b. + The deprecated constructor was being used in the same file + for the exact use case of enabling namespaces that are not + specified by the parameter. There is no replacement for + supporting a dynamic server lookup, however the parameter + logic could still use simplification. +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Henning Kayser, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Fix warning and cleanup unneeded placeholders (`#2566 `_) diff --git a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt index 7a5725fec4..38a203fe27 100644 --- a/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt +++ b/moveit_plugins/moveit_ros_control_interface/CMakeLists.txt @@ -4,13 +4,8 @@ project(moveit_ros_control_interface LANGUAGES CXX) find_package(ament_cmake REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp_action - controller_manager_msgs - moveit_core - moveit_simple_controller_manager - pluginlib - trajectory_msgs -) + rclcpp_action controller_manager_msgs moveit_core + moveit_simple_controller_manager pluginlib trajectory_msgs) foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) find_package(${dependency} REQUIRED) @@ -24,89 +19,72 @@ moveit_package() include(ConfigExtras.cmake) add_library(moveit_ros_control_interface_plugin SHARED - src/controller_manager_plugin.cpp -) -set_target_properties(moveit_ros_control_interface_plugin PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}") + src/controller_manager_plugin.cpp) +set_target_properties( + moveit_ros_control_interface_plugin + PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}") target_include_directories(moveit_ros_control_interface_plugin PRIVATE include) ament_target_dependencies(moveit_ros_control_interface_plugin - ${THIS_PACKAGE_INCLUDE_DEPENDS} - Boost -) + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) add_library(moveit_ros_control_interface_trajectory_plugin SHARED - src/joint_trajectory_controller_plugin.cpp -) -set_target_properties(moveit_ros_control_interface_trajectory_plugin PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}") -target_include_directories(moveit_ros_control_interface_trajectory_plugin PRIVATE include) + src/joint_trajectory_controller_plugin.cpp) +set_target_properties( + moveit_ros_control_interface_trajectory_plugin + PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}") +target_include_directories(moveit_ros_control_interface_trajectory_plugin + PRIVATE include) ament_target_dependencies(moveit_ros_control_interface_trajectory_plugin - ${THIS_PACKAGE_INCLUDE_DEPENDS} - Boost -) + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) add_library(moveit_ros_control_interface_gripper_plugin SHARED - src/gripper_controller_plugin.cpp -) -set_target_properties(moveit_ros_control_interface_gripper_plugin PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}") -target_include_directories(moveit_ros_control_interface_gripper_plugin PRIVATE include) + src/gripper_controller_plugin.cpp) +set_target_properties( + moveit_ros_control_interface_gripper_plugin + PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}") +target_include_directories(moveit_ros_control_interface_gripper_plugin + PRIVATE include) ament_target_dependencies(moveit_ros_control_interface_gripper_plugin - ${THIS_PACKAGE_INCLUDE_DEPENDS} - Boost -) + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) add_library(moveit_ros_control_interface_empty_plugin SHARED - src/empty_controller_plugin.cpp -) -set_target_properties(moveit_ros_control_interface_empty_plugin PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}") -target_include_directories(moveit_ros_control_interface_empty_plugin PRIVATE include) + src/empty_controller_plugin.cpp) +set_target_properties( + moveit_ros_control_interface_empty_plugin + PROPERTIES VERSION "${moveit_ros_control_interface_VERSION}") +target_include_directories(moveit_ros_control_interface_empty_plugin + PRIVATE include) ament_target_dependencies(moveit_ros_control_interface_empty_plugin - ${THIS_PACKAGE_INCLUDE_DEPENDS} - Boost -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() -endif() - -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## set(TARGET_LIBRARIES - moveit_ros_control_interface_plugin - moveit_ros_control_interface_trajectory_plugin - moveit_ros_control_interface_gripper_plugin - moveit_ros_control_interface_empty_plugin -) - -## Mark executables and/or libraries for installation -install(TARGETS ${TARGET_LIBRARIES} + moveit_ros_control_interface_plugin + moveit_ros_control_interface_trajectory_plugin + moveit_ros_control_interface_gripper_plugin + moveit_ros_control_interface_empty_plugin) + +# Mark executables and/or libraries for installation +install( + TARGETS ${TARGET_LIBRARIES} EXPORT moveit_ros_control_interfaceTargets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_control_interface -) + INCLUDES + DESTINATION include/moveit_ros_control_interface) -## Mark cpp header files for installation -install(DIRECTORY include/ - DESTINATION include/moveit_ros_control_interface -) +# Mark cpp header files for installation +install(DIRECTORY include/ DESTINATION include/moveit_ros_control_interface) pluginlib_export_plugin_description_file(moveit_core moveit_core_plugins.xml) -pluginlib_export_plugin_description_file(moveit_ros_control_interface moveit_ros_control_interface_plugins.xml) +pluginlib_export_plugin_description_file( + moveit_ros_control_interface moveit_ros_control_interface_plugins.xml) ament_export_targets(moveit_ros_control_interfaceTargets HAS_LIBRARY_TARGET) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} - Boost -) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_plugins/moveit_ros_control_interface/package.xml b/moveit_plugins/moveit_ros_control_interface/package.xml index 49d6a4a22c..e8a28c550f 100644 --- a/moveit_plugins/moveit_ros_control_interface/package.xml +++ b/moveit_plugins/moveit_ros_control_interface/package.xml @@ -2,7 +2,7 @@ moveit_ros_control_interface - 2.9.0 + 2.10.0 ros_control controller manager interface for MoveIt Henning Kayser Tyler Weaver @@ -21,9 +21,6 @@ pluginlib trajectory_msgs - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp index 65d8310b2a..eb7b2e8205 100644 --- a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp +++ b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp @@ -65,7 +65,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ros_control_interface"); + return moveit::getLogger("moveit.plugins.ros_control_interface"); } } // namespace @@ -255,8 +255,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan * \brief Configure interface with namespace * @param ns namespace of ros_control node (without /controller_manager/) */ - [[deprecated("Ros2ControlManager constructor with namespace is deprecated. Set namespace via the " - "ros_control_namespace parameter.")]] Ros2ControlManager(const std::string& ns) + Ros2ControlManager(const std::string& ns) : ns_(ns), loader_("moveit_ros_control_interface", "moveit_ros_control_interface::ControllerHandleAllocator") { RCLCPP_INFO_STREAM(getLogger(), "Started moveit_ros_control_interface::Ros2ControlManager for namespace " << ns_); @@ -265,12 +264,18 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan void initialize(const rclcpp::Node::SharedPtr& node) override { node_ = node; - // Set the namespace from the ros_control_namespace parameter, or default to "/" - if (!node_->has_parameter("ros_control_namespace")) + if (!ns_.empty()) { - ns_ = node_->declare_parameter("ros_control_namespace", "/"); + if (!node_->has_parameter("ros_control_namespace")) + { + ns_ = node_->declare_parameter("ros_control_namespace", "/"); + } + else + { + node_->get_parameter("ros_control_namespace", ns_); + } } - else + else if (node->has_parameter("ros_control_namespace")) { node_->get_parameter("ros_control_namespace", ns_); RCLCPP_INFO_STREAM(getLogger(), "Namespace for controller manager was specified, namespace: " << ns_); diff --git a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst index 5ab67379be..f94faab05e 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst +++ b/moveit_plugins/moveit_simple_controller_manager/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package moveit_simple_controller_manager ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt index c51930686f..9905ae3238 100644 --- a/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt +++ b/moveit_plugins/moveit_simple_controller_manager/CMakeLists.txt @@ -15,29 +15,22 @@ find_package(rclcpp_action REQUIRED) # Finds Boost Components include(ConfigExtras.cmake) -set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - rclcpp - rclcpp_action - moveit_core - pluginlib -) +set(THIS_PACKAGE_INCLUDE_DEPENDS control_msgs rclcpp rclcpp_action moveit_core + pluginlib) -include_directories( - include -) +include_directories(include) -add_library(moveit_simple_controller_manager SHARED +add_library( + moveit_simple_controller_manager SHARED src/moveit_simple_controller_manager.cpp - src/follow_joint_trajectory_controller_handle.cpp -) + src/follow_joint_trajectory_controller_handle.cpp) -set_target_properties(moveit_simple_controller_manager PROPERTIES VERSION "${moveit_simple_controller_manager_VERSION}") +set_target_properties( + moveit_simple_controller_manager + PROPERTIES VERSION "${moveit_simple_controller_manager_VERSION}") ament_target_dependencies(moveit_simple_controller_manager - ${THIS_PACKAGE_INCLUDE_DEPENDS} - Boost -) + ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) install( TARGETS moveit_simple_controller_manager @@ -45,28 +38,14 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_simple_controller_manager -) + INCLUDES + DESTINATION include/moveit_simple_controller_manager) install(DIRECTORY include/ DESTINATION include/moveit_simple_controller_manager) ament_export_targets(moveit_simple_controller_managerTargets HAS_LIBRARY_TARGET) -ament_export_dependencies( - ${THIS_PACKAGE_INCLUDE_DEPENDS} - Boost -) +ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) -pluginlib_export_plugin_description_file(moveit_core moveit_simple_controller_manager_plugin_description.xml) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_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() -endif() +pluginlib_export_plugin_description_file( + moveit_core moveit_simple_controller_manager_plugin_description.xml) ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_plugins/moveit_simple_controller_manager/package.xml b/moveit_plugins/moveit_simple_controller_manager/package.xml index 1e9307f957..d11a5cda19 100644 --- a/moveit_plugins/moveit_simple_controller_manager/package.xml +++ b/moveit_plugins/moveit_simple_controller_manager/package.xml @@ -2,7 +2,7 @@ moveit_simple_controller_manager - 2.9.0 + 2.10.0 A generic, simple controller manager plugin for MoveIt. Michael Görner Henning Kayser @@ -12,8 +12,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Michael Ferguson @@ -26,9 +26,6 @@ control_msgs rclcpp_action - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp index acbfe27598..4d4cedb8f7 100644 --- a/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp +++ b/moveit_plugins/moveit_simple_controller_manager/src/moveit_simple_controller_manager.cpp @@ -87,7 +87,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_simple_controller_manager"); + return moveit::getLogger("moveit.plugins.simple_controller_manager"); } } // namespace static const std::string PARAM_BASE_NAME = "moveit_simple_controller_manager"; diff --git a/moveit_py/CHANGELOG.rst b/moveit_py/CHANGELOG.rst index 6f6ba0fa95..00211b554f 100644 --- a/moveit_py/CHANGELOG.rst +++ b/moveit_py/CHANGELOG.rst @@ -2,6 +2,23 @@ Changelog for package moveit_py ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* Get configuration values of traj_exec_man (`#2702 `_) + * (ros_planning) get configuration values of traj_exec_man + * (py) get configuration values of traj_exec_man +* CMake format and lint in pre-commit (`#2683 `_) +* log after rclcpp init +* Contributors: Henning Kayser, Matthijs van der Burgh, Robert Haschke, Sebastian Jahr, Tyler Weaver, peterdavidfagan + 2.9.0 (2024-01-09) ------------------ * [PSM] Process collision object color when adding object trough the planning scene monitor (`#2567 `_) diff --git a/moveit_py/CITATION.cff b/moveit_py/CITATION.cff index ab0a59f44b..61e387555b 100644 --- a/moveit_py/CITATION.cff +++ b/moveit_py/CITATION.cff @@ -5,4 +5,4 @@ authors: date-released: 2023-03-20 message: "If you use this software, please cite it as below." title: "MoveIt 2 Python Library: A Software Library for Robotics Education and Research" -url: "https://github.com/ros-planning/moveit2/tree/main/moveit_py" +url: "https://github.com/moveit/moveit2/tree/main/moveit_py" diff --git a/moveit_py/CMakeLists.txt b/moveit_py/CMakeLists.txt index 304babdf19..99aab735d7 100644 --- a/moveit_py/CMakeLists.txt +++ b/moveit_py/CMakeLists.txt @@ -20,76 +20,83 @@ add_subdirectory(src/moveit/moveit_py_utils) ament_python_install_package(moveit) # Set the build location and install location for a CPython extension -function(configure_build_install_location _library_name) - # Install into test_moveit folder in build space for unit tests to import - set_target_properties(${_library_name} PROPERTIES - # Use generator expression to avoid prepending a build type specific directory on Windows - LIBRARY_OUTPUT_DIRECTORY $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit> - RUNTIME_OUTPUT_DIRECTORY $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit>) +function(configure_build_install_location library_name) + # Install into test_moveit folder in build space for unit tests to import + set_target_properties( + ${library_name} + PROPERTIES # Use generator expression to avoid prepending a build type + # specific directory on Windows + LIBRARY_OUTPUT_DIRECTORY + $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit> + RUNTIME_OUTPUT_DIRECTORY + $<1:${CMAKE_CURRENT_BINARY_DIR}/test_moveit>) - install(TARGETS ${_library_name} - DESTINATION "${PYTHON_INSTALL_DIR}/moveit" - ) + install(TARGETS ${library_name} DESTINATION "${PYTHON_INSTALL_DIR}/moveit") endfunction() -pybind11_add_module(core - src/moveit/core.cpp - src/moveit/moveit_core/collision_detection/collision_common.cpp - src/moveit/moveit_core/collision_detection/collision_matrix.cpp - src/moveit/moveit_core/collision_detection/world.cpp - src/moveit/moveit_core/controller_manager/controller_manager.cpp - src/moveit/moveit_core/kinematic_constraints/utils.cpp - src/moveit/moveit_core/planning_interface/planning_response.cpp - src/moveit/moveit_core/planning_scene/planning_scene.cpp - src/moveit/moveit_core/transforms/transforms.cpp - src/moveit/moveit_core/robot_model/joint_model.cpp - src/moveit/moveit_core/robot_model/joint_model_group.cpp - src/moveit/moveit_core/robot_model/robot_model.cpp - src/moveit/moveit_core/robot_state/robot_state.cpp - src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp - ) -target_link_libraries(core PRIVATE moveit_ros_planning::moveit_cpp - rclcpp::rclcpp - moveit_core::moveit_transforms - moveit_core::moveit_kinematic_constraints - moveit_core::moveit_planning_interface - moveit_core::moveit_planning_scene - moveit_core::moveit_utils - moveit_core::moveit_robot_model - moveit_core::moveit_robot_state - moveit_py_utils) +pybind11_add_module( + core + src/moveit/core.cpp + src/moveit/moveit_core/collision_detection/collision_common.cpp + src/moveit/moveit_core/collision_detection/collision_matrix.cpp + src/moveit/moveit_core/collision_detection/world.cpp + src/moveit/moveit_core/controller_manager/controller_manager.cpp + src/moveit/moveit_core/kinematic_constraints/utils.cpp + src/moveit/moveit_core/planning_interface/planning_response.cpp + src/moveit/moveit_core/planning_scene/planning_scene.cpp + src/moveit/moveit_core/transforms/transforms.cpp + src/moveit/moveit_core/robot_model/joint_model.cpp + src/moveit/moveit_core/robot_model/joint_model_group.cpp + src/moveit/moveit_core/robot_model/robot_model.cpp + src/moveit/moveit_core/robot_state/robot_state.cpp + src/moveit/moveit_core/robot_trajectory/robot_trajectory.cpp) +target_link_libraries( + core + PRIVATE moveit_ros_planning::moveit_cpp + rclcpp::rclcpp + moveit_core::moveit_transforms + moveit_core::moveit_kinematic_constraints + moveit_core::moveit_planning_interface + moveit_core::moveit_planning_scene + moveit_core::moveit_utils + moveit_core::moveit_robot_model + moveit_core::moveit_robot_state + moveit_py_utils) configure_build_install_location(core) -pybind11_add_module(planning - src/moveit/planning.cpp - src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp - src/moveit/moveit_ros/moveit_cpp/planning_component.cpp - src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp - src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp -) -target_link_libraries(planning PRIVATE moveit_ros_planning::moveit_cpp - moveit_ros_planning::moveit_planning_scene_monitor - moveit_ros_planning::moveit_trajectory_execution_manager - moveit_core::moveit_utils - rclcpp::rclcpp - moveit_py_utils +pybind11_add_module( + planning + src/moveit/planning.cpp + src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp + src/moveit/moveit_ros/moveit_cpp/planning_component.cpp + src/moveit/moveit_ros/planning_scene_monitor/planning_scene_monitor.cpp + src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp ) +target_link_libraries( + planning + PRIVATE moveit_ros_planning::moveit_cpp + moveit_ros_planning::moveit_planning_scene_monitor + moveit_ros_planning::moveit_trajectory_execution_manager + moveit_core::moveit_utils + rclcpp::rclcpp + moveit_py_utils) configure_build_install_location(planning) - if(BUILD_TESTING) find_package(ament_cmake_pytest REQUIRED) - set(_pytest_tests - test/unit/test_robot_model.py - test/unit/test_robot_state.py - ) - foreach(_test_path ${_pytest_tests}) - get_filename_component(_test_name ${_test_path} NAME_WE) - ament_add_pytest_test(${_test_name} ${_test_path} - APPEND_ENV AMENT_PREFIX_INDEX=${ament_index_build_path} PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} - TIMEOUT 60 - WORKING_DIRECTORY "${CMAKE_SOURCE_DIR}" - ) + set(_pytest_tests test/unit/test_robot_model.py test/unit/test_robot_state.py) + foreach(test_path ${_pytest_tests}) + get_filename_component(_test_name ${test_path} NAME_WE) + ament_add_pytest_test( + ${_test_name} + ${test_path} + APPEND_ENV + AMENT_PREFIX_INDEX=${ament_index_build_path} + PYTHONPATH=${CMAKE_CURRENT_BINARY_DIR} + TIMEOUT + 60 + WORKING_DIRECTORY + "${CMAKE_SOURCE_DIR}") endforeach() endif() diff --git a/moveit_py/README.md b/moveit_py/README.md index 8156807910..e903761ee5 100644 --- a/moveit_py/README.md +++ b/moveit_py/README.md @@ -26,7 +26,7 @@ If you use this library in your work please use the following citation: @software{fagan2023moveitpy, author = {Fagan, Peter David}, title = {{MoveIt 2 Python Library: A Software Library for Robotics Education and Research}}, - url = {https://github.com/ros-planning/moveit2/tree/main/moveit_py}, + url = {https://github.com/moveit/moveit2/tree/main/moveit_py}, year = {2023} } ``` diff --git a/moveit_py/moveit/planning.pyi b/moveit_py/moveit/planning.pyi index e8adcfd8c2..09435678ff 100644 --- a/moveit_py/moveit/planning.pyi +++ b/moveit_py/moveit/planning.pyi @@ -68,6 +68,9 @@ class PlanningSceneMonitor: class TrajectoryExecutionManager: def __init__(self, *args, **kwargs) -> None: ... + def allowed_execution_duration_scaling(self, *args, **kwargs) -> Any: ... + def allowed_goal_duration_margin(self, *args, **kwargs) -> Any: ... + def allowed_start_tolerance(self, *args, **kwargs) -> Any: ... def are_controllers_active(self, *args, **kwargs) -> Any: ... def clear(self, *args, **kwargs) -> Any: ... def enable_execution_duration_monitoring(self, *args, **kwargs) -> Any: ... @@ -77,6 +80,8 @@ class TrajectoryExecutionManager: def ensure_active_controllers_for_joints(self, *args, **kwargs) -> Any: ... def execute(self, *args, **kwargs) -> Any: ... def execute_and_wait(self, *args, **kwargs) -> Any: ... + def execution_duration_monitoring(self, *args, **kwargs) -> Any: ... + def execution_velocity_scaling(self, *args, **kwargs) -> Any: ... def get_last_execution_status(self, *args, **kwargs) -> Any: ... def is_controller_active(self, *args, **kwargs) -> Any: ... def is_managing_controllers(self, *args, **kwargs) -> Any: ... @@ -89,3 +94,4 @@ class TrajectoryExecutionManager: def set_wait_for_trajectory_completion(self, *args, **kwargs) -> Any: ... def stop_execution(self, *args, **kwargs) -> Any: ... def wait_for_execution(self, *args, **kwargs) -> Any: ... + def wait_for_trajectory_completion(self, *args, **kwargs) -> Any: ... diff --git a/moveit_py/package.xml b/moveit_py/package.xml index f20bf5f2a5..cbc800cca0 100644 --- a/moveit_py/package.xml +++ b/moveit_py/package.xml @@ -2,7 +2,7 @@ moveit_py - 2.9.0 + 2.10.0 Python binding for MoveIt 2 Peter David Fagan diff --git a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h index e688705898..7ac1497b29 100644 --- a/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h +++ b/moveit_py/src/moveit/moveit_core/robot_state/robot_state.h @@ -38,7 +38,12 @@ #include #include +#pragma GCC diagnostic push +#if defined(__GNUC__) && !defined(__clang__) +#pragma GCC diagnostic ignored "-Wmaybe-uninitialized" +#endif #include +#pragma GCC diagnostic pop #include #include #include diff --git a/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt b/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt index f54faad6e4..132da7425c 100644 --- a/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt +++ b/moveit_py/src/moveit/moveit_py_utils/CMakeLists.txt @@ -1,12 +1,13 @@ add_library(moveit_py_utils SHARED src/copy_ros_msg.cpp src/ros_msg_typecasters.cpp) -target_include_directories(moveit_py_utils PUBLIC - $ - $ -) -set_target_properties(moveit_py_utils PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_include_directories( + moveit_py_utils PUBLIC $ + $) +set_target_properties(moveit_py_utils PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_py_utils rclcpp moveit_msgs geometry_msgs pybind11) +ament_target_dependencies(moveit_py_utils rclcpp moveit_msgs geometry_msgs + pybind11) install( TARGETS moveit_py_utils diff --git a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp index eaf8eee2fa..955e66d09d 100644 --- a/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp +++ b/moveit_py/src/moveit/moveit_ros/moveit_cpp/moveit_cpp.cpp @@ -43,7 +43,7 @@ namespace bind_moveit_cpp { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_cpp_initializer"); + return moveit::getLogger("moveit.py.cpp_initializer"); } std::shared_ptr @@ -87,7 +87,6 @@ void initMoveitPy(py::module& m) // Initialize ROS, pass launch arguments with rclcpp::init() if (!rclcpp::ok()) { - RCLCPP_INFO(getLogger(), "Initialize rclcpp"); std::vector chars; chars.reserve(launch_arguments.size()); for (const auto& arg : launch_arguments) @@ -96,6 +95,7 @@ void initMoveitPy(py::module& m) } rclcpp::init(launch_arguments.size(), chars.data()); + RCLCPP_INFO(getLogger(), "Initialize rclcpp"); } // Build NodeOptions diff --git a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp index 2cdfeb0356..3872d17665 100644 --- a/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp +++ b/moveit_py/src/moveit/moveit_ros/trajectory_execution_manager/trajectory_execution_manager.cpp @@ -153,7 +153,7 @@ void initTrajectoryExecutionManager(py::module& m) )") // ToDo(MatthijsBurgh) - // See https://github.com/ros-planning/moveit2/issues/2442 + // See https://github.com/moveit/moveit2/issues/2442 // get_trajectories .def("execute", py::overload_cast`_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Fix CI for Rolling / Ubuntu Noble (`#2793 `_) + * docker.yaml: Enable caching + * [TEMP] moveit2_rolling.repos: add not yet released packages + * Skip broken ci-testing image: osrf/ros2:testing doesn't contain /opt/ros! + * use boost::timer::progress_display if available + check for header to stay compatible with ubuntu 20.04. + Support boost >= 1.83 + Slightly ugly due to the double alias, but boost::timer was a class + before 1.72, so using `boost::timer::progress_display` in the code + breaks with older versions. + * cherry-pick of `#3547 `_ from MoveIt1 + * Tag ci image as ci-testing as well + --------- + Co-authored-by: Michael Görner + Co-authored-by: Sebastian Jahr + Co-authored-by: Henning Kayser +* Update deprecated include of boost/progress.hpp to boost/timer/progress_display.hpp (`#2811 `_) +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Sebastian Jahr, Stephanie Eng, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * [Planning Pipeline Refactoring] `#2 `_ Enable chaining planners (`#2457 `_) diff --git a/moveit_ros/benchmarks/CMakeLists.txt b/moveit_ros/benchmarks/CMakeLists.txt index fa1c4360d3..58538a70b9 100644 --- a/moveit_ros/benchmarks/CMakeLists.txt +++ b/moveit_ros/benchmarks/CMakeLists.txt @@ -19,32 +19,26 @@ include(ConfigExtras.cmake) include_directories(include) set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - Boost - tf2_eigen - moveit_core - moveit_ros_planning - moveit_ros_warehouse - pluginlib -) - -add_library(moveit_ros_benchmarks SHARED - src/BenchmarkOptions.cpp - src/BenchmarkExecutor.cpp -) -set_target_properties(moveit_ros_benchmarks PROPERTIES VERSION "${moveit_ros_benchmarks_VERSION}") + rclcpp + Boost + tf2_eigen + moveit_core + moveit_ros_planning + moveit_ros_warehouse + pluginlib) + +add_library(moveit_ros_benchmarks SHARED src/BenchmarkOptions.cpp + src/BenchmarkExecutor.cpp) +set_target_properties(moveit_ros_benchmarks + PROPERTIES VERSION "${moveit_ros_benchmarks_VERSION}") if(WIN32) set(EXTRA_LIB ws2_32.lib) endif() -ament_target_dependencies(moveit_ros_benchmarks - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) +ament_target_dependencies(moveit_ros_benchmarks ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_ros_benchmarks ${EXTRA_LIB}) add_executable(moveit_run_benchmark src/RunBenchmark.cpp) -ament_target_dependencies(moveit_run_benchmark - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) +ament_target_dependencies(moveit_run_benchmark ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_run_benchmark moveit_ros_benchmarks) install( @@ -53,38 +47,17 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_benchmarks -) + INCLUDES + DESTINATION include/moveit_ros_benchmarks) ament_export_targets(moveit_ros_benchmarksTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -install( - TARGETS - moveit_run_benchmark - DESTINATION lib/moveit_ros_benchmarks -) +install(TARGETS moveit_run_benchmark DESTINATION lib/moveit_ros_benchmarks) -install(DIRECTORY include/ - DESTINATION include/moveit_ros_benchmarks -) +install(DIRECTORY include/ DESTINATION include/moveit_ros_benchmarks) install(PROGRAMS scripts/moveit_benchmark_statistics.py - DESTINATION lib/moveit_ros_benchmarks -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) - set(ament_cmake_pep257_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() -endif() + DESTINATION lib/moveit_ros_benchmarks) ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_ros/benchmarks/README.md b/moveit_ros/benchmarks/README.md index 4823b68253..3201e2cf08 100644 --- a/moveit_ros/benchmarks/README.md +++ b/moveit_ros/benchmarks/README.md @@ -2,4 +2,4 @@ This package provides methods to benchmark motion planning algorithms and aggregate/plot statistics. Results can be viewed in [Planner Arena](http://plannerarena.org/). -For more information and usage example please see [moveit tutorials](https://ros-planning.github.io/moveit_tutorials/doc/benchmarking/benchmarking_tutorial.html). +For more information and usage example please see [moveit tutorials](https://moveit.github.io/moveit_tutorials/doc/benchmarking/benchmarking_tutorial.html). diff --git a/moveit_ros/benchmarks/package.xml b/moveit_ros/benchmarks/package.xml index 1c6cf7e014..ce22554a6f 100644 --- a/moveit_ros/benchmarks/package.xml +++ b/moveit_ros/benchmarks/package.xml @@ -2,7 +2,7 @@ moveit_ros_benchmarks - 2.9.0 + 2.10.0 Enhanced tools for benchmarks in MoveIt Henning Kayser Tyler Weaver @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ryan Luna @@ -35,9 +35,6 @@ moveit_configs_utils launch_param_builder - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp index f108fec0db..252d2a98d6 100644 --- a/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkExecutor.cpp @@ -43,13 +43,21 @@ #include #include -// TODO(henningkayser): Switch to boost/timer/progress_display.hpp with Boost 1.72 -// boost/progress.hpp is deprecated and will be replaced by boost/timer/progress_display.hpp in Boost 1.72. -// Until then we need to suppress the deprecation warning. -#define BOOST_ALLOW_DEPRECATED_HEADERS #include + +#if __has_include() +#include +using boost_progress_display = boost::timer::progress_display; +#else +// boost < 1.72 +#define BOOST_TIMER_ENABLE_DEPRECATED 1 #include -#undef BOOST_ALLOW_DEPRECATED_HEADERS +#undef BOOST_TIMER_ENABLE_DEPRECATED +using boost_progress_display = boost::progress_display; +#endif + +#include +#include #include #include #include @@ -68,7 +76,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("benchmark_executor"); + return moveit::getLogger("moveit.benchmarks.executor"); } } // namespace @@ -776,7 +784,7 @@ void BenchmarkExecutor::runBenchmark(moveit_msgs::msg::MotionPlanRequest request } num_planners += options.parallel_planning_pipelines.size(); - boost::progress_display progress(num_planners * options.runs, std::cout); + boost_progress_display progress(num_planners * options.runs, std::cout); // Iterate through all planning pipelines auto planning_pipelines = moveit_cpp_->getPlanningPipelines(); diff --git a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp index 54ca8fee0f..c27f3c9cc1 100644 --- a/moveit_ros/benchmarks/src/BenchmarkOptions.cpp +++ b/moveit_ros/benchmarks/src/BenchmarkOptions.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("benchmark_options"); + return moveit::getLogger("moveit.benchmarks.options"); } } // namespace diff --git a/moveit_ros/hybrid_planning/CHANGELOG.rst b/moveit_ros/hybrid_planning/CHANGELOG.rst index 8a85416307..291384011e 100644 --- a/moveit_ros/hybrid_planning/CHANGELOG.rst +++ b/moveit_ros/hybrid_planning/CHANGELOG.rst @@ -2,6 +2,21 @@ Changelog for package moveit_hybrid_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Remove HP error codes interface (`#2774 `_) +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Clean-up hybrid planning package (`#2603 `_) +* Contributors: Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Update ros2_control usage (`#2620 `_) diff --git a/moveit_ros/hybrid_planning/CMakeLists.txt b/moveit_ros/hybrid_planning/CMakeLists.txt index 6c90110a96..0f90c56033 100644 --- a/moveit_ros/hybrid_planning/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/CMakeLists.txt @@ -7,6 +7,7 @@ moveit_package() # find dependencies find_package(ament_cmake REQUIRED) +find_package(generate_parameter_library REQUIRED) find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) find_package(moveit_ros_planning REQUIRED) @@ -21,42 +22,42 @@ find_package(tf2_ros REQUIRED) find_package(trajectory_msgs REQUIRED) set(LIBRARIES - # Components - moveit_global_planner_component - moveit_hybrid_planning_manager - moveit_local_planner_component - # Plugins - forward_trajectory_plugin - motion_planning_pipeline_plugin - replan_invalidated_trajectory_plugin - simple_sampler_plugin - single_plan_execution_plugin -) + # Components + moveit_global_planner_component + moveit_hybrid_planning_manager + moveit_local_planner_component + # Plugins + forward_trajectory_plugin + motion_planning_pipeline_plugin + replan_invalidated_trajectory_plugin + simple_sampler_plugin + single_plan_execution_plugin + # Parameters + local_planner_parameters + hp_manager_parameters) set(THIS_PACKAGE_INCLUDE_DEPENDS - moveit_core - moveit_msgs - moveit_ros_planning - moveit_ros_planning_interface - pluginlib - rclcpp - rclcpp_action - rclcpp_components - std_msgs - std_srvs - tf2_ros - trajectory_msgs -) + moveit_core + moveit_msgs + moveit_ros_planning + moveit_ros_planning_interface + pluginlib + rclcpp + rclcpp_action + rclcpp_components + std_msgs + std_srvs + tf2_ros + trajectory_msgs) set(THIS_PACKAGE_INCLUDE_DIRS - global_planner/global_planner_component/include/ - global_planner/global_planner_plugins/include/ - hybrid_planning_manager/hybrid_planning_manager_component/include/ - hybrid_planning_manager/planner_logic_plugins/include/ - local_planner/local_planner_component/include/ - local_planner/trajectory_operator_plugins/include/ - local_planner/local_constraint_solver_plugins/include/ -) + global_planner/global_planner_component/include/ + global_planner/global_planner_plugins/include/ + hybrid_planning_manager/hybrid_planning_manager_component/include/ + hybrid_planning_manager/planner_logic_plugins/include/ + local_planner/local_planner_component/include/ + local_planner/trajectory_operator_plugins/include/ + local_planner/local_constraint_solver_plugins/include/) include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) add_subdirectory(hybrid_planning_manager) @@ -64,33 +65,49 @@ add_subdirectory(global_planner) add_subdirectory(local_planner) add_subdirectory(test) -rclcpp_components_register_nodes(moveit_hybrid_planning_manager "moveit::hybrid_planning::HybridPlanningManager") -rclcpp_components_register_nodes(moveit_global_planner_component "moveit::hybrid_planning::GlobalPlannerComponent") -rclcpp_components_register_nodes(moveit_local_planner_component "moveit::hybrid_planning::LocalPlannerComponent") +rclcpp_components_register_nodes( + moveit_hybrid_planning_manager + "moveit::hybrid_planning::HybridPlanningManager") +rclcpp_components_register_nodes( + moveit_global_planner_component + "moveit::hybrid_planning::GlobalPlannerComponent") +rclcpp_components_register_nodes( + moveit_local_planner_component + "moveit::hybrid_planning::LocalPlannerComponent") -install(TARGETS ${LIBRARIES} +install( + TARGETS ${LIBRARIES} EXPORT moveit_hybrid_planningTargets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_hybrid_planning) + INCLUDES + DESTINATION include/moveit_hybrid_planning) -install(TARGETS cancel_action hybrid_planning_demo_node +install( + TARGETS cancel_action ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/moveit_hybrid_planning - INCLUDES DESTINATION include/moveit_hybrid_planning) + INCLUDES + DESTINATION include/moveit_hybrid_planning) -install(DIRECTORY ${THIS_PACKAGE_INCLUDE_DIRS} DESTINATION include/moveit_hybrid_planning) +install(DIRECTORY ${THIS_PACKAGE_INCLUDE_DIRS} + DESTINATION include/moveit_hybrid_planning) install(DIRECTORY test/launch DESTINATION share/moveit_hybrid_planning) install(DIRECTORY test/config DESTINATION share/moveit_hybrid_planning) -pluginlib_export_plugin_description_file(moveit_hybrid_planning single_plan_execution_plugin.xml) -pluginlib_export_plugin_description_file(moveit_hybrid_planning moveit_planning_pipeline_plugin.xml) -pluginlib_export_plugin_description_file(moveit_hybrid_planning simple_sampler_plugin.xml) -pluginlib_export_plugin_description_file(moveit_hybrid_planning replan_invalidated_trajectory_plugin.xml) -pluginlib_export_plugin_description_file(moveit_hybrid_planning forward_trajectory_plugin.xml) +pluginlib_export_plugin_description_file(moveit_hybrid_planning + single_plan_execution_plugin.xml) +pluginlib_export_plugin_description_file(moveit_hybrid_planning + moveit_planning_pipeline_plugin.xml) +pluginlib_export_plugin_description_file(moveit_hybrid_planning + simple_sampler_plugin.xml) +pluginlib_export_plugin_description_file( + moveit_hybrid_planning replan_invalidated_trajectory_plugin.xml) +pluginlib_export_plugin_description_file(moveit_hybrid_planning + forward_trajectory_plugin.xml) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) ament_export_targets(moveit_hybrid_planningTargets HAS_LIBRARY_TARGET) diff --git a/moveit_ros/hybrid_planning/README.md b/moveit_ros/hybrid_planning/README.md index defb1475c7..e31a4c5cee 100644 --- a/moveit_ros/hybrid_planning/README.md +++ b/moveit_ros/hybrid_planning/README.md @@ -1,5 +1,5 @@ # Hybrid Planning -A Hybrid Planning architecture. You can find more information in the project's issues[#300](https://github.com/ros-planning/moveit2/issues/300), [#433](https://github.com/ros-planning/moveit2/issues/433) and on the [MoveIt 2 roadmap](https://moveit.ros.org/documentation/contributing/roadmap/). Furthermore, there is an extensive tutorial available [here](https://github.com/ros-planning/moveit2_tutorials/pull/97). +A Hybrid Planning architecture. You can find more information in the project's issues[#300](https://github.com/moveit/moveit2/issues/300), [#433](https://github.com/moveit/moveit2/issues/433) and on the [MoveIt 2 roadmap](https://moveit.ros.org/documentation/contributing/roadmap/). Furthermore, there is an extensive tutorial available [here](https://github.com/moveit/moveit2_tutorials/pull/97). ## Getting started To start the demo run: diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/CMakeLists.txt b/moveit_ros/hybrid_planning/global_planner/global_planner_component/CMakeLists.txt index e39676fd67..8fea54c86a 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/CMakeLists.txt @@ -1,6 +1,7 @@ # Global planner component add_library(moveit_global_planner_component SHARED - src/global_planner_component.cpp -) -set_target_properties(moveit_global_planner_component PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_global_planner_component ${THIS_PACKAGE_INCLUDE_DEPENDS}) + src/global_planner_component.cpp) +set_target_properties(moveit_global_planner_component + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_global_planner_component + ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp index 804bc9bbdb..33b86f8325 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_component/src/global_planner_component.cpp @@ -62,17 +62,9 @@ GlobalPlannerComponent::GlobalPlannerComponent(const rclcpp::NodeOptions& option bool GlobalPlannerComponent::initializeGlobalPlanner() { // Initialize global planning request action server - std::string global_planning_action_name = ""; - node_->declare_parameter("global_planning_action_name", ""); - node_->get_parameter("global_planning_action_name", global_planning_action_name); - if (global_planning_action_name.empty()) - { - RCLCPP_ERROR(node_->get_logger(), "global_planning_action_name was not defined"); - return false; - } cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); global_planning_request_server_ = rclcpp_action::create_server( - node_, global_planning_action_name, + node_, "global_planning_action", // Goal callback [this](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr& /*unused*/) { diff --git a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/CMakeLists.txt index 05fba889c9..5537737681 100644 --- a/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/global_planner/global_planner_plugins/CMakeLists.txt @@ -1,5 +1,6 @@ add_library(motion_planning_pipeline_plugin SHARED - src/moveit_planning_pipeline.cpp -) -set_target_properties(motion_planning_pipeline_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(motion_planning_pipeline_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) + src/moveit_planning_pipeline.cpp) +set_target_properties(motion_planning_pipeline_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(motion_planning_pipeline_plugin + ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt index c3948543bc..60aa2980b6 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/CMakeLists.txt @@ -1,4 +1,9 @@ # Hybrid planning manager component -add_library(moveit_hybrid_planning_manager SHARED src/hybrid_planning_manager.cpp) -set_target_properties(moveit_hybrid_planning_manager PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_hybrid_planning_manager ${THIS_PACKAGE_INCLUDE_DEPENDS}) +generate_parameter_library(hp_manager_parameters res/hp_manager_parameters.yaml) +add_library(moveit_hybrid_planning_manager SHARED + src/hybrid_planning_manager.cpp) +set_target_properties(moveit_hybrid_planning_manager + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_hybrid_planning_manager hp_manager_parameters) +ament_target_dependencies(moveit_hybrid_planning_manager + ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h index 4deeebdee1..3336370d4d 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/hybrid_planning_events.h @@ -50,13 +50,27 @@ enum class HybridPlanningEvent GLOBAL_PLANNING_ACTION_SUCCESSFUL, GLOBAL_PLANNING_ACTION_ABORTED, GLOBAL_PLANNING_ACTION_CANCELED, + GLOBAL_PLANNING_ACTION_REJECTED, // Indicates that the global planner found a solution (This solution is not necessarily the last or best solution) GLOBAL_SOLUTION_AVAILABLE, // Result of the local planning action LOCAL_PLANNING_ACTION_SUCCESSFUL, LOCAL_PLANNING_ACTION_ABORTED, LOCAL_PLANNING_ACTION_CANCELED, + LOCAL_PLANNING_ACTION_REJECTED, // Undefined event to allow empty reaction events to indicate failure UNDEFINED }; + +/** + * Enum class HybridPlanningAction - This class defines the basic actions that the HP manager can perform + */ +enum class HybridPlanningAction +{ + DO_NOTHING, + RETURN_HP_SUCCESS, + RETURN_HP_FAILURE, + SEND_GLOBAL_SOLVER_REQUEST, + SEND_LOCAL_SOLVER_REQUEST +}; } // namespace moveit::hybrid_planning 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 025f95524a..9944622202 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 @@ -54,14 +54,14 @@ namespace moveit::hybrid_planning /** * Class HybridPlanningManager - ROS 2 component node that implements the hybrid planning manager. */ -class HybridPlanningManager : public rclcpp::Node +class HybridPlanningManager { public: /** \brief Constructor */ HybridPlanningManager(const rclcpp::NodeOptions& options); /** \brief Destructor */ - ~HybridPlanningManager() override + ~HybridPlanningManager() { // Join the thread used for long-running callbacks if (long_callback_thread_.joinable()) @@ -70,21 +70,19 @@ class HybridPlanningManager : public rclcpp::Node } } - /** - * Allows creation of a smart pointer that references to instances of this object - * @return shared pointer of the HybridPlanningManager instance that called the function - */ - std::shared_ptr shared_from_this() - { - return std::static_pointer_cast(Node::shared_from_this()); - } - /** * Load and initialized planner logic plugin and ROS 2 action and topic interfaces * @return Initialization successful yes/no */ bool initialize(); + // This function is required to make this class a valid NodeClass + // see https://docs.ros2.org/latest/api/rclcpp_components/register__node__macro_8hpp.html + rclcpp::node_interfaces::NodeBaseInterface::SharedPtr get_node_base_interface() // NOLINT + { + return node_->get_node_base_interface(); // NOLINT + } + /** * Cancel any active action goals, including global and local planners */ @@ -115,19 +113,22 @@ class HybridPlanningManager : public rclcpp::Node */ void sendHybridPlanningResponse(bool success); + /** + * @brief Process the action result and do an action call if necessary + * + * @param result Result to an event + */ + void processReactionResult(const ReactionResult& result); + private: + std::shared_ptr node_; + // Planner logic plugin loader std::unique_ptr> planner_logic_plugin_loader_; // Planner logic instance to implement reactive behavior std::shared_ptr planner_logic_instance_; - // Timer to trigger events periodically - rclcpp::TimerBase::SharedPtr timer_; - - // Flag that indicates whether the manager is initialized - bool initialized_; - // Flag that indicates hybrid planning has been canceled std::atomic stop_hybrid_planning_; diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h index 971b4858eb..4b10cea752 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/planner_logic_interface.h @@ -40,16 +40,17 @@ #include #include +#include #include -#include namespace moveit::hybrid_planning { // Describes the outcome of a reaction to an event in the hybrid planning architecture struct ReactionResult { - ReactionResult(const HybridPlanningEvent& planning_event, const std::string& error_msg, int error_code) - : error_message(error_msg), error_code(error_code) + ReactionResult(const HybridPlanningEvent& planning_event, const std::string& error_msg, const int error_code, + const HybridPlanningAction& action = HybridPlanningAction::DO_NOTHING) + : error_message(error_msg), error_code(error_code), action(action) { switch (planning_event) { @@ -79,10 +80,18 @@ struct ReactionResult break; case HybridPlanningEvent::UNDEFINED: event = "Undefined event"; + break; + case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_REJECTED: + event = "Global planning action rejected"; + break; + case HybridPlanningEvent::LOCAL_PLANNING_ACTION_REJECTED: + event = "Local planning action rejected"; + break; } }; - ReactionResult(const std::string& event, const std::string& error_msg, int error_code) - : event(event), error_message(error_msg), error_code(error_code){}; + ReactionResult(const std::string& event, const std::string& error_msg, const int error_code, + const HybridPlanningAction& action = HybridPlanningAction::DO_NOTHING) + : event(event), error_message(error_msg), error_code(error_code), action(action){}; // Event that triggered the reaction std::string event; @@ -91,10 +100,11 @@ struct ReactionResult std::string error_message; // Error code - MoveItErrorCode error_code; -}; + moveit::core::MoveItErrorCode error_code; -class HybridPlanningManager; // Forward declaration + // Action to that needs to be performed by the HP manager + HybridPlanningAction action; +}; /** * Class PlannerLogicInterface - Base class for a planner logic. The logic defines how to react to different events that @@ -113,10 +123,12 @@ class PlannerLogicInterface /** * Initialize the planner logic - * @param hybrid_planning_manager The hybrid planning manager instance to initialize this logic with. * @return true if initialization was successful */ - virtual bool initialize(const std::shared_ptr& hybrid_planning_manager) = 0; + virtual bool initialize() + { + return true; + }; /** * React to event defined in HybridPlanningEvent enum @@ -131,9 +143,5 @@ class PlannerLogicInterface * @return Reaction result that summarizes the outcome of the reaction */ virtual ReactionResult react(const std::string& event) = 0; - -protected: - // The hybrid planning manager instance that runs this logic plugin - std::shared_ptr hybrid_planning_manager_ = nullptr; }; } // namespace moveit::hybrid_planning diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/res/hp_manager_parameters.yaml b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/res/hp_manager_parameters.yaml new file mode 100644 index 0000000000..d76d1b52f2 --- /dev/null +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/res/hp_manager_parameters.yaml @@ -0,0 +1,6 @@ +hp_manager_parameters: + planner_logic_plugin_name: { + type: string, + description: "Name of the planner logic plugin", + default_value: "moveit::hybrid_planning/ReplanInvalidatedTrajectory", + } 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 7b6044d6fe..b069b9a19c 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 @@ -34,35 +34,23 @@ #include #include +#include +#include namespace moveit::hybrid_planning { using namespace std::chrono_literals; -HybridPlanningManager::HybridPlanningManager(const rclcpp::NodeOptions& options) - : Node("hybrid_planning_manager", options), initialized_(false), stop_hybrid_planning_(false) +namespace { - // Initialize hybrid planning component after construction - // TODO(sjahr) Remove once life cycle component nodes are available - timer_ = create_wall_timer(1ms, [this]() { - if (initialized_) - { - timer_->cancel(); - } - else - { - if (!initialize()) - { - const std::string error = "Failed to initialize global planner"; - timer_->cancel(); - throw std::runtime_error(error); - } - initialized_ = true; - } - }); +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.hybrid_planning.manager"); } +} // namespace -bool HybridPlanningManager::initialize() +HybridPlanningManager::HybridPlanningManager(const rclcpp::NodeOptions& options) + : node_{ std::make_shared("hybrid_planning_manager", options) }, stop_hybrid_planning_(false) { // Load planning logic plugin try @@ -72,91 +60,63 @@ bool HybridPlanningManager::initialize() } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(get_logger(), "Exception while creating planner logic plugin loader '%s'", ex.what()); - } - // TODO(sjahr) Refactor parameter declaration and use repository wide solution - std::string logic_plugin_name = ""; - if (has_parameter("planner_logic_plugin_name")) - { - get_parameter("planner_logic_plugin_name", logic_plugin_name); - } - else - { - logic_plugin_name = declare_parameter("planner_logic_plugin_name", - "moveit::hybrid_planning/ReplanInvalidatedTrajectory"); + RCLCPP_ERROR(getLogger(), "Exception while creating planner logic plugin loader '%s'", ex.what()); } + + auto param_listener = hp_manager_parameters::ParamListener(node_, ""); + const auto params = param_listener.get_params(); try { - planner_logic_instance_ = planner_logic_plugin_loader_->createUniqueInstance(logic_plugin_name); - if (!planner_logic_instance_->initialize(HybridPlanningManager::shared_from_this())) + planner_logic_instance_ = planner_logic_plugin_loader_->createUniqueInstance(params.planner_logic_plugin_name); + if (!planner_logic_instance_->initialize()) { throw std::runtime_error("Unable to initialize planner logic plugin"); } - RCLCPP_INFO(get_logger(), "Using planner logic interface '%s'", logic_plugin_name.c_str()); + RCLCPP_INFO(getLogger(), "Using planner logic interface '%s'", params.planner_logic_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { - RCLCPP_ERROR(get_logger(), "Exception while loading planner logic '%s': '%s'", logic_plugin_name.c_str(), ex.what()); + RCLCPP_ERROR(getLogger(), "Exception while loading planner logic '%s': '%s'", + params.planner_logic_plugin_name.c_str(), ex.what()); + return; } // Initialize local planning action client - std::string local_planning_action_name = declare_parameter("local_planning_action_name", ""); - get_parameter("local_planning_action_name", local_planning_action_name); - if (local_planning_action_name.empty()) - { - RCLCPP_ERROR(get_logger(), "local_planning_action_name parameter was not defined"); - return false; - } local_planner_action_client_ = - rclcpp_action::create_client(this, local_planning_action_name); + rclcpp_action::create_client(node_, "local_planning_action"); if (!local_planner_action_client_->wait_for_action_server(2s)) { - RCLCPP_ERROR(get_logger(), "Local planner action server not available after waiting"); - return false; + RCLCPP_ERROR(getLogger(), "Local planner action server not available after waiting"); + return; } // Initialize global planning action client - std::string global_planning_action_name = declare_parameter("global_planning_action_name", ""); - get_parameter("global_planning_action_name", global_planning_action_name); - if (global_planning_action_name.empty()) - { - RCLCPP_ERROR(get_logger(), "global_planning_action_name parameter was not defined"); - return false; - } global_planner_action_client_ = - rclcpp_action::create_client(this, global_planning_action_name); + rclcpp_action::create_client(node_, "global_planning_action"); if (!global_planner_action_client_->wait_for_action_server(2s)) { - RCLCPP_ERROR(get_logger(), "Global planner action server not available after waiting"); - return false; + RCLCPP_ERROR(getLogger(), "Global planner action server not available after waiting"); + return; } // Initialize hybrid planning action server - std::string hybrid_planning_action_name = declare_parameter("hybrid_planning_action_name", ""); - get_parameter("hybrid_planning_action_name", hybrid_planning_action_name); - if (hybrid_planning_action_name.empty()) - { - RCLCPP_ERROR(get_logger(), "hybrid_planning_action_name parameter was not defined"); - return false; - } - cb_group_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); hybrid_planning_request_server_ = rclcpp_action::create_server( - get_node_base_interface(), get_node_clock_interface(), get_node_logging_interface(), - get_node_waitables_interface(), hybrid_planning_action_name, + node_, "run_hybrid_planning", // Goal callback - [this](const rclcpp_action::GoalUUID& /*unused*/, - const std::shared_ptr& /*unused*/) { - RCLCPP_INFO(get_logger(), "Received goal request"); + [](const rclcpp_action::GoalUUID& /*unused*/, + const std::shared_ptr& /*unused*/) { + RCLCPP_INFO(getLogger(), "Received goal request"); return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE; }, // Cancel callback - [this](const std::shared_ptr>& /*unused*/) { + [&](const std::shared_ptr>& /*unused*/) { cancelHybridManagerGoals(); - RCLCPP_INFO(get_logger(), "Received request to cancel goal"); + RCLCPP_INFO(getLogger(), "Received request to cancel goal"); return rclcpp_action::CancelResponse::ACCEPT; }, // Execution callback - [this](const std::shared_ptr>& goal_handle) { + [&](const std::shared_ptr>& goal_handle) { // this needs to return quickly to avoid blocking the executor, so spin up a new thread if (long_callback_thread_.joinable()) { @@ -167,21 +127,12 @@ bool HybridPlanningManager::initialize() rcl_action_server_get_default_options(), cb_group_); // Initialize global solution subscriber - global_solution_sub_ = create_subscription( + global_solution_sub_ = node_->create_subscription( "global_trajectory", rclcpp::SystemDefaultsQoS(), - [this](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& /* unused */) { + [&](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& /* unused */) { // react is defined in a hybrid_planning_manager plugin - ReactionResult reaction_result = planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE); - 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(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); - } + processReactionResult(planner_logic_instance_->react(HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE)); }); - return true; } void HybridPlanningManager::cancelHybridManagerGoals() noexcept @@ -208,16 +159,7 @@ void HybridPlanningManager::executeHybridPlannerGoal( 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(get_logger(), "Hybrid Planning Manager failed to react to " << reaction_result.event); - } + processReactionResult(planner_logic_instance_->react(HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED)); } bool HybridPlanningManager::sendGlobalPlannerAction() @@ -226,7 +168,7 @@ bool HybridPlanningManager::sendGlobalPlannerAction() // Add goal response callback global_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [&](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { auto planning_progress = std::make_shared(); auto& feedback = planning_progress->feedback; if (!goal_handle) @@ -241,7 +183,7 @@ bool HybridPlanningManager::sendGlobalPlannerAction() }; // Add result callback global_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult& global_result) { + [&](const rclcpp_action::ClientGoalHandle::WrappedResult& global_result) { // Reaction result from the latest event ReactionResult reaction_result = ReactionResult(HybridPlanningEvent::UNDEFINED, "", moveit_msgs::msg::MoveItErrorCodes::FAILURE); @@ -260,15 +202,7 @@ bool HybridPlanningManager::sendGlobalPlannerAction() break; } // 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(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); - } + processReactionResult(reaction_result); }; // Forward global trajectory goal from hybrid planning request TODO(sjahr) pass goal as function argument @@ -296,7 +230,7 @@ bool HybridPlanningManager::sendLocalPlannerAction() // Add goal response callback local_goal_options.goal_response_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { + [&](const rclcpp_action::ClientGoalHandle::SharedPtr& goal_handle) { auto planning_progress = std::make_shared(); auto& feedback = planning_progress->feedback; if (!goal_handle) @@ -312,23 +246,14 @@ bool HybridPlanningManager::sendLocalPlannerAction() // Add feedback callback local_goal_options.feedback_callback = - [this](const rclcpp_action::ClientGoalHandle::SharedPtr& /*unused*/, - const std::shared_ptr& local_planner_feedback) { - // react is defined in a hybrid_planning_manager plugin - ReactionResult reaction_result = planner_logic_instance_->react(local_planner_feedback->feedback); - 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(get_logger(), "Hybrid Planning Manager failed to react to '%s'", reaction_result.event.c_str()); - } + [&](const rclcpp_action::ClientGoalHandle::SharedPtr& /*unused*/, + const std::shared_ptr& local_planner_feedback) { + processReactionResult(planner_logic_instance_->react(local_planner_feedback->feedback)); }; // Add result callback to print the result local_goal_options.result_callback = - [this](const rclcpp_action::ClientGoalHandle::WrappedResult& local_result) { + [&](const rclcpp_action::ClientGoalHandle::WrappedResult& local_result) { // Reaction result from the latest event ReactionResult reaction_result = ReactionResult(HybridPlanningEvent::UNDEFINED, "", moveit_msgs::msg::MoveItErrorCodes::FAILURE); @@ -346,18 +271,7 @@ bool HybridPlanningManager::sendLocalPlannerAction() default: break; } - // Abort hybrid planning if reaction fails - if (reaction_result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) - { - 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(get_logger(), "Hybrid Planning Manager failed to react to " << reaction_result.event); - } + processReactionResult(reaction_result); }; if (stop_hybrid_planning_) @@ -385,6 +299,40 @@ void HybridPlanningManager::sendHybridPlanningResponse(bool success) hybrid_planning_goal_handle_->abort(result); } } + +void HybridPlanningManager::processReactionResult(const ReactionResult& result) +{ + if (result.error_code.val != moveit_msgs::msg::MoveItErrorCodes::SUCCESS) + { + auto hp_result = std::make_shared(); + hp_result->error_code.val = result.error_code.val; + hp_result->error_message = result.error_message; + hybrid_planning_goal_handle_->abort(hp_result); + RCLCPP_ERROR(getLogger(), "Hybrid Planning Manager failed to react to '%s'", result.event.c_str()); + return; + } + + switch (result.action) + { + case HybridPlanningAction::DO_NOTHING: + // Do nothing + break; + case HybridPlanningAction::RETURN_HP_SUCCESS: + sendHybridPlanningResponse(true); + break; + case HybridPlanningAction::RETURN_HP_FAILURE: + sendHybridPlanningResponse(false); + break; + case HybridPlanningAction::SEND_GLOBAL_SOLVER_REQUEST: + sendGlobalPlannerAction(); + break; + case HybridPlanningAction::SEND_LOCAL_SOLVER_REQUEST: + sendLocalPlannerAction(); + break; + default: + RCLCPP_ERROR(getLogger(), "Unknown reaction result code. No actions taken by the hybrid planning manager."); + } +} } // namespace moveit::hybrid_planning // Register the component with class_loader diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/CMakeLists.txt index 1e103bb714..17cb49aa7c 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/CMakeLists.txt @@ -1,13 +1,17 @@ -add_library(single_plan_execution_plugin SHARED - src/single_plan_execution.cpp -) -set_target_properties(single_plan_execution_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(single_plan_execution_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(single_plan_execution_plugin moveit_hybrid_planning_manager) +add_library(single_plan_execution_plugin SHARED src/single_plan_execution.cpp) +set_target_properties(single_plan_execution_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(single_plan_execution_plugin + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(single_plan_execution_plugin + moveit_hybrid_planning_manager) add_library(replan_invalidated_trajectory_plugin SHARED - src/replan_invalidated_trajectory.cpp -) -set_target_properties(replan_invalidated_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(replan_invalidated_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(replan_invalidated_trajectory_plugin moveit_hybrid_planning_manager single_plan_execution_plugin) + src/replan_invalidated_trajectory.cpp) +set_target_properties(replan_invalidated_trajectory_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(replan_invalidated_trajectory_plugin + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries( + replan_invalidated_trajectory_plugin moveit_hybrid_planning_manager + single_plan_execution_plugin) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h index 62b6e713a6..3378db79f0 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/include/moveit/planner_logic_plugins/single_plan_execution.h @@ -47,7 +47,6 @@ class SinglePlanExecution : public PlannerLogicInterface public: SinglePlanExecution() = default; ~SinglePlanExecution() override = default; - bool initialize(const std::shared_ptr& hybrid_planning_manager) override; ReactionResult react(const HybridPlanningEvent& event) override; ReactionResult react(const std::string& event) override; 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 5c99f03126..6af466bc1a 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 @@ -42,11 +42,8 @@ ReactionResult ReplanInvalidatedTrajectory::react(const std::string& event) if ((event == toString(LocalFeedbackEnum::COLLISION_AHEAD)) || (event == toString(LocalFeedbackEnum::LOCAL_PLANNER_STUCK))) { - if (!hybrid_planning_manager_->sendGlobalPlannerAction()) // Start global planning - { - hybrid_planning_manager_->sendHybridPlanningResponse(false); - } - return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS, + HybridPlanningAction::SEND_GLOBAL_SOLVER_REQUEST); } else { diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp index 4d0ce03c3e..5b1170ad24 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp +++ b/moveit_ros/hybrid_planning/hybrid_planning_manager/planner_logic_plugins/src/single_plan_execution.cpp @@ -36,37 +36,26 @@ namespace moveit::hybrid_planning { -bool SinglePlanExecution::initialize(const std::shared_ptr& hybrid_planning_manager) -{ - hybrid_planning_manager_ = hybrid_planning_manager; - return true; -} - ReactionResult SinglePlanExecution::react(const HybridPlanningEvent& event) { switch (event) { case HybridPlanningEvent::HYBRID_PLANNING_REQUEST_RECEIVED: - // Handle new hybrid planning request - if (!hybrid_planning_manager_->sendGlobalPlannerAction()) // Start global planning - { - hybrid_planning_manager_->sendHybridPlanningResponse(false); - } // Reset local planner started flag local_planner_started_ = false; - return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + // Handle new hybrid planning request + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS, + HybridPlanningAction::SEND_GLOBAL_SOLVER_REQUEST); case HybridPlanningEvent::GLOBAL_SOLUTION_AVAILABLE: // Do nothing since we wait for the global planning action to finish return ReactionResult(event, "Do nothing", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_SUCCESSFUL: // Activate local planner once global solution is available if (!local_planner_started_) - { // ensure the local planner is not started twice - if (!hybrid_planning_manager_->sendLocalPlannerAction()) // Start local planning - { - hybrid_planning_manager_->sendHybridPlanningResponse(false); - } + { // ensure the local planner is not started twice local_planner_started_ = true; + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS, + HybridPlanningAction::SEND_LOCAL_SOLVER_REQUEST); } return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); case HybridPlanningEvent::GLOBAL_PLANNING_ACTION_ABORTED: @@ -75,15 +64,16 @@ ReactionResult SinglePlanExecution::react(const HybridPlanningEvent& event) moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); case HybridPlanningEvent::LOCAL_PLANNING_ACTION_SUCCESSFUL: // Finish hybrid planning action successfully because local planning action succeeded - hybrid_planning_manager_->sendHybridPlanningResponse(true); - return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS); + return ReactionResult(event, "", moveit_msgs::msg::MoveItErrorCodes::SUCCESS, + HybridPlanningAction::RETURN_HP_SUCCESS); case HybridPlanningEvent::LOCAL_PLANNING_ACTION_ABORTED: // Local planning failed so abort hybrid planning return ReactionResult(event, "Local planner failed to find a solution", moveit_msgs::msg::MoveItErrorCodes::PLANNING_FAILED); default: // Unknown event, abort hybrid planning - return ReactionResult(event, "Unknown event", moveit_msgs::msg::MoveItErrorCodes::FAILURE); + return ReactionResult(event, "Unknown event", moveit_msgs::msg::MoveItErrorCodes::FAILURE, + HybridPlanningAction::RETURN_HP_SUCCESS); } } diff --git a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt index fa68289600..4c0e0e6c89 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/local_planner/local_constraint_solver_plugins/CMakeLists.txt @@ -1,5 +1,5 @@ -add_library(forward_trajectory_plugin SHARED - src/forward_trajectory.cpp -) -set_target_properties(forward_trajectory_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(forward_trajectory_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) +add_library(forward_trajectory_plugin SHARED src/forward_trajectory.cpp) +set_target_properties(forward_trajectory_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(forward_trajectory_plugin + ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/local_planner_component/CMakeLists.txt index 5c5a80b20d..a4de092130 100644 --- a/moveit_ros/hybrid_planning/local_planner/local_planner_component/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/CMakeLists.txt @@ -1,6 +1,10 @@ # Local_planner_component +generate_parameter_library(local_planner_parameters + res/local_planner_parameters.yaml) add_library(moveit_local_planner_component SHARED - src/local_planner_component.cpp -) -set_target_properties(moveit_local_planner_component PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_local_planner_component ${THIS_PACKAGE_INCLUDE_DEPENDS}) + src/local_planner_component.cpp) +set_target_properties(moveit_local_planner_component + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_local_planner_component local_planner_parameters) +ament_target_dependencies(moveit_local_planner_component + ${THIS_PACKAGE_INCLUDE_DEPENDS}) 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 15ff513da6..1ce83d7efc 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 @@ -61,32 +61,13 @@ #include #include -namespace moveit::hybrid_planning -{ -// TODO(sjahr) Refactor and use repository wide solution -template -void declareOrGetParam(const std::string& param_name, T& output_value, const T& default_value, - const rclcpp::Node::SharedPtr& node) +// Forward declaration of parameter class allows users to implement custom parameters +namespace local_planner_parameters { - try - { - if (node->has_parameter(param_name)) - { - node->get_parameter(param_name, output_value); - } - else - { - output_value = node->declare_parameter(param_name, default_value); - } - } - catch (const rclcpp::exceptions::InvalidParameterTypeException& e) - { - RCLCPP_ERROR_STREAM(node->get_logger(), - "Error getting parameter '" << param_name << "', check parameter type in YAML file"); - throw e; - } +MOVEIT_STRUCT_FORWARD(Params); } - +namespace moveit::hybrid_planning +{ /// Internal local planner states /// TODO(sjahr) Use lifecycle node? enum class LocalPlannerState : int8_t @@ -104,48 +85,6 @@ enum class LocalPlannerState : int8_t class LocalPlannerComponent { public: - /// Struct that contains configuration of the local planner component node - struct LocalPlannerConfig - { - void load(const rclcpp::Node::SharedPtr& node) - { - std::string undefined = ""; - declareOrGetParam("group_name", group_name, undefined, node); - declareOrGetParam("trajectory_operator_plugin_name", trajectory_operator_plugin_name, undefined, - node); - declareOrGetParam("local_constraint_solver_plugin_name", local_constraint_solver_plugin_name, - undefined, node); - declareOrGetParam("local_planning_action_name", local_planning_action_name, undefined, node); - declareOrGetParam("local_planning_frequency", local_planning_frequency, 1.0, node); - declareOrGetParam("global_solution_topic", global_solution_topic, undefined, node); - declareOrGetParam("local_solution_topic", local_solution_topic, undefined, node); - declareOrGetParam("local_solution_topic_type", local_solution_topic_type, undefined, node); - declareOrGetParam("publish_joint_positions", publish_joint_positions, false, node); - declareOrGetParam("publish_joint_velocities", publish_joint_velocities, false, node); - // Planning scene monitor options - declareOrGetParam("monitored_planning_scene", monitored_planning_scene_topic, undefined, node); - declareOrGetParam("collision_object_topic", collision_object_topic, undefined, node); - declareOrGetParam("joint_states_topic", joint_states_topic, undefined, node); - } - - std::string group_name; - std::string robot_description; - std::string robot_description_semantic; - std::string publish_planning_scene_topic; - std::string trajectory_operator_plugin_name; - std::string local_constraint_solver_plugin_name; - std::string local_planning_action_name; - std::string global_solution_topic; - std::string local_solution_topic; - std::string local_solution_topic_type; - bool publish_joint_positions; - bool publish_joint_velocities; - double local_planning_frequency; - std::string monitored_planning_scene_topic; - std::string collision_object_topic; - std::string joint_states_topic; - }; - /** \brief Constructor */ LocalPlannerComponent(const rclcpp::NodeOptions& options); @@ -186,7 +125,7 @@ class LocalPlannerComponent std::shared_ptr node_; // Planner configuration - LocalPlannerConfig config_; + std::shared_ptr config_; // Current planner state. Must be thread-safe std::atomic state_; diff --git a/moveit_ros/hybrid_planning/local_planner/local_planner_component/res/local_planner_parameters.yaml b/moveit_ros/hybrid_planning/local_planner/local_planner_component/res/local_planner_parameters.yaml new file mode 100644 index 0000000000..52b9c993b0 --- /dev/null +++ b/moveit_ros/hybrid_planning/local_planner/local_planner_component/res/local_planner_parameters.yaml @@ -0,0 +1,61 @@ +local_planner_parameters: + group_name: { + type: string, + description: "Planning group to be used", + default_value: "UNKNOWN", + } + trajectory_operator_plugin_name: { + type: string, + description: "Trajectory operator plugin", + default_value: "UNKNOWN", + } + local_constraint_solver_plugin_name: { + type: string, + description: "Local constraint solver plugin", + default_value: "UNKNOWN", + } + local_planning_frequency: { + type: double, + description: "Spinning frequency of the local planner [Hz].", + default_value: 1.0, + } + global_solution_topic: { + type: string, + description: "Name of the topic where the global solution is published", + default_value: "UNKNOWN", + } + local_solution_topic: { + type: string, + description: "Name of the topic where the local solution is published", + default_value: "UNKNOWN", + } + local_solution_topic_type: { + type: string, + description: "Topic type of local solution", + default_value: "UNKNOWN", + } + publish_joint_positions: { + type: bool, + description: "Whether or not the local solver publishes position commands", + default_value: false, + } + publish_joint_velocities: { + type: bool, + description: "Whether or not the local solver publishes velocity commands", + default_value: false, + } + monitored_planning_scene_topic: { + type: string, + description: "Name of the topic where the reference planning scene is published", + default_value: "/planning_scene", + } + collision_object_topic: { + type: string, + description: "Name of the topic where the collision objects are published", + default_value: "/collision_object", + } + joint_states_topic: { + type: string, + description: "Name of the topic where the joint states are published", + default_value: "/joint_states", + } 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 fb91d7856f..ab4bb28c66 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 @@ -33,6 +33,7 @@ *********************************************************************/ #include +#include #include #include @@ -68,13 +69,14 @@ LocalPlannerComponent::LocalPlannerComponent(const rclcpp::NodeOptions& options) bool LocalPlannerComponent::initialize() { // Load planner parameter - config_.load(node_); + auto param_listener = local_planner_parameters::ParamListener(node_, ""); + config_ = std::make_shared(param_listener.get_params()); // Validate config - if (config_.local_solution_topic_type == "std_msgs/Float64MultiArray") + if (config_->local_solution_topic_type == "std_msgs/Float64MultiArray") { - if ((config_.publish_joint_positions && config_.publish_joint_velocities) || - (!config_.publish_joint_positions && !config_.publish_joint_velocities)) + if ((config_->publish_joint_positions && config_->publish_joint_velocities) || + (!config_->publish_joint_positions && !config_->publish_joint_velocities)) { RCLCPP_ERROR(node_->get_logger(), "When publishing a std_msgs/Float64MultiArray, you must select positions OR velocities. " @@ -93,9 +95,9 @@ bool LocalPlannerComponent::initialize() } // Start state and scene monitors - planning_scene_monitor_->startSceneMonitor(config_.monitored_planning_scene_topic); - planning_scene_monitor_->startWorldGeometryMonitor(config_.collision_object_topic); - planning_scene_monitor_->startStateMonitor(config_.joint_states_topic, "/attached_collision_object"); + planning_scene_monitor_->startSceneMonitor(config_->monitored_planning_scene_topic); + planning_scene_monitor_->startWorldGeometryMonitor(config_->collision_object_topic); + planning_scene_monitor_->startStateMonitor(config_->joint_states_topic, "/attached_collision_object"); planning_scene_monitor_->monitorDiffs(true); planning_scene_monitor_->stopPublishingPlanningScene(); @@ -113,17 +115,17 @@ bool LocalPlannerComponent::initialize() try { trajectory_operator_instance_ = - trajectory_operator_loader_->createUniqueInstance(config_.trajectory_operator_plugin_name); + trajectory_operator_loader_->createUniqueInstance(config_->trajectory_operator_plugin_name); if (!trajectory_operator_instance_->initialize(node_, planning_scene_monitor_->getRobotModel(), - config_.group_name)) // TODO(sjahr) add default group param + config_->group_name)) // TODO(sjahr) add default group param throw std::runtime_error("Unable to initialize trajectory operator plugin"); RCLCPP_INFO(node_->get_logger(), "Using trajectory operator interface '%s'", - config_.trajectory_operator_plugin_name.c_str()); + config_->trajectory_operator_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { RCLCPP_ERROR(node_->get_logger(), "Exception while loading trajectory operator '%s': '%s'", - config_.trajectory_operator_plugin_name.c_str(), ex.what()); + config_->trajectory_operator_plugin_name.c_str(), ex.what()); return false; } @@ -141,23 +143,23 @@ bool LocalPlannerComponent::initialize() try { local_constraint_solver_instance_ = - local_constraint_solver_plugin_loader_->createUniqueInstance(config_.local_constraint_solver_plugin_name); - if (!local_constraint_solver_instance_->initialize(node_, planning_scene_monitor_, config_.group_name)) + local_constraint_solver_plugin_loader_->createUniqueInstance(config_->local_constraint_solver_plugin_name); + if (!local_constraint_solver_instance_->initialize(node_, planning_scene_monitor_, config_->group_name)) throw std::runtime_error("Unable to initialize constraint solver plugin"); RCLCPP_INFO(node_->get_logger(), "Using constraint solver interface '%s'", - config_.local_constraint_solver_plugin_name.c_str()); + config_->local_constraint_solver_plugin_name.c_str()); } catch (pluginlib::PluginlibException& ex) { RCLCPP_ERROR(node_->get_logger(), "Exception while loading constraint solver '%s': '%s'", - config_.local_constraint_solver_plugin_name.c_str(), ex.what()); + config_->local_constraint_solver_plugin_name.c_str(), ex.what()); return false; } // Initialize local planning request action server cb_group_ = node_->create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); local_planning_request_server_ = rclcpp_action::create_server( - node_, config_.local_planning_action_name, + node_, "local_planning_action", // Goal callback [this](const rclcpp_action::GoalUUID& /*unused*/, const std::shared_ptr& /*unused*/) { @@ -197,7 +199,7 @@ bool LocalPlannerComponent::initialize() // This needs to return quickly to avoid blocking the executor, so run the local planner in a new thread. auto local_planner_timer = [&]() { timer_ = - node_->create_wall_timer(1s / config_.local_planning_frequency, [this]() { return executeIteration(); }); + node_->create_wall_timer(1s / config_->local_planning_frequency, [this]() { return executeIteration(); }); }; long_callback_thread_ = std::thread(local_planner_timer); }, @@ -205,7 +207,7 @@ bool LocalPlannerComponent::initialize() // Initialize global trajectory listener global_solution_subscriber_ = node_->create_subscription( - config_.global_solution_topic, rclcpp::SystemDefaultsQoS(), + config_->global_solution_topic, rclcpp::SystemDefaultsQoS(), [this](const moveit_msgs::msg::MotionPlanResponse::ConstSharedPtr& msg) { // Add received trajectory to internal reference trajectory robot_trajectory::RobotTrajectory new_trajectory(planning_scene_monitor_->getRobotModel(), msg->group_name); @@ -226,18 +228,19 @@ bool LocalPlannerComponent::initialize() }); // Initialize local solution publisher - RCLCPP_INFO(node_->get_logger(), "Using '%s' as local solution topic type", config_.local_solution_topic_type.c_str()); - if (config_.local_solution_topic_type == "trajectory_msgs/JointTrajectory") + RCLCPP_INFO(node_->get_logger(), "Using '%s' as local solution topic type", + config_->local_solution_topic_type.c_str()); + if (config_->local_solution_topic_type == "trajectory_msgs/JointTrajectory") { local_trajectory_publisher_ = - node_->create_publisher(config_.local_solution_topic, 1); + node_->create_publisher(config_->local_solution_topic, 1); } - else if (config_.local_solution_topic_type == "std_msgs/Float64MultiArray") + else if (config_->local_solution_topic_type == "std_msgs/Float64MultiArray") { local_solution_publisher_ = - node_->create_publisher(config_.local_solution_topic, 1); + node_->create_publisher(config_->local_solution_topic, 1); } - else if (config_.local_solution_topic_type == "CUSTOM") + else if (config_->local_solution_topic_type == "CUSTOM") { // Local solution publisher is defined by the local constraint solver plugin } @@ -285,7 +288,7 @@ void LocalPlannerComponent::executeIteration() // Get local goal trajectory to follow robot_trajectory::RobotTrajectory local_trajectory = - robot_trajectory::RobotTrajectory(planning_scene_monitor_->getRobotModel(), config_.group_name); + robot_trajectory::RobotTrajectory(planning_scene_monitor_->getRobotModel(), config_->group_name); *local_planner_feedback_ = trajectory_operator_instance_->getLocalTrajectory(current_robot_state, local_trajectory); @@ -315,31 +318,31 @@ void LocalPlannerComponent::executeIteration() } // Use a configurable message interface like MoveIt servo - // (See https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/src/servo_calcs.cpp) + // (See https://github.com/moveit/moveit2/blob/main/moveit_ros/moveit_servo/src/servo_calcs.cpp) // Format outgoing msg in the right format // (trajectory_msgs/JointTrajectory or joint positions/velocities in form of std_msgs/Float64MultiArray). - if (config_.local_solution_topic_type == "trajectory_msgs/JointTrajectory") + if (config_->local_solution_topic_type == "trajectory_msgs/JointTrajectory") { local_trajectory_publisher_->publish(local_solution); } - else if (config_.local_solution_topic_type == "std_msgs/Float64MultiArray") + else if (config_->local_solution_topic_type == "std_msgs/Float64MultiArray") { // Transform "trajectory_msgs/JointTrajectory" to "std_msgs/Float64MultiArray" auto joints = std::make_unique(); if (!local_solution.points.empty()) { - if (config_.publish_joint_positions) + if (config_->publish_joint_positions) { joints->data = local_solution.points[0].positions; } - else if (config_.publish_joint_velocities) + else if (config_->publish_joint_velocities) { joints->data = local_solution.points[0].velocities; } } local_solution_publisher_->publish(std::move(joints)); } - else if (config_.local_solution_topic_type == "CUSTOM") + else if (config_->local_solution_topic_type == "CUSTOM") { // Local solution publisher is defined by the local constraint solver plugin } diff --git a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/CMakeLists.txt b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/CMakeLists.txt index 18e40dfc57..b9ba389698 100644 --- a/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/local_planner/trajectory_operator_plugins/CMakeLists.txt @@ -1,5 +1,4 @@ -add_library(simple_sampler_plugin SHARED - src/simple_sampler.cpp -) -set_target_properties(simple_sampler_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +add_library(simple_sampler_plugin SHARED src/simple_sampler.cpp) +set_target_properties(simple_sampler_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(simple_sampler_plugin ${THIS_PACKAGE_INCLUDE_DEPENDS}) diff --git a/moveit_ros/hybrid_planning/package.xml b/moveit_ros/hybrid_planning/package.xml index d353765615..d61e4be425 100644 --- a/moveit_ros/hybrid_planning/package.xml +++ b/moveit_ros/hybrid_planning/package.xml @@ -1,6 +1,6 @@ moveit_hybrid_planning - 2.9.0 + 2.10.0 Hybrid planning components of MoveIt 2 Sebastian Jahr @@ -9,8 +9,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 ament_cmake moveit_common @@ -36,9 +36,13 @@ moveit_resources_panda_moveit_config ament_cmake_gtest - ament_lint_auto - ament_lint_common + controller_manager + moveit_configs_utils moveit_planners_ompl + moveit_resources_panda_moveit_config + moveit_simple_controller_manager + position_controllers + robot_state_publisher ros_testing diff --git a/moveit_ros/hybrid_planning/test/CMakeLists.txt b/moveit_ros/hybrid_planning/test/CMakeLists.txt index b3a5d3dbce..6d62727f0a 100644 --- a/moveit_ros/hybrid_planning/test/CMakeLists.txt +++ b/moveit_ros/hybrid_planning/test/CMakeLists.txt @@ -1,30 +1,17 @@ add_executable(cancel_action cancel_action.cpp) ament_target_dependencies(cancel_action PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) -add_executable(hybrid_planning_demo_node hybrid_planning_demo_node.cpp) -ament_target_dependencies(hybrid_planning_demo_node PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS}) - 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_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() - - # TODO (vatanaksoytezer / andyze: Flaky behaviour, investigate and re-enable this test asap) - # 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}") + # TODO (vatanaksoytezer / andyze: Flaky behaviour, investigate and re-enable + # this test asap) 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/config/common_hybrid_planning_params.yaml b/moveit_ros/hybrid_planning/test/config/common_hybrid_planning_params.yaml deleted file mode 100644 index 1825256505..0000000000 --- a/moveit_ros/hybrid_planning/test/config/common_hybrid_planning_params.yaml +++ /dev/null @@ -1,6 +0,0 @@ -# Parameters that are shared across several component nodes - -# A namespace can be added if multiple hybrid planners are launched -global_planning_action_name: "/test/hybrid_planning/global_planning_action" -local_planning_action_name: "/test/hybrid_planning/local_planning_action" -hybrid_planning_action_name: "/test/hybrid_planning/run_hybrid_planning" diff --git a/moveit_ros/hybrid_planning/test/config/global_planner.yaml b/moveit_ros/hybrid_planning/test/config/global_planner.yaml index a25d4ce475..a52f448879 100644 --- a/moveit_ros/hybrid_planning/test/config/global_planner.yaml +++ b/moveit_ros/hybrid_planning/test/config/global_planner.yaml @@ -9,7 +9,7 @@ planning_scene_monitor_options: # Subscribe to this topic (The name comes from the perspective of moveit_cpp) publish_planning_scene_topic: "/planning_scene" # Publish this topic, e.g. to visualize with RViz - monitored_planning_scene_topic: "/global_planner/planning_scene" + monitored_planning_scene_topic: "/planning_scene" wait_for_initial_state_timeout: 10.0 planning_pipelines: #namespace: "moveit_cpp" # optional, default is ~ diff --git a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp b/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp deleted file mode 100644 index 647b7558e9..0000000000 --- a/moveit_ros/hybrid_planning/test/hybrid_planning_demo_node.cpp +++ /dev/null @@ -1,321 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, 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: Sebastian Jahr - Description: A demonstration that re-plans around a moving box. - */ - -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#if RCLCPP_VERSION_GTE(20, 0, 0) -#include -#else -#include -#endif -#include -#include -#include -#include -#include -#include - -using namespace std::chrono_literals; -namespace -{ -rclcpp::Logger getLogger() -{ - return moveit::getLogger("test_hybrid_planning_client"); -} -} // namespace - -class HybridPlanningDemo -{ -public: - HybridPlanningDemo(const rclcpp::Node::SharedPtr& node) - { - node_ = node; - - std::string 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(getLogger(), "hybrid_planning_action_name parameter was not defined"); - std::exit(EXIT_FAILURE); - } - hp_action_client_ = - rclcpp_action::create_client(node_, hybrid_planning_action_name); - - collision_object_1_.header.frame_id = "panda_link0"; - collision_object_1_.id = "box1"; - - collision_object_2_.header.frame_id = "panda_link0"; - collision_object_2_.id = "box2"; - - collision_object_3_.header.frame_id = "panda_link0"; - collision_object_3_.id = "box3"; - - box_1_.type = box_1_.BOX; - box_1_.dimensions = { 0.5, 0.8, 0.01 }; - - box_2_.type = box_2_.BOX; - box_2_.dimensions = { 1.0, 0.4, 0.01 }; - - // 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::ConstSharedPtr& /* unused */) { - // Remove old collision objects - collision_object_1_.operation = collision_object_1_.REMOVE; - - // Add new collision objects - geometry_msgs::msg::Pose box_pose_2; - box_pose_2.position.x = 0.2; - box_pose_2.position.y = 0.4; - box_pose_2.position.z = 0.95; - - collision_object_2_.primitives.push_back(box_2_); - collision_object_2_.primitive_poses.push_back(box_pose_2); - collision_object_2_.operation = collision_object_2_.ADD; - - geometry_msgs::msg::Pose box_pose_3; - box_pose_3.position.x = 0.2; - box_pose_3.position.y = -0.4; - box_pose_3.position.z = 0.95; - - collision_object_3_.primitives.push_back(box_2_); - collision_object_3_.primitive_poses.push_back(box_pose_3); - collision_object_3_.operation = collision_object_3_.ADD; - - // Add object to planning scene - { // Lock PlanningScene - planning_scene_monitor::LockedPlanningSceneRW scene(planning_scene_monitor_); - scene->processCollisionObjectMsg(collision_object_2_); - scene->processCollisionObjectMsg(collision_object_3_); - scene->processCollisionObjectMsg(collision_object_1_); - } // Unlock PlanningScene - }); - } - - void run() - { - RCLCPP_INFO(getLogger(), "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(getLogger(), "The planning scene was not retrieved!"); - return; - } - else - { - planning_scene_monitor_->startStateMonitor(); - planning_scene_monitor_->providePlanningSceneService(); // let RViz display query PlanningScene - 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(getLogger(), "Timeout when waiting for /joint_states updates. Is the robot running?"); - return; - } - - if (!hp_action_client_->wait_for_action_server(20s)) - { - RCLCPP_ERROR(getLogger(), "Hybrid planning action server not available after waiting"); - return; - } - - geometry_msgs::msg::Pose box_pose; - box_pose.position.x = 0.4; - box_pose.position.y = 0.0; - box_pose.position.z = 0.85; - - collision_object_1_.primitives.push_back(box_1_); - collision_object_1_.primitive_poses.push_back(box_pose); - collision_object_1_.operation = collision_object_1_.ADD; - - // Add object to planning scene - { // Lock PlanningScene - planning_scene_monitor::LockedPlanningSceneRW scene(planning_scene_monitor_); - scene->processCollisionObjectMsg(collision_object_1_); - } // Unlock PlanningScene - - RCLCPP_INFO(getLogger(), "Wait 2s for the collision object"); - rclcpp::sleep_for(2s); - - // 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); - } - - // Create desired motion goal - 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"; - - 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); - - goal_motion_request.goal_constraints.resize(1); - goal_motion_request.goal_constraints[0] = - kinematic_constraints::constructGoalConstraints(goal_state, joint_model_group); - - // 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); - - auto goal_action_request = moveit_msgs::action::HybridPlanner::Goal(); - goal_action_request.planning_group = planning_group; - goal_action_request.motion_sequence = sequence_request; - - auto send_goal_options = rclcpp_action::Client::SendGoalOptions(); - send_goal_options.result_callback = - [](const rclcpp_action::ClientGoalHandle::WrappedResult& result) { - switch (result.code) - { - case rclcpp_action::ResultCode::SUCCEEDED: - RCLCPP_INFO(getLogger(), "Hybrid planning goal succeeded"); - break; - case rclcpp_action::ResultCode::ABORTED: - RCLCPP_ERROR(getLogger(), "Hybrid planning goal was aborted"); - return; - case rclcpp_action::ResultCode::CANCELED: - RCLCPP_ERROR(getLogger(), "Hybrid planning goal was canceled"); - return; - default: - RCLCPP_ERROR(getLogger(), "Unknown hybrid planning result code"); - return; - RCLCPP_INFO(getLogger(), "Hybrid planning result received"); - } - }; - send_goal_options.feedback_callback = - [](const rclcpp_action::ClientGoalHandle::SharedPtr& /*unused*/, - const std::shared_ptr& feedback) { - RCLCPP_INFO_STREAM(getLogger(), feedback->feedback); - }; - - RCLCPP_INFO(getLogger(), "Sending hybrid planning goal"); - // Ask server to achieve some goal and wait until it's accepted - auto goal_handle_future = hp_action_client_->async_send_goal(goal_action_request, send_goal_options); - } - -private: - rclcpp::Node::SharedPtr node_; - rclcpp_action::Client::SharedPtr hp_action_client_; - rclcpp::Subscription::SharedPtr global_solution_subscriber_; - planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; - rclcpp::TimerBase::SharedPtr timer_; - - moveit_msgs::msg::CollisionObject collision_object_1_; - moveit_msgs::msg::CollisionObject collision_object_2_; - moveit_msgs::msg::CollisionObject collision_object_3_; - shape_msgs::msg::SolidPrimitive box_1_; - shape_msgs::msg::SolidPrimitive box_2_; - - // TF - std::shared_ptr tf_buffer_; -}; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions node_options; - node_options.automatically_declare_parameters_from_overrides(true); - - rclcpp::Node::SharedPtr node = std::make_shared("hybrid_planning_test_node", "", node_options); - - 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(); - }); - - rclcpp::spin(node); - run_demo.join(); - rclcpp::shutdown(); - return 0; -} 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 ea4f1fbea3..c2d1faab87 100644 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py +++ b/moveit_ros/hybrid_planning/test/launch/hybrid_planning_common.py @@ -1,26 +1,11 @@ # Return a list of nodes we commonly launch for the demo. Nice for testing use. import os -import xacro import yaml from ament_index_python.packages import get_package_share_directory -from launch.actions import ExecuteProcess -from launch_ros.actions import Node from launch_ros.descriptions import ComposableNode from launch_ros.actions import ComposableNodeContainer - - -def load_file(package_name, file_path): - package_path = get_package_share_directory(package_name) - absolute_file_path = os.path.join(package_path, file_path) - - try: - with open(absolute_file_path, "r") as file: - return file.read() - except ( - EnvironmentError - ): # parent of IOError, OSError *and* WindowsError where available - return None +from moveit_configs_utils import MoveItConfigsBuilder def load_yaml(package_name, file_path): @@ -36,71 +21,21 @@ def load_yaml(package_name, file_path): return None -def get_robot_description(): - robot_description_config = xacro.process_file( - os.path.join( - get_package_share_directory("moveit_resources_panda_moveit_config"), - "config", - "panda.urdf.xacro", - ) - ) - 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" - ) - - # The global planner uses the typical OMPL parameters - planning_pipelines_config = { - "ompl": { - "planning_plugins": ["ompl_interface/OMPLPlanner"], - "request_adapters": [ - "default_planning_request_adapters/ResolveConstraintFrames", - "default_planning_request_adapters/ValidateWorkspaceBounds", - "default_planning_request_adapters/CheckStartStateBounds", - "default_planning_request_adapters/CheckStartStateCollision", - ], - "response_adapters": [ - "default_planning_response_adapters/AddTimeOptimalParameterization", - "default_planning_response_adapters/ValidateSolution", - "default_planning_response_adapters/DisplayMotionPath", - ], - } - } - ompl_planning_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/ompl_planning.yaml" - ) - planning_pipelines_config["ompl"].update(ompl_planning_yaml) - - moveit_simple_controllers_yaml = load_yaml( - "moveit_resources_panda_moveit_config", "config/moveit_controllers.yaml" + # Load the moveit configuration + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .planning_pipelines("ompl", ["ompl"]) + .trajectory_execution(file_path="config/moveit_controllers.yaml") + .to_moveit_configs() ) - moveit_controllers = { - "moveit_simple_controller_manager": moveit_simple_controllers_yaml, - "moveit_controller_manager": "moveit_simple_controller_manager/MoveItSimpleControllerManager", - } + # Hybrid planning parameter # Any parameters that are unique to your plugins go here - common_hybrid_planning_param = load_yaml( - "moveit_hybrid_planning", "config/common_hybrid_planning_params.yaml" - ) global_planner_param = load_yaml( "moveit_hybrid_planning", "config/global_planner.yaml" ) @@ -123,13 +58,8 @@ def generate_common_hybrid_launch_description(): plugin="moveit::hybrid_planning::GlobalPlannerComponent", name="global_planner", parameters=[ - common_hybrid_planning_param, global_planner_param, - robot_description, - robot_description_semantic, - kinematics_yaml, - planning_pipelines_config, - moveit_controllers, + moveit_config.to_dict(), ], ), ComposableNode( @@ -137,11 +67,8 @@ def generate_common_hybrid_launch_description(): plugin="moveit::hybrid_planning::LocalPlannerComponent", name="local_planner", parameters=[ - common_hybrid_planning_param, local_planner_param, - robot_description, - robot_description_semantic, - kinematics_yaml, + moveit_config.to_dict(), ], ), ComposableNode( @@ -149,7 +76,6 @@ def generate_common_hybrid_launch_description(): plugin="moveit::hybrid_planning::HybridPlanningManager", name="hybrid_planning_manager", parameters=[ - common_hybrid_planning_param, hybrid_planning_manager_param, ], ), @@ -157,82 +83,8 @@ def generate_common_hybrid_launch_description(): output="screen", ) - # RViz - rviz_config_file = ( - get_package_share_directory("moveit_hybrid_planning") - + "/config/hybrid_planning_demo.rviz" - ) - rviz_node = Node( - package="rviz2", - executable="rviz2", - name="rviz2", - output="log", - arguments=["-d", rviz_config_file], - parameters=[robot_description, robot_description_semantic], - ) - - # Static TF - static_tf = Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_transform_publisher", - output="log", - arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], - ) - - # Publish TF - robot_state_publisher = Node( - package="robot_state_publisher", - executable="robot_state_publisher", - name="robot_state_publisher", - output="both", - parameters=[robot_description], - ) - - # ros2_control using FakeSystem as hardware - ros2_controllers_path = os.path.join( - get_package_share_directory("moveit_hybrid_planning"), - "config", - "demo_controller.yaml", - ) - ros2_control_node = Node( - package="controller_manager", - executable="ros2_control_node", - parameters=[ros2_controllers_path], - remappings=[ - ("/controller_manager/robot_description", "/robot_description"), - ], - output="screen", - ) - - joint_state_broadcaster_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "joint_state_broadcaster", - "--controller-manager", - "/controller_manager", - ], - ) - - panda_joint_group_position_controller_spawner = Node( - package="controller_manager", - executable="spawner", - arguments=[ - "panda_joint_group_position_controller", - "-c", - "/controller_manager", - ], - ) - launched_nodes = [ container, - static_tf, - rviz_node, - robot_state_publisher, - ros2_control_node, - joint_state_broadcaster_spawner, - panda_joint_group_position_controller_spawner, ] return launched_nodes 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 deleted file mode 100644 index dd91e47f50..0000000000 --- a/moveit_ros/hybrid_planning/test/launch/hybrid_planning_demo.launch.py +++ /dev/null @@ -1,38 +0,0 @@ -import launch -import os -import sys - -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_yaml, -) - - -def generate_launch_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() - - # 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 index 1b8ccefd34..e079883102 100644 --- a/moveit_ros/hybrid_planning/test/launch/test_basic_integration.test.py +++ b/moveit_ros/hybrid_planning/test/launch/test_basic_integration.test.py @@ -3,17 +3,16 @@ import sys import unittest +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription -from launch.actions import DeclareLaunchArgument, ExecuteProcess, TimerAction +from launch.actions import DeclareLaunchArgument, TimerAction from launch.substitutions import LaunchConfiguration, PathJoinSubstitution from launch_ros.actions import Node +from moveit_configs_utils import MoveItConfigsBuilder 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, ) @@ -21,11 +20,10 @@ 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" + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() ) hybrid_planning_gtest = Node( @@ -33,13 +31,62 @@ def generate_test_description(): [LaunchConfiguration("test_binary_dir"), "test_basic_integration"] ), parameters=[ - robot_description, - robot_description_semantic, - common_hybrid_planning_param, + moveit_config.to_dict(), ], output="screen", ) + # Static TF + static_tf = Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + # ros2_control using FakeSystem as hardware + ros2_controllers_path = os.path.join( + get_package_share_directory("moveit_hybrid_planning"), + "config", + "demo_controller.yaml", + ) + ros2_control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[moveit_config.robot_description, ros2_controllers_path], + output="screen", + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + ) + + panda_joint_group_position_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[ + "panda_joint_group_position_controller", + "-c", + "/controller_manager", + ], + ) + return LaunchDescription( [ DeclareLaunchArgument( @@ -51,6 +98,13 @@ def generate_test_description(): launch_testing.actions.ReadyToTest(), ] + common_launch + + [ + static_tf, + robot_state_publisher, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_joint_group_position_controller_spawner, + ] ), { "hybrid_planning_gtest": hybrid_planning_gtest, } diff --git a/moveit_ros/hybrid_planning/test/test_basic_integration.cpp b/moveit_ros/hybrid_planning/test/test_basic_integration.cpp index 20f7e1efe4..9a6beb355e 100644 --- a/moveit_ros/hybrid_planning/test/test_basic_integration.cpp +++ b/moveit_ros/hybrid_planning/test/test_basic_integration.cpp @@ -63,25 +63,12 @@ class HybridPlanningFixture : public ::testing::Test 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); + hp_action_client_ = rclcpp_action::create_client(node_, "run_hybrid_planning"); // 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 */) {}); + [](const moveit_msgs::msg::MotionPlanResponse::SharedPtr /* unused */) {}); RCLCPP_INFO(node_->get_logger(), "Initialize Planning Scene Monitor"); tf_buffer_ = std::make_shared(node_->get_clock()); @@ -195,7 +182,7 @@ class HybridPlanningFixture : public ::testing::Test 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_INFO(node_->get_logger(), "%s", feedback->feedback.c_str()); }; } @@ -235,7 +222,8 @@ TEST_F(HybridPlanningFixture, ActionCompletion) } // Make a hybrid planning request then abort it -TEST_F(HybridPlanningFixture, ActionAbortion) +// TODO(sjahr): Fix and re-enable +/*TEST_F(HybridPlanningFixture, ActionAbortion) { std::thread run_thread([this]() { // Send the goal @@ -254,7 +242,7 @@ TEST_F(HybridPlanningFixture, ActionAbortion) run_thread.join(); ASSERT_FALSE(action_successful_); ASSERT_TRUE(action_aborted_); -} +}*/ } // namespace moveit_hybrid_planning int main(int argc, char** argv) diff --git a/moveit_ros/move_group/CHANGELOG.rst b/moveit_ros/move_group/CHANGELOG.rst index 33222e381c..198f64aae0 100644 --- a/moveit_ros/move_group/CHANGELOG.rst +++ b/moveit_ros/move_group/CHANGELOG.rst @@ -2,6 +2,27 @@ Changelog for package moveit_ros_move_group ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* Remove extraneous error message from URDF service (`#2736 `_) +* Add parameter api integration test (`#2662 `_) +* CMake format and lint in pre-commit (`#2683 `_) +* Get robot description from topic in GetUrdfService (`#2681 `_) +* Fix bug in GetUrdfService move_group capability (`#2669 `_) + * Take both possible closings for links into account + * Make CI happy +* avoid a relative jump threshold of 0.0 (`#2654 `_) +* Add get group urdf capability (`#2649 `_) +* Contributors: Abishalini Sivaraman, Ezra Brooks, Mario Prats, Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Fix warning and cleanup unneeded placeholders (`#2566 `_) diff --git a/moveit_ros/move_group/CMakeLists.txt b/moveit_ros/move_group/CMakeLists.txt index 5e52563b90..6c4cdc9521 100644 --- a/moveit_ros/move_group/CMakeLists.txt +++ b/moveit_ros/move_group/CMakeLists.txt @@ -9,112 +9,93 @@ moveit_package() include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DEPENDS - fmt - ament_cmake - moveit_core - moveit_ros_occupancy_map_monitor - moveit_ros_planning - pluginlib - rclcpp - rclcpp_action - std_srvs - tf2 - tf2_geometry_msgs - tf2_ros -) - -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) + fmt + ament_cmake + moveit_core + moveit_ros_occupancy_map_monitor + moveit_ros_planning + pluginlib + rclcpp + rclcpp_action + std_srvs + tf2 + tf2_geometry_msgs + tf2_ros) + +foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${dependency} REQUIRED) endforeach() add_library(moveit_move_group_capabilities_base SHARED - src/move_group_context.cpp - src/move_group_capability.cpp -) -target_include_directories(moveit_move_group_capabilities_base PUBLIC - $ - $ -) -set_target_properties(moveit_move_group_capabilities_base PROPERTIES VERSION "${moveit_ros_move_group_VERSION}") -ament_target_dependencies(moveit_move_group_capabilities_base ${THIS_PACKAGE_INCLUDE_DEPENDS}) - -add_library(moveit_move_group_default_capabilities SHARED + src/move_group_context.cpp src/move_group_capability.cpp) +target_include_directories( + moveit_move_group_capabilities_base + PUBLIC $ + $) +set_target_properties(moveit_move_group_capabilities_base + PROPERTIES VERSION "${moveit_ros_move_group_VERSION}") +ament_target_dependencies(moveit_move_group_capabilities_base + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +add_library( + moveit_move_group_default_capabilities SHARED src/default_capabilities/apply_planning_scene_service_capability.cpp src/default_capabilities/cartesian_path_service_capability.cpp src/default_capabilities/clear_octomap_service_capability.cpp src/default_capabilities/execute_trajectory_action_capability.cpp + src/default_capabilities/get_group_urdf_capability.cpp src/default_capabilities/get_planning_scene_service_capability.cpp src/default_capabilities/kinematics_service_capability.cpp src/default_capabilities/move_action_capability.cpp src/default_capabilities/plan_service_capability.cpp src/default_capabilities/query_planners_service_capability.cpp src/default_capabilities/state_validation_service_capability.cpp - src/default_capabilities/tf_publisher_capability.cpp -) -target_include_directories(moveit_move_group_default_capabilities PUBLIC - $ - $ -) -set_target_properties(moveit_move_group_default_capabilities PROPERTIES VERSION "${moveit_ros_move_group_VERSION}") -ament_target_dependencies(moveit_move_group_default_capabilities ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(moveit_move_group_default_capabilities moveit_move_group_capabilities_base) + src/default_capabilities/tf_publisher_capability.cpp) +target_include_directories( + moveit_move_group_default_capabilities + PUBLIC $ + $) +set_target_properties(moveit_move_group_default_capabilities + PROPERTIES VERSION "${moveit_ros_move_group_VERSION}") +ament_target_dependencies(moveit_move_group_default_capabilities + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(moveit_move_group_default_capabilities + moveit_move_group_capabilities_base) add_executable(move_group src/move_group.cpp) target_include_directories(move_group PUBLIC include) -ament_target_dependencies(move_group ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) +ament_target_dependencies(move_group ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost) target_link_libraries(move_group moveit_move_group_capabilities_base) add_executable(list_move_group_capabilities src/list_capabilities.cpp) -ament_target_dependencies(list_move_group_capabilities ${THIS_PACKAGE_INCLUDE_DEPENDS}) -target_link_libraries(list_move_group_capabilities moveit_move_group_capabilities_base fmt) +ament_target_dependencies(list_move_group_capabilities + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(list_move_group_capabilities + moveit_move_group_capabilities_base fmt) -install( - TARGETS - move_group - list_move_group_capabilities - RUNTIME - DESTINATION lib/moveit_ros_move_group -) +install(TARGETS move_group list_move_group_capabilities + RUNTIME DESTINATION lib/moveit_ros_move_group) install( - TARGETS - moveit_move_group_default_capabilities - moveit_move_group_capabilities_base + TARGETS moveit_move_group_default_capabilities + moveit_move_group_capabilities_base EXPORT moveit_ros_move_groupTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) -install( - DIRECTORY include/ - DESTINATION include/moveit_ros_move_group -) +install(DIRECTORY include/ DESTINATION include/moveit_ros_move_group) -install( - PROGRAMS - scripts/load_map - scripts/save_map - DESTINATION lib/moveit_ros_move_group -) +install(PROGRAMS scripts/load_map scripts/save_map + DESTINATION lib/moveit_ros_move_group) -pluginlib_export_plugin_description_file(moveit_ros_move_group default_capabilities_plugin_description.xml) +pluginlib_export_plugin_description_file( + moveit_ros_move_group default_capabilities_plugin_description.xml) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_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() - -# TODO(henningkayser): enable rostests -# find_package(rostest REQUIRED) # rostest under development in ROS2 https://github.com/ros-planning/moveit2/issues/23 - # this test is flaky + # TODO(henningkayser): enable rostests find_package(rostest REQUIRED) # + # rostest under development in ROS2 + # https://github.com/moveit/moveit2/issues/23 this test is flaky # add_rostest(test/test_cancel_before_plan_execution.test) # add_rostest(test/test_check_state_validity_in_empty_scene.test) endif() diff --git a/moveit_ros/move_group/ConfigExtras.cmake b/moveit_ros/move_group/ConfigExtras.cmake index 65db6bd813..30817cf001 100644 --- a/moveit_ros/move_group/ConfigExtras.cmake +++ b/moveit_ros/move_group/ConfigExtras.cmake @@ -1,3 +1,10 @@ # Extras module needed for dependencies to find boost components -find_package(Boost REQUIRED system filesystem date_time program_options thread) +find_package( + Boost + REQUIRED + system + filesystem + date_time + program_options + thread) diff --git a/moveit_ros/move_group/default_capabilities_plugin_description.xml b/moveit_ros/move_group/default_capabilities_plugin_description.xml index 93361e30cd..35e379e1f2 100644 --- a/moveit_ros/move_group/default_capabilities_plugin_description.xml +++ b/moveit_ros/move_group/default_capabilities_plugin_description.xml @@ -72,4 +72,10 @@ + + + Provide a capability which creates an URDF string with joints and links of a requested joint model group + + + diff --git a/moveit_ros/move_group/include/moveit/move_group/capability_names.h b/moveit_ros/move_group/include/moveit/move_group/capability_names.h index c84e049184..f5bc6cadb2 100644 --- a/moveit_ros/move_group/include/moveit/move_group/capability_names.h +++ b/moveit_ros/move_group/include/moveit/move_group/capability_names.h @@ -62,4 +62,6 @@ static const std::string APPLY_PLANNING_SCENE_SERVICE_NAME = "apply_planning_scene"; // name of the service that applies a given planning scene static const std::string CLEAR_OCTOMAP_SERVICE_NAME = "clear_octomap"; // name of the service that can be used to clear the octomap +static const std::string GET_URDF_SERVICE_NAME = + "get_urdf"; // name of the service that can be used to request the urdf of a planning group } // namespace move_group diff --git a/moveit_ros/move_group/package.xml b/moveit_ros/move_group/package.xml index e050894fdb..68cfb989ed 100644 --- a/moveit_ros/move_group/package.xml +++ b/moveit_ros/move_group/package.xml @@ -2,7 +2,7 @@ moveit_ros_move_group - 2.9.0 + 2.10.0 The move_group node for MoveIt Michael Görner Henning Kayser @@ -12,8 +12,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan Sachin Chitta @@ -36,9 +36,10 @@ moveit_kinematics moveit_resources_fanuc_moveit_config - - ament_lint_auto - ament_lint_common + ament_cmake_gtest + moveit_configs_utils + moveit_resources_panda_moveit_config + ros_testing diff --git a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp index 8ab96e122f..ec3725694e 100644 --- a/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/apply_planning_scene_service_capability.cpp @@ -67,7 +67,7 @@ bool ApplyPlanningSceneService::applyScene(const std::shared_ptrplanning_scene_monitor_) { - RCLCPP_ERROR(moveit::getLogger("ApplyPlanningSceneService"), + RCLCPP_ERROR(moveit::getLogger("moveit.ros.move_group.apply_planning_scene_service"), "Cannot apply PlanningScene as no scene is monitored."); return true; } diff --git a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp index 8e2c696672..c868454fed 100644 --- a/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/cartesian_path_service_capability.cpp @@ -68,7 +68,7 @@ bool isStateValid(const planning_scene::PlanningScene* planning_scene, rclcpp::Logger getLogger() { - return moveit::getLogger("cartesian_path_service_capability"); + return moveit::getLogger("moveit.ros.move_group.cartesian_path_service_capability"); } } // namespace @@ -177,11 +177,15 @@ bool MoveGroupCartesianPathService::computeService( "and jump threshold %lf (in %s reference frame)", static_cast(waypoints.size()), link_name.c_str(), req->max_step, req->jump_threshold, global_frame ? "global" : "link"); + moveit::core::JumpThreshold jump_threshold = moveit::core::JumpThreshold::disabled(); + if (req->jump_threshold > 0.0) + { + jump_threshold = moveit::core::JumpThreshold::relative(req->jump_threshold); + } std::vector traj; res->fraction = moveit::core::CartesianInterpolator::computeCartesianPath( &start_state, jmg, traj, start_state.getLinkModel(link_name), waypoints, global_frame, - moveit::core::MaxEEFStep(req->max_step), moveit::core::JumpThreshold::relative(req->jump_threshold), - constraint_fn); + moveit::core::MaxEEFStep(req->max_step), jump_threshold, constraint_fn); moveit::core::robotStateToRobotStateMsg(start_state, res->start_state); robot_trajectory::RobotTrajectory rt(context_->planning_scene_monitor_->getRobotModel(), req->group_name); diff --git a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp index 4af2520f3d..a62b8b5ad5 100644 --- a/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/clear_octomap_service_capability.cpp @@ -43,7 +43,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ClearOctomapService"); + return moveit::getLogger("moveit.ros.move_group.clear_octomap_service"); } } // namespace diff --git a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp index 1866128e5e..8d78f2ac41 100644 --- a/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/execute_trajectory_action_capability.cpp @@ -49,7 +49,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("ClearOctomapService"); + return moveit::getLogger("moveit.ros.move_group.clear_octomap_service"); } } // namespace diff --git a/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp new file mode 100644 index 0000000000..fffdbf52f9 --- /dev/null +++ b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.cpp @@ -0,0 +1,168 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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: Sebastian Jahr */ + +#include "get_group_urdf_capability.h" + +#include +#include +#include +#include + +namespace move_group +{ + +namespace +{ +rclcpp::Logger getLogger() +{ + return moveit::getLogger("moveit.ros.move_group.get_urdf_service"); +} +const auto JOINT_ELEMENT_CLOSING = std::string(""); +const auto LINK_ELEMENT_CLOSING = std::string(""); +const auto ROBOT_ELEMENT_CLOSING = std::string(""); +const auto GENERAL_ELEMENT_CLOSING = std::string("/>"); +} // namespace + +GetUrdfService::GetUrdfService() : MoveGroupCapability("get_group_urdf") +{ +} + +void GetUrdfService::initialize() +{ + get_urdf_service_ = context_->moveit_cpp_->getNode()->create_service( + GET_URDF_SERVICE_NAME, + [this](const std::shared_ptr& req, + const std::shared_ptr& res) { + res->error_code.source = std::string("GetUrdfService"); + const auto subgroup = context_->moveit_cpp_->getRobotModel()->getJointModelGroup(req->group_name); + // Check if group exists in loaded robot model + if (!subgroup) + { + const auto error_string = std::string("Cannot create URDF because planning group '") + req->group_name + + std::string("' does not exist"); + RCLCPP_ERROR(getLogger(), "%s", error_string.c_str()); + res->error_code.message = error_string; + res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + + std::string full_urdf_string = + context_->moveit_cpp_->getPlanningSceneMonitor()->getRobotModelLoader()->getRDFLoader()->getURDFString(); + + // Check if robot description string is empty + if (full_urdf_string.empty()) + { + const auto error_string = std::string("Couldn't get the robot description string from MoveItCpp"); + RCLCPP_ERROR(getLogger(), "%s", error_string.c_str()); + res->error_code.message = error_string; + res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + + // Create subgroup urdf + // Create header + res->urdf_string = std::string("group_name + + std::string("\" xmlns:xacro=\"http://ros.org/wiki/xacro\">"); + + // Create links + auto link_names = subgroup->getLinkModelNames(); + // Remove duplicates + std::sort(link_names.begin(), link_names.end()); + link_names.erase(std::unique(link_names.begin(), link_names.end()), link_names.end()); + + for (const auto& link_name : link_names) + { + const auto start = full_urdf_string.find("" or "" so we need to consider both cases + auto const substring_without_opening = substring.substr(1, substring.size() - 2); + auto const general_opening_pos_a = substring_without_opening.find('<'); + auto const link_closing_pos_b = substring.find(GENERAL_ELEMENT_CLOSING); + // Case "/>" + if (link_closing_pos_b < general_opening_pos_a) + { + res->urdf_string += substring.substr(0, link_closing_pos_b + GENERAL_ELEMENT_CLOSING.size()); + } + // Case + else + { + res->urdf_string += substring.substr(0, substring.find(LINK_ELEMENT_CLOSING) + LINK_ELEMENT_CLOSING.size()); + } + } + + // Create joints + auto joint_names = subgroup->getJointModelNames(); + // Remove duplicates + std::sort(joint_names.begin(), joint_names.end()); + joint_names.erase(std::unique(joint_names.begin(), joint_names.end()), joint_names.end()); + for (const auto& joint_name : joint_names) + { + const auto start = full_urdf_string.find("urdf_string += substring.substr(0, substring.find(JOINT_ELEMENT_CLOSING) + JOINT_ELEMENT_CLOSING.size()); + + // If parent link model is not part of the joint group, add it + const auto parent_link_element = subgroup->getJointModel(joint_name)->getParentLinkModel()->getName(); + if (std::find(link_names.begin(), link_names.end(), parent_link_element) == link_names.end()) + { + auto const base_link_element = ""; + res->urdf_string += base_link_element; + link_names.push_back(parent_link_element); + } + } + + // Add closing + res->urdf_string += ROBOT_ELEMENT_CLOSING; + + // Validate urdf file + if (!urdf::parseURDF(res->urdf_string)) + { + const std::string error_string = std::string("Failed to create valid urdf"); + RCLCPP_ERROR(getLogger(), "%s", error_string.c_str()); + res->error_code.message = error_string; + res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::FAILURE; + return; + } + res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::SUCCESS; + } // End of callback function + ); +} +} // namespace move_group + +#include + +PLUGINLIB_EXPORT_CLASS(move_group::GetUrdfService, move_group::MoveGroupCapability) diff --git a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/moveit_error_code_interface.h b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h similarity index 72% rename from moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/moveit_error_code_interface.h rename to moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h index 8f60e16a8c..05fb6b10ff 100644 --- a/moveit_ros/hybrid_planning/hybrid_planning_manager/hybrid_planning_manager_component/include/moveit/hybrid_planning_manager/moveit_error_code_interface.h +++ b/moveit_ros/move_group/src/default_capabilities/get_group_urdf_capability.h @@ -1,7 +1,7 @@ /********************************************************************* * Software License Agreement (BSD License) * - * Copyright (c) 2020, PickNik Inc. + * Copyright (c) 2024, PickNik Inc. * All rights reserved. * * Redistribution and use in source and binary forms, with or without @@ -33,39 +33,35 @@ *********************************************************************/ /* Author: Sebastian Jahr - Description: Defines an interface for setting and comparing MoveIt error codes. - */ + Desc: This capability creates an URDF string with joints and links of a requested joint model group */ #pragma once -namespace moveit::hybrid_planning +#include +#include + +namespace move_group { -class MoveItErrorCode : public moveit_msgs::msg::MoveItErrorCodes +/** + * @brief Move group capability to create an URDF string for a joint model group + * + */ +class GetUrdfService : public MoveGroupCapability { public: - MoveItErrorCode() - { - val = 0; - } - MoveItErrorCode(int code) - { - val = code; - } - MoveItErrorCode(const moveit_msgs::msg::MoveItErrorCodes& code) - { - val = code.val; - } - explicit operator bool() const - { - return val == moveit_msgs::msg::MoveItErrorCodes::SUCCESS; - } - bool operator==(const int code) const - { - return val == code; - } - bool operator!=(const int code) const - { - return val != code; - } + /** + * @brief Constructor + * + */ + GetUrdfService(); + + /** + * @brief Initializes service when plugin is loaded + * + */ + void initialize() override; + +private: + rclcpp::Service::SharedPtr get_urdf_service_; }; -} // namespace moveit::hybrid_planning +} // namespace move_group diff --git a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp index f27073e6fa..6101ab60cc 100644 --- a/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/kinematics_service_capability.cpp @@ -205,7 +205,7 @@ bool MoveGroupKinematicsService::computeFKService(const std::shared_ptrfk_link_names.empty()) { - RCLCPP_ERROR(moveit::getLogger("MoveGroupKinematicsService"), "No links specified for FK request"); + RCLCPP_ERROR(moveit::getLogger("moveit.ros.move_group.kinematics_service"), "No links specified for FK request"); res->error_code.val = moveit_msgs::msg::MoveItErrorCodes::INVALID_LINK_NAME; return true; } diff --git a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp index c0cc597390..ba7e77bfae 100644 --- a/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/move_action_capability.cpp @@ -52,7 +52,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("MoveGroupMoveAction"); + return moveit::getLogger("moveit.ros.move_group.move_action"); } } // namespace diff --git a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp index 4a44fcdb18..0a5018d2c3 100644 --- a/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/plan_service_capability.cpp @@ -48,7 +48,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("MoveGroupPlanService"); + return moveit::getLogger("moveit.ros.move_group.plan_service"); } } // namespace diff --git a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp index 1bdc92881e..9ea896bdc9 100644 --- a/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/query_planners_service_capability.cpp @@ -46,7 +46,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("query_planners_service_capability"); + return moveit::getLogger("moveit.ros.move_group.query_planners_service_capability"); } } // namespace MoveGroupQueryPlannersService::MoveGroupQueryPlannersService() : MoveGroupCapability("query_planners_service") diff --git a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp index 95925840d9..8ed2af48c1 100644 --- a/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp +++ b/moveit_ros/move_group/src/default_capabilities/tf_publisher_capability.cpp @@ -132,8 +132,8 @@ void TfPublisher::initialize() keep_running_ = true; - RCLCPP_INFO(moveit::getLogger("TfPublisher"), "Initializing MoveGroupTfPublisher with a frame publishing rate of %d", - rate_); + RCLCPP_INFO(moveit::getLogger("moveit.ros.move_group.tf_publisher"), + "Initializing MoveGroupTfPublisher with a frame publishing rate of %d", rate_); thread_ = std::thread(&TfPublisher::publishPlanningSceneFrames, this); } } // namespace move_group diff --git a/moveit_ros/move_group/src/move_group.cpp b/moveit_ros/move_group/src/move_group.cpp index 0550c42860..e67ad74963 100644 --- a/moveit_ros/move_group/src/move_group.cpp +++ b/moveit_ros/move_group/src/move_group.cpp @@ -55,13 +55,14 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("move_group"); + return moveit::getLogger("moveit.ros.move_group.executable"); } } // namespace // These capabilities are loaded unless listed in disable_capabilities // clang-format off static const char* const DEFAULT_CAPABILITIES[] = { + "move_group/GetUrdfService", "move_group/MoveGroupCartesianPathService", "move_group/MoveGroupKinematicsService", "move_group/MoveGroupExecuteTrajectoryAction", diff --git a/moveit_ros/move_group/src/move_group_capability.cpp b/moveit_ros/move_group/src/move_group_capability.cpp index aa8a1bdb9a..3a33da9393 100644 --- a/moveit_ros/move_group/src/move_group_capability.cpp +++ b/moveit_ros/move_group/src/move_group_capability.cpp @@ -50,7 +50,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("move_group_capability"); + return moveit::getLogger("moveit.ros.move_group.capability"); } } // namespace diff --git a/moveit_ros/move_group/src/move_group_context.cpp b/moveit_ros/move_group/src/move_group_context.cpp index df35342e66..2d11ab6563 100644 --- a/moveit_ros/move_group/src/move_group_context.cpp +++ b/moveit_ros/move_group/src/move_group_context.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("move_group_context"); + return moveit::getLogger("moveit.ros.move_group.context"); } } // namespace diff --git a/moveit_ros/moveit_ros/CHANGELOG.rst b/moveit_ros/moveit_ros/CHANGELOG.rst index ae0b079857..4df947049d 100644 --- a/moveit_ros/moveit_ros/CHANGELOG.rst +++ b/moveit_ros/moveit_ros/CHANGELOG.rst @@ -2,6 +2,17 @@ Changelog for package moveit_ros ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit_ros/moveit_ros/CMakeLists.txt b/moveit_ros/moveit_ros/CMakeLists.txt index 960c65c4a7..2855e2a392 100644 --- a/moveit_ros/moveit_ros/CMakeLists.txt +++ b/moveit_ros/moveit_ros/CMakeLists.txt @@ -2,9 +2,4 @@ cmake_minimum_required(VERSION 3.22) project(moveit_ros) find_package(ament_cmake REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/moveit_ros/moveit_ros/package.xml b/moveit_ros/moveit_ros/package.xml index c1920684d7..358f961e7d 100644 --- a/moveit_ros/moveit_ros/package.xml +++ b/moveit_ros/moveit_ros/package.xml @@ -2,7 +2,7 @@ moveit_ros - 2.9.0 + 2.10.0 Components of MoveIt that use ROS Michael Görner Henning Kayser @@ -12,8 +12,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan Sachin Chitta @@ -33,9 +33,6 @@ moveit_ros_visualization moveit_ros_move_group - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_ros/moveit_servo/CHANGELOG.rst b/moveit_ros/moveit_servo/CHANGELOG.rst index 191f8398c5..3a6ca13f28 100644 --- a/moveit_ros/moveit_servo/CHANGELOG.rst +++ b/moveit_ros/moveit_servo/CHANGELOG.rst @@ -2,6 +2,34 @@ Changelog for package moveit_servo ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* remove intraprocess comm warning (`#2752 `_) +* Fix error message text in servo.cpp (`#2769 `_) +* Fix launch parameters in Servo demos (`#2735 `_) + Co-authored-by: Sebastian Jahr +* [Servo] Fix collision checking with attached objects (`#2747 `_) +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Attempt to use SCHED_FIFO for Servo regardless of RT kernel (`#2653 `_) + * Attempt to use SCHED_FIFO for Servo regardless of RT kernel + * Update warning message if Servo fails to use SCHED_FIFO + Co-authored-by: AndyZe + * Update moveit_ros/moveit_servo/src/servo_node.cpp + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> + --------- + Co-authored-by: AndyZe + Co-authored-by: Sebastian Castro <4603398+sea-bass@users.noreply.github.com> +* Acceleration Limited Smoothing Plugin for Servo (`#2651 `_) +* Contributors: Marc Bestmann, Nathan Brooks, Paul Gesel, Robert Haschke, Sebastian Castro, Sebastian Jahr, Stephanie Eng, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Add command queue to servo to account for latency (`#2594 `_) diff --git a/moveit_ros/moveit_servo/CMakeLists.txt b/moveit_ros/moveit_servo/CMakeLists.txt index f04a0f446a..6ec080d195 100644 --- a/moveit_ros/moveit_servo/CMakeLists.txt +++ b/moveit_ros/moveit_servo/CMakeLists.txt @@ -6,123 +6,105 @@ find_package(moveit_common REQUIRED) moveit_package() set(THIS_PACKAGE_INCLUDE_DEPENDS - control_msgs - geometry_msgs - moveit_core - moveit_msgs - moveit_ros_planning - moveit_ros_planning_interface - pluginlib - rclcpp - rclcpp_components - realtime_tools - sensor_msgs - std_msgs - std_srvs - tf2_eigen - trajectory_msgs -) + control_msgs + geometry_msgs + moveit_core + moveit_msgs + moveit_ros_planning + moveit_ros_planning_interface + pluginlib + rclcpp + rclcpp_components + realtime_tools + sensor_msgs + std_msgs + std_srvs + tf2_eigen + trajectory_msgs) find_package(ament_cmake REQUIRED) find_package(generate_parameter_library REQUIRED) -foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) - find_package(${Dependency} REQUIRED) +foreach(dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${dependency} REQUIRED) endforeach() -include_directories( - include -) +include_directories(include) -################### -## C++ Libraries ## -################### +# ############################################################################## +# C++ Libraries ## +# ############################################################################## # This library provides a way of loading parameters for servo -generate_parameter_library( - moveit_servo_lib_parameters - config/servo_parameters.yaml -) - -# This library provides a C++ interface for sending realtime twist or joint commands to a robot -add_library(moveit_servo_lib_cpp SHARED - src/collision_monitor.cpp - src/servo.cpp - src/utils/common.cpp - src/utils/command.cpp - -) -set_target_properties(moveit_servo_lib_cpp PROPERTIES VERSION "${moveit_servo_VERSION}") +generate_parameter_library(moveit_servo_lib_parameters + config/servo_parameters.yaml) + +# This library provides a C++ interface for sending realtime twist or joint +# commands to a robot +add_library( + moveit_servo_lib_cpp SHARED src/collision_monitor.cpp src/servo.cpp + src/utils/common.cpp src/utils/command.cpp) +set_target_properties(moveit_servo_lib_cpp PROPERTIES VERSION + "${moveit_servo_VERSION}") target_link_libraries(moveit_servo_lib_cpp moveit_servo_lib_parameters) ament_target_dependencies(moveit_servo_lib_cpp ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_library(moveit_servo_lib_ros SHARED src/servo_node.cpp) -set_target_properties(moveit_servo_lib_ros PROPERTIES VERSION "${moveit_servo_VERSION}") +set_target_properties(moveit_servo_lib_ros PROPERTIES VERSION + "${moveit_servo_VERSION}") target_link_libraries(moveit_servo_lib_ros moveit_servo_lib_cpp) ament_target_dependencies(moveit_servo_lib_ros ${THIS_PACKAGE_INCLUDE_DEPENDS}) -###################### -## Components ## -###################### +# ############################################################################## +# Components ## +# ############################################################################## -rclcpp_components_register_node( - moveit_servo_lib_ros - PLUGIN "moveit_servo::ServoNode" - EXECUTABLE servo_node -) +rclcpp_components_register_node(moveit_servo_lib_ros PLUGIN + "moveit_servo::ServoNode" EXECUTABLE servo_node) -###################### -## Executable Nodes ## -###################### +# ############################################################################## +# Executable Nodes ## +# ############################################################################## # Executable node for the joint jog demo -add_executable(demo_joint_jog demos/cpp_interface/demo_joint_jog.cpp ) +add_executable(demo_joint_jog demos/cpp_interface/demo_joint_jog.cpp) target_link_libraries(demo_joint_jog moveit_servo_lib_cpp) ament_target_dependencies(demo_joint_jog ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Executable node for the twist demo -add_executable(demo_twist demos/cpp_interface/demo_twist.cpp ) +add_executable(demo_twist demos/cpp_interface/demo_twist.cpp) target_link_libraries(demo_twist moveit_servo_lib_cpp) ament_target_dependencies(demo_twist ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Executable node for the pose demo -add_executable(demo_pose demos/cpp_interface/demo_pose.cpp ) +add_executable(demo_pose demos/cpp_interface/demo_pose.cpp) target_link_libraries(demo_pose moveit_servo_lib_cpp) ament_target_dependencies(demo_pose ${THIS_PACKAGE_INCLUDE_DEPENDS}) # Keyboard control example for servo add_executable(servo_keyboard_input demos/servo_keyboard_input.cpp) target_include_directories(servo_keyboard_input PUBLIC include) -ament_target_dependencies(servo_keyboard_input ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(servo_keyboard_input ${THIS_PACKAGE_INCLUDE_DEPENDS}) -############# -## Install ## -############# +# ############################################################################## +# Install ## +# ############################################################################## # Install Libraries install( - TARGETS - moveit_servo_lib_cpp - moveit_servo_lib_ros - moveit_servo_lib_parameters + TARGETS moveit_servo_lib_cpp moveit_servo_lib_ros moveit_servo_lib_parameters EXPORT moveit_servoTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_servo -) + INCLUDES + DESTINATION include/moveit_servo) # Install Binaries install( - TARGETS - demo_joint_jog - demo_twist - demo_pose - servo_node - servo_keyboard_input + TARGETS demo_joint_jog demo_twist demo_pose servo_node servo_keyboard_input ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION lib/moveit_servo -) + RUNTIME DESTINATION lib/moveit_servo) # Install include, launch, config directories install(DIRECTORY include/ DESTINATION include/moveit_servo) @@ -132,25 +114,34 @@ install(DIRECTORY config DESTINATION share/moveit_servo) ament_export_targets(moveit_servoTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) - if(BUILD_TESTING) - find_package(ament_cmake_gtest REQUIRED) - find_package(ros_testing REQUIRED) - - ament_add_gtest_executable(moveit_servo_utils_test tests/test_utils.cpp) - target_link_libraries(moveit_servo_utils_test moveit_servo_lib_cpp) - ament_target_dependencies(moveit_servo_utils_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(tests/launch/servo_utils.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - ament_add_gtest_executable(moveit_servo_cpp_integration_test tests/test_integration.cpp tests/servo_cpp_fixture.hpp) - target_link_libraries(moveit_servo_cpp_integration_test moveit_servo_lib_cpp) - ament_target_dependencies(moveit_servo_cpp_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(tests/launch/servo_cpp_integration.test.py TIMEOUT 30 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - - ament_add_gtest_executable(moveit_servo_ros_integration_test tests/test_ros_integration.cpp tests/servo_ros_fixture.hpp) - ament_target_dependencies(moveit_servo_ros_integration_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - add_ros_test(tests/launch/servo_ros_integration.test.py TIMEOUT 120 ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + find_package(ament_cmake_gtest REQUIRED) + find_package(ros_testing REQUIRED) + + ament_add_gtest_executable(moveit_servo_utils_test tests/test_utils.cpp) + target_link_libraries(moveit_servo_utils_test moveit_servo_lib_cpp) + ament_target_dependencies(moveit_servo_utils_test + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_utils.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + ament_add_gtest_executable( + moveit_servo_cpp_integration_test tests/test_integration.cpp + tests/servo_cpp_fixture.hpp) + target_link_libraries(moveit_servo_cpp_integration_test moveit_servo_lib_cpp) + ament_target_dependencies(moveit_servo_cpp_integration_test + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_cpp_integration.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") + + ament_add_gtest_executable( + moveit_servo_ros_integration_test tests/test_ros_integration.cpp + tests/servo_ros_fixture.hpp) + ament_target_dependencies(moveit_servo_ros_integration_test + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + add_ros_test(tests/launch/servo_ros_integration.test.py TIMEOUT 120 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/moveit_servo/README.md b/moveit_ros/moveit_servo/README.md index 3bd2a95969..0a8c1203df 100644 --- a/moveit_ros/moveit_servo/README.md +++ b/moveit_ros/moveit_servo/README.md @@ -1,3 +1,3 @@ -# Moveit Servo +# MoveIt Servo See the [Realtime Arm Servoing Tutorial](https://moveit.picknik.ai/main/doc/examples/realtime_servo/realtime_servo_tutorial.html) for installation instructions, quick-start guide, an overview about `moveit_servo`, and to learn how to set it up on your robot. diff --git a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml index 878f27b141..b5d57bfcb9 100644 --- a/moveit_ros/moveit_servo/config/panda_simulated_config.yaml +++ b/moveit_ros/moveit_servo/config/panda_simulated_config.yaml @@ -28,7 +28,7 @@ publish_joint_accelerations: false ## Plugins for smoothing outgoing commands use_smoothing: true -smoothing_filter_plugin_name: "online_signal_smoothing::ButterworthFilterPlugin" +smoothing_filter_plugin_name: "online_signal_smoothing::AccelerationLimitedPlugin" # If is_primary_planning_scene_monitor is set to true, the Servo server's PlanningScene advertises the /get_planning_scene service, # which other nodes can use as a source for information about the planning environment. @@ -44,7 +44,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. -leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) +leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/moveit_ros/moveit_servo/config/test_config_panda.yaml b/moveit_ros/moveit_servo/config/test_config_panda.yaml index 0698fd846a..5324ce9bf1 100644 --- a/moveit_ros/moveit_servo/config/test_config_panda.yaml +++ b/moveit_ros/moveit_servo/config/test_config_panda.yaml @@ -43,7 +43,7 @@ move_group_name: panda_arm # Often 'manipulator' or 'arm' lower_singularity_threshold: 17.0 # Start decelerating when the condition number hits this (close to singularity) hard_stop_singularity_threshold: 30.0 # Stop when the condition number hits this joint_limit_margins: [0.1, 0.1, 0.1, 0.1, 0.1, 0.1, 0.1] # added as a buffer to joint limits [radians or meters]. If moving quickly, make this larger. -leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/ros-planning/moveit2/pull/620) +leaving_singularity_threshold_multiplier: 2.0 # Multiply the hard stop limit by this when leaving singularity (see https://github.com/moveit/moveit2/pull/620) ## Topic names cartesian_command_in_topic: ~/delta_twist_cmds # Topic for incoming Cartesian twist commands diff --git a/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp index 34e020376c..a68260bd8d 100644 --- a/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp +++ b/moveit_ros/moveit_servo/include/moveit_servo/collision_monitor.hpp @@ -67,8 +67,9 @@ class CollisionMonitor // Variables const servo::Params& servo_params_; - moveit::core::RobotStatePtr robot_state_; + const planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + moveit::core::RobotState robot_state_; // The collision monitor thread. std::thread monitor_thread_; diff --git a/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py index 77cecddbd6..4ed51a26cb 100644 --- a/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_joint_jog.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .joint_limits() + .joint_limits(file_path="config/hard_joint_limits.yaml") .robot_description_kinematics() .to_moveit_configs() ) @@ -22,8 +22,9 @@ def generate_launch_description(): .to_dict() } - # This filter parameter should be >1. Increase it for greater smoothing but slower motion. - low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} # RViz rviz_config_file = ( @@ -104,7 +105,8 @@ def generate_launch_description(): executable="demo_joint_jog", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/moveit_ros/moveit_servo/launch/demo_pose.launch.py b/moveit_ros/moveit_servo/launch/demo_pose.launch.py index aef3d2bda0..348dbe5c48 100644 --- a/moveit_ros/moveit_servo/launch/demo_pose.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_pose.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .joint_limits() + .joint_limits(file_path="config/hard_joint_limits.yaml") .robot_description_kinematics() .to_moveit_configs() ) @@ -22,8 +22,9 @@ def generate_launch_description(): .to_dict() } - # This filter parameter should be >1. Increase it for greater smoothing but slower motion. - low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} # RViz rviz_config_file = ( @@ -104,7 +105,8 @@ def generate_launch_description(): executable="demo_pose", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py b/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py index 2768ea9268..b2d7699dc1 100644 --- a/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_ros_api.launch.py @@ -12,8 +12,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .joint_limits() - .robot_description_kinematics() + .joint_limits(file_path="config/hard_joint_limits.yaml") .to_moveit_configs() ) @@ -29,8 +28,9 @@ def generate_launch_description(): .to_dict() } - # This filter parameter should be >1. Increase it for greater smoothing but slower motion. - low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} # RViz rviz_config_file = ( @@ -98,7 +98,8 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, @@ -129,7 +130,8 @@ def generate_launch_description(): name="servo_node", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/moveit_ros/moveit_servo/launch/demo_twist.launch.py b/moveit_ros/moveit_servo/launch/demo_twist.launch.py index 1230aa168c..d95300dad8 100644 --- a/moveit_ros/moveit_servo/launch/demo_twist.launch.py +++ b/moveit_ros/moveit_servo/launch/demo_twist.launch.py @@ -10,7 +10,7 @@ def generate_launch_description(): moveit_config = ( MoveItConfigsBuilder("moveit_resources_panda") .robot_description(file_path="config/panda.urdf.xacro") - .joint_limits() + .joint_limits(file_path="config/hard_joint_limits.yaml") .robot_description_kinematics() .to_moveit_configs() ) @@ -22,8 +22,9 @@ def generate_launch_description(): .to_dict() } - # This filter parameter should be >1. Increase it for greater smoothing but slower motion. - low_pass_filter_coeff = {"butterworth_filter_coeff": 1.5} + # This sets the update rate and planning group name for the acceleration limiting filter. + acceleration_filter_update_period = {"update_period": 0.01} + planning_group_name = {"planning_group_name": "panda_arm"} # RViz rviz_config_file = ( @@ -104,7 +105,8 @@ def generate_launch_description(): executable="demo_twist", parameters=[ servo_params, - low_pass_filter_coeff, + acceleration_filter_update_period, + planning_group_name, moveit_config.robot_description, moveit_config.robot_description_semantic, moveit_config.robot_description_kinematics, diff --git a/moveit_ros/moveit_servo/package.xml b/moveit_ros/moveit_servo/package.xml index 75d8854fd8..574d89dad8 100644 --- a/moveit_ros/moveit_servo/package.xml +++ b/moveit_ros/moveit_servo/package.xml @@ -2,7 +2,7 @@ moveit_servo - 2.9.0 + 2.10.0 Provides real-time manipulator Cartesian and joint servoing. Blake Anderson Andy Zelenak @@ -11,7 +11,7 @@ BSD-3-Clause - https://ros-planning.github.io/moveit_tutorials + https://moveit.github.io/moveit_tutorials Brian O'Neil Andy Zelenak @@ -49,8 +49,6 @@ tf2_ros ament_cmake_gtest - ament_lint_auto - ament_lint_common moveit_resources_panda_moveit_config ros_testing diff --git a/moveit_ros/moveit_servo/src/collision_monitor.cpp b/moveit_ros/moveit_servo/src/collision_monitor.cpp index b137999537..c3a886cff8 100644 --- a/moveit_ros/moveit_servo/src/collision_monitor.cpp +++ b/moveit_ros/moveit_servo/src/collision_monitor.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("CollisionMonitor"); + return moveit::getLogger("moveit.ros.move_group.collision_monitor"); } } // namespace @@ -55,6 +55,7 @@ CollisionMonitor::CollisionMonitor(const planning_scene_monitor::PlanningSceneMo const servo::Params& servo_params, std::atomic& collision_velocity_scale) : servo_params_(servo_params) , planning_scene_monitor_(planning_scene_monitor) + , robot_state_(planning_scene_monitor->getPlanningScene()->getCurrentState()) , collision_velocity_scale_(collision_velocity_scale) { scene_collision_request_.distance = true; @@ -104,10 +105,11 @@ void CollisionMonitor::checkCollisions() if (servo_params_.check_collisions) { - // Fetch latest robot state. - robot_state_ = planning_scene_monitor_->getStateMonitor()->getCurrentState(); + // Fetch latest robot state using planning scene instead of state monitor due to + // https://github.com/moveit/moveit2/issues/2748 + robot_state_ = planning_scene_monitor_->getPlanningScene()->getCurrentState(); // This must be called before doing collision checking. - robot_state_->updateCollisionBodyTransforms(); + robot_state_.updateCollisionBodyTransforms(); // Get a read-only copy of planning scene. planning_scene_monitor::LockedPlanningSceneRO locked_scene(planning_scene_monitor_); @@ -115,12 +117,12 @@ void CollisionMonitor::checkCollisions() // Check collision with environment. scene_collision_result_.clear(); locked_scene->getCollisionEnv()->checkRobotCollision(scene_collision_request_, scene_collision_result_, - *robot_state_, locked_scene->getAllowedCollisionMatrix()); + robot_state_, locked_scene->getAllowedCollisionMatrix()); // Check robot self collision. self_collision_result_.clear(); locked_scene->getCollisionEnvUnpadded()->checkSelfCollision( - self_collision_request_, self_collision_result_, *robot_state_, locked_scene->getAllowedCollisionMatrix()); + self_collision_request_, self_collision_result_, robot_state_, locked_scene->getAllowedCollisionMatrix()); // If collision detected scale velocity to 0, else start decelerating exponentially. // velocity_scale = e ^ k * (collision_distance - threshold) diff --git a/moveit_ros/moveit_servo/src/servo.cpp b/moveit_ros/moveit_servo/src/servo.cpp index 09ef4a477f..bfac5ae194 100644 --- a/moveit_ros/moveit_servo/src/servo.cpp +++ b/moveit_ros/moveit_servo/src/servo.cpp @@ -58,7 +58,7 @@ namespace moveit_servo Servo::Servo(const rclcpp::Node::SharedPtr& node, std::shared_ptr servo_param_listener, const planning_scene_monitor::PlanningSceneMonitorPtr& planning_scene_monitor) : node_(node) - , logger_(moveit::getLogger("servo")) + , logger_(moveit::getLogger("moveit.ros.servo")) , servo_param_listener_{ std::move(servo_param_listener) } , planning_scene_monitor_{ planning_scene_monitor } { @@ -222,7 +222,7 @@ bool Servo::validateParams(const servo::Params& servo_params) const RCLCPP_ERROR_STREAM( logger_, "When publishing a std_msgs/Float64MultiArray, " "either the parameter 'publish_joint_positions' OR the parameter 'publish_joint_velocities' must " - "be set to true. But both are set to false." + "be set to true. But both are set to true." << check_yaml_string); params_valid = false; } @@ -514,16 +514,6 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot // Compute the joint velocities required to reach positions target_state.velocities = joint_position_delta / servo_params_.publish_period; - // Apply smoothing to the positions if a smoother was provided. - doSmoothing(target_state); - - // Apply collision scaling to the joint position delta - target_state.positions = - current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions); - - // Compute velocities based on smoothed joint positions - target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period; - // Scale down the velocity based on joint velocity limit or user defined scaling if applicable. const double joint_velocity_limit_scale = jointLimitVelocityScalingFactor( target_state.velocities, joint_bounds, servo_params_.override_velocity_scaling_factor); @@ -536,6 +526,16 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot // Adjust joint position based on scaled down velocity target_state.positions = current_state.positions + (target_state.velocities * servo_params_.publish_period); + // Apply smoothing to the positions if a smoother was provided. + doSmoothing(target_state); + + // Apply collision scaling to the joint position delta + target_state.positions = + current_state.positions + collision_velocity_scale_ * (target_state.positions - current_state.positions); + + // Compute velocities based on smoothed joint positions + target_state.velocities = (target_state.positions - current_state.positions) / servo_params_.publish_period; + // Check if any joints are going past joint position limits const std::vector joints_to_halt = jointsToHalt(target_state.positions, target_state.velocities, joint_bounds, servo_params_.joint_limit_margins); @@ -548,6 +548,9 @@ KinematicState Servo::getNextJointState(const moveit::core::RobotStatePtr& robot } } + // Update internal state of filter with final calculated command. + resetSmoothing(target_state); + return target_state; } @@ -678,6 +681,7 @@ std::pair Servo::smoothHalt(const KinematicState& halt_sta } } + resetSmoothing(target_state); return std::make_pair(stopped, target_state); } diff --git a/moveit_ros/moveit_servo/src/servo_node.cpp b/moveit_ros/moveit_servo/src/servo_node.cpp index 0afd05979e..deb8b02d01 100644 --- a/moveit_ros/moveit_servo/src/servo_node.cpp +++ b/moveit_ros/moveit_servo/src/servo_node.cpp @@ -67,27 +67,18 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options) { moveit::setNodeLoggerName(node_->get_name()); - if (!options.use_intra_process_comms()) + // Configure SCHED_FIFO and priority + if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority)) { - RCLCPP_WARN_STREAM(node_->get_logger(), - "Intra-process communication is disabled, consider enabling it by adding: " - "\nextra_arguments=[{'use_intra_process_comms' : True}]\nto the Servo composable node " - "in the launch file"); + RCLCPP_INFO_STREAM(node_->get_logger(), "Enabled SCHED_FIFO and higher thread priority."); } - - // Check if a realtime kernel is available - if (realtime_tools::has_realtime_kernel()) + else { - if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority)) - { - RCLCPP_INFO_STREAM(node_->get_logger(), "Realtime kernel available, higher thread priority has been set."); - } - else - { - RCLCPP_WARN_STREAM(node_->get_logger(), "Could not enable FIFO RT scheduling policy."); - } + RCLCPP_WARN_STREAM(node_->get_logger(), "Could not enable FIFO RT scheduling policy. Continuing with the default."); } - else + + // Check if a realtime kernel is available + if (!realtime_tools::has_realtime_kernel()) { RCLCPP_WARN_STREAM(node_->get_logger(), "Realtime kernel is recommended for better performance."); } diff --git a/moveit_ros/moveit_servo/src/utils/command.cpp b/moveit_ros/moveit_servo/src/utils/command.cpp index 62a40cdd1e..fe4cb62546 100644 --- a/moveit_ros/moveit_servo/src/utils/command.cpp +++ b/moveit_ros/moveit_servo/src/utils/command.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("servo"); + return moveit::getLogger("moveit.ros.servo"); } /** diff --git a/moveit_ros/moveit_servo/src/utils/common.cpp b/moveit_ros/moveit_servo/src/utils/common.cpp index ea1512984e..66b0866819 100644 --- a/moveit_ros/moveit_servo/src/utils/common.cpp +++ b/moveit_ros/moveit_servo/src/utils/common.cpp @@ -343,7 +343,7 @@ std::pair velocityScalingFactorForSingularity(const moveit:: const bool moving_towards_singularity = vector_towards_singularity.dot(target_delta_x) > 0; // Compute upper condition variable threshold based on if we are moving towards or away from singularity. - // See https://github.com/ros-planning/moveit2/pull/620#issuecomment-1201418258 for visual explanation. + // See https://github.com/moveit/moveit2/pull/620#issuecomment-1201418258 for visual explanation. double upper_threshold; if (moving_towards_singularity) { diff --git a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst index fb88df21b6..98b1330284 100644 --- a/moveit_ros/occupancy_map_monitor/CHANGELOG.rst +++ b/moveit_ros/occupancy_map_monitor/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package moveit_ros_occupancy_map_monitor ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Enforce liboctomap-dev by using a cmake version range +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Henning Kayser, Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging in moveit_core (`#2503 `_) diff --git a/moveit_ros/occupancy_map_monitor/CMakeLists.txt b/moveit_ros/occupancy_map_monitor/CMakeLists.txt index e7bda9b16a..b8dd549bcb 100644 --- a/moveit_ros/occupancy_map_monitor/CMakeLists.txt +++ b/moveit_ros/occupancy_map_monitor/CMakeLists.txt @@ -13,44 +13,39 @@ find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) find_package(pluginlib REQUIRED) find_package(Eigen3 REQUIRED) -find_package(octomap REQUIRED) +# Enforce system version liboctomap-dev +# https://github.com/moveit/moveit2/issues/2862 +find_package(octomap 1.9.7...<1.10.0 REQUIRED) find_package(geometric_shapes REQUIRED) find_package(tf2_ros REQUIRED) - include_directories(include) -include_directories(SYSTEM - ${EIGEN3_INCLUDE_DIRS} - ${X11_INCLUDE_DIR} -) +include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS} ${X11_INCLUDE_DIR}) set(THIS_PACKAGE_INCLUDE_DEPENDS - rclcpp - moveit_core - moveit_msgs - pluginlib - octomap - geometric_shapes - Boost -) - -add_library(moveit_ros_occupancy_map_monitor SHARED - src/occupancy_map_monitor.cpp - src/occupancy_map_monitor_middleware_handle.cpp - src/occupancy_map_updater.cpp -) -set_target_properties(moveit_ros_occupancy_map_monitor PROPERTIES VERSION "${moveit_ros_occupancy_map_monitor_VERSION}") + rclcpp + moveit_core + moveit_msgs + pluginlib + octomap + geometric_shapes + Boost) + +add_library( + moveit_ros_occupancy_map_monitor SHARED + src/occupancy_map_monitor.cpp src/occupancy_map_monitor_middleware_handle.cpp + src/occupancy_map_updater.cpp) +set_target_properties( + moveit_ros_occupancy_map_monitor + PROPERTIES VERSION "${moveit_ros_occupancy_map_monitor_VERSION}") ament_target_dependencies(moveit_ros_occupancy_map_monitor - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) + ${THIS_PACKAGE_INCLUDE_DEPENDS}) add_executable(moveit_ros_occupancy_map_server src/occupancy_map_server.cpp) -ament_target_dependencies(moveit_ros_occupancy_map_server - PUBLIC ${THIS_PACKAGE_INCLUDE_DEPENDS} -) +ament_target_dependencies(moveit_ros_occupancy_map_server PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_ros_occupancy_map_server - PRIVATE - moveit_ros_occupancy_map_monitor) + PRIVATE moveit_ros_occupancy_map_monitor) install( TARGETS moveit_ros_occupancy_map_monitor @@ -58,36 +53,23 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_occupancy_map_monitor -) + INCLUDES + DESTINATION include/moveit_ros_occupancy_map_monitor) install(TARGETS moveit_ros_occupancy_map_server - DESTINATION lib/moveit_ros_occupancy_map_monitor -) + DESTINATION lib/moveit_ros_occupancy_map_monitor) install(DIRECTORY include/ DESTINATION include/moveit_ros_occupancy_map_monitor) ament_export_targets(moveit_ros_occupancy_map_monitorTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS} eigen3_cmake_module) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_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() - find_package(ament_cmake_gmock REQUIRED) ament_add_gmock(occupancy_map_monitor_tests - test/occupancy_map_monitor_tests.cpp - ) + test/occupancy_map_monitor_tests.cpp) target_link_libraries(occupancy_map_monitor_tests - moveit_ros_occupancy_map_monitor - ) + moveit_ros_occupancy_map_monitor) endif() ament_package() diff --git a/moveit_ros/occupancy_map_monitor/package.xml b/moveit_ros/occupancy_map_monitor/package.xml index 6241090bd2..6ba31b2f19 100644 --- a/moveit_ros/occupancy_map_monitor/package.xml +++ b/moveit_ros/occupancy_map_monitor/package.xml @@ -2,7 +2,7 @@ moveit_ros_occupancy_map_monitor - 2.9.0 + 2.10.0 Components of MoveIt connecting to occupancy map Henning Kayser Tyler Weaver @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan Jon Binney @@ -26,7 +26,13 @@ rclcpp moveit_core moveit_msgs + pluginlib tf2_ros geometric_shapes @@ -36,9 +42,6 @@ ament_cmake_gmock - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp index 5153362fe5..abe0819917 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor.cpp @@ -73,7 +73,7 @@ OccupancyMapMonitor::OccupancyMapMonitor(std::unique_ptr middl , debug_info_{ false } , mesh_handle_count_{ 0 } , active_{ false } - , logger_(moveit::getLogger("occupancy_map_monitor")) + , logger_(moveit::getLogger("moveit.ros.occupancy_map_monitor")) { if (middleware_handle_ == nullptr) { diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp index 92d39d9f1c..dd2eec7f22 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_monitor_middleware_handle.cpp @@ -53,7 +53,9 @@ namespace occupancy_map_monitor OccupancyMapMonitorMiddlewareHandle::OccupancyMapMonitorMiddlewareHandle(const rclcpp::Node::SharedPtr& node, double map_resolution, const std::string& map_frame) - : node_{ node }, parameters_{ map_resolution, map_frame, {} }, logger_(moveit::getLogger("occupancy_map_monitor")) + : node_{ node } + , parameters_{ map_resolution, map_frame, {} } + , logger_(moveit::getLogger("moveit.ros.occupancy_map_monitor")) { try { diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp index 1eaaf258cd..1d09c9d604 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_server.cpp @@ -61,7 +61,7 @@ static void publishOctomap(const rclcpp::Publisher:: occupancy_map_monitor::OccupancyMapMonitor& server) { octomap_msgs::msg::Octomap map; - const auto logger = moveit::getLogger("occupancy_map_monitor"); + const auto logger = moveit::getLogger("moveit.ros.occupancy_map_monitor"); map.header.frame_id = server.getMapFrame(); map.header.stamp = rclcpp::Clock().now(); diff --git a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp index d16c79291a..95d98a2ae3 100644 --- a/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp +++ b/moveit_ros/occupancy_map_monitor/src/occupancy_map_updater.cpp @@ -48,7 +48,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("OccupancyMapUpdater"); + return moveit::getLogger("moveit.ros.occupancy_map_updater"); } } // namespace diff --git a/moveit_ros/perception/CHANGELOG.rst b/moveit_ros/perception/CHANGELOG.rst index e058f161d3..e347a0bdcd 100644 --- a/moveit_ros/perception/CHANGELOG.rst +++ b/moveit_ros/perception/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package moveit_ros_perception ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Fix segmentation fault in mesh_filter/gl_renderer (`#2834 `_) +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: CihatAltiparmak, Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * (moveit_ros) add missing CYLINDER check (`#2640 `_) diff --git a/moveit_ros/perception/CMakeLists.txt b/moveit_ros/perception/CMakeLists.txt index 94fce63091..0452e04858 100644 --- a/moveit_ros/perception/CMakeLists.txt +++ b/moveit_ros/perception/CMakeLists.txt @@ -15,7 +15,7 @@ if(WITH_OPENGL) find_package(OpenGL REQUIRED) find_package(GLEW REQUIRED) - set(gl_LIBS ${gl_LIBS} ${OPENGL_LIBRARIES}) + set(GL_LIBS ${GL_LIBS} ${OPENGL_LIBRARIES}) if(APPLE) find_package(FreeGLUT REQUIRED) set(SYSTEM_GL_LIBRARIES ${GLEW_LIBRARIES} GLEW::GLEW FreeGLUT::freeglut) @@ -27,7 +27,8 @@ if(WITH_OPENGL) set(SYSTEM_GL_LIBRARIES ${GLEW_LIBRARIES} GLUT::GLUT) endif() endif() - set(perception_GL_INCLUDE_DIRS "mesh_filter/include" "depth_image_octomap_updater/include") + set(PERCEPTION_GL_INCLUDE_DIRS "mesh_filter/include" + "depth_image_octomap_updater/include") set(SYSTEM_GL_INCLUDE_DIRS ${GLEW_INCLUDE_DIR} ${GLUT_INCLUDE_DIR}) endif() @@ -59,46 +60,39 @@ find_package(OpenCV) include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DIRS - depth_image_octomap_updater/include - lazy_free_space_updater/include - mesh_filter/include - point_containment_filter/include - pointcloud_octomap_updater/include - semantic_world/include - ${perception_GL_INCLUDE_DIRS} -) + depth_image_octomap_updater/include + lazy_free_space_updater/include + mesh_filter/include + point_containment_filter/include + pointcloud_octomap_updater/include + semantic_world/include + ${PERCEPTION_GL_INCLUDE_DIRS}) set(THIS_PACKAGE_LIBRARIES - moveit_depth_image_octomap_updater - moveit_depth_image_octomap_updater_core - moveit_lazy_free_space_updater - moveit_mesh_filter - moveit_point_containment_filter - moveit_pointcloud_octomap_updater - moveit_pointcloud_octomap_updater_core - moveit_semantic_world -) + moveit_depth_image_octomap_updater + moveit_depth_image_octomap_updater_core + moveit_lazy_free_space_updater + moveit_mesh_filter + moveit_point_containment_filter + moveit_pointcloud_octomap_updater + moveit_pointcloud_octomap_updater_core + moveit_semantic_world) set(THIS_PACKAGE_INCLUDE_DEPENDS - image_transport - moveit_core - moveit_msgs - moveit_ros_occupancy_map_monitor - object_recognition_msgs - rclcpp - sensor_msgs - tf2_geometry_msgs - Eigen3 -) + image_transport + moveit_core + moveit_msgs + moveit_ros_occupancy_map_monitor + object_recognition_msgs + rclcpp + sensor_msgs + tf2_geometry_msgs + Eigen3) include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) -include_directories(SYSTEM - ${Boost_INCLUDE_DIRS} - ${EIGEN3_INCLUDE_DIRS} - ${OpenCV_INCLUDE_DIRS} - ${SYSTEM_GL_INCLUDE_DIR} - ${X11_INCLUDE_DIR} -) +include_directories( + SYSTEM ${Boost_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} + ${SYSTEM_GL_INCLUDE_DIR} ${X11_INCLUDE_DIR}) add_subdirectory(lazy_free_space_updater) add_subdirectory(point_containment_filter) @@ -116,25 +110,17 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_perception -) + INCLUDES + DESTINATION include/moveit_ros_perception) ament_export_targets(moveit_ros_perceptionTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -pluginlib_export_plugin_description_file(moveit_ros_occupancy_map_monitor "pointcloud_octomap_updater_plugin_description.xml") -pluginlib_export_plugin_description_file(moveit_ros_occupancy_map_monitor "depth_image_octomap_updater_plugin_description.xml") - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_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() -endif() +pluginlib_export_plugin_description_file( + moveit_ros_occupancy_map_monitor + "pointcloud_octomap_updater_plugin_description.xml") +pluginlib_export_plugin_description_file( + moveit_ros_occupancy_map_monitor + "depth_image_octomap_updater_plugin_description.xml") ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt index 8b8268b357..752666a5dc 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/depth_image_octomap_updater/CMakeLists.txt @@ -1,6 +1,9 @@ -add_library(moveit_depth_image_octomap_updater_core SHARED src/depth_image_octomap_updater.cpp) -set_target_properties(moveit_depth_image_octomap_updater_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_depth_image_octomap_updater_core +add_library(moveit_depth_image_octomap_updater_core SHARED + src/depth_image_octomap_updater.cpp) +set_target_properties(moveit_depth_image_octomap_updater_core + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_depth_image_octomap_updater_core rclcpp moveit_core image_transport @@ -8,13 +11,15 @@ ament_target_dependencies(moveit_depth_image_octomap_updater_core tf2 tf2_geometry_msgs geometric_shapes - moveit_ros_occupancy_map_monitor -) -target_link_libraries(moveit_depth_image_octomap_updater_core moveit_lazy_free_space_updater moveit_mesh_filter) + moveit_ros_occupancy_map_monitor) +target_link_libraries(moveit_depth_image_octomap_updater_core + moveit_lazy_free_space_updater moveit_mesh_filter) add_library(moveit_depth_image_octomap_updater SHARED src/updater_plugin.cpp) -set_target_properties(moveit_depth_image_octomap_updater PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_depth_image_octomap_updater +set_target_properties(moveit_depth_image_octomap_updater + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_depth_image_octomap_updater rclcpp moveit_core image_transport @@ -23,8 +28,8 @@ ament_target_dependencies(moveit_depth_image_octomap_updater tf2_geometry_msgs geometric_shapes moveit_ros_occupancy_map_monitor - pluginlib -) -target_link_libraries(moveit_depth_image_octomap_updater moveit_depth_image_octomap_updater_core) + pluginlib) +target_link_libraries(moveit_depth_image_octomap_updater + moveit_depth_image_octomap_updater_core) install(DIRECTORY include/ DESTINATION include/moveit_ros_perception) diff --git a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp index 7f06e53c90..5e2fceb8a0 100644 --- a/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp +++ b/moveit_ros/perception/depth_image_octomap_updater/src/depth_image_octomap_updater.cpp @@ -71,7 +71,7 @@ DepthImageOctomapUpdater::DepthImageOctomapUpdater() , K2_(0.0) , K4_(0.0) , K5_(0.0) - , logger_(moveit::getLogger("depth_image_octomap_updater")) + , logger_(moveit::getLogger("moveit.ros.depth_image_octomap_updater")) { } diff --git a/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt b/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt index eed138660a..ce2ce81c29 100644 --- a/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt +++ b/moveit_ros/perception/lazy_free_space_updater/CMakeLists.txt @@ -1,14 +1,16 @@ -add_library(moveit_lazy_free_space_updater SHARED src/lazy_free_space_updater.cpp) -set_target_properties(moveit_lazy_free_space_updater PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(moveit_lazy_free_space_updater PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -set_target_properties(moveit_lazy_free_space_updater PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") +add_library(moveit_lazy_free_space_updater SHARED + src/lazy_free_space_updater.cpp) +set_target_properties(moveit_lazy_free_space_updater + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties( + moveit_lazy_free_space_updater + PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set_target_properties(moveit_lazy_free_space_updater + PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") if(APPLE) target_link_libraries(moveit_lazy_free_space_updater OpenMP::OpenMP_CXX) endif() -ament_target_dependencies(moveit_lazy_free_space_updater - rclcpp - moveit_ros_occupancy_map_monitor - sensor_msgs -) +ament_target_dependencies(moveit_lazy_free_space_updater rclcpp + moveit_ros_occupancy_map_monitor sensor_msgs) install(DIRECTORY include/ DESTINATION include/moveit_ros_perception) diff --git a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp index 2486738ad6..fff1edf040 100644 --- a/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp +++ b/moveit_ros/perception/lazy_free_space_updater/src/lazy_free_space_updater.cpp @@ -51,7 +51,7 @@ LazyFreeSpaceUpdater::LazyFreeSpaceUpdater(const collision_detection::OccMapTree , process_model_cells_set_(nullptr) , update_thread_([this] { lazyUpdateThread(); }) , process_thread_([this] { processThread(); }) - , logger_(moveit::getLogger("lazy_free_space_updater")) + , logger_(moveit::getLogger("moveit.ros.lazy_free_space_updater")) { } diff --git a/moveit_ros/perception/mesh_filter/CMakeLists.txt b/moveit_ros/perception/mesh_filter/CMakeLists.txt index 12be3698fe..ee1844df52 100644 --- a/moveit_ros/perception/mesh_filter/CMakeLists.txt +++ b/moveit_ros/perception/mesh_filter/CMakeLists.txt @@ -1,43 +1,34 @@ -add_library(moveit_mesh_filter SHARED - src/mesh_filter_base.cpp - src/sensor_model.cpp - src/stereo_camera_model.cpp - src/gl_renderer.cpp - src/gl_mesh.cpp -) +add_library( + moveit_mesh_filter SHARED + src/mesh_filter_base.cpp src/sensor_model.cpp src/stereo_camera_model.cpp + src/gl_renderer.cpp src/gl_mesh.cpp) include(GenerateExportHeader) generate_export_header(moveit_mesh_filter) -target_include_directories(moveit_mesh_filter PUBLIC $) -set_target_properties(moveit_mesh_filter PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_mesh_filter - rclcpp - moveit_core - geometric_shapes - Eigen3 -) +target_include_directories( + moveit_mesh_filter PUBLIC $) +set_target_properties(moveit_mesh_filter + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_mesh_filter rclcpp moveit_core + geometric_shapes Eigen3) -target_link_libraries(moveit_mesh_filter ${gl_LIBS} ${SYSTEM_GL_LIBRARIES}) +target_link_libraries(moveit_mesh_filter ${GL_LIBS} ${SYSTEM_GL_LIBRARIES}) -# TODO: Port to ROS2 -# add_library(moveit_depth_self_filter SHARED -# src/depth_self_filter_nodelet.cpp -# src/transform_provider.cpp -# ) -# set_target_properties(moveit_depth_self_filter PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +# TODO: Port to ROS2 add_library(moveit_depth_self_filter SHARED +# src/depth_self_filter_nodelet.cpp src/transform_provider.cpp ) +# set_target_properties(moveit_depth_self_filter PROPERTIES VERSION +# "${${PROJECT_NAME}_VERSION}") # -# target_link_libraries(moveit_depth_self_filter ${catkin_LIBRARIES} moveit_mesh_filter) +# target_link_libraries(moveit_depth_self_filter ${catkin_LIBRARIES} +# moveit_mesh_filter) -# TODO: enable testing -# if(CATKIN_ENABLE_TESTING) -# #catkin_lint: ignore_once env_var -# # Can only run this test if we have a display -# if(DEFINED ENV{DISPLAY} AND NOT $ENV{DISPLAY} STREQUAL "") -# catkin_add_gtest(mesh_filter_test test/mesh_filter_test.cpp) -# target_link_libraries(mesh_filter_test ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_mesh_filter) -# else() -# message("No display, will not configure tests for moveit_ros_perception/mesh_filter") -# endif() -# endif() +# TODO: enable testing if(CATKIN_ENABLE_TESTING) #catkin_lint: ignore_once +# env_var # Can only run this test if we have a display if(DEFINED ENV{DISPLAY} +# AND NOT $ENV{DISPLAY} STREQUAL "") catkin_add_gtest(mesh_filter_test +# test/mesh_filter_test.cpp) target_link_libraries(mesh_filter_test +# ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_mesh_filter) else() message("No +# display, will not configure tests for moveit_ros_perception/mesh_filter") +# endif() endif() install(DIRECTORY include/ DESTINATION include/moveit_ros_perception) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_mesh_filter_export.h DESTINATION include/moveit_ros_perception) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_mesh_filter_export.h + DESTINATION include/moveit_ros_perception) diff --git a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp index b8f03d45fc..3c2d7d29e3 100644 --- a/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp +++ b/moveit_ros/perception/mesh_filter/src/gl_renderer.cpp @@ -344,7 +344,7 @@ GLuint mesh_filter::GLRenderer::loadShaders(const string& vertex_source, const s glGetProgramInfoLog(program_id, info_log_length, nullptr, &program_error_message[0]); std::size_t l = strnlen(&program_error_message[0], program_error_message.size()); if (l > 0) - RCLCPP_ERROR(moveit::getLogger("gl_renderer"), "%s\n", &program_error_message[0]); + RCLCPP_ERROR(moveit::getLogger("moveit.ros.gl_renderer"), "%s\n", &program_error_message[0]); } if (vertex_shader_id) @@ -387,7 +387,7 @@ void mesh_filter::GLRenderer::createGLContext() if (context_it == s_context.end()) { - s_context.at(thread_id) = std::pair(1, 0); + s_context.insert({ thread_id, std::pair(1, 0) }); glutInitWindowPosition(glutGet(GLUT_SCREEN_WIDTH) + 30000, 0); glutInitWindowSize(1, 1); diff --git a/moveit_ros/perception/package.xml b/moveit_ros/perception/package.xml index 4f3701d9fd..a50c122553 100644 --- a/moveit_ros/perception/package.xml +++ b/moveit_ros/perception/package.xml @@ -2,7 +2,7 @@ moveit_ros_perception - 2.9.0 + 2.10.0 Components of MoveIt connecting to perception Henning Kayser Tyler Weaver @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan Jon Binney @@ -45,8 +45,6 @@ eigen - ament_lint_auto - ament_lint_common ament_cmake diff --git a/moveit_ros/perception/point_containment_filter/CMakeLists.txt b/moveit_ros/perception/point_containment_filter/CMakeLists.txt index 44f72bd3f1..f13a319e1e 100644 --- a/moveit_ros/perception/point_containment_filter/CMakeLists.txt +++ b/moveit_ros/perception/point_containment_filter/CMakeLists.txt @@ -1,12 +1,12 @@ add_library(moveit_point_containment_filter SHARED src/shape_mask.cpp) -set_target_properties(moveit_point_containment_filter PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(moveit_point_containment_filter PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -set_target_properties(moveit_point_containment_filter PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") -ament_target_dependencies(moveit_point_containment_filter - rclcpp - sensor_msgs - geometric_shapes - moveit_core -) +set_target_properties(moveit_point_containment_filter + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties( + moveit_point_containment_filter + PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set_target_properties(moveit_point_containment_filter + PROPERTIES LINK_FLAGS "${OpenMP_CXX_FLAGS}") +ament_target_dependencies(moveit_point_containment_filter rclcpp sensor_msgs + geometric_shapes moveit_core) install(DIRECTORY include/ DESTINATION include/moveit_ros_perception) diff --git a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp index fde28345d2..db931d5b54 100644 --- a/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp +++ b/moveit_ros/perception/point_containment_filter/src/shape_mask.cpp @@ -45,7 +45,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("shape_mask"); + return moveit::getLogger("moveit.ros.shape_mask"); } } // namespace diff --git a/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt b/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt index 3d4a95a147..6b928b506d 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt +++ b/moveit_ros/perception/pointcloud_octomap_updater/CMakeLists.txt @@ -1,6 +1,9 @@ -add_library(moveit_pointcloud_octomap_updater_core SHARED src/pointcloud_octomap_updater.cpp) -set_target_properties(moveit_pointcloud_octomap_updater_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_pointcloud_octomap_updater_core +add_library(moveit_pointcloud_octomap_updater_core SHARED + src/pointcloud_octomap_updater.cpp) +set_target_properties(moveit_pointcloud_octomap_updater_core + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_pointcloud_octomap_updater_core rclcpp moveit_core tf2_ros @@ -8,15 +11,21 @@ ament_target_dependencies(moveit_pointcloud_octomap_updater_core sensor_msgs moveit_ros_occupancy_map_monitor tf2_geometry_msgs - tf2 -) -target_link_libraries(moveit_pointcloud_octomap_updater_core moveit_point_containment_filter) -set_target_properties(moveit_pointcloud_octomap_updater_core PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") -set_target_properties(moveit_pointcloud_octomap_updater_core PROPERTIES LINK_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") + tf2) +target_link_libraries(moveit_pointcloud_octomap_updater_core + moveit_point_containment_filter) +set_target_properties( + moveit_pointcloud_octomap_updater_core + PROPERTIES COMPILE_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") +set_target_properties( + moveit_pointcloud_octomap_updater_core + PROPERTIES LINK_FLAGS "${CMAKE_CXX_FLAGS} ${OpenMP_CXX_FLAGS}") add_library(moveit_pointcloud_octomap_updater SHARED src/plugin_init.cpp) -set_target_properties(moveit_pointcloud_octomap_updater PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_pointcloud_octomap_updater +set_target_properties(moveit_pointcloud_octomap_updater + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_pointcloud_octomap_updater rclcpp moveit_core tf2_ros @@ -25,8 +34,8 @@ ament_target_dependencies(moveit_pointcloud_octomap_updater moveit_ros_occupancy_map_monitor tf2_geometry_msgs tf2 - pluginlib -) -target_link_libraries(moveit_pointcloud_octomap_updater moveit_pointcloud_octomap_updater_core) + pluginlib) +target_link_libraries(moveit_pointcloud_octomap_updater + moveit_pointcloud_octomap_updater_core) install(DIRECTORY include/ DESTINATION include/moveit_ros_perception) diff --git a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp index 315bb5d607..6e2b8678d8 100644 --- a/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp +++ b/moveit_ros/perception/pointcloud_octomap_updater/src/pointcloud_octomap_updater.cpp @@ -58,7 +58,7 @@ PointCloudOctomapUpdater::PointCloudOctomapUpdater() , max_update_rate_(0) , point_cloud_subscriber_(nullptr) , point_cloud_filter_(nullptr) - , logger_(moveit::getLogger("pointcloud_octomap_updater")) + , logger_(moveit::getLogger("moveit.ros.pointcloud_octomap_updater")) { } diff --git a/moveit_ros/perception/semantic_world/CMakeLists.txt b/moveit_ros/perception/semantic_world/CMakeLists.txt index a4bd574133..dc8d941daa 100644 --- a/moveit_ros/perception/semantic_world/CMakeLists.txt +++ b/moveit_ros/perception/semantic_world/CMakeLists.txt @@ -1,6 +1,8 @@ add_library(moveit_semantic_world SHARED src/semantic_world.cpp) -set_target_properties(moveit_semantic_world PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_semantic_world +set_target_properties(moveit_semantic_world + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_semantic_world PUBLIC rclcpp moveit_core @@ -11,7 +13,6 @@ ament_target_dependencies(moveit_semantic_world moveit_msgs tf2_eigen Eigen3 - Boost -) + Boost) install(DIRECTORY include/ DESTINATION include/moveit_ros_perception) diff --git a/moveit_ros/perception/semantic_world/src/semantic_world.cpp b/moveit_ros/perception/semantic_world/src/semantic_world.cpp index 5be32f7a2a..337630c267 100644 --- a/moveit_ros/perception/semantic_world/src/semantic_world.cpp +++ b/moveit_ros/perception/semantic_world/src/semantic_world.cpp @@ -67,7 +67,7 @@ namespace semantic_world SemanticWorld::SemanticWorld(const rclcpp::Node::SharedPtr& node, const planning_scene::PlanningSceneConstPtr& planning_scene) - : planning_scene_(planning_scene), node_handle_(node), logger_(moveit::getLogger("semantic_world")) + : planning_scene_(planning_scene), node_handle_(node), logger_(moveit::getLogger("moveit.ros.semantic_world")) { table_subscriber_ = node_handle_->create_subscription( diff --git a/moveit_ros/planning/CHANGELOG.rst b/moveit_ros/planning/CHANGELOG.rst index e4d9da718e..b8b17e2d80 100644 --- a/moveit_ros/planning/CHANGELOG.rst +++ b/moveit_ros/planning/CHANGELOG.rst @@ -2,6 +2,74 @@ Changelog for package moveit_ros_planning ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Apply clang-tidy fixes +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Enable mdof trajectory execution (`#2740 `_) + * Add RobotTrajectory conversion from MDOF to joints + * Convert MDOF trajectories to joint trajectories in planning interfaces + * Treat mdof joint variables as common joints in + TrajectoryExecutionManager + * Convert multi-DOF trajectories to joints in TEM + * Revert "Convert MDOF trajectories to joint trajectories in planning interfaces" + This reverts commit 885ee2718594859555b73dc341311a859d31216e. + * Handle multi-DOF variables in TEM's bound checking + * Add parameter to optionally enable multi-dof conversion + * Improve error message about unknown controllers + * Fix name ordering in JointTrajectory conversion + * Improve DEBUG output in TEM + * Comment RobotTrajectory test + * add acceleration to avoid out of bounds read + --------- + Co-authored-by: Paul Gesel + Co-authored-by: Abishalini Sivaraman + Co-authored-by: Ezra Brooks +* Skip flaky PSM launch test (`#2822 `_) +* change default to 1e308 (`#2801 `_) +* PSM: keep references to scene\_ valid upon receiving full scenes (`#2745 `_) + plan_execution-related modules rely on `plan.planning_scene\_` in many places + to point to the currently monitored scene (or a diff on top of it). + Before this patch, if the PSM would receive full scenes during execution, + `plan.planning_scene\_` would not include later incremental updates anymore + because the monitor created a new diff scene. + --------- + Co-authored-by: v4hn +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* Print links in collision (`#2727 `_) +* Do not overwrite the error code in planWithSinglePipeline (`#2723 `_) + * Do not overwrite the error code in planWithSinglePipeline + Return the `MotionPlanResponse` as-is. + * Do not rely on generatePlan() to set error code + Do not rely on generatePlan() to set the error code in all cases and + ensure that the error code is set to FAILURE if `generatePlan()` returns + false. + --------- + Co-authored-by: Gaël Écorchard +* Get configuration values of traj_exec_man (`#2702 `_) + * (ros_planning) get configuration values of traj_exec_man + * (py) get configuration values of traj_exec_man + --------- + Co-authored-by: Sebastian Jahr +* Exit earlier on failure in `generatePlan` (`#2726 `_) + With this change, the `PlanningPipeline::generatePlan()` exits as soon + as a failure is detected. Before this, `break` was used to exit the + current loop of request adapters, planners, or response adapters, but + the function continued to the next loop. For example, if a planner would + fail, the response adapters would still be executed. + Co-authored-by: Gaël Écorchard +* srdf publisher node (`#2682 `_) +* CMake format and lint in pre-commit (`#2683 `_) +* Get robot description from topic in GetUrdfService (`#2681 `_) +* Shut down PSM publishing before starting to publish on a potentially new topic (`#2680 `_) +* Contributors: Abishalini Sivaraman, Ezra Brooks, Gaël Écorchard, Henning Kayser, Matthijs van der Burgh, Paul Gesel, Robert Haschke, Sebastian Castro, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * [PSM] Process collision object color when adding object trough the planning scene monitor (`#2567 `_) diff --git a/moveit_ros/planning/CMakeLists.txt b/moveit_ros/planning/CMakeLists.txt index 4a4edb8476..17071ad226 100644 --- a/moveit_ros/planning/CMakeLists.txt +++ b/moveit_ros/planning/CMakeLists.txt @@ -6,83 +6,85 @@ find_package(moveit_common REQUIRED) moveit_package() find_package(ament_cmake REQUIRED) +find_package(ament_index_cpp REQUIRED) +find_package(Eigen3 REQUIRED) find_package(fmt REQUIRED) find_package(generate_parameter_library REQUIRED) +find_package(message_filters REQUIRED) +find_package(moveit_core REQUIRED) find_package(moveit_msgs REQUIRED) -# find_package(moveit_ros_perception REQUIRED) +find_package(moveit_ros_occupancy_map_monitor REQUIRED) find_package(pluginlib REQUIRED) find_package(rclcpp REQUIRED) -find_package(message_filters REQUIRED) +find_package(rclcpp_components REQUIRED) find_package(srdfdom REQUIRED) -find_package(urdf REQUIRED) +find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) -find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_eigen REQUIRED) +find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_msgs REQUIRED) find_package(tf2_ros REQUIRED) -find_package(Eigen3 REQUIRED) -find_package(moveit_core REQUIRED) -find_package(ament_index_cpp REQUIRED) -find_package(moveit_ros_occupancy_map_monitor REQUIRED) +find_package(urdf REQUIRED) +# find_package(moveit_ros_perception REQUIRED) # Finds Boost Components include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DIRS - rdf_loader/include - kinematics_plugin_loader/include - robot_model_loader/include + collision_plugin_loader/include constraint_sampler_manager_loader/include - planning_pipeline/include + kinematics_plugin_loader/include + moveit_cpp/include + plan_execution/include planning_pipeline_interfaces/include + planning_pipeline/include planning_scene_monitor/include - trajectory_execution_manager/include - plan_execution/include - collision_plugin_loader/include - moveit_cpp/include -) + rdf_loader/include + robot_model_loader/include + trajectory_execution_manager/include) set(THIS_PACKAGE_LIBRARIES - moveit_rdf_loader - moveit_kinematics_plugin_loader - moveit_robot_model_loader - moveit_constraint_sampler_manager_loader - planning_pipeline_parameters - moveit_planning_pipeline - moveit_planning_pipeline_interfaces - moveit_trajectory_execution_manager - moveit_plan_execution - moveit_planning_scene_monitor - moveit_collision_plugin_loader - default_request_adapter_parameters - default_response_adapter_parameters - moveit_default_planning_request_adapter_plugins - moveit_default_planning_response_adapter_plugins - moveit_cpp -) + default_request_adapter_parameters + default_response_adapter_parameters + moveit_collision_plugin_loader + moveit_constraint_sampler_manager_loader + moveit_cpp + moveit_default_planning_request_adapter_plugins + moveit_default_planning_response_adapter_plugins + moveit_kinematics_plugin_loader + moveit_plan_execution + moveit_planning_pipeline + moveit_planning_pipeline_interfaces + moveit_planning_scene_monitor + moveit_rdf_loader + moveit_robot_model_loader + moveit_trajectory_execution_manager + planning_pipeline_parameters + srdf_publisher_node) set(THIS_PACKAGE_INCLUDE_DEPENDS - pluginlib - generate_parameter_library - rclcpp - message_filters - srdfdom - urdf - tf2 - tf2_eigen - tf2_ros - Eigen3 - moveit_core - # moveit_ros_perception - moveit_ros_occupancy_map_monitor - moveit_msgs - tf2_msgs - tf2_geometry_msgs + Eigen3 + generate_parameter_library + message_filters + moveit_core + moveit_msgs + moveit_ros_occupancy_map_monitor + pluginlib + rclcpp + rclcpp_components + srdfdom + std_msgs + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_msgs + tf2_ros + urdf + # moveit_ros_perception ) include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) -include_directories(SYSTEM - ${EIGEN3_INCLUDE_DIRS}) +include_directories(SYSTEM ${EIGEN3_INCLUDE_DIRS}) add_subdirectory(collision_plugin_loader) add_subdirectory(constraint_sampler_manager_loader) @@ -90,13 +92,14 @@ add_subdirectory(kinematics_plugin_loader) add_subdirectory(moveit_cpp) add_subdirectory(plan_execution) add_subdirectory(planning_components_tools) -add_subdirectory(planning_pipeline) add_subdirectory(planning_pipeline_interfaces) +add_subdirectory(planning_pipeline) add_subdirectory(planning_request_adapter_plugins) add_subdirectory(planning_response_adapter_plugins) add_subdirectory(planning_scene_monitor) add_subdirectory(rdf_loader) add_subdirectory(robot_model_loader) +add_subdirectory(srdf_publisher_node) add_subdirectory(trajectory_execution_manager) install( @@ -105,26 +108,23 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_planning -) + INCLUDES + DESTINATION include/moveit_ros_planning) ament_export_targets(moveit_ros_planningTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -pluginlib_export_plugin_description_file(moveit_core "default_request_adapters_plugin_description.xml") -pluginlib_export_plugin_description_file(moveit_core "default_response_adapters_plugin_description.xml") -if(BUILD_TESTING) - pluginlib_export_plugin_description_file(moveit_core "planning_pipeline_test_plugins_description.xml") - - find_package(ament_lint_auto REQUIRED) - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_FOUND TRUE) - set(ament_cmake_uncrustify_FOUND TRUE) - set(ament_cmake_flake8_FOUND TRUE) +rclcpp_components_register_node( + srdf_publisher_node PLUGIN "moveit_ros_planning::SrdfPublisher" EXECUTABLE + srdf_publisher) - # Run all lint tests in package.xml except those listed above - ament_lint_auto_find_test_dependencies() +pluginlib_export_plugin_description_file( + moveit_core "default_request_adapters_plugin_description.xml") +pluginlib_export_plugin_description_file( + moveit_core "default_response_adapters_plugin_description.xml") +if(BUILD_TESTING) + pluginlib_export_plugin_description_file( + moveit_core "planning_pipeline_test_plugins_description.xml") endif() ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_ros/planning/ConfigExtras.cmake b/moveit_ros/planning/ConfigExtras.cmake index ecb95ca17c..3a527d95c1 100644 --- a/moveit_ros/planning/ConfigExtras.cmake +++ b/moveit_ros/planning/ConfigExtras.cmake @@ -1,11 +1,11 @@ # Extras module needed for dependencies to find boost components find_package( - Boost REQUIRED + Boost + REQUIRED system filesystem date_time program_options thread - chrono -) + chrono) diff --git a/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt b/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt index 6f9b4bcf47..bc34ed5eca 100644 --- a/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt +++ b/moveit_ros/planning/collision_plugin_loader/CMakeLists.txt @@ -1,9 +1,8 @@ -add_library(moveit_collision_plugin_loader SHARED src/collision_plugin_loader.cpp) -set_target_properties(moveit_collision_plugin_loader PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_collision_plugin_loader - moveit_core - rclcpp - pluginlib -) +add_library(moveit_collision_plugin_loader SHARED + src/collision_plugin_loader.cpp) +set_target_properties(moveit_collision_plugin_loader + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_collision_plugin_loader moveit_core rclcpp + pluginlib) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp index 5c1b9dc99d..ea362c574b 100644 --- a/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp +++ b/moveit_ros/planning/collision_plugin_loader/src/collision_plugin_loader.cpp @@ -37,7 +37,7 @@ namespace collision_detection { -CollisionPluginLoader::CollisionPluginLoader() : logger_(moveit::getLogger("collision_plugin_loader")) +CollisionPluginLoader::CollisionPluginLoader() : logger_(moveit::getLogger("moveit.ros.collision_plugin_loader")) { } diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt b/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt index 76d240d28f..222a564a78 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt +++ b/moveit_ros/planning/constraint_sampler_manager_loader/CMakeLists.txt @@ -1,15 +1,11 @@ add_library(moveit_constraint_sampler_manager_loader SHARED - src/constraint_sampler_manager_loader.cpp) + src/constraint_sampler_manager_loader.cpp) -set_target_properties(moveit_constraint_sampler_manager_loader PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_constraint_sampler_manager_loader - moveit_core - rclcpp - Boost - pluginlib -) +set_target_properties(moveit_constraint_sampler_manager_loader + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_constraint_sampler_manager_loader moveit_core + rclcpp Boost pluginlib) target_link_libraries(moveit_constraint_sampler_manager_loader - moveit_rdf_loader -) + moveit_rdf_loader) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp index 2d92f3195d..cd069ca821 100644 --- a/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp +++ b/moveit_ros/planning/constraint_sampler_manager_loader/src/constraint_sampler_manager_loader.cpp @@ -50,7 +50,7 @@ class ConstraintSamplerManagerLoader::Helper { public: Helper(const rclcpp::Node::SharedPtr& node, const constraint_samplers::ConstraintSamplerManagerPtr& csm) - : node_(node), logger_(moveit::getLogger("constraint_sampler_manager_loader")) + : node_(node), logger_(moveit::getLogger("moveit.ros.constraint_sampler_manager_loader")) { if (node_->has_parameter("constraint_samplers")) { diff --git a/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt b/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt index 0eb3afca0b..cbc9d6d0ff 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt +++ b/moveit_ros/planning/kinematics_plugin_loader/CMakeLists.txt @@ -1,29 +1,23 @@ -add_library(moveit_kinematics_plugin_loader SHARED src/kinematics_plugin_loader.cpp) -set_target_properties(moveit_kinematics_plugin_loader PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_kinematics_plugin_loader - rclcpp - urdf - pluginlib - class_loader - moveit_core -) +add_library(moveit_kinematics_plugin_loader SHARED + src/kinematics_plugin_loader.cpp) +set_target_properties(moveit_kinematics_plugin_loader + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_kinematics_plugin_loader rclcpp urdf pluginlib + class_loader moveit_core) generate_parameter_library( kinematics_parameters # cmake target name for the parameter library src/kinematics_parameters.yaml # path to input yaml file ) -target_link_libraries(moveit_kinematics_plugin_loader - moveit_rdf_loader - kinematics_parameters -) +target_link_libraries(moveit_kinematics_plugin_loader moveit_rdf_loader + kinematics_parameters) install( TARGETS kinematics_parameters EXPORT ${PROJECT_NAME}Targets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib - RUNTIME DESTINATION bin -) + RUNTIME DESTINATION bin) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h index 4adf24ee4f..96c645da15 100644 --- a/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h +++ b/moveit_ros/planning/kinematics_plugin_loader/include/moveit/kinematics_plugin_loader/kinematics_plugin_loader.h @@ -57,7 +57,9 @@ class KinematicsPluginLoader well as used to read the SRDF document when needed. */ KinematicsPluginLoader(const rclcpp::Node::SharedPtr& node, const std::string& robot_description = "robot_description") - : node_(node), robot_description_(robot_description), logger_(moveit::getLogger("kinematics_plugin_loader")) + : node_(node) + , robot_description_(robot_description) + , logger_(moveit::getLogger("moveit.ros.kinematics_plugin_loader")) { } diff --git a/moveit_ros/planning/moveit_cpp/CMakeLists.txt b/moveit_ros/planning/moveit_cpp/CMakeLists.txt index facf2e1310..59850df90f 100644 --- a/moveit_ros/planning/moveit_cpp/CMakeLists.txt +++ b/moveit_ros/planning/moveit_cpp/CMakeLists.txt @@ -1,25 +1,17 @@ -add_library(moveit_cpp SHARED - src/moveit_cpp.cpp - src/planning_component.cpp -) -set_target_properties(moveit_cpp PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_cpp - rclcpp - moveit_core -) -target_link_libraries(moveit_cpp - moveit_planning_scene_monitor - moveit_planning_pipeline - moveit_planning_pipeline_interfaces - moveit_trajectory_execution_manager) +add_library(moveit_cpp SHARED src/moveit_cpp.cpp src/planning_component.cpp) +set_target_properties(moveit_cpp PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_cpp rclcpp moveit_core) +target_link_libraries( + moveit_cpp moveit_planning_scene_monitor moveit_planning_pipeline + moveit_planning_pipeline_interfaces moveit_trajectory_execution_manager) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) -# TODO: Port MoveItCpp test -#if (BUILD_TESTING) -# find_package(moveit_resources_panda_moveit_config REQUIRED) -# find_package(rostest REQUIRED) +# TODO: Port MoveItCpp test if (BUILD_TESTING) +# find_package(moveit_resources_panda_moveit_config REQUIRED) +# find_package(rostest REQUIRED) # -# add_rostest_gtest(moveit_cpp_test test/moveit_cpp_test.test test/moveit_cpp_test.cpp) -# target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) -#endif() +# add_rostest_gtest(moveit_cpp_test test/moveit_cpp_test.test +# test/moveit_cpp_test.cpp) target_link_libraries(moveit_cpp_test moveit_cpp +# ${catkin_LIBRARIES}) endif() diff --git a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp index 9f12282120..410edc25ff 100644 --- a/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp +++ b/moveit_ros/planning/moveit_cpp/src/moveit_cpp.cpp @@ -50,7 +50,7 @@ MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node) : MoveItCpp(node, Opti } MoveItCpp::MoveItCpp(const rclcpp::Node::SharedPtr& node, const Options& options) - : node_(node), logger_(moveit::getLogger("moveit_cpp")) + : node_(node), logger_(moveit::getLogger("moveit.ros.moveit_cpp")) { // Configure planning scene monitor if (!loadPlanningSceneMonitor(options.planning_scene_monitor_options)) diff --git a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp index 270b9357cc..b3494a4a50 100644 --- a/moveit_ros/planning/moveit_cpp/src/planning_component.cpp +++ b/moveit_ros/planning/moveit_cpp/src/planning_component.cpp @@ -51,7 +51,7 @@ PlanningComponent::PlanningComponent(const std::string& group_name, const MoveIt : node_(moveit_cpp->getNode()) , moveit_cpp_(moveit_cpp) , group_name_(group_name) - , logger_(moveit::getLogger("planning_component")) + , logger_(moveit::getLogger("moveit.ros.planning_component")) { joint_model_group_ = moveit_cpp_->getRobotModel()->getJointModelGroup(group_name); if (!joint_model_group_) diff --git a/moveit_ros/planning/package.xml b/moveit_ros/planning/package.xml index 226ccd1c41..099dae1d69 100644 --- a/moveit_ros/planning/package.xml +++ b/moveit_ros/planning/package.xml @@ -2,7 +2,7 @@ moveit_ros_planning - 2.9.0 + 2.10.0 Planning components of MoveIt that use ROS Henning Kayser Tyler Weaver @@ -12,8 +12,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan Sachin Chitta @@ -24,30 +24,32 @@ eigen3_cmake_module + pluginlib ament_index_cpp + eigen + fmt generate_parameter_library + message_filters moveit_core - moveit_ros_occupancy_map_monitor moveit_msgs - message_filters - pluginlib + moveit_ros_occupancy_map_monitor rclcpp_action + rclcpp_components rclcpp srdfdom - urdf - tf2 + std_msgs tf2_eigen tf2_geometry_msgs tf2_msgs tf2_ros - eigen - fmt + tf2 + urdf - ament_lint_auto - ament_lint_common ament_cmake_gmock ament_cmake_gtest + moveit_configs_utils ros_testing + launch_testing_ament_cmake moveit_resources_panda_moveit_config diff --git a/moveit_ros/planning/plan_execution/CMakeLists.txt b/moveit_ros/planning/plan_execution/CMakeLists.txt index 5921f416e5..95c11fd8e5 100644 --- a/moveit_ros/planning/plan_execution/CMakeLists.txt +++ b/moveit_ros/planning/plan_execution/CMakeLists.txt @@ -1,17 +1,10 @@ -add_library(moveit_plan_execution SHARED - src/plan_execution.cpp) -set_target_properties(moveit_plan_execution PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_plan_execution - moveit_planning_pipeline - moveit_planning_scene_monitor - moveit_trajectory_execution_manager -) -ament_target_dependencies(moveit_plan_execution - moveit_core - rclcpp - Boost - class_loader - pluginlib -) +add_library(moveit_plan_execution SHARED src/plan_execution.cpp) +set_target_properties(moveit_plan_execution + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries( + moveit_plan_execution moveit_planning_pipeline moveit_planning_scene_monitor + moveit_trajectory_execution_manager) +ament_target_dependencies(moveit_plan_execution moveit_core rclcpp Boost + class_loader pluginlib) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/plan_execution/src/plan_execution.cpp b/moveit_ros/planning/plan_execution/src/plan_execution.cpp index 76cb40c836..6e0452f410 100644 --- a/moveit_ros/planning/plan_execution/src/plan_execution.cpp +++ b/moveit_ros/planning/plan_execution/src/plan_execution.cpp @@ -83,7 +83,7 @@ plan_execution::PlanExecution::PlanExecution( : node_(node) , planning_scene_monitor_(planning_scene_monitor) , trajectory_execution_manager_(trajectory_execution) - , logger_(moveit::getLogger("add_time_optimal_parameterization")) + , logger_(moveit::getLogger("moveit.ros.add_time_optimal_parameterization")) { if (!trajectory_execution_manager_) { diff --git a/moveit_ros/planning/planning_components_tools/CMakeLists.txt b/moveit_ros/planning/planning_components_tools/CMakeLists.txt index d5c475b6d5..0eb2bd3fab 100644 --- a/moveit_ros/planning/planning_components_tools/CMakeLists.txt +++ b/moveit_ros/planning/planning_components_tools/CMakeLists.txt @@ -1,62 +1,56 @@ -# NOTE: For some reason ament_lint_cmake [linelength] only complains about this file -# TODO(henningkayser): Disable linelength filter once number of digits is configurable - set to 120 -# lint_cmake: -linelength -add_executable(moveit_print_planning_model_info src/print_planning_model_info.cpp) -target_link_libraries(moveit_print_planning_model_info moveit_robot_model_loader) -ament_target_dependencies(moveit_print_planning_model_info - rclcpp -) +# NOTE: For some reason ament_lint_cmake [linelength] only caware omplains about +# this file +# TODO(henningkayser): Disable linelength filter once number of digits is +# configurable - set to 120 lint_cmake: -linelength +add_executable(moveit_print_planning_model_info + src/print_planning_model_info.cpp) +target_link_libraries(moveit_print_planning_model_info + moveit_robot_model_loader) +ament_target_dependencies(moveit_print_planning_model_info rclcpp) -add_executable(moveit_print_planning_scene_info src/print_planning_scene_info.cpp) -target_link_libraries(moveit_print_planning_scene_info moveit_planning_scene_monitor) -ament_target_dependencies(moveit_print_planning_scene_info - rclcpp -) +add_executable(moveit_print_planning_scene_info + src/print_planning_scene_info.cpp) +target_link_libraries(moveit_print_planning_scene_info + moveit_planning_scene_monitor) +ament_target_dependencies(moveit_print_planning_scene_info rclcpp) add_executable(moveit_display_random_state src/display_random_state.cpp) target_link_libraries(moveit_display_random_state moveit_planning_scene_monitor) -ament_target_dependencies(moveit_display_random_state - rclcpp -) +ament_target_dependencies(moveit_display_random_state rclcpp) -add_executable(moveit_visualize_robot_collision_volume src/visualize_robot_collision_volume.cpp) -target_link_libraries(moveit_visualize_robot_collision_volume PRIVATE moveit_planning_scene_monitor) -ament_target_dependencies(moveit_visualize_robot_collision_volume - PUBLIC - rclcpp - tf2_ros -) +add_executable(moveit_visualize_robot_collision_volume + src/visualize_robot_collision_volume.cpp) +target_link_libraries(moveit_visualize_robot_collision_volume + PRIVATE moveit_planning_scene_monitor) +ament_target_dependencies(moveit_visualize_robot_collision_volume PUBLIC rclcpp + tf2_ros) -add_executable(moveit_evaluate_collision_checking_speed src/evaluate_collision_checking_speed.cpp) -target_link_libraries(moveit_evaluate_collision_checking_speed moveit_planning_scene_monitor) -ament_target_dependencies(moveit_evaluate_collision_checking_speed - rclcpp - Boost -) +add_executable(moveit_evaluate_collision_checking_speed + src/evaluate_collision_checking_speed.cpp) +target_link_libraries(moveit_evaluate_collision_checking_speed + moveit_planning_scene_monitor) +ament_target_dependencies(moveit_evaluate_collision_checking_speed rclcpp Boost) if("${catkin_LIBRARIES}" MATCHES "moveit_collision_detection_bullet") - add_executable(moveit_compare_collision_checking_speed_fcl_bullet src/compare_collision_speed_checking_fcl_bullet.cpp) - target_link_libraries(moveit_compare_collision_checking_speed_fcl_bullet - moveit_planning_scene_monitor - ${catkin_LIBRARIES} - ${Boost_LIBRARIES} - ) + add_executable(moveit_compare_collision_checking_speed_fcl_bullet + src/compare_collision_speed_checking_fcl_bullet.cpp) + target_link_libraries( + moveit_compare_collision_checking_speed_fcl_bullet + moveit_planning_scene_monitor ${catkin_LIBRARIES} ${Boost_LIBRARIES}) endif() add_executable(moveit_publish_scene_from_text src/publish_scene_from_text.cpp) -target_link_libraries(moveit_publish_scene_from_text PRIVATE moveit_planning_scene_monitor moveit_robot_model_loader) -ament_target_dependencies(moveit_publish_scene_from_text - PUBLIC - rclcpp -) +target_link_libraries( + moveit_publish_scene_from_text PRIVATE moveit_planning_scene_monitor + moveit_robot_model_loader) +ament_target_dependencies(moveit_publish_scene_from_text PUBLIC rclcpp) -install(TARGETS - moveit_print_planning_model_info - moveit_print_planning_scene_info - moveit_display_random_state - moveit_visualize_robot_collision_volume - moveit_evaluate_collision_checking_speed - moveit_publish_scene_from_text - RUNTIME DESTINATION lib/${PROJECT_NAME} -) +install( + TARGETS moveit_print_planning_model_info + moveit_print_planning_scene_info + moveit_display_random_state + moveit_visualize_robot_collision_volume + moveit_evaluate_collision_checking_speed + moveit_publish_scene_from_text + RUNTIME DESTINATION lib/${PROJECT_NAME}) # lint_cmake: diff --git a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp index 8c94f8433f..93b8443fac 100644 --- a/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp +++ b/moveit_ros/planning/planning_components_tools/src/evaluate_collision_checking_speed.cpp @@ -47,7 +47,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("evaluate_collision_checking_speed"); + return moveit::getLogger("moveit.ros.evaluate_collision_checking_speed"); } } // namespace diff --git a/moveit_ros/planning/planning_pipeline/CMakeLists.txt b/moveit_ros/planning/planning_pipeline/CMakeLists.txt index 44116520ac..d3e9c75790 100644 --- a/moveit_ros/planning/planning_pipeline/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline/CMakeLists.txt @@ -1,53 +1,50 @@ -generate_parameter_library(planning_pipeline_parameters res/planning_pipeline_parameters.yaml) +generate_parameter_library(planning_pipeline_parameters + res/planning_pipeline_parameters.yaml) add_library(moveit_planning_pipeline SHARED src/planning_pipeline.cpp) target_link_libraries(moveit_planning_pipeline planning_pipeline_parameters) include(GenerateExportHeader) generate_export_header(moveit_planning_pipeline) -target_include_directories(moveit_planning_pipeline PUBLIC $) -set_target_properties(moveit_planning_pipeline PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_include_directories( + moveit_planning_pipeline + PUBLIC $) +set_target_properties(moveit_planning_pipeline + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_planning_pipeline - moveit_core - moveit_msgs - rclcpp - pluginlib -) +ament_target_dependencies(moveit_planning_pipeline moveit_core moveit_msgs + rclcpp pluginlib) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) add_library(moveit_pipeline_test_plugins SHARED - test/planning_pipeline_test_plugins.cpp - ) - target_include_directories(moveit_pipeline_test_plugins PUBLIC $) - ament_target_dependencies(moveit_pipeline_test_plugins - moveit_core - rclcpp - pluginlib - ) - set_target_properties(moveit_pipeline_test_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + test/planning_pipeline_test_plugins.cpp) + target_include_directories( + moveit_pipeline_test_plugins + PUBLIC $) + ament_target_dependencies(moveit_pipeline_test_plugins moveit_core rclcpp + pluginlib) + set_target_properties(moveit_pipeline_test_plugins + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - ament_add_gtest(moveit_planning_pipeline_test test/planning_pipeline_tests.cpp - APPEND_LIBRARY_DIRS "${append_library_dirs}") - target_include_directories(moveit_planning_pipeline_test PUBLIC $) - ament_target_dependencies(moveit_planning_pipeline_test - moveit_core - moveit_msgs - rclcpp - pluginlib - ) - target_link_libraries(moveit_planning_pipeline_test - moveit_planning_pipeline - ) + ament_add_gtest( + moveit_planning_pipeline_test test/planning_pipeline_tests.cpp + APPEND_LIBRARY_DIRS "${APPEND_LIBRARY_DIRS}") + target_include_directories( + moveit_planning_pipeline_test + PUBLIC $) + ament_target_dependencies(moveit_planning_pipeline_test moveit_core + moveit_msgs rclcpp pluginlib) + target_link_libraries(moveit_planning_pipeline_test moveit_planning_pipeline) - install(TARGETS moveit_pipeline_test_plugins + install( + TARGETS moveit_pipeline_test_plugins EXPORT moveit_pipeline_test_pluginsTargets LIBRARY DESTINATION lib ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - ) + RUNTIME DESTINATION bin) endif() install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_pipeline_export.h DESTINATION include/moveit_ros_planning) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_pipeline_export.h + DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp index cbc3d20356..882ab97b13 100644 --- a/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp +++ b/moveit_ros/planning/planning_pipeline/src/planning_pipeline.cpp @@ -44,7 +44,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("planning_pipeline"); + return moveit::getLogger("moveit.ros.planning_pipeline"); } } // namespace @@ -87,7 +87,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model , node_(node) , parameter_namespace_(parameter_namespace) , robot_model_(model) - , logger_(moveit::getLogger("planning_pipeline")) + , logger_(moveit::getLogger("moveit.ros.planning_pipeline")) { auto param_listener = planning_pipeline_parameters::ParamListener(node, parameter_namespace); pipeline_parameters_ = param_listener.get_params(); @@ -104,7 +104,7 @@ PlanningPipeline::PlanningPipeline(const moveit::core::RobotModelConstPtr& model , node_(node) , parameter_namespace_(parameter_namespace) , robot_model_(model) - , logger_(moveit::getLogger("planning_pipeline")) + , logger_(moveit::getLogger("moveit.ros.planning_pipeline")) { pipeline_parameters_.planning_plugins = planner_plugin_names; pipeline_parameters_.request_adapters = request_adapter_plugin_names; @@ -286,7 +286,8 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& RCLCPP_ERROR(node_->get_logger(), "PlanningRequestAdapter '%s' failed, because '%s'. Aborting planning pipeline.", req_adapter->getDescription().c_str(), status.message.c_str()); - break; + active_ = false; + return false; } } @@ -309,7 +310,8 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& "Failed to create PlanningContext for planner '%s'. Aborting planning pipeline.", planner->getDescription().c_str()); res.error_code = moveit::core::MoveItErrorCode::PLANNING_FAILED; - break; + active_ = false; + return false; } // Run planner @@ -320,8 +322,10 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // If planner does not succeed, break chain and return false if (!res.error_code) { - RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed", planner->getDescription().c_str()); - break; + RCLCPP_ERROR(node_->get_logger(), "Planner '%s' failed with error code %s", planner->getDescription().c_str(), + errorCodeToString(res.error_code).c_str()); + active_ = false; + return false; } } @@ -338,9 +342,10 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // If adapter does not succeed, break chain and return false if (!res.error_code) { - RCLCPP_ERROR(node_->get_logger(), "PlanningResponseAdapter '%s' failed", - res_adapter->getDescription().c_str()); - break; + RCLCPP_ERROR(node_->get_logger(), "PlanningResponseAdapter '%s' failed with error code %s", + res_adapter->getDescription().c_str(), errorCodeToString(res.error_code).c_str()); + active_ = false; + return false; } } } @@ -365,7 +370,7 @@ bool PlanningPipeline::generatePlan(const planning_scene::PlanningSceneConstPtr& // Set planning pipeline to inactive active_ = false; - return bool(res); + return static_cast(res); } void PlanningPipeline::terminate() const diff --git a/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt b/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt index 21025589ab..a6ccb228ca 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt +++ b/moveit_ros/planning/planning_pipeline_interfaces/CMakeLists.txt @@ -1,21 +1,22 @@ -add_library(moveit_planning_pipeline_interfaces SHARED - src/planning_pipeline_interfaces.cpp - src/plan_responses_container.cpp - src/solution_selection_functions.cpp - src/stopping_criterion_function.cpp -) +add_library( + moveit_planning_pipeline_interfaces SHARED + src/planning_pipeline_interfaces.cpp src/plan_responses_container.cpp + src/solution_selection_functions.cpp src/stopping_criterion_function.cpp) include(GenerateExportHeader) generate_export_header(moveit_planning_pipeline_interfaces) -target_include_directories(moveit_planning_pipeline_interfaces PUBLIC $) -set_target_properties(moveit_planning_pipeline_interfaces PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_planning_pipeline_interfaces moveit_planning_pipeline) +target_include_directories( + moveit_planning_pipeline_interfaces + PUBLIC $) +set_target_properties(moveit_planning_pipeline_interfaces + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_planning_pipeline_interfaces + moveit_planning_pipeline) -ament_target_dependencies(moveit_planning_pipeline_interfaces - moveit_core - moveit_msgs - rclcpp -) +ament_target_dependencies(moveit_planning_pipeline_interfaces moveit_core + moveit_msgs rclcpp) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_pipeline_interfaces_export.h DESTINATION include/moveit_ros_planning) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_pipeline_interfaces_export.h + DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp index 73abd5fe4c..e76b549ef6 100644 --- a/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp +++ b/moveit_ros/planning/planning_pipeline_interfaces/src/planning_pipeline_interfaces.cpp @@ -46,7 +46,7 @@ namespace planning_pipeline_interfaces rclcpp::Logger getLogger() { - return moveit::getLogger("planning_pipeline_interfaces"); + return moveit::getLogger("moveit.ros.planning_pipeline_interfaces"); } ::planning_interface::MotionPlanResponse @@ -65,7 +65,13 @@ planWithSinglePipeline(const ::planning_interface::MotionPlanRequest& motion_pla const planning_pipeline::PlanningPipelinePtr pipeline = it->second; if (!pipeline->generatePlan(planning_scene, motion_plan_request, motion_plan_response)) { - motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE; + if ((motion_plan_response.error_code.val == moveit::core::MoveItErrorCode::SUCCESS) || + (motion_plan_response.error_code.val == moveit::core::MoveItErrorCode::UNDEFINED)) + { + RCLCPP_ERROR(getLogger(), "Planning pipeline '%s' failed to plan, but did not set an error code", + motion_plan_request.pipeline_id.c_str()); + motion_plan_response.error_code = moveit::core::MoveItErrorCode::FAILURE; + } } return motion_plan_response; } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt index fe60a67adb..5ad3751114 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_request_adapter_plugins/CMakeLists.txt @@ -1,18 +1,16 @@ -generate_parameter_library(default_request_adapter_parameters res/default_request_adapter_params.yaml) +generate_parameter_library(default_request_adapter_parameters + res/default_request_adapter_params.yaml) -add_library(moveit_default_planning_request_adapter_plugins SHARED - src/check_for_stacked_constraints.cpp - src/check_start_state_bounds.cpp - src/check_start_state_collision.cpp - src/validate_workspace_bounds.cpp - src/resolve_constraint_frames.cpp -) +add_library( + moveit_default_planning_request_adapter_plugins SHARED + src/check_for_stacked_constraints.cpp src/check_start_state_bounds.cpp + src/check_start_state_collision.cpp src/validate_workspace_bounds.cpp + src/resolve_constraint_frames.cpp) -target_link_libraries(moveit_default_planning_request_adapter_plugins default_request_adapter_parameters) +target_link_libraries(moveit_default_planning_request_adapter_plugins + default_request_adapter_parameters) -set_target_properties(moveit_default_planning_request_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(moveit_default_planning_request_adapter_plugins + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(moveit_default_planning_request_adapter_plugins - moveit_core - rclcpp - pluginlib -) + moveit_core rclcpp pluginlib) diff --git a/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml b/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml index 859c32c1bb..9af11adad9 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml +++ b/moveit_ros/planning/planning_request_adapter_plugins/res/default_request_adapter_params.yaml @@ -8,5 +8,5 @@ default_request_adapter_parameters: default_workspace_bounds: { type: double, description: "ValidateWorkspaceBounds: Default workspace bounds representing a cube around the robot's origin whose edge length this parameter defines.", - default_value: 10.0, + default_value: 1000000000000.0, # TODO ideally, this should be inf or 1e308, but this notation is not working in version 0.3.8 } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp index 07f29b6cd9..6aed9555b2 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_for_stacked_constraints.cpp @@ -52,7 +52,7 @@ namespace default_planning_request_adapters class CheckForStackedConstraints : public planning_interface::PlanningRequestAdapter { public: - CheckForStackedConstraints() : logger_(moveit::getLogger("check_for_stacked_constraints")) + CheckForStackedConstraints() : logger_(moveit::getLogger("moveit.ros.check_for_stacked_constraints")) { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp index 0d938f2384..bde29d5e12 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_bounds.cpp @@ -62,7 +62,7 @@ namespace default_planning_request_adapters class CheckStartStateBounds : public planning_interface::PlanningRequestAdapter { public: - CheckStartStateBounds() : logger_(moveit::getLogger("check_start_state_bounds")) + CheckStartStateBounds() : logger_(moveit::getLogger("moveit.ros.check_start_state_bounds")) { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp index f17c89f0b4..3f3d22f6f2 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/check_start_state_collision.cpp @@ -54,7 +54,7 @@ namespace default_planning_request_adapters class CheckStartStateCollision : public planning_interface::PlanningRequestAdapter { public: - CheckStartStateCollision() : logger_(moveit::getLogger("validate_start_state")) + CheckStartStateCollision() : logger_(moveit::getLogger("moveit.ros.validate_start_state")) { } @@ -85,8 +85,18 @@ class CheckStartStateCollision : public planning_interface::PlanningRequestAdapt } else { + collision_detection::CollisionResult::ContactMap contacts; + planning_scene->getCollidingPairs(contacts); + + std::string contact_information = std::to_string(contacts.size()) + " contact(s) detected : "; + + for (const auto& [contact_pair, contact_info] : contacts) + { + contact_information.append(contact_pair.first + " - " + contact_pair.second + ", "); + } + status.val = moveit_msgs::msg::MoveItErrorCodes::START_STATE_IN_COLLISION; - status.message = std::string("Start state in collision."); + status.message = std::string(contact_information); } status.source = getDescription(); return status; diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp index d7a5d3e886..55b0562b79 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/resolve_constraint_frames.cpp @@ -49,7 +49,7 @@ namespace default_planning_request_adapters class ResolveConstraintFrames : public planning_interface::PlanningRequestAdapter { public: - ResolveConstraintFrames() : logger_(moveit::getLogger("resolve_constraint_frames")) + ResolveConstraintFrames() : logger_(moveit::getLogger("moveit.ros.resolve_constraint_frames")) { } diff --git a/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp index 5fcff9504a..de0510ef3a 100644 --- a/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp +++ b/moveit_ros/planning/planning_request_adapter_plugins/src/validate_workspace_bounds.cpp @@ -53,7 +53,7 @@ namespace default_planning_request_adapters class ValidateWorkspaceBounds : public planning_interface::PlanningRequestAdapter { public: - ValidateWorkspaceBounds() : logger_(moveit::getLogger("validate_workspace_bounds")) + ValidateWorkspaceBounds() : logger_(moveit::getLogger("moveit.ros.validate_workspace_bounds")) { } diff --git a/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt b/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt index ad68bf15ee..7471eefa34 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt +++ b/moveit_ros/planning/planning_response_adapter_plugins/CMakeLists.txt @@ -1,18 +1,15 @@ -generate_parameter_library(default_response_adapter_parameters res/default_response_adapter_params.yaml) +generate_parameter_library(default_response_adapter_parameters + res/default_response_adapter_params.yaml) -add_library(moveit_default_planning_response_adapter_plugins SHARED - src/add_ruckig_traj_smoothing.cpp - src/add_time_optimal_parameterization.cpp - src/display_motion_path.cpp - src/validate_path.cpp -) +add_library( + moveit_default_planning_response_adapter_plugins SHARED + src/add_ruckig_traj_smoothing.cpp src/add_time_optimal_parameterization.cpp + src/display_motion_path.cpp src/validate_path.cpp) -target_link_libraries(moveit_default_planning_response_adapter_plugins default_response_adapter_parameters) +target_link_libraries(moveit_default_planning_response_adapter_plugins + default_response_adapter_parameters) -set_target_properties(moveit_default_planning_response_adapter_plugins PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(moveit_default_planning_response_adapter_plugins + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") ament_target_dependencies(moveit_default_planning_response_adapter_plugins - Boost - moveit_core - rclcpp - pluginlib -) + Boost moveit_core rclcpp pluginlib) diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp index 1034f95ff5..fd6a5b4204 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/add_ruckig_traj_smoothing.cpp @@ -52,7 +52,7 @@ using namespace trajectory_processing; class AddRuckigTrajectorySmoothing : public planning_interface::PlanningResponseAdapter { public: - AddRuckigTrajectorySmoothing() : logger_(moveit::getLogger("add_ruckig_trajectory_smoothing")) + AddRuckigTrajectorySmoothing() : logger_(moveit::getLogger("moveit.ros.add_ruckig_trajectory_smoothing")) { } diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp index 9901bc08d3..7b8ed0b13c 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/add_time_optimal_parameterization.cpp @@ -49,7 +49,7 @@ using namespace trajectory_processing; class AddTimeOptimalParameterization : public planning_interface::PlanningResponseAdapter { public: - AddTimeOptimalParameterization() : logger_(moveit::getLogger("add_time_optimal_parameterization")) + AddTimeOptimalParameterization() : logger_(moveit::getLogger("moveit.ros.add_time_optimal_parameterization")) { } diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp index 9b2f98cff0..e5fcb1bc03 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/display_motion_path.cpp @@ -54,7 +54,7 @@ namespace default_planning_response_adapters class DisplayMotionPath : public planning_interface::PlanningResponseAdapter { public: - DisplayMotionPath() : logger_(moveit::getLogger("display_motion_path")) + DisplayMotionPath() : logger_(moveit::getLogger("moveit.ros.display_motion_path")) { } diff --git a/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp b/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp index d91b05c27a..c5e47cda1f 100644 --- a/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp +++ b/moveit_ros/planning/planning_response_adapter_plugins/src/validate_path.cpp @@ -52,7 +52,7 @@ namespace default_planning_response_adapters class ValidateSolution : public planning_interface::PlanningResponseAdapter { public: - ValidateSolution() : logger_(moveit::getLogger("validate_solution")) + ValidateSolution() : logger_(moveit::getLogger("moveit.ros.validate_solution")) { } diff --git a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt index b25347f88e..8aa26aa4a7 100644 --- a/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt +++ b/moveit_ros/planning/planning_scene_monitor/CMakeLists.txt @@ -1,61 +1,63 @@ -add_library(moveit_planning_scene_monitor SHARED - src/planning_scene_monitor.cpp - src/current_state_monitor.cpp - src/current_state_monitor_middleware_handle.cpp - src/trajectory_monitor.cpp - src/trajectory_monitor_middleware_handle.cpp -) +add_library( + moveit_planning_scene_monitor SHARED + src/planning_scene_monitor.cpp src/current_state_monitor.cpp + src/current_state_monitor_middleware_handle.cpp src/trajectory_monitor.cpp + src/trajectory_monitor_middleware_handle.cpp) include(GenerateExportHeader) generate_export_header(moveit_planning_scene_monitor) -target_include_directories(moveit_planning_scene_monitor PUBLIC $) -set_target_properties(moveit_planning_scene_monitor PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_planning_scene_monitor +target_include_directories( + moveit_planning_scene_monitor + PUBLIC $) +set_target_properties(moveit_planning_scene_monitor + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_planning_scene_monitor moveit_ros_occupancy_map_monitor message_filters urdf pluginlib rclcpp fmt - moveit_msgs -) -target_link_libraries(moveit_planning_scene_monitor - moveit_robot_model_loader - moveit_collision_plugin_loader -) + moveit_msgs) +target_link_libraries(moveit_planning_scene_monitor moveit_robot_model_loader + moveit_collision_plugin_loader) add_executable(demo_scene demos/demo_scene.cpp) -ament_target_dependencies(demo_scene +ament_target_dependencies( + demo_scene PUBLIC rclcpp fmt moveit_msgs urdf message_filters - pluginlib -) + pluginlib) target_link_libraries(demo_scene PRIVATE moveit_planning_scene_monitor) -install(TARGETS demo_scene - DESTINATION lib/${PROJECT_NAME} -) +install(TARGETS demo_scene DESTINATION lib/${PROJECT_NAME}) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_monitor_export.h DESTINATION include/moveit_ros_planning) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_monitor_export.h + DESTINATION include/moveit_ros_planning) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ament_cmake_gmock REQUIRED) + find_package(ros_testing REQUIRED) ament_add_gmock(current_state_monitor_tests - test/current_state_monitor_tests.cpp - ) + test/current_state_monitor_tests.cpp) target_link_libraries(current_state_monitor_tests - moveit_planning_scene_monitor - ) - ament_add_gmock(trajectory_monitor_tests - test/trajectory_monitor_tests.cpp - ) - target_link_libraries(trajectory_monitor_tests - moveit_planning_scene_monitor - ) + moveit_planning_scene_monitor) + ament_add_gmock(trajectory_monitor_tests test/trajectory_monitor_tests.cpp) + target_link_libraries(trajectory_monitor_tests moveit_planning_scene_monitor) + + ament_add_gtest_executable(planning_scene_monitor_test + test/planning_scene_monitor_test.cpp) + target_link_libraries(planning_scene_monitor_test + moveit_planning_scene_monitor) + ament_target_dependencies(planning_scene_monitor_test moveit_core rclcpp + moveit_msgs) + add_ros_test(test/launch/planning_scene_monitor.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") endif() diff --git a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp index bf7efdc410..572e2fd188 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/current_state_monitor.cpp @@ -60,7 +60,7 @@ CurrentStateMonitor::CurrentStateMonitor(std::unique_ptr::epsilon()) , use_sim_time_(use_sim_time) - , logger_(moveit::getLogger("current_state_monitor")) + , logger_(moveit::getLogger("moveit.ros.current_state_monitor")) { robot_state_.setToDefaultValues(); } diff --git a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp index c030a2d0a4..648cd47162 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp @@ -96,7 +96,7 @@ PlanningSceneMonitor::PlanningSceneMonitor(const rclcpp::Node::SharedPtr& node, , dt_state_update_(0.0) , shape_transform_cache_lookup_wait_time_(0, 0) , rm_loader_(rm_loader) - , logger_(moveit::getLogger("planning_scene_monitor")) + , logger_(moveit::getLogger("moveit.ros.planning_scene_monitor")) { std::vector new_args = rclcpp::NodeOptions().arguments(); new_args.push_back("--ros-args"); @@ -416,13 +416,24 @@ void PlanningSceneMonitor::startPublishingPlanningScene(SceneUpdateType update_t const std::string& planning_scene_topic) { publish_update_types_ = update_type; - if (!publish_planning_scene_ && scene_) + + if (publish_planning_scene_) + { + RCLCPP_INFO(logger_, "Stopping existing planning scene publisher."); + stopPublishingPlanningScene(); + } + + if (scene_) { planning_scene_publisher_ = pnode_->create_publisher(planning_scene_topic, 100); RCLCPP_INFO(logger_, "Publishing maintained planning scene on '%s'", planning_scene_topic.c_str()); monitorDiffs(true); publish_planning_scene_ = std::make_unique([this] { scenePublishingThread(); }); } + else + { + RCLCPP_WARN(logger_, "Did not find a planning scene, so cannot publish it."); + } } void PlanningSceneMonitor::scenePublishingThread() @@ -531,7 +542,7 @@ void PlanningSceneMonitor::getMonitoredTopics(std::vector& topics) if (collision_object_subscriber_) { // TODO (anasarrak) This has been changed to subscriber on Moveit, look at - // https://github.com/ros-planning/moveit/pull/1406/files/cb9488312c00e9c8949d89b363766f092330954d#diff-fb6e26ecc9a73d59dbdae3f3e08145e6 + // https://github.com/moveit/moveit/pull/1406/files/cb9488312c00e9c8949d89b363766f092330954d#diff-fb6e26ecc9a73d59dbdae3f3e08145e6 topics.push_back(collision_object_subscriber_->get_topic_name()); } if (planning_scene_world_subscriber_) @@ -717,7 +728,21 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann RCLCPP_DEBUG(logger_, "scene update %f robot stamp: %f", fmod(last_update_time_.seconds(), 10.), fmod(last_robot_motion_time_.seconds(), 10.)); old_scene_name = scene_->getName(); - result = scene_->usePlanningSceneMsg(scene); + + if (!scene.is_diff && parent_scene_) + { + // clear maintained (diff) scene_ and set the full new scene in parent_scene_ instead + scene_->clearDiffs(); + result = parent_scene_->setPlanningSceneMsg(scene); + // There were no callbacks for individual object changes, so rebuild the octree masks + excludeAttachedBodiesFromOctree(); + excludeWorldObjectsFromOctree(); + } + else + { + result = scene_->setPlanningSceneDiffMsg(scene); + } + if (octomap_monitor_) { if (!scene.is_diff && scene.world.octomap.octomap.data.empty()) @@ -729,23 +754,6 @@ bool PlanningSceneMonitor::newPlanningSceneMessage(const moveit_msgs::msg::Plann } robot_model_ = scene_->getRobotModel(); - // if we just reset the scene completely but we were maintaining diffs, we need to fix that - if (!scene.is_diff && parent_scene_) - { - // the scene is now decoupled from the parent, since we just reset it - scene_->setAttachedBodyUpdateCallback(moveit::core::AttachedBodyCallback()); - scene_->setCollisionObjectUpdateCallback(collision_detection::World::ObserverCallbackFn()); - parent_scene_ = scene_; - scene_ = parent_scene_->diff(); - scene_const_ = scene_; - scene_->setAttachedBodyUpdateCallback([this](moveit::core::AttachedBody* body, bool attached) { - currentStateAttachedBodyUpdateCallback(body, attached); - }); - scene_->setCollisionObjectUpdateCallback( - [this](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) { - currentWorldObjectUpdateCallback(object, action); - }); - } if (octomap_monitor_) { excludeAttachedBodiesFromOctree(); // in case updates have happened to the attached bodies, put them in diff --git a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp index 34095d4a23..1fe9ca0f4a 100644 --- a/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp +++ b/moveit_ros/planning/planning_scene_monitor/src/trajectory_monitor.cpp @@ -55,7 +55,7 @@ planning_scene_monitor::TrajectoryMonitor::TrajectoryMonitor( , middleware_handle_(std::move(middleware_handle)) , sampling_frequency_(sampling_frequency) , trajectory_(current_state_monitor_->getRobotModel(), "") - , logger_(moveit::getLogger("trajectory_monitor")) + , logger_(moveit::getLogger("moveit.ros.trajectory_monitor")) { setSamplingFrequency(sampling_frequency); } diff --git a/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py b/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py new file mode 100644 index 0000000000..0f785d4d7d --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/test/launch/planning_scene_monitor.test.py @@ -0,0 +1,94 @@ +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 + + +def generate_test_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description(file_path="config/panda.urdf.xacro") + .to_moveit_configs() + ) + + # 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 = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager-timeout", + "300", + "--controller-manager", + "/controller_manager", + ], + output="screen", + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + psm_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "planning_scene_monitor_test", + ] + ), + parameters=[ + moveit_config.to_dict(), + ], + output="screen", + ) + + return launch.LaunchDescription( + [ + launch.actions.TimerAction(period=2.0, actions=[ros2_control_node]), + launch.actions.TimerAction( + period=4.0, actions=[joint_state_broadcaster_spawner] + ), + launch.actions.TimerAction( + period=6.0, actions=[panda_arm_controller_spawner] + ), + launch.actions.TimerAction(period=9.0, actions=[psm_gtest]), + launch_testing.actions.ReadyToTest(), + ] + ), { + "psm_gtest": psm_gtest, + } + + +class TestGTestWaitForCompletion(unittest.TestCase): + @unittest.skip("Flaky test on humble, see moveit2#2821") + # Waits for test to complete, then waits a bit to make sure result files are generated + def test_gtest_run_complete(self, psm_gtest): + self.proc_info.assertWaitForShutdown(psm_gtest, timeout=4000.0) + + +@launch_testing.post_shutdown_test() +class TestGTestProcessPostShutdown(unittest.TestCase): + @unittest.skip("Flaky test on humble, see moveit2#2821") + # Checks if the test has been completed with acceptable exit codes (successful codes) + def test_gtest_pass(self, proc_info, psm_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=psm_gtest) diff --git a/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp new file mode 100644 index 0000000000..806a88f3ac --- /dev/null +++ b/moveit_ros/planning/planning_scene_monitor/test/planning_scene_monitor_test.cpp @@ -0,0 +1,154 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2023, University of Hamburg + * 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 the copyright holder 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: Michael 'v4hn' Goerner + Desc: Tests for PlanningSceneMonitor +*/ + +// ROS +#include + +// Testing +#include + +// Main class +#include +#include + +class PlanningSceneMonitorTest : public ::testing::Test +{ +public: + void SetUp() override + { + test_node_ = std::make_shared("moveit_planning_scene_monitor_test"); + executor_ = std::make_shared(); + planning_scene_monitor_ = std::make_unique( + test_node_, "robot_description", "planning_scene_monitor"); + planning_scene_monitor_->monitorDiffs(true); + scene_ = planning_scene_monitor_->getPlanningScene(); + executor_->add_node(test_node_); + executor_thread_ = std::thread([this]() { executor_->spin(); }); + } + + void TearDown() override + { + scene_.reset(); + executor_->cancel(); + if (executor_thread_.joinable()) + { + executor_thread_.join(); + } + } + +protected: + std::shared_ptr test_node_; + + // Executor and a thread to run the executor. + rclcpp::Executor::SharedPtr executor_; + std::thread executor_thread_; + + planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_; + planning_scene::PlanningScenePtr scene_; +}; + +// various code expects the monitored scene to remain the same +TEST_F(PlanningSceneMonitorTest, TestPersistentScene) +{ + auto scene{ planning_scene_monitor_->getPlanningScene() }; + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = msg.robot_state.is_diff = true; + planning_scene_monitor_->newPlanningSceneMessage(msg); + EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene()); + msg.is_diff = msg.robot_state.is_diff = false; + planning_scene_monitor_->newPlanningSceneMessage(msg); + EXPECT_EQ(scene, planning_scene_monitor_->getPlanningScene()); +} + +using UpdateType = planning_scene_monitor::PlanningSceneMonitor::SceneUpdateType; + +#define TRIGGERS_UPDATE(msg, expected_update_type) \ + { \ + planning_scene_monitor_->clearUpdateCallbacks(); \ + auto received_update_type{ UpdateType::UPDATE_NONE }; \ + planning_scene_monitor_->addUpdateCallback([&](auto type) { received_update_type = type; }); \ + planning_scene_monitor_->newPlanningSceneMessage(msg); \ + EXPECT_EQ(received_update_type, expected_update_type); \ + } + +TEST_F(PlanningSceneMonitorTest, UpdateTypes) +{ + moveit_msgs::msg::PlanningScene msg; + msg.is_diff = msg.robot_state.is_diff = true; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_NONE); + + msg.fixed_frame_transforms.emplace_back(); + msg.fixed_frame_transforms.back().header.frame_id = "base_link"; + msg.fixed_frame_transforms.back().child_frame_id = "object"; + msg.fixed_frame_transforms.back().transform.rotation.w = 1.0; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_TRANSFORMS); + msg.fixed_frame_transforms.clear(); + moveit::core::robotStateToRobotStateMsg(scene_->getCurrentState(), msg.robot_state, false); + msg.robot_state.is_diff = true; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE); + + msg.robot_state.is_diff = false; + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_STATE | UpdateType::UPDATE_GEOMETRY); + + msg.robot_state = moveit_msgs::msg::RobotState{}; + msg.robot_state.is_diff = true; + moveit_msgs::msg::CollisionObject collision_object; + collision_object.header.frame_id = "base_link"; + collision_object.id = "object"; + collision_object.operation = moveit_msgs::msg::CollisionObject::ADD; + collision_object.pose.orientation.w = 1.0; + collision_object.primitives.emplace_back(); + collision_object.primitives.back().type = shape_msgs::msg::SolidPrimitive::SPHERE; + collision_object.primitives.back().dimensions = { 1.0 }; + msg.world.collision_objects.emplace_back(collision_object); + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_GEOMETRY); + + msg.world.collision_objects.clear(); + msg.is_diff = false; + + TRIGGERS_UPDATE(msg, UpdateType::UPDATE_SCENE); +} + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/planning/rdf_loader/CMakeLists.txt b/moveit_ros/planning/rdf_loader/CMakeLists.txt index df319b18fa..70b73b8b59 100644 --- a/moveit_ros/planning/rdf_loader/CMakeLists.txt +++ b/moveit_ros/planning/rdf_loader/CMakeLists.txt @@ -1,38 +1,24 @@ -add_library(moveit_rdf_loader SHARED src/rdf_loader.cpp src/synchronized_string_parameter.cpp) -set_target_properties(moveit_rdf_loader PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_rdf_loader - rclcpp - ament_index_cpp - urdf - srdfdom - moveit_core) +add_library(moveit_rdf_loader SHARED src/rdf_loader.cpp + src/synchronized_string_parameter.cpp) +set_target_properties(moveit_rdf_loader PROPERTIES VERSION + "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_rdf_loader rclcpp ament_index_cpp urdf srdfdom + moveit_core) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) -install(DIRECTORY - test/data - test/launch - DESTINATION share/${PROJECT_NAME}/rdf_loader/test -) +install(DIRECTORY test/data test/launch + DESTINATION share/${PROJECT_NAME}/rdf_loader/test) -# TODO(sjahr) Debug and re-enable -#if(BUILD_TESTING) -# find_package(ament_cmake_gtest REQUIRED) -# find_package(ros_testing REQUIRED) +# TODO(sjahr) Debug and re-enable if(BUILD_TESTING) +# find_package(ament_cmake_gtest REQUIRED) find_package(ros_testing REQUIRED) # -# ament_add_gtest_executable(test_rdf_integration -# test/test_rdf_integration.cpp -# ) -# target_link_libraries(test_rdf_integration moveit_rdf_loader) -# add_ros_test(test/launch/test_rdf_integration.test.py TIMEOUT 120) +# ament_add_gtest_executable(test_rdf_integration test/test_rdf_integration.cpp +# ) target_link_libraries(test_rdf_integration moveit_rdf_loader) +# add_ros_test(test/launch/test_rdf_integration.test.py TIMEOUT 120) # -# add_executable(boring_string_publisher test/boring_string_publisher.cpp) -# target_link_libraries(boring_string_publisher moveit_rdf_loader) +# add_executable(boring_string_publisher test/boring_string_publisher.cpp) +# target_link_libraries(boring_string_publisher moveit_rdf_loader) # -# install( -# TARGETS -# test_rdf_integration boring_string_publisher -# ARCHIVE DESTINATION lib -# LIBRARY DESTINATION lib -# RUNTIME DESTINATION lib/${PROJECT_NAME} -# ) -#endif() +# install( TARGETS test_rdf_integration boring_string_publisher ARCHIVE +# DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION +# lib/${PROJECT_NAME} ) endif() diff --git a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h index 85d1fa28aa..cd89eda6a6 100644 --- a/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h +++ b/moveit_ros/planning/rdf_loader/include/moveit/rdf_loader/rdf_loader.h @@ -81,6 +81,12 @@ class RDFLoader return ros_name_; } + /** @brief Get the URDF string*/ + const std::string& getURDFString() const + { + return urdf_string_; + } + /** @brief Get the parsed URDF model*/ const urdf::ModelInterfaceSharedPtr& getURDF() const { diff --git a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp index 7ed8b3b273..e96337a83e 100644 --- a/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp +++ b/moveit_ros/planning/rdf_loader/src/rdf_loader.cpp @@ -60,7 +60,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("RDFLoader"); + return moveit::getLogger("moveit.ros.rdf_loader"); } } // namespace diff --git a/moveit_ros/planning/robot_model_loader/CMakeLists.txt b/moveit_ros/planning/robot_model_loader/CMakeLists.txt index 867ef0500a..284ea4a363 100644 --- a/moveit_ros/planning/robot_model_loader/CMakeLists.txt +++ b/moveit_ros/planning/robot_model_loader/CMakeLists.txt @@ -3,17 +3,11 @@ if(CMAKE_CXX_COMPILER_ID STREQUAL "Clang") endif() add_library(moveit_robot_model_loader SHARED src/robot_model_loader.cpp) -set_target_properties(moveit_robot_model_loader PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_robot_model_loader - rclcpp - urdf - Boost - moveit_core - moveit_msgs -) -target_link_libraries(moveit_robot_model_loader - moveit_rdf_loader - moveit_kinematics_plugin_loader -) +set_target_properties(moveit_robot_model_loader + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_robot_model_loader rclcpp urdf Boost + moveit_core moveit_msgs) +target_link_libraries(moveit_robot_model_loader moveit_rdf_loader + moveit_kinematics_plugin_loader) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) diff --git a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp index 91dd24b22a..40a86a1483 100644 --- a/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp +++ b/moveit_ros/planning/robot_model_loader/src/robot_model_loader.cpp @@ -49,7 +49,7 @@ namespace robot_model_loader RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const std::string& robot_description, bool load_kinematics_solvers) - : node_(node), logger_(moveit::getLogger("robot_model_loader")) + : node_(node), logger_(moveit::getLogger("moveit.ros.robot_model_loader")) { Options opt(robot_description); opt.load_kinematics_solvers = load_kinematics_solvers; @@ -57,7 +57,7 @@ RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const st } RobotModelLoader::RobotModelLoader(const rclcpp::Node::SharedPtr& node, const Options& opt) - : node_(node), logger_(moveit::getLogger("robot_model_loader")) + : node_(node), logger_(moveit::getLogger("moveit.ros.robot_model_loader")) { configure(opt); } diff --git a/moveit_ros/planning/srdf_publisher_node/CMakeLists.txt b/moveit_ros/planning/srdf_publisher_node/CMakeLists.txt new file mode 100644 index 0000000000..d548d2f1e6 --- /dev/null +++ b/moveit_ros/planning/srdf_publisher_node/CMakeLists.txt @@ -0,0 +1,9 @@ +add_library(srdf_publisher_node SHARED src/srdf_publisher_node.cpp) +ament_target_dependencies(srdf_publisher_node PUBLIC std_msgs rclcpp + rclcpp_components) + +if(BUILD_TESTING) + find_package(launch_testing_ament_cmake REQUIRED) + + add_launch_test(test/srdf_publisher_test.py TARGET test-srdf_publisher) +endif() diff --git a/moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp b/moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp new file mode 100644 index 0000000000..6684e39b16 --- /dev/null +++ b/moveit_ros/planning/srdf_publisher_node/src/srdf_publisher_node.cpp @@ -0,0 +1,101 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, PickNik Robotics 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 Robotics 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. + *********************************************************************/ + +// SrdfPublisher Node +// Author: Tyler Weaver +// +// Node with a single parameter and single publisher using transient local QoS. +// Publishes string set on parameter `robot_description_semantic` to topic +// `/robot_description_semantic`. +// +// This is similar to what [robot_state_publisher](https://github.com/ros/robot_state_publisher) +// does for the URDF using parameter `robot_description` and topic `/robot_description`. +// +// As MoveIt subscribes to the topic `/robot_description_semantic` for the srdf +// this node can be used when you want to set the SRDF parameter on only one node +// and have the rest of your system use a subscriber to that topic to get the +// SRDF. + +#include +#include + +#include + +namespace moveit_ros_planning +{ + +class SrdfPublisher : public rclcpp::Node +{ + rclcpp::Publisher::SharedPtr srdf_publisher_; + rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr on_set_parameters_handle_; + +public: + SrdfPublisher(const rclcpp::NodeOptions& options) : rclcpp::Node("srdf_publisher", options) + { + srdf_publisher_ = this->create_publisher("robot_description_semantic", + rclcpp::QoS(1).transient_local().reliable()); + + // TODO: Update the callback used here once Humble is EOL + // Using add_on_set_parameters_callback as it is the only parameter callback available in Humble. + // This is also why we have to return an always success validation. + // Once Humble is EOL use add_post_set_parameters_callback. + on_set_parameters_handle_ = + this->add_on_set_parameters_callback([this](const std::vector& parameters) { + for (auto const& parameter : parameters) + { + if (parameter.get_name() == "robot_description_semantic") + { + std_msgs::msg::String msg; + msg.data = parameter.get_value(); + srdf_publisher_->publish(msg); + break; + } + } + + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + return result; + }); + + rcl_interfaces::msg::ParameterDescriptor descriptor; + descriptor.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING; + descriptor.description = "Semantic Robot Description Format"; + this->declare_parameter("robot_description_semantic", rclcpp::ParameterType::PARAMETER_STRING, descriptor); + } +}; + +} // namespace moveit_ros_planning + +#include "rclcpp_components/register_node_macro.hpp" +RCLCPP_COMPONENTS_REGISTER_NODE(moveit_ros_planning::SrdfPublisher) diff --git a/moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py b/moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py new file mode 100644 index 0000000000..88d875bf10 --- /dev/null +++ b/moveit_ros/planning/srdf_publisher_node/test/srdf_publisher_test.py @@ -0,0 +1,124 @@ +# Software License Agreement (BSD License) +# +# Copyright (c) 2024, PickNik Robotics 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 Robotics 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: Tyler Weaver + +import unittest +from threading import Event +from threading import Thread + +import launch +import launch_ros +import launch_testing +import rclpy +from rclpy.node import Node +from rclpy.qos import ( + QoSProfile, + QoSReliabilityPolicy, + QoSDurabilityPolicy, +) +from std_msgs.msg import String + +import pytest + + +@pytest.mark.launch_test +def generate_test_description(): + srdf_publisher = launch_ros.actions.Node( + package="moveit_ros_planning", + parameters=[{"robot_description_semantic": "test value"}], + executable="srdf_publisher", + output="screen", + ) + + return launch.LaunchDescription( + [ + srdf_publisher, + # Start tests right away - no need to wait for anything + launch_testing.actions.ReadyToTest(), + ] + ), {"srdf_publisher": srdf_publisher} + + +class SrdfObserverNode(Node): + def __init__(self, name="srdf_observer_node"): + super().__init__(name) + self.msg_event_object = Event() + self.srdf = "" + + def start_subscriber(self): + # Create a subscriber + self.subscription = self.create_subscription( + String, + "robot_description_semantic", + self.subscriber_callback, + qos_profile=QoSProfile( + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL, + depth=1, + ), + ) + + # Add a spin thread + self.ros_spin_thread = Thread( + target=lambda node: rclpy.spin(node), args=(self,) + ) + self.ros_spin_thread.start() + + def subscriber_callback(self, msg): + self.srdf = msg.data + self.msg_event_object.set() + + +# These tests will run concurrently with the dut process. After all these tests are done, +# the launch system will shut down the processes that it started up +class TestFixture(unittest.TestCase): + def test_rosout_msgs_published(self, proc_output): + rclpy.init() + try: + node = SrdfObserverNode() + node.start_subscriber() + msgs_received_flag = node.msg_event_object.wait(timeout=5.0) + assert ( + msgs_received_flag + ), "Did not receive message on `/robot_description_semantic`!" + assert node.srdf == "test value" + finally: + rclpy.shutdown() + + +@launch_testing.post_shutdown_test() +class TestProcessOutput(unittest.TestCase): + def test_exit_code(self, proc_info): + # Check that all processes in the launch (in this case, there's just one) exit + # with code 0 + launch_testing.asserts.assertExitCodes(proc_info) diff --git a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt index d23277f592..929e8c88f6 100644 --- a/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt +++ b/moveit_ros/planning/trajectory_execution_manager/CMakeLists.txt @@ -1,9 +1,14 @@ -add_library(moveit_trajectory_execution_manager SHARED src/trajectory_execution_manager.cpp) +add_library(moveit_trajectory_execution_manager SHARED + src/trajectory_execution_manager.cpp) include(GenerateExportHeader) generate_export_header(moveit_trajectory_execution_manager) -target_include_directories(moveit_trajectory_execution_manager PUBLIC $) -set_target_properties(moveit_trajectory_execution_manager PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_trajectory_execution_manager +target_include_directories( + moveit_trajectory_execution_manager + PUBLIC $) +set_target_properties(moveit_trajectory_execution_manager + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies( + moveit_trajectory_execution_manager moveit_core moveit_ros_occupancy_map_monitor rclcpp @@ -11,25 +16,27 @@ ament_target_dependencies(moveit_trajectory_execution_manager std_msgs sensor_msgs moveit_msgs - tf2_eigen -) + tf2_eigen) target_link_libraries(moveit_trajectory_execution_manager - moveit_planning_scene_monitor -) + moveit_planning_scene_monitor) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_trajectory_execution_manager_export.h DESTINATION include/moveit_ros_planning) +install( + FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_trajectory_execution_manager_export.h + DESTINATION include/moveit_ros_planning) if(CATKIN_ENABLE_TESTING) -## This needs further cleanup before it can run -# add_library(test_controller_manager_plugin test/test_moveit_controller_manager_plugin.cpp) -# set_target_properties(test_controller_manager_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -# target_link_libraries(test_controller_manager_plugin moveit_trajectory_execution_manager ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -# -# find_package(moveit_resources_panda_moveit_config REQUIRED) -# find_package(rostest REQUIRED) -# add_rostest_gtest(test_execution_manager -# test/test_execution_manager.test -# test/test_execution_manager.cpp) -# target_link_libraries(test_execution_manager moveit_trajectory_execution_manager ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + # This needs further cleanup before it can run + # add_library(test_controller_manager_plugin + # test/test_moveit_controller_manager_plugin.cpp) + # set_target_properties(test_controller_manager_plugin PROPERTIES VERSION + # "${${PROJECT_NAME}_VERSION}") + # target_link_libraries(test_controller_manager_plugin + # moveit_trajectory_execution_manager ${catkin_LIBRARIES} ${Boost_LIBRARIES}) + # + # find_package(moveit_resources_panda_moveit_config REQUIRED) + # find_package(rostest REQUIRED) add_rostest_gtest(test_execution_manager + # test/test_execution_manager.test test/test_execution_manager.cpp) + # target_link_libraries(test_execution_manager + # moveit_trajectory_execution_manager ${catkin_LIBRARIES} ${Boost_LIBRARIES}) endif() diff --git a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h index 18ab7c32bf..1bb055e20d 100644 --- a/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h +++ b/moveit_ros/planning/trajectory_execution_manager/include/moveit/trajectory_execution_manager/trajectory_execution_manager.h @@ -193,24 +193,42 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager /// longer than expected, the trajectory is canceled void enableExecutionDurationMonitoring(bool flag); + /// Get the current status of the monitoring of trajectory execution duration. + bool executionDurationMonitoring() const; + /// When determining the expected duration of a trajectory, this multiplicative factor is applied /// to get the allowed duration of execution void setAllowedExecutionDurationScaling(double scaling); + /// Get the current scaling of the duration of a trajectory to get the allowed duration of execution. + double allowedExecutionDurationScaling() const; + /// When determining the expected duration of a trajectory, this multiplicative factor is applied /// to allow more than the expected execution time before triggering trajectory cancel void setAllowedGoalDurationMargin(double margin); + /// Get the current margin of the duration of a trajectory to get the allowed duration of execution. + double allowedGoalDurationMargin() const; + /// Before sending a trajectory to a controller, scale the velocities by the factor specified. /// By default, this is 1.0 void setExecutionVelocityScaling(double scaling); + /// Get the current scaling of the execution velocities. + double executionVelocityScaling() const; + /// Set joint-value tolerance for validating trajectory's start point against current robot state void setAllowedStartTolerance(double tolerance); + /// Get the current joint-value for validating trajectory's start point against current robot state. + double allowedStartTolerance() const; + /// Enable or disable waiting for trajectory completion void setWaitForTrajectoryCompletion(bool flag); + /// Get the current state of waiting for the trajectory being completed. + bool waitForTrajectoryCompletion() const; + rclcpp::Node::SharedPtr getControllerManagerNode() { return controller_mgr_node_; @@ -315,6 +333,7 @@ class MOVEIT_TRAJECTORY_EXECUTION_MANAGER_EXPORT TrajectoryExecutionManager // 'global' values double allowed_execution_duration_scaling_; double allowed_goal_duration_margin_; + bool control_multi_dof_joint_variables_; // controller-specific values // override the 'global' values std::map controller_allowed_execution_duration_scaling_; diff --git a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp index 886f9faccc..8a863ef80d 100644 --- a/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp +++ b/moveit_ros/planning/trajectory_execution_manager/src/trajectory_execution_manager.cpp @@ -36,7 +36,9 @@ #include #include +#include #include +#include #include #include @@ -51,11 +53,15 @@ static const double DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN = 0.5; // allow 0.5 // after scaling) static const double DEFAULT_CONTROLLER_GOAL_DURATION_SCALING = 1.1; // allow the execution of a trajectory to take more time than expected (scaled by a value > 1) +static const bool DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES = false; TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::SharedPtr& node, const moveit::core::RobotModelConstPtr& robot_model, const planning_scene_monitor::CurrentStateMonitorPtr& csm) - : node_(node), logger_(moveit::getLogger("trajectory_execution_manager")), robot_model_(robot_model), csm_(csm) + : node_(node) + , logger_(moveit::getLogger("moveit.ros.trajectory_execution_manager")) + , robot_model_(robot_model) + , csm_(csm) { if (!node_->get_parameter("moveit_manage_controllers", manage_controllers_)) manage_controllers_ = false; @@ -67,7 +73,7 @@ TrajectoryExecutionManager::TrajectoryExecutionManager(const rclcpp::Node::Share const planning_scene_monitor::CurrentStateMonitorPtr& csm, bool manage_controllers) : node_(node) - , logger_(moveit::getLogger("trajectory_execution_manager")) + , logger_(moveit::getLogger("moveit.ros.trajectory_execution_manager")) , robot_model_(robot_model) , csm_(csm) , manage_controllers_(manage_controllers) @@ -94,6 +100,7 @@ void TrajectoryExecutionManager::initialize() execution_velocity_scaling_ = 1.0; allowed_start_tolerance_ = 0.01; wait_for_trajectory_completion_ = true; + control_multi_dof_joint_variables_ = DEFAULT_CONTROL_MULTI_DOF_JOINT_VARIABLES; allowed_execution_duration_scaling_ = DEFAULT_CONTROLLER_GOAL_DURATION_SCALING; allowed_goal_duration_margin_ = DEFAULT_CONTROLLER_GOAL_DURATION_MARGIN; @@ -160,7 +167,7 @@ void TrajectoryExecutionManager::initialize() rclcpp::NodeOptions opt; opt.allow_undeclared_parameters(true); opt.automatically_declare_parameters_from_overrides(true); - controller_mgr_node_.reset(new rclcpp::Node("moveit_simple_controller_manager", opt)); + controller_mgr_node_ = std::make_shared("moveit_simple_controller_manager", opt); auto all_params = node_->get_node_parameters_interface()->get_parameter_overrides(); for (const auto& param : all_params) @@ -200,6 +207,8 @@ void TrajectoryExecutionManager::initialize() controller_mgr_node_->get_parameter("trajectory_execution.allowed_goal_duration_margin", allowed_goal_duration_margin_); controller_mgr_node_->get_parameter("trajectory_execution.allowed_start_tolerance", allowed_start_tolerance_); + controller_mgr_node_->get_parameter("trajectory_execution.control_multi_dof_joint_variables", + control_multi_dof_joint_variables_); if (manage_controllers_) { @@ -255,31 +264,61 @@ void TrajectoryExecutionManager::enableExecutionDurationMonitoring(bool flag) execution_duration_monitoring_ = flag; } +bool TrajectoryExecutionManager::executionDurationMonitoring() const +{ + return execution_duration_monitoring_; +} + void TrajectoryExecutionManager::setAllowedExecutionDurationScaling(double scaling) { allowed_execution_duration_scaling_ = scaling; } +double TrajectoryExecutionManager::allowedExecutionDurationScaling() const +{ + return allowed_execution_duration_scaling_; +} + void TrajectoryExecutionManager::setAllowedGoalDurationMargin(double margin) { allowed_goal_duration_margin_ = margin; } +double TrajectoryExecutionManager::allowedGoalDurationMargin() const +{ + return allowed_goal_duration_margin_; +} + void TrajectoryExecutionManager::setExecutionVelocityScaling(double scaling) { execution_velocity_scaling_ = scaling; } +double TrajectoryExecutionManager::executionVelocityScaling() const +{ + return execution_velocity_scaling_; +} + void TrajectoryExecutionManager::setAllowedStartTolerance(double tolerance) { allowed_start_tolerance_ = tolerance; } +double TrajectoryExecutionManager::allowedStartTolerance() const +{ + return allowed_start_tolerance_; +} + void TrajectoryExecutionManager::setWaitForTrajectoryCompletion(bool flag) { wait_for_trajectory_completion_ = flag; } +bool TrajectoryExecutionManager::waitForTrajectoryCompletion() const +{ + return wait_for_trajectory_completion_; +} + bool TrajectoryExecutionManager::isManagingControllers() const { return manage_controllers_; @@ -350,8 +389,45 @@ bool TrajectoryExecutionManager::push(const moveit_msgs::msg::RobotTrajectory& t return false; } + // Optionally, convert multi dof waypoints to joint states and replace trajectory for execution + std::optional replaced_trajectory; + if (control_multi_dof_joint_variables_ && !trajectory.multi_dof_joint_trajectory.points.empty()) + { + // We convert the trajectory message into a RobotTrajectory first, + // since the conversion to a combined JointTrajectory depends on the local variables + // of the Multi-DOF joint types. + moveit::core::RobotState reference_state(robot_model_); + reference_state.setToDefaultValues(); + robot_trajectory::RobotTrajectory tmp_trajectory(robot_model_); + tmp_trajectory.setRobotTrajectoryMsg(reference_state, trajectory); + + // Combine all joints for filtering the joint trajectory waypoints + std::vector all_trajectory_joints = trajectory.joint_trajectory.joint_names; + for (const auto& mdof_joint : trajectory.multi_dof_joint_trajectory.joint_names) + { + all_trajectory_joints.push_back(mdof_joint); + } + + // Convert back to single joint trajectory including the MDOF joint variables, e.g. position/x, position/y, ... + const auto joint_trajectory = + robot_trajectory::toJointTrajectory(tmp_trajectory, true /* include_mdof_joints */, all_trajectory_joints); + + // Check success of conversion + // This should never happen when using MoveIt's interfaces, but users can pass anything into TEM::push() directly + if (!joint_trajectory.has_value()) + { + RCLCPP_ERROR(logger_, "Failed to convert multi-DOF trajectory to joint trajectory, aborting execution!"); + return false; + } + + // Create a new robot trajectory message that only contains the combined joint trajectory + RCLCPP_DEBUG(logger_, "Successfully converted multi-DOF trajectory to joint trajectory for execution."); + replaced_trajectory = moveit_msgs::msg::RobotTrajectory(); + replaced_trajectory->joint_trajectory = joint_trajectory.value(); + } + TrajectoryExecutionContext* context = new TrajectoryExecutionContext(); - if (configure(*context, trajectory, controllers)) + if (configure(*context, replaced_trajectory.value_or(trajectory), controllers)) { if (verbose_) { @@ -704,12 +780,12 @@ bool TrajectoryExecutionManager::distributeTrajectory(const moveit_msgs::msg::Ro std::set actuated_joints_single; for (const std::string& joint_name : trajectory.joint_trajectory.joint_names) { - const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_name); + const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name); if (jm) { if (jm->isPassive() || jm->getMimic() != nullptr || jm->getType() == moveit::core::JointModel::FIXED) continue; - actuated_joints_single.insert(jm->getName()); + actuated_joints_single.insert(joint_name); } } @@ -849,7 +925,7 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont RCLCPP_WARN(logger_, "Failed to validate trajectory: couldn't receive full current joint state within 1s"); return false; } - + moveit::core::RobotState reference_state(*current_state); for (const auto& trajectory : context.trajectory_parts_) { if (!trajectory.joint_trajectory.points.empty()) @@ -863,30 +939,47 @@ bool TrajectoryExecutionManager::validate(const TrajectoryExecutionContext& cont return false; } - for (std::size_t i = 0, end = joint_names.size(); i < end; ++i) + std::set joints; + for (const auto& joint_name : joint_names) { - const moveit::core::JointModel* jm = current_state->getJointModel(joint_names[i]); + const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name); if (!jm) { - RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_names[i]); + RCLCPP_ERROR_STREAM(logger_, "Unknown joint in trajectory: " << joint_name); return false; } - double cur_position = current_state->getJointPositions(jm)[0]; - double traj_position = positions[i]; - // normalize positions and compare - jm->enforcePositionBounds(&cur_position); - jm->enforcePositionBounds(&traj_position); - if (jm->distance(&cur_position, &traj_position) > allowed_start_tolerance_) + joints.insert(jm); + } + + // Copy all variable positions to reference state, and then compare start state joint distance within bounds + // Note on multi-DOF joints: Instead of comparing the translation and rotation distances like it's done for + // the multi-dof trajectory, this check will use the joint's internal distance implementation instead. + // This is more accurate, but may require special treatment for cases like the diff drive's turn path geometry. + reference_state.setVariablePositions(joint_names, positions); + + for (const auto joint : joints) + { + reference_state.enforcePositionBounds(joint); + current_state->enforcePositionBounds(joint); + if (reference_state.distance(*current_state, joint) > allowed_start_tolerance_) { RCLCPP_ERROR(logger_, - "\nInvalid Trajectory: start point deviates from current robot state more than %g" - "\njoint '%s': expected: %g, current: %g", - allowed_start_tolerance_, joint_names[i].c_str(), traj_position, cur_position); + "Invalid Trajectory: start point deviates from current robot state more than %g at joint '%s'." + "\nEnable DEBUG for detailed state info.", + allowed_start_tolerance_, joint->getName().c_str()); + RCLCPP_DEBUG(logger_, "| Joint | Expected | Current |"); + for (const auto& joint_name : joint_names) + { + RCLCPP_DEBUG(logger_, "| %s | %g | %g |", joint_name.c_str(), + reference_state.getVariablePosition(joint_name), + current_state->getVariablePosition(joint_name)); + } return false; } } } + if (!trajectory.multi_dof_joint_trajectory.points.empty()) { // Check multi-dof trajectory @@ -947,7 +1040,7 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, std::set actuated_joints; auto is_actuated = [this](const std::string& joint_name) -> bool { - const moveit::core::JointModel* jm = robot_model_->getJointModel(joint_name); + const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_name); return (jm && !jm->isPassive() && !jm->getMimic() && jm->getType() != moveit::core::JointModel::FIXED); }; for (const std::string& joint_name : trajectory.multi_dof_joint_trajectory.joint_names) @@ -1015,7 +1108,13 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, { if (known_controllers_.find(controller) == known_controllers_.end()) { - RCLCPP_ERROR(logger_, "Controller '%s' is not known", controller.c_str()); + std::stringstream stream; + for (const auto& controller : known_controllers_) + { + stream << " `" << controller.first << '`'; + } + RCLCPP_ERROR_STREAM(logger_, + "Controller " << controller << " is not known. Known controllers: " << stream.str()); return false; } } @@ -1045,6 +1144,13 @@ bool TrajectoryExecutionManager::configure(TrajectoryExecutionContext& context, } } RCLCPP_ERROR(logger_, "Known controllers and their joints:\n%s", ss2.str().c_str()); + + if (!trajectory.multi_dof_joint_trajectory.joint_names.empty()) + { + RCLCPP_WARN(logger_, "Hint: You can control multi-dof waypoints as joint trajectory by setting " + "`trajectory_execution.control_multi_dof_joint_variables=True`"); + } + return false; } @@ -1490,11 +1596,12 @@ bool TrajectoryExecutionManager::waitForRobotToStop(const TrajectoryExecutionCon for (std::size_t i = 0; i < n && !moved; ++i) { - const moveit::core::JointModel* jm = cur_state->getJointModel(joint_names[i]); + const moveit::core::JointModel* jm = robot_model_->getJointOfVariable(joint_names[i]); if (!jm) continue; // joint vanished from robot state (shouldn't happen), but we don't care - if (fabs(cur_state->getJointPositions(jm)[0] - prev_state->getJointPositions(jm)[0]) > allowed_start_tolerance_) + if (fabs(jm->distance(cur_state->getJointPositions(jm), prev_state->getJointPositions(jm))) > + allowed_start_tolerance_) { moved = true; no_motion_count = 0; diff --git a/moveit_ros/planning_interface/CHANGELOG.rst b/moveit_ros/planning_interface/CHANGELOG.rst index 8e57a03914..e1c2331636 100644 --- a/moveit_ros/planning_interface/CHANGELOG.rst +++ b/moveit_ros/planning_interface/CHANGELOG.rst @@ -2,6 +2,26 @@ Changelog for package moveit_ros_planning_interface ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Apply clang-tidy fixes +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Add getNodeHandle impl (`#2840 `_) + Co-authored-by: Sebastian Jahr +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* Add parameter api integration test (`#2662 `_) +* CMake format and lint in pre-commit (`#2683 `_) +* Remove unused python content from planning interface directory (`#2665 `_) + * Remove unused python content from planning interface directory + * Cleanup test cmake +* Contributors: Robert Haschke, Sebastian Jahr, Tyler Weaver, methylDragon + 2.9.0 (2024-01-09) ------------------ * Update ros2_control usage (`#2620 `_) diff --git a/moveit_ros/planning_interface/CMakeLists.txt b/moveit_ros/planning_interface/CMakeLists.txt index 9b121ca81f..47470e0a76 100644 --- a/moveit_ros/planning_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/CMakeLists.txt @@ -18,76 +18,59 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_action REQUIRED) -find_package(rclpy REQUIRED) - -# find_package(PythonInterp REQUIRED) -# find_package(PythonLibs "${PYTHON_VERSION_MAJOR}.${PYTHON_VERSION_MINOR}" REQUIRED) find_package(Eigen3 REQUIRED) -# find_package(eigenpy REQUIRED) - -# Finds Boost Components -include(ConfigExtras.cmake) set(THIS_PACKAGE_INCLUDE_DIRS -# py_bindings_tools/include - common_planning_interface_objects/include - planning_scene_interface/include - move_group_interface/include -) + common_planning_interface_objects/include planning_scene_interface/include + move_group_interface/include) set(THIS_PACKAGE_LIBRARIES - moveit_common_planning_interface_objects - moveit_planning_scene_interface - moveit_move_group_interface -# moveit_py_bindings_tools -# moveit_py_bindings_tools_python -) + moveit_common_planning_interface_objects moveit_planning_scene_interface + moveit_move_group_interface) set(THIS_PACKAGE_INCLUDE_DEPENDS - geometry_msgs - moveit_msgs - moveit_core - moveit_ros_planning - moveit_ros_warehouse - moveit_ros_move_group - tf2 - tf2_eigen - tf2_geometry_msgs - tf2_ros - rclcpp - rclcpp_action - Eigen3 - Boost -) + geometry_msgs + moveit_msgs + moveit_core + moveit_ros_planning + moveit_ros_warehouse + moveit_ros_move_group + tf2 + tf2_eigen + tf2_geometry_msgs + tf2_ros + rclcpp + rclcpp_action + Eigen3) include_directories(${THIS_PACKAGE_INCLUDE_DIRS}) -# add_subdirectory(py_bindings_tools) add_subdirectory(common_planning_interface_objects) add_subdirectory(planning_scene_interface) add_subdirectory(move_group_interface) -# add_subdirectory(robot_interface) # add_subdirectory(test) -############# -## TESTING ## -############# +# ############################################################################## +# TESTING ## +# ############################################################################## if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) find_package(ros_testing REQUIRED) - # TODO (vatanaksoytezer): Enable once this test is not flaky - # Move group interface cpp ompl constrained planning integration test + # TODO (vatanaksoytezer): Enable once this test is not flaky Move group + # interface cpp ompl constrained planning integration test # ament_add_gtest_executable(move_group_ompl_constraints_test - # test/move_group_ompl_constraints_test.cpp - # ) + # test/move_group_ompl_constraints_test.cpp ) - # target_link_libraries(move_group_ompl_constraints_test ${THIS_PACKAGE_LIBRARIES}) - # ament_target_dependencies(move_group_ompl_constraints_test ${THIS_PACKAGE_INCLUDE_DEPENDS}) - # add_ros_test(test/launch/move_group_ompl_constraints.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") - # end move group interface cpp ompl constrained planning integration test + # target_link_libraries(move_group_ompl_constraints_test + # ${THIS_PACKAGE_LIBRARIES}) + # ament_target_dependencies(move_group_ompl_constraints_test + # ${THIS_PACKAGE_INCLUDE_DEPENDS}) + # add_ros_test(test/launch/move_group_ompl_constraints.test.py ARGS + # "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") end move group interface cpp + # ompl constrained planning integration test endif() @@ -97,23 +80,10 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_planning_interface -) + INCLUDES + DESTINATION include/moveit_ros_planning_interface) ament_export_targets(moveit_ros_planning_interfaceTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_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() -endif() - ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_ros/planning_interface/ConfigExtras.cmake b/moveit_ros/planning_interface/ConfigExtras.cmake index c1b88a79a6..63a5f425e4 100644 --- a/moveit_ros/planning_interface/ConfigExtras.cmake +++ b/moveit_ros/planning_interface/ConfigExtras.cmake @@ -1,16 +1,11 @@ # Extras module needed for dependencies to find boost components -#if(Boost_VERSION LESS 106700) -# set(BOOST_PYTHON_COMPONENT python) -#else() -# set(BOOST_PYTHON_COMPONENT python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) -#endif() +# if(Boost_VERSION LESS 106700) set(BOOST_PYTHON_COMPONENT python) else() +# set(BOOST_PYTHON_COMPONENT +# python${PYTHON_VERSION_MAJOR}${PYTHON_VERSION_MINOR}) endif() -find_package(Boost REQUIRED COMPONENTS - date_time - filesystem - program_options - #${BOOST_PYTHON_COMPONENT} - system - thread -) +find_package( + Boost REQUIRED + COMPONENTS date_time filesystem program_options + # ${BOOST_PYTHON_COMPONENT} + system thread) diff --git a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt index 16817ca264..eb8715926b 100644 --- a/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt +++ b/moveit_ros/planning_interface/common_planning_interface_objects/CMakeLists.txt @@ -1,9 +1,8 @@ -add_library(moveit_common_planning_interface_objects SHARED src/common_objects.cpp) -set_target_properties(moveit_common_planning_interface_objects PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_common_planning_interface_objects - rclcpp - moveit_ros_planning - tf2_ros -) +add_library(moveit_common_planning_interface_objects SHARED + src/common_objects.cpp) +set_target_properties(moveit_common_planning_interface_objects + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_common_planning_interface_objects rclcpp + moveit_ros_planning tf2_ros) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning_interface) diff --git a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt index d7917bb550..04367e9a7b 100644 --- a/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/move_group_interface/CMakeLists.txt @@ -1,28 +1,40 @@ add_library(moveit_move_group_interface SHARED src/move_group_interface.cpp) include(GenerateExportHeader) generate_export_header(moveit_move_group_interface) -target_include_directories(moveit_move_group_interface PUBLIC $) -set_target_properties(moveit_move_group_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_move_group_interface moveit_common_planning_interface_objects moveit_planning_scene_interface ${Boost_THREAD_LIBRARY}) -ament_target_dependencies(moveit_move_group_interface +target_include_directories( + moveit_move_group_interface + PUBLIC $) +set_target_properties(moveit_move_group_interface + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries( + moveit_move_group_interface moveit_common_planning_interface_objects + moveit_planning_scene_interface ${Boost_THREAD_LIBRARY}) +ament_target_dependencies( + moveit_move_group_interface moveit_core moveit_msgs moveit_ros_move_group moveit_ros_occupancy_map_monitor moveit_ros_planning - moveit_ros_warehouse -) + moveit_ros_warehouse) # TODO (ddengster) : port wrap_python_move_group -#add_library(moveit_move_group_interface_python src/wrap_python_move_group.cpp) -#target_link_libraries(moveit_move_group_interface_python moveit_move_group_interface ${eigenpy_LIBRARIES} ${PYTHON_LIBRARIES} ${LIBS} ${Boost_LIBRARIES} moveit_py_bindings_tools) -#add_dependencies(moveit_move_group_interface_python) -#set_target_properties(moveit_move_group_interface_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -#set_target_properties(moveit_move_group_interface_python PROPERTIES OUTPUT_NAME _moveit_move_group_interface PREFIX "") -#set_target_properties(moveit_move_group_interface_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "bin") +# add_library(moveit_move_group_interface_python src/wrap_python_move_group.cpp) +# target_link_libraries(moveit_move_group_interface_python +# moveit_move_group_interface ${eigenpy_LIBRARIES} ${PYTHON_LIBRARIES} ${LIBS} +# ${Boost_LIBRARIES} moveit_py_bindings_tools) +# add_dependencies(moveit_move_group_interface_python) +# set_target_properties(moveit_move_group_interface_python PROPERTIES VERSION +# "${${PROJECT_NAME}_VERSION}") +# set_target_properties(moveit_move_group_interface_python PROPERTIES +# OUTPUT_NAME _moveit_move_group_interface PREFIX "") +# set_target_properties(moveit_move_group_interface_python PROPERTIES +# LIBRARY_OUTPUT_DIRECTORY "bin") if(WIN32) -# set_target_properties(moveit_move_group_interface_python PROPERTIES SUFFIX .pyd) + # set_target_properties(moveit_move_group_interface_python PROPERTIES SUFFIX + # .pyd) endif() install(DIRECTORY include/ DESTINATION include/moveit_ros_planning_interface) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_move_group_interface_export.h DESTINATION include/moveit_ros_planning_interface) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_move_group_interface_export.h + DESTINATION include/moveit_ros_planning_interface) diff --git a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h index 38ec463c9e..f3c7b6f506 100644 --- a/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h +++ b/moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h @@ -164,7 +164,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface moveit::core::RobotModelConstPtr getRobotModel() const; /** \brief Get the ROS node handle of this instance operates on */ - rclcpp::Node::SharedPtr getNodeHandle(); + const rclcpp::Node::SharedPtr& getNode() const; /** \brief Get the name of the frame in which the robot is planning */ const std::string& getPlanningFrame() const; diff --git a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp index 652569658a..4f93f72781 100644 --- a/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp +++ b/moveit_ros/planning_interface/move_group_interface/src/move_group_interface.cpp @@ -110,7 +110,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl public: MoveGroupInterfaceImpl(const rclcpp::Node::SharedPtr& node, const Options& opt, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) - : opt_(opt), node_(node), logger_(moveit::getLogger("move_group_interface")), tf_buffer_(tf_buffer) + : opt_(opt), node_(node), logger_(moveit::getLogger("moveit.ros.move_group_interface")), tf_buffer_(tf_buffer) { // We have no control on how the passed node is getting executed. To make sure MGI is functional, we're creating // our own callback group which is managed in a separate callback thread @@ -1319,7 +1319,7 @@ class MoveGroupInterface::MoveGroupInterfaceImpl MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const std::string& group_name, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) - : logger_(moveit::getLogger("move_group_interface")) + : logger_(moveit::getLogger("moveit.ros.move_group_interface")) { if (!rclcpp::ok()) throw std::runtime_error("ROS does not seem to be running"); @@ -1330,7 +1330,7 @@ MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, cons MoveGroupInterface::MoveGroupInterface(const rclcpp::Node::SharedPtr& node, const Options& opt, const std::shared_ptr& tf_buffer, const rclcpp::Duration& wait_for_servers) - : logger_(moveit::getLogger("move_group_interface")) + : logger_(moveit::getLogger("moveit.ros.move_group_interface")) { impl_ = new MoveGroupInterfaceImpl(node, opt, tf_buffer ? tf_buffer : getSharedTF(), wait_for_servers); } @@ -2295,6 +2295,11 @@ void MoveGroupInterface::setSupportSurfaceName(const std::string& name) impl_->setSupportSurfaceName(name); } +const rclcpp::Node::SharedPtr& MoveGroupInterface::getNode() const +{ + return impl_->node_; +} + const std::string& MoveGroupInterface::getPlanningFrame() const { return impl_->getRobotModel()->getModelFrame(); diff --git a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp b/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp deleted file mode 100644 index 9c5c8dd466..0000000000 --- a/moveit_ros/planning_interface/move_group_interface/src/wrap_python_move_group.cpp +++ /dev/null @@ -1,821 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, 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 Willow Garage 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: Ioan Sucan */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include - -/** @cond IGNORE */ - -namespace bp = boost::python; - -using moveit::py_bindings_tools::GILReleaser; - -namespace moveit -{ -namespace planning_interface -{ -class MoveGroupInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public MoveGroupInterface -{ -public: - // ROSInitializer is constructed first, and ensures ros::init() was called, if - // needed - MoveGroupInterfaceWrapper(const std::string& group_name, const std::string& robot_description, - const std::string& ns = "", double wait_for_servers = 5.0) - : py_bindings_tools::ROScppInitializer() - , MoveGroupInterface(Options(group_name, robot_description, ros::NodeHandle(ns)), - std::shared_ptr(), ros::WallDuration(wait_for_servers)) - { - } - - bool setJointValueTargetPerJointPythonList(const std::string& joint, bp::list& values) - { - return setJointValueTarget(joint, py_bindings_tools::doubleFromList(values)); - } - - bool setJointValueTargetPythonIterable(bp::object& values) - { - return setJointValueTarget(py_bindings_tools::doubleFromList(values)); - } - - bool setJointValueTargetPythonDict(bp::dict& values) - { - bp::list k = values.keys(); - int l = bp::len(k); - std::map v; - for (int i = 0; i < l; ++i) - v[bp::extract(k[i])] = bp::extract(values[k[i]]); - return setJointValueTarget(v); - } - - bool setJointValueTargetFromPosePython(const py_bindings_tools::ByteString& pose_str, const std::string& eef, - bool approx) - { - geometry_msgs::Pose pose_msg; - py_bindings_tools::deserializeMsg(pose_str, pose_msg); - return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); - } - - bool setJointValueTargetFromPoseStampedPython(const py_bindings_tools::ByteString& pose_str, const std::string& eef, - bool approx) - { - geometry_msgs::PoseStamped pose_msg; - py_bindings_tools::deserializeMsg(pose_str, pose_msg); - return approx ? setApproximateJointValueTarget(pose_msg, eef) : setJointValueTarget(pose_msg, eef); - } - - bool setJointValueTargetFromJointStatePython(const py_bindings_tools::ByteString& js_str) - { - sensor_msgs::JointState js_msg; - py_bindings_tools::deserializeMsg(js_str, js_msg); - return setJointValueTarget(js_msg); - } - - bp::list getJointValueTargetPythonList() - { - std::vector values; - MoveGroupInterface::getJointValueTarget(values); - bp::list l; - for (const double value : values) - l.append(value); - return l; - } - - py_bindings_tools::ByteString getJointValueTarget() - { - moveit_msgs::msg::RobotState msg; - const moveit::core::RobotState state = moveit::planning_interface::MoveGroupInterface::getTargetRobotState(); - moveit::core::robotStateToRobotStateMsg(state, msg); - return py_bindings_tools::serializeMsg(msg); - } - - void rememberJointValuesFromPythonList(const std::string& string, bp::list& values) - { - rememberJointValues(string, py_bindings_tools::doubleFromList(values)); - } - - const char* getPlanningFrameCStr() const - { - return getPlanningFrame().c_str(); - } - - py_bindings_tools::ByteString getInterfaceDescriptionPython() - { - moveit_msgs::msg::PlannerInterfaceDescription msg; - getInterfaceDescription(msg); - return py_bindings_tools::serializeMsg(msg); - } - - bp::list getActiveJointsList() const - { - return py_bindings_tools::listFromString(getActiveJoints()); - } - - bp::list getJointsList() const - { - return py_bindings_tools::listFromString(getJoints()); - } - - bp::list getCurrentJointValuesList() - { - return py_bindings_tools::listFromDouble(getCurrentJointValues()); - } - - bp::list getRandomJointValuesList() - { - return py_bindings_tools::listFromDouble(getRandomJointValues()); - } - - bp::dict getRememberedJointValuesPython() const - { - const std::map>& rv = getRememberedJointValues(); - bp::dict d; - for (const std::pair>& it : rv) - d[it.first] = py_bindings_tools::listFromDouble(it.second); - return d; - } - - bp::list convertPoseToList(const geometry_msgs::Pose& pose) const - { - std::vector v(7); - v[0] = pose.position.x; - v[1] = pose.position.y; - v[2] = pose.position.z; - v[3] = pose.orientation.x; - v[4] = pose.orientation.y; - v[5] = pose.orientation.z; - v[6] = pose.orientation.w; - return moveit::py_bindings_tools::listFromDouble(v); - } - - bp::list convertTransformToList(const geometry_msgs::Transform& tr) const - { - std::vector v(7); - v[0] = tr.translation.x; - v[1] = tr.translation.y; - v[2] = tr.translation.z; - v[3] = tr.rotation.x; - v[4] = tr.rotation.y; - v[5] = tr.rotation.z; - v[6] = tr.rotation.w; - return py_bindings_tools::listFromDouble(v); - } - - void convertListToTransform(const bp::list& l, geometry_msgs::Transform& tr) const - { - std::vector v = py_bindings_tools::doubleFromList(l); - tr.translation.x = v[0]; - tr.translation.y = v[1]; - tr.translation.z = v[2]; - tr.rotation.x = v[3]; - tr.rotation.y = v[4]; - tr.rotation.z = v[5]; - tr.rotation.w = v[6]; - } - - void convertListToPose(const bp::list& l, geometry_msgs::Pose& p) const - { - std::vector v = py_bindings_tools::doubleFromList(l); - p.position.x = v[0]; - p.position.y = v[1]; - p.position.z = v[2]; - p.orientation.x = v[3]; - p.orientation.y = v[4]; - p.orientation.z = v[5]; - p.orientation.w = v[6]; - } - - bp::list getCurrentRPYPython(const std::string& end_effector_link = "") - { - return py_bindings_tools::listFromDouble(getCurrentRPY(end_effector_link)); - } - - bp::list getCurrentPosePython(const std::string& end_effector_link = "") - { - geometry_msgs::PoseStamped pose = getCurrentPose(end_effector_link); - return convertPoseToList(pose.pose); - } - - bp::list getRandomPosePython(const std::string& end_effector_link = "") - { - geometry_msgs::PoseStamped pose = getRandomPose(end_effector_link); - return convertPoseToList(pose.pose); - } - - bp::list getKnownConstraintsList() const - { - return py_bindings_tools::listFromString(getKnownConstraints()); - } - - bool placePose(const std::string& object_name, const bp::list& pose, bool plan_only = false) - { - geometry_msgs::PoseStamped msg; - convertListToPose(pose, msg.pose); - msg.header.frame_id = getPoseReferenceFrame(); - msg.header.stamp = ros::Time::now(); - GILReleaser gr; - return place(object_name, msg, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool placePoses(const std::string& object_name, const bp::list& poses_list, bool plan_only = false) - { - int l = bp::len(poses_list); - std::vector poses(l); - for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(poses_list[i]), poses[i]); - GILReleaser gr; - return place(object_name, poses, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool placeLocation(const std::string& object_name, const py_bindings_tools::ByteString& location_str, - bool plan_only = false) - { - std::vector locations(1); - py_bindings_tools::deserializeMsg(location_str, locations[0]); - GILReleaser gr; - return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool placeLocations(const std::string& object_name, const bp::list& location_list, bool plan_only = false) - { - int l = bp::len(location_list); - std::vector locations(l); - for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(location_list[i]), locations[i]); - GILReleaser gr; - return place(object_name, std::move(locations), plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool placeAnywhere(const std::string& object_name, bool plan_only = false) - { - GILReleaser gr; - return place(object_name, plan_only) == moveit::core::MoveItErrorCode::SUCCESS; - } - - void convertListToArrayOfPoses(const bp::list& poses, std::vector& msg) - { - int l = bp::len(poses); - for (int i = 0; i < l; ++i) - { - const bp::list& pose = bp::extract(poses[i]); - std::vector v = py_bindings_tools::doubleFromList(pose); - if (v.size() == 6 || v.size() == 7) - { - Eigen::Isometry3d p; - if (v.size() == 6) - { - tf2::Quaternion tq; - tq.setRPY(v[3], v[4], v[5]); - Eigen::Quaterniond eq; - tf2::convert(tq, eq); - p = Eigen::Isometry3d(eq); - } - else - p = Eigen::Isometry3d(Eigen::Quaterniond(v[6], v[3], v[4], v[5])); - p.translation() = Eigen::Vector3d(v[0], v[1], v[2]); - geometry_msgs::Pose pm = tf2::toMsg(p); - msg.push_back(pm); - } - else - ROS_WARN("Incorrect number of values for a pose: %u", (unsigned int)v.size()); - } - } - - bp::dict getCurrentStateBoundedPython() - { - moveit::core::RobotStatePtr current = getCurrentState(); - current->enforceBounds(); - moveit_msgs::msg::RobotState rsmv; - moveit::core::robotStateToRobotStateMsg(*current, rsmv); - bp::dict output; - for (size_t x = 0; x < rsmv.joint_state.name.size(); ++x) - output[rsmv.joint_state.name[x]] = rsmv.joint_state.position[x]; - return output; - } - - py_bindings_tools::ByteString getCurrentStatePython() - { - moveit::core::RobotStatePtr current_state = getCurrentState(); - moveit_msgs::RobotState state_message; - moveit::core::robotStateToRobotStateMsg(*current_state, state_message); - return py_bindings_tools::serializeMsg(state_message); - } - - void setStartStatePython(const py_bindings_tools::ByteString& msg_str) - { - moveit_msgs::msg::RobotState msg; - py_bindings_tools::deserializeMsg(msg_str, msg); - setStartState(msg); - } - - bool setPoseTargetsPython(bp::list& poses, const std::string& end_effector_link = "") - { - std::vector msg; - convertListToArrayOfPoses(poses, msg); - return setPoseTargets(msg, end_effector_link); - } - py_bindings_tools::ByteString getPoseTargetPython(const std::string& end_effector_link) - { - geometry_msgs::PoseStamped pose = moveit::planning_interface::MoveGroupInterface::getPoseTarget(end_effector_link); - return py_bindings_tools::serializeMsg(pose); - } - - bool setPoseTargetPython(bp::list& pose, const std::string& end_effector_link = "") - { - std::vector v = py_bindings_tools::doubleFromList(pose); - geometry_msgs::Pose msg; - if (v.size() == 6) - { - tf2::Quaternion q; - q.setRPY(v[3], v[4], v[5]); - tf2::convert(q, msg.orientation); - } - else if (v.size() == 7) - { - msg.orientation.x = v[3]; - msg.orientation.y = v[4]; - msg.orientation.z = v[5]; - msg.orientation.w = v[6]; - } - else - { - ROS_ERROR("Pose description expected to consist of either 6 or 7 values"); - return false; - } - msg.position.x = v[0]; - msg.position.y = v[1]; - msg.position.z = v[2]; - return setPoseTarget(msg, end_effector_link); - } - - const char* getEndEffectorLinkCStr() const - { - return getEndEffectorLink().c_str(); - } - - const char* getPoseReferenceFrameCStr() const - { - return getPoseReferenceFrame().c_str(); - } - - const char* getNameCStr() const - { - return getName().c_str(); - } - - const char* getPlannerIdCStr() const - { - return getPlannerId().c_str(); - } - - const char* getPlanningPipelineIdCStr() const - { - return getPlanningPipelineId().c_str(); - } - - bp::dict getNamedTargetValuesPython(const std::string& name) - { - bp::dict output; - std::map positions = getNamedTargetValues(name); - std::map::iterator iterator; - - for (iterator = positions.begin(); iterator != positions.end(); ++iterator) - output[iterator->first] = iterator->second; - return output; - } - - bp::list getNamedTargetsPython() - { - return py_bindings_tools::listFromString(getNamedTargets()); - } - - bool movePython() - { - GILReleaser gr; - return move() == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool asyncMovePython() - { - return asyncMove() == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool attachObjectPython(const std::string& object_name, const std::string& link_name, const bp::list& touch_links) - { - return attachObject(object_name, link_name, py_bindings_tools::stringFromList(touch_links)); - } - - bool executePython(const py_bindings_tools::ByteString& plan_str) - { - MoveGroupInterface::Plan plan; - py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); - GILReleaser gr; - return execute(plan) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bool asyncExecutePython(const py_bindings_tools::ByteString& plan_str) - { - MoveGroupInterface::Plan plan; - py_bindings_tools::deserializeMsg(plan_str, plan.trajectory_); - return asyncExecute(plan) == moveit::core::MoveItErrorCode::SUCCESS; - } - - bp::tuple planPython() - { - MoveGroupInterface::Plan plan; - moveit_msgs::MoveItErrorCodes res; - { - GILReleaser gr; - res = MoveGroupInterface::plan(plan); - } - return bp::make_tuple(py_bindings_tools::serializeMsg(res), py_bindings_tools::serializeMsg(plan.trajectory_), - plan.planning_time); - } - - py_bindings_tools::ByteString constructMotionPlanRequestPython() - { - moveit_msgs::MotionPlanRequest request; - constructMotionPlanRequest(request); - return py_bindings_tools::serializeMsg(request); - } - - bp::tuple computeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions) - { - moveit_msgs::msg::Constraints path_constraints_tmp; - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints_tmp); - } - - bp::tuple computeCartesianPathConstrainedPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, - const py_bindings_tools::ByteString& path_constraints_str) - { - moveit_msgs::msg::Constraints path_constraints; - py_bindings_tools::deserializeMsg(path_constraints_str, path_constraints); - return doComputeCartesianPathPython(waypoints, eef_step, jump_threshold, avoid_collisions, path_constraints); - } - - bp::tuple doComputeCartesianPathPython(const bp::list& waypoints, double eef_step, double jump_threshold, - bool avoid_collisions, const moveit_msgs::msg::Constraints& path_constraints) - { - std::vector poses; - convertListToArrayOfPoses(waypoints, poses); - moveit_msgs::msg::RobotTrajectory trajectory; - double fraction; - { - GILReleaser gr; - fraction = computeCartesianPath(poses, eef_step, jump_threshold, trajectory, path_constraints, avoid_collisions); - } - return bp::make_tuple(py_bindings_tools::serializeMsg(trajectory), fraction); - } - - int pickGrasp(const std::string& object, const py_bindings_tools::ByteString& grasp_str, bool plan_only = false) - { - moveit_msgs::msg::Grasp grasp; - py_bindings_tools::deserializeMsg(grasp_str, grasp); - GILReleaser gr; - return pick(object, grasp, plan_only).val; - } - - int pickGrasps(const std::string& object, const bp::list& grasp_list, bool plan_only = false) - { - int l = bp::len(grasp_list); - std::vector grasps(l); - for (int i = 0; i < l; ++i) - py_bindings_tools::deserializeMsg(py_bindings_tools::ByteString(grasp_list[i]), grasps[i]); - GILReleaser gr; - return pick(object, std::move(grasps), plan_only).val; - } - - void setPathConstraintsFromMsg(const py_bindings_tools::ByteString& constraints_str) - { - moveit_msgs::msg::Constraints constraints_msg; - py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); - setPathConstraints(constraints_msg); - } - - py_bindings_tools::ByteString getPathConstraintsPython() - { - moveit_msgs::msg::Constraints constraints_msg(getPathConstraints()); - return py_bindings_tools::serializeMsg(constraints_msg); - } - - void setTrajectoryConstraintsFromMsg(const py_bindings_tools::ByteString& constraints_str) - { - moveit_msgs::TrajectoryConstraints constraints_msg; - py_bindings_tools::deserializeMsg(constraints_str, constraints_msg); - setTrajectoryConstraints(constraints_msg); - } - - py_bindings_tools::ByteString getTrajectoryConstraintsPython() - { - moveit_msgs::TrajectoryConstraints constraints_msg(getTrajectoryConstraints()); - return py_bindings_tools::serializeMsg(constraints_msg); - } - - py_bindings_tools::ByteString retimeTrajectory(const py_bindings_tools::ByteString& ref_state_str, - const py_bindings_tools::ByteString& traj_str, - double velocity_scaling_factor, double acceleration_scaling_factor, - const std::string& algorithm) - { - // Convert reference state message to object - moveit_msgs::msg::RobotState ref_state_msg; - py_bindings_tools::deserializeMsg(ref_state_str, ref_state_msg); - moveit::core::RobotState ref_state_obj(getRobotModel()); - if (moveit::core::robotStateMsgToRobotState(ref_state_msg, ref_state_obj, true)) - { - // Convert trajectory message to object - moveit_msgs::msg::RobotTrajectory traj_msg; - py_bindings_tools::deserializeMsg(traj_str, traj_msg); - bool algorithm_found = true; - { - GILReleaser gr; - robot_trajectory::RobotTrajectory traj_obj(getRobotModel(), getName()); - traj_obj.setRobotTrajectoryMsg(ref_state_obj, traj_msg); - - // Do the actual retiming - if (algorithm == "iterative_time_parameterization") - { - trajectory_processing::IterativeParabolicTimeParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else if (algorithm == "iterative_spline_parameterization") - { - trajectory_processing::IterativeSplineParameterization time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else if (algorithm == "time_optimal_trajectory_generation") - { - trajectory_processing::TimeOptimalTrajectoryGeneration time_param; - time_param.computeTimeStamps(traj_obj, velocity_scaling_factor, acceleration_scaling_factor); - } - else - { - ROS_ERROR_STREAM_NAMED("move_group_py", "Unknown time parameterization algorithm: " << algorithm); - algorithm_found = false; - traj_msg = moveit_msgs::RobotTrajectory(); - } - - if (algorithm_found) - // Convert the retimed trajectory back into a message - traj_obj.getRobotTrajectoryMsg(traj_msg); - } - return py_bindings_tools::serializeMsg(traj_msg); - } - else - { - ROS_ERROR("Unable to convert RobotState message to RobotState instance."); - return py_bindings_tools::ByteString(""); - } - } - - Eigen::MatrixXd getJacobianMatrixPython(const bp::list& joint_values, const bp::object& reference_point = bp::object()) - { - const std::vector v = py_bindings_tools::doubleFromList(joint_values); - std::vector ref; - if (reference_point.is_none()) - ref = { 0.0, 0.0, 0.0 }; - else - ref = py_bindings_tools::doubleFromList(reference_point); - if (ref.size() != 3) - throw std::invalid_argument("reference point needs to have 3 elements, got " + std::to_string(ref.size())); - - moveit::core::RobotState state(getRobotModel()); - state.setToDefaultValues(); - auto group = state.getJointModelGroup(getName()); - state.setJointGroupPositions(group, v); - return state.getJacobian(group, Eigen::Map(&ref[0])); - } - - py_bindings_tools::ByteString enforceBoundsPython(const py_bindings_tools::ByteString& msg_str) - { - moveit_msgs::RobotState state_msg; - py_bindings_tools::deserializeMsg(msg_str, state_msg); - moveit::core::RobotState state(getRobotModel()); - if (moveit::core::robotStateMsgToRobotState(state_msg, state, true)) - { - state.enforceBounds(); - moveit::core::robotStateToRobotStateMsg(state, state_msg); - return py_bindings_tools::serializeMsg(state_msg); - } - else - { - ROS_ERROR("Unable to convert RobotState message to RobotState instance."); - return py_bindings_tools::ByteString(""); - } - } -}; - -BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(getJacobianMatrixOverloads, getJacobianMatrixPython, 1, 2) - -static void wrap_move_group_interface() -{ - eigenpy::enableEigenPy(); - - bp::class_ move_group_interface_class( - "MoveGroupInterface", bp::init>()); - - move_group_interface_class.def("async_move", &MoveGroupInterfaceWrapper::asyncMovePython); - move_group_interface_class.def("move", &MoveGroupInterfaceWrapper::movePython); - move_group_interface_class.def("execute", &MoveGroupInterfaceWrapper::executePython); - move_group_interface_class.def("async_execute", &MoveGroupInterfaceWrapper::asyncExecutePython); - moveit::core::MoveItErrorCode (MoveGroupInterfaceWrapper::*pick_1)(const std::string&, bool) = - &MoveGroupInterfaceWrapper::pick; - move_group_interface_class.def("pick", pick_1); - move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasp); - move_group_interface_class.def("pick", &MoveGroupInterfaceWrapper::pickGrasps); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placePose); - move_group_interface_class.def("place_poses_list", &MoveGroupInterfaceWrapper::placePoses); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeLocation); - move_group_interface_class.def("place_locations_list", &MoveGroupInterfaceWrapper::placeLocations); - move_group_interface_class.def("place", &MoveGroupInterfaceWrapper::placeAnywhere); - move_group_interface_class.def("stop", &MoveGroupInterfaceWrapper::stop); - - move_group_interface_class.def("get_name", &MoveGroupInterfaceWrapper::getNameCStr); - move_group_interface_class.def("get_planning_frame", &MoveGroupInterfaceWrapper::getPlanningFrameCStr); - move_group_interface_class.def("get_interface_description", &MoveGroupInterfaceWrapper::getInterfaceDescriptionPython); - - move_group_interface_class.def("get_active_joints", &MoveGroupInterfaceWrapper::getActiveJointsList); - move_group_interface_class.def("get_joints", &MoveGroupInterfaceWrapper::getJointsList); - move_group_interface_class.def("get_variable_count", &MoveGroupInterfaceWrapper::getVariableCount); - move_group_interface_class.def("allow_looking", &MoveGroupInterfaceWrapper::allowLooking); - move_group_interface_class.def("allow_replanning", &MoveGroupInterfaceWrapper::allowReplanning); - - move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); - - move_group_interface_class.def("set_pose_reference_frame", &MoveGroupInterfaceWrapper::setPoseReferenceFrame); - move_group_interface_class.def("set_end_effector_link", &MoveGroupInterfaceWrapper::setEndEffectorLink); - move_group_interface_class.def("get_end_effector_link", &MoveGroupInterfaceWrapper::getEndEffectorLinkCStr); - move_group_interface_class.def("get_pose_reference_frame", &MoveGroupInterfaceWrapper::getPoseReferenceFrameCStr); - - move_group_interface_class.def("set_pose_target", &MoveGroupInterfaceWrapper::setPoseTargetPython); - - move_group_interface_class.def("set_pose_targets", &MoveGroupInterfaceWrapper::setPoseTargetsPython); - - move_group_interface_class.def("set_position_target", &MoveGroupInterfaceWrapper::setPositionTarget); - move_group_interface_class.def("set_rpy_target", &MoveGroupInterfaceWrapper::setRPYTarget); - move_group_interface_class.def("set_orientation_target", &MoveGroupInterfaceWrapper::setOrientationTarget); - - move_group_interface_class.def("get_current_pose", &MoveGroupInterfaceWrapper::getCurrentPosePython); - move_group_interface_class.def("get_current_rpy", &MoveGroupInterfaceWrapper::getCurrentRPYPython); - - move_group_interface_class.def("get_random_pose", &MoveGroupInterfaceWrapper::getRandomPosePython); - - move_group_interface_class.def("clear_pose_target", &MoveGroupInterfaceWrapper::clearPoseTarget); - move_group_interface_class.def("clear_pose_targets", &MoveGroupInterfaceWrapper::clearPoseTargets); - - move_group_interface_class.def("set_joint_value_target", - &MoveGroupInterfaceWrapper::setJointValueTargetPythonIterable); - move_group_interface_class.def("set_joint_value_target", &MoveGroupInterfaceWrapper::setJointValueTargetPythonDict); - - move_group_interface_class.def("set_joint_value_target", - &MoveGroupInterfaceWrapper::setJointValueTargetPerJointPythonList); - bool (MoveGroupInterfaceWrapper::*set_joint_value_target_4)(const std::string&, double) = - &MoveGroupInterfaceWrapper::setJointValueTarget; - move_group_interface_class.def("set_joint_value_target", set_joint_value_target_4); - - move_group_interface_class.def("set_joint_value_target_from_pose", - &MoveGroupInterfaceWrapper::setJointValueTargetFromPosePython); - move_group_interface_class.def("set_joint_value_target_from_pose_stamped", - &MoveGroupInterfaceWrapper::setJointValueTargetFromPoseStampedPython); - move_group_interface_class.def("set_joint_value_target_from_joint_state_message", - &MoveGroupInterfaceWrapper::setJointValueTargetFromJointStatePython); - - move_group_interface_class.def("get_joint_value_target", &MoveGroupInterfaceWrapper::getJointValueTargetPythonList); - - move_group_interface_class.def("set_named_target", &MoveGroupInterfaceWrapper::setNamedTarget); - move_group_interface_class.def("set_random_target", &MoveGroupInterfaceWrapper::setRandomTarget); - - void (MoveGroupInterfaceWrapper::*remember_joint_values_2)(const std::string&) = - &MoveGroupInterfaceWrapper::rememberJointValues; - move_group_interface_class.def("remember_joint_values", remember_joint_values_2); - - move_group_interface_class.def("remember_joint_values", &MoveGroupInterfaceWrapper::rememberJointValuesFromPythonList); - - move_group_interface_class.def("start_state_monitor", &MoveGroupInterfaceWrapper::startStateMonitor); - move_group_interface_class.def("get_current_joint_values", &MoveGroupInterfaceWrapper::getCurrentJointValuesList); - move_group_interface_class.def("get_random_joint_values", &MoveGroupInterfaceWrapper::getRandomJointValuesList); - move_group_interface_class.def("get_remembered_joint_values", - &MoveGroupInterfaceWrapper::getRememberedJointValuesPython); - - move_group_interface_class.def("forget_joint_values", &MoveGroupInterfaceWrapper::forgetJointValues); - - move_group_interface_class.def("get_goal_joint_tolerance", &MoveGroupInterfaceWrapper::getGoalJointTolerance); - move_group_interface_class.def("get_goal_position_tolerance", &MoveGroupInterfaceWrapper::getGoalPositionTolerance); - move_group_interface_class.def("get_goal_orientation_tolerance", - &MoveGroupInterfaceWrapper::getGoalOrientationTolerance); - - move_group_interface_class.def("set_goal_joint_tolerance", &MoveGroupInterfaceWrapper::setGoalJointTolerance); - move_group_interface_class.def("set_goal_position_tolerance", &MoveGroupInterfaceWrapper::setGoalPositionTolerance); - move_group_interface_class.def("set_goal_orientation_tolerance", - &MoveGroupInterfaceWrapper::setGoalOrientationTolerance); - move_group_interface_class.def("set_goal_tolerance", &MoveGroupInterfaceWrapper::setGoalTolerance); - - move_group_interface_class.def("set_start_state_to_current_state", - &MoveGroupInterfaceWrapper::setStartStateToCurrentState); - move_group_interface_class.def("set_start_state", &MoveGroupInterfaceWrapper::setStartStatePython); - - bool (MoveGroupInterfaceWrapper::*set_path_constraints_1)(const std::string&) = - &MoveGroupInterfaceWrapper::setPathConstraints; - move_group_interface_class.def("set_path_constraints", set_path_constraints_1); - move_group_interface_class.def("set_path_constraints_from_msg", &MoveGroupInterfaceWrapper::setPathConstraintsFromMsg); - move_group_interface_class.def("get_path_constraints", &MoveGroupInterfaceWrapper::getPathConstraintsPython); - move_group_interface_class.def("clear_path_constraints", &MoveGroupInterfaceWrapper::clearPathConstraints); - - move_group_interface_class.def("set_trajectory_constraints_from_msg", - &MoveGroupInterfaceWrapper::setTrajectoryConstraintsFromMsg); - move_group_interface_class.def("get_trajectory_constraints", - &MoveGroupInterfaceWrapper::getTrajectoryConstraintsPython); - move_group_interface_class.def("clear_trajectory_constraints", &MoveGroupInterfaceWrapper::clearTrajectoryConstraints); - move_group_interface_class.def("get_known_constraints", &MoveGroupInterfaceWrapper::getKnownConstraintsList); - move_group_interface_class.def("set_constraints_database", &MoveGroupInterfaceWrapper::setConstraintsDatabase); - move_group_interface_class.def("set_workspace", &MoveGroupInterfaceWrapper::setWorkspace); - move_group_interface_class.def("set_planning_time", &MoveGroupInterfaceWrapper::setPlanningTime); - move_group_interface_class.def("get_planning_time", &MoveGroupInterfaceWrapper::getPlanningTime); - move_group_interface_class.def("set_max_velocity_scaling_factor", - &MoveGroupInterfaceWrapper::setMaxVelocityScalingFactor); - move_group_interface_class.def("set_max_acceleration_scaling_factor", - &MoveGroupInterfaceWrapper::setMaxAccelerationScalingFactor); - move_group_interface_class.def("set_planner_id", &MoveGroupInterfaceWrapper::setPlannerId); - move_group_interface_class.def("get_planner_id", &MoveGroupInterfaceWrapper::getPlannerIdCStr); - move_group_interface_class.def("set_planning_pipeline_id", &MoveGroupInterfaceWrapper::setPlanningPipelineId); - move_group_interface_class.def("get_planning_pipeline_id", &MoveGroupInterfaceWrapper::getPlanningPipelineIdCStr); - move_group_interface_class.def("set_num_planning_attempts", &MoveGroupInterfaceWrapper::setNumPlanningAttempts); - move_group_interface_class.def("plan", &MoveGroupInterfaceWrapper::planPython); - move_group_interface_class.def("construct_motion_plan_request", - &MoveGroupInterfaceWrapper::constructMotionPlanRequestPython); - move_group_interface_class.def("compute_cartesian_path", &MoveGroupInterfaceWrapper::computeCartesianPathPython); - move_group_interface_class.def("compute_cartesian_path", - &MoveGroupInterfaceWrapper::computeCartesianPathConstrainedPython); - move_group_interface_class.def("set_support_surface_name", &MoveGroupInterfaceWrapper::setSupportSurfaceName); - move_group_interface_class.def("attach_object", &MoveGroupInterfaceWrapper::attachObjectPython); - move_group_interface_class.def("detach_object", &MoveGroupInterfaceWrapper::detachObject); - move_group_interface_class.def("retime_trajectory", &MoveGroupInterfaceWrapper::retimeTrajectory); - move_group_interface_class.def("get_named_targets", &MoveGroupInterfaceWrapper::getNamedTargetsPython); - move_group_interface_class.def("get_named_target_values", &MoveGroupInterfaceWrapper::getNamedTargetValuesPython); - move_group_interface_class.def("get_current_state_bounded", &MoveGroupInterfaceWrapper::getCurrentStateBoundedPython); - move_group_interface_class.def("get_current_state", &MoveGroupInterfaceWrapper::getCurrentStatePython); - move_group_interface_class.def("get_jacobian_matrix", &MoveGroupInterfaceWrapper::getJacobianMatrixPython, - getJacobianMatrixOverloads()); - move_group_interface_class.def("enforce_bounds", &MoveGroupInterfaceWrapper::enforceBoundsPython); -} -} // namespace planning_interface -} // namespace moveit - -BOOST_PYTHON_MODULE(_moveit_move_group_interface) -{ - using namespace moveit::planning_interface; - wrap_move_group_interface(); -} - -/** @endcond */ diff --git a/moveit_ros/planning_interface/package.xml b/moveit_ros/planning_interface/package.xml index 71b5ab0d8c..b42df59a24 100644 --- a/moveit_ros/planning_interface/package.xml +++ b/moveit_ros/planning_interface/package.xml @@ -2,7 +2,7 @@ moveit_ros_planning_interface - 2.9.0 + 2.10.0 Components of MoveIt that offer simpler remote (as from another ROS 2 node) interfaces to planning and execution Henning Kayser Tyler Weaver @@ -12,8 +12,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan @@ -42,11 +42,9 @@ moveit_resources_fanuc_moveit_config moveit_resources_panda_moveit_config moveit_simple_controller_manager - moveit_planners_ompl rviz2 ros_testing ament_cmake_gtest - ament_lint_auto moveit_configs_utils diff --git a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt index d6a7edd4e2..10264ea6d7 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt +++ b/moveit_ros/planning_interface/planning_scene_interface/CMakeLists.txt @@ -1,19 +1,28 @@ -add_library(moveit_planning_scene_interface SHARED src/planning_scene_interface.cpp) -set_target_properties(moveit_planning_scene_interface PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -ament_target_dependencies(moveit_planning_scene_interface moveit_msgs moveit_core moveit_ros_move_group) +add_library(moveit_planning_scene_interface SHARED + src/planning_scene_interface.cpp) +set_target_properties(moveit_planning_scene_interface + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +ament_target_dependencies(moveit_planning_scene_interface moveit_msgs + moveit_core moveit_ros_move_group) # TODO(JafarAbdi): Support python wrapper -#add_library(moveit_planning_scene_interface_python src/wrap_python_planning_scene_interface.cpp) -#set_target_properties(moveit_planning_scene_interface_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -#target_link_libraries(moveit_planning_scene_interface_python moveit_planning_scene_interface ${PYTHON_LIBRARIES} ${LIBS} ${Boost_LIBRARIES} moveit_py_bindings_tools) -#set_target_properties(moveit_planning_scene_interface_python PROPERTIES OUTPUT_NAME _moveit_planning_scene_interface PREFIX "") -#set_target_properties(moveit_planning_scene_interface_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "bin") +# add_library(moveit_planning_scene_interface_python +# src/wrap_python_planning_scene_interface.cpp) +# set_target_properties(moveit_planning_scene_interface_python PROPERTIES +# VERSION "${${PROJECT_NAME}_VERSION}") +# target_link_libraries(moveit_planning_scene_interface_python +# moveit_planning_scene_interface ${PYTHON_LIBRARIES} ${LIBS} ${Boost_LIBRARIES} +# moveit_py_bindings_tools) +# set_target_properties(moveit_planning_scene_interface_python PROPERTIES +# OUTPUT_NAME _moveit_planning_scene_interface PREFIX "") +# set_target_properties(moveit_planning_scene_interface_python PROPERTIES +# LIBRARY_OUTPUT_DIRECTORY "bin") if(WIN32) -# set_target_properties(moveit_planning_scene_interface_python PROPERTIES SUFFIX .pyd) + # set_target_properties(moveit_planning_scene_interface_python PROPERTIES + # SUFFIX .pyd) endif() -#install(TARGETS moveit_planning_scene_interface_python -# EXPORT moveit_planning_scene_interface_python -# DESTINATION bin) +# install(TARGETS moveit_planning_scene_interface_python EXPORT +# moveit_planning_scene_interface_python DESTINATION bin) install(DIRECTORY include/ DESTINATION include/moveit_ros_planning_interface) diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp index 2e589cf982..a4277f0731 100644 --- a/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp +++ b/moveit_ros/planning_interface/planning_scene_interface/src/planning_scene_interface.cpp @@ -127,8 +127,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl bool good = true; for (const geometry_msgs::msg::Pose& mesh_pose : collision_object.mesh_poses) { - if (!(mesh_pose.position.x >= minx && mesh_pose.position.x <= maxx && mesh_pose.position.y >= miny && - mesh_pose.position.y <= maxy && mesh_pose.position.z >= minz && mesh_pose.position.z <= maxz)) + if (mesh_pose.position.x < minx || mesh_pose.position.x > maxx || mesh_pose.position.y < miny || + mesh_pose.position.y > maxy || mesh_pose.position.z < minz || mesh_pose.position.z > maxz) { good = false; break; @@ -136,9 +136,8 @@ class PlanningSceneInterface::PlanningSceneInterfaceImpl } for (const geometry_msgs::msg::Pose& primitive_pose : collision_object.primitive_poses) { - if (!(primitive_pose.position.x >= minx && primitive_pose.position.x <= maxx && - primitive_pose.position.y >= miny && primitive_pose.position.y <= maxy && - primitive_pose.position.z >= minz && primitive_pose.position.z <= maxz)) + if (primitive_pose.position.x < minx || primitive_pose.position.x > maxx || primitive_pose.position.y < miny || + primitive_pose.position.y > maxy || primitive_pose.position.z < minz || primitive_pose.position.z > maxz) { good = false; break; diff --git a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp b/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp deleted file mode 100644 index 5455bf1ff1..0000000000 --- a/moveit_ros/planning_interface/planning_scene_interface/src/wrap_python_planning_scene_interface.cpp +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Willow Garage, 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 Willow Garage 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: Ioan Sucan */ - -#include -#include -#include -#include - -#include -#include - -/** @cond IGNORE */ - -namespace bp = boost::python; - -namespace moveit -{ -namespace planning_interface -{ -class PlanningSceneInterfaceWrapper : protected py_bindings_tools::ROScppInitializer, public PlanningSceneInterface -{ -public: - // ROSInitializer is constructed first, and ensures ros::init() was called, if needed - PlanningSceneInterfaceWrapper(const std::string& ns = "") - : py_bindings_tools::ROScppInitializer(), PlanningSceneInterface(ns) - { - } - - bp::list getKnownObjectNamesPython(bool with_type = false) - { - return py_bindings_tools::listFromString(getKnownObjectNames(with_type)); - } - - bp::list getKnownObjectNamesInROIPython(double minx, double miny, double minz, double maxx, double maxy, double maxz, - bool with_type = false) - { - return py_bindings_tools::listFromString(getKnownObjectNamesInROI(minx, miny, minz, maxx, maxy, maxz, with_type)); - } - - bp::dict getObjectPosesPython(const bp::list& object_ids) - { - std::map ops = getObjectPoses(py_bindings_tools::stringFromList(object_ids)); - std::map ser_ops; - for (std::map::const_iterator it = ops.begin(); it != ops.end(); ++it) - ser_ops[it->first] = py_bindings_tools::serializeMsg(it->second); - - return py_bindings_tools::dictFromType(ser_ops); - } - - bp::dict getObjectsPython(const bp::list& object_ids) - { - std::map objs = - getObjects(py_bindings_tools::stringFromList(object_ids)); - std::map ser_objs; - for (std::map::const_iterator it = objs.begin(); it != objs.end(); - ++it) - ser_objs[it->first] = py_bindings_tools::serializeMsg(it->second); - - return py_bindings_tools::dictFromType(ser_objs); - } - - bp::dict getAttachedObjectsPython(const bp::list& object_ids) - { - std::map aobjs = - getAttachedObjects(py_bindings_tools::stringFromList(object_ids)); - std::map ser_aobjs; - for (std::map::const_iterator it = aobjs.begin(); - it != aobjs.end(); ++it) - ser_aobjs[it->first] = py_bindings_tools::serializeMsg(it->second); - - return py_bindings_tools::dictFromType(ser_aobjs); - } - - bool applyPlanningScenePython(const py_bindings_tools::ByteString& ps_str) - { - moveit_msgs::msg::PlanningScene ps_msg; - py_bindings_tools::deserializeMsg(ps_str, ps_msg); - return applyPlanningScene(ps_msg); - } -}; - -static void wrap_planning_scene_interface() -{ - bp::class_ planning_scene_class("PlanningSceneInterface", - bp::init>()); - - planning_scene_class.def("get_known_object_names", &PlanningSceneInterfaceWrapper::getKnownObjectNamesPython); - planning_scene_class.def("get_known_object_names_in_roi", - &PlanningSceneInterfaceWrapper::getKnownObjectNamesInROIPython); - planning_scene_class.def("get_object_poses", &PlanningSceneInterfaceWrapper::getObjectPosesPython); - planning_scene_class.def("get_objects", &PlanningSceneInterfaceWrapper::getObjectsPython); - planning_scene_class.def("get_attached_objects", &PlanningSceneInterfaceWrapper::getAttachedObjectsPython); - planning_scene_class.def("apply_planning_scene", &PlanningSceneInterfaceWrapper::applyPlanningScenePython); -} -} // namespace planning_interface -} // namespace moveit - -BOOST_PYTHON_MODULE(_moveit_planning_scene_interface) -{ - using namespace moveit::planning_interface; - wrap_planning_scene_interface(); -} - -/** @endcond */ diff --git a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt b/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt deleted file mode 100644 index 07739486f8..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/CMakeLists.txt +++ /dev/null @@ -1,26 +0,0 @@ -add_library(moveit_py_bindings_tools src/roscpp_initializer.cpp) -set_target_properties(moveit_py_bindings_tools PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_py_bindings_tools ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${PYTHON_LIBRARIES}) - -install(TARGETS moveit_py_bindings_tools - EXPORT moveit_py_bindings_tools - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION} -) - -add_library(moveit_py_bindings_tools_python src/wrap_python_roscpp_initializer.cpp) -target_link_libraries(moveit_py_bindings_tools_python moveit_py_bindings_tools ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) -set_target_properties(moveit_py_bindings_tools_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(moveit_py_bindings_tools_python PROPERTIES OUTPUT_NAME _moveit_roscpp_initializer PREFIX "") -set_target_properties(moveit_py_bindings_tools_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") -if(WIN32) - set_target_properties(moveit_py_bindings_tools_python PROPERTIES SUFFIX .pyd) -endif() - -install(TARGETS moveit_py_bindings_tools_python - EXPORT moveit_py_bindings_tools_python - DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} -) - -install(DIRECTORY include/ DESTINATION ${CATKIN_GLOBAL_INCLUDE_DESTINATION}) diff --git a/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox b/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox deleted file mode 100644 index 061314d0d8..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/dox/py_bindings.dox +++ /dev/null @@ -1,48 +0,0 @@ -/**\page py_bindings_roscpp Creating Python bindings for C++ classes - - \par Problem - When creating Python bindings of C++ classes it may be - necessary to have roscpp initialized (ros::init() called) when the - instance of a particular C++ class is created. - \par Example - For example, say we need python bindings for class Foo; - A common approach to define functionality specific for the wrappers is to - define: - \code - class FooWrapper : public Foo - { - public: - ... - }; - \endcode - This allows defining additional functions in FooWrapper that are - more amenable to the construction of Python bindings. - Say that FooWrapper is now constructed because a Python programmer - created the instance of the wrapped object. At that point, if Foo - needs ros::init() to have been called, there is an error, because - even if rospy is initialized, roscpp is not. - @par Solutions - We can update class FooWrapper using the moveit_py_bindings_tools::ROScppInitializer class - like so: - \code - class FooWrapper : protected ROScppInitializer - public Foo - { - public: - FooWrapper(...) : ROScppInitializer(), - Foo(...) - { } - ... - }; - \endcode - This way it is ensured that the constructor of ROScppInitialier is - called right before the instance of Foo is constructed (in the - process of constructing FooWrapper). - The advantage of this solution is that the Python programmer could not care less - what roscpp is doing. Things will "just work". - The downside of this solution is that if any command line arguments - have been passed to the Python program, they are not forwarded to roscpp. - But to forward these parameters an explicit function call needs to be made. - For that we provide a function call - -*/ diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h deleted file mode 100644 index e7dfe3ecc3..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/gil_releaser.h +++ /dev/null @@ -1,96 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, RWTH Aachen University - * 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 RWTH Aachen University 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: Bjarne von Horn */ - -#pragma once - -#include -#include - -namespace moveit -{ -namespace py_bindings_tools -{ -/** \brief RAII Helper to release the Global Interpreter Lock (GIL) - * - * Use this helper class to release the GIL before doing long computations - * or blocking calls. Note that without the GIL Python-related functions must not be called. - * So, before releasing the GIL all \c boost::python variables have to be converted to e.g. \c std::vector. - * Before converting the result back to e.g. a moveit::py_bindings_tools::ByteString instance, the GIL has to be - * reacquired. - */ -class GILReleaser -{ - PyThreadState* thread_state_; - -public: - /** \brief Release the GIL on construction */ - GILReleaser() noexcept - { - thread_state_ = PyEval_SaveThread(); - } - /** \brief Reacquire the GIL on destruction */ - ~GILReleaser() noexcept - { - if (thread_state_) - { - PyEval_RestoreThread(thread_state_); - thread_state_ = nullptr; - } - } - - GILReleaser(const GILReleaser&) = delete; - GILReleaser(GILReleaser&& other) noexcept - { - thread_state_ = other.thread_state_; - other.thread_state_ = nullptr; - } - - GILReleaser& operator=(const GILReleaser&) = delete; - GILReleaser& operator=(GILReleaser&& other) noexcept - { - GILReleaser copy(std::move(other)); - swap(copy); - return *this; - } - - void swap(GILReleaser& other) noexcept - { - std::swap(other.thread_state_, thread_state_); - } -}; - -} // namespace py_bindings_tools -} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h deleted file mode 100644 index acbbde0d2b..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/py_conversions.h +++ /dev/null @@ -1,96 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, 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 Willow Garage 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: Ioan Sucan */ - -#pragma once - -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace py_bindings_tools -{ -template -std::vector typeFromList(const boost::python::object& values) -{ - boost::python::stl_input_iterator begin(values), end; - std::vector v; - v.assign(begin, end); - return v; -} - -template -boost::python::list listFromType(const std::vector& v) -{ - boost::python::list l; - for (std::size_t i = 0; i < v.size(); ++i) - l.append(v[i]); - return l; -} - -template -boost::python::dict dictFromType(const std::map& v) -{ - boost::python::dict d; - for (typename std::map::const_iterator it = v.begin(); it != v.end(); ++it) - d[it->first] = it->second; - return d; -} - -std::vector doubleFromList(const boost::python::object& values) -{ - return typeFromList(values); -} - -std::vector stringFromList(const boost::python::object& values) -{ - return typeFromList(values); -} - -boost::python::list listFromDouble(const std::vector& v) -{ - return listFromType(v); -} - -boost::python::list listFromString(const std::vector& v) -{ - return listFromType(v); -} -} // namespace py_bindings_tools -} // namespace moveit diff --git a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h b/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h deleted file mode 100644 index 61514fdb60..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/include/moveit/py_bindings_tools/serialize_msg.h +++ /dev/null @@ -1,137 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, 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 Willow Garage 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: Ioan Sucan */ - -#pragma once - -#include -#include -#include -#include -#include -#include - -namespace moveit -{ -namespace py_bindings_tools -{ -/** \brief C++ Wrapper class for Python 3 \c Bytes Object */ -class ByteString : public boost::python::object -{ -public: - // constructors for bp::handle and friends - BOOST_PYTHON_FORWARD_OBJECT_CONSTRUCTORS(ByteString, boost::python::object) - ByteString() : boost::python::object(boost::python::handle<>(PyBytes_FromString(""))) - { - } - explicit ByteString(const char* s) : boost::python::object(boost::python::handle<>(PyBytes_FromString(s))) - { - } - explicit ByteString(const std::string& s) - : boost::python::object(boost::python::handle<>(PyBytes_FromStringAndSize(s.c_str(), s.size()))) - { - } - // bp::list[] returns a proxy which has to be converted to an object first - template - explicit ByteString(const boost::python::api::proxy& proxy) : boost::python::object(proxy) - { - } - /** \brief Serializes a ROS message into a Python Bytes object - * The second template parameter ensures that this overload is only chosen with a ROS message argument - */ - template ::value, int>::type = 0> - explicit ByteString(const T& msg) - : boost::python::object( - boost::python::handle<>(PyBytes_FromStringAndSize(nullptr, ros::serialization::serializationLength(msg)))) - { - ros::serialization::OStream stream_arg(reinterpret_cast(PyBytes_AS_STRING(ptr())), - PyBytes_GET_SIZE(ptr())); - ros::serialization::serialize(stream_arg, msg); - } - - /** \brief Convert content to a ROS message */ - template - void deserialize(T& msg) const - { - static_assert(sizeof(uint8_t) == sizeof(char), "ros/python buffer layout mismatch"); - char* buf = PyBytes_AsString(ptr()); - // buf == nullptr on error - if (!buf) - { - throw std::runtime_error("Underlying python object is not a Bytes/String instance"); - } - // unfortunately no constructor with const uint8_t - ros::serialization::IStream stream_arg(reinterpret_cast(buf), PyBytes_GET_SIZE(ptr())); - ros::serialization::deserialize(stream_arg, msg); - } -}; - -/** \brief Convert a ROS message to a Python Bytestring */ -template -ByteString serializeMsg(const T& msg) -{ - return ByteString(msg); -} - -/** \brief Convert a Python Bytestring to a ROS message */ -template -void deserializeMsg(const ByteString& data, T& msg) -{ - data.deserialize(msg); -} - -} // namespace py_bindings_tools -} // namespace moveit - -namespace boost -{ -namespace python -{ -namespace converter -{ -// only accept Python 3 Bytes / Python 2 String instance when used as C++ function parameter -template <> -struct object_manager_traits -#if PY_VERSION_HEX >= 0x03000000 - : pytype_object_manager_traits<&PyBytes_Type, moveit::py_bindings_tools::ByteString> -#else - : pytype_object_manager_traits<&PyString_Type, moveit::py_bindings_tools::ByteString> -#endif - -{ -}; -} // namespace converter -} // namespace python -} // namespace boost diff --git a/moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp b/moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp deleted file mode 100644 index b0a0dccf0a..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/src/roscpp_initializer.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, 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 Willow Garage 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: Ioan Sucan */ - -#include -#include -#include -#include -#include - -static std::vector& ROScppArgs() -{ - static std::vector args; - return args; -} - -static std::string& ROScppNodeName() -{ - static std::string node_name("moveit_python_wrappers"); - return node_name; -} - -void moveit::py_bindings_tools::roscpp_set_arguments(const std::string& node_name, boost::python::list& argv) -{ - ROScppNodeName() = node_name; - ROScppArgs() = stringFromList(argv); -} - -namespace -{ -struct InitProxy -{ - InitProxy() - { - const std::vector& args = ROScppArgs(); - int fake_argc = args.size(); - char** fake_argv = new char*[args.size()]; - for (std::size_t i = 0; i < args.size(); ++i) - fake_argv[i] = strdup(args[i].c_str()); - - ros::init(fake_argc, fake_argv, ROScppNodeName(), - ros::init_options::AnonymousName | ros::init_options::NoSigintHandler); - for (int i = 0; i < fake_argc; ++i) - delete[] fake_argv[i]; - delete[] fake_argv; - } - - ~InitProxy() - { - if (ros::isInitialized() && !ros::isShuttingDown()) - ros::shutdown(); - } -}; -} // namespace - -static void roscpp_init_or_stop(bool init) -{ - // ensure we do not accidentally initialize ROS multiple times per process - static std::mutex lock; - std::mutex::scoped_lock slock(lock); - - // once per process, we start a spinner - static bool once = true; - static std::unique_ptr proxy; - static std::unique_ptr spinner; - - // initialize only once - if (once && init) - { - once = false; - - // if ROS (cpp) is not initialized, we initialize it - if (!ros::isInitialized()) - { - proxy = std::make_unique(); - spinner = std::make_unique(1); - spinner->start(); - } - } - - // shutdown if needed - if (!init) - { - once = false; - proxy.reset(); - spinner.reset(); - } -} - -void moveit::py_bindings_tools::roscpp_init() -{ - roscpp_init_or_stop(true); -} - -void moveit::py_bindings_tools::roscpp_init(const std::string& node_name, boost::python::list& argv) -{ - roscpp_set_arguments(node_name, argv); - roscpp_init(); -} - -void moveit::py_bindings_tools::roscpp_init(boost::python::list& argv) -{ - ROScppArgs() = stringFromList(argv); - roscpp_init(); -} - -void moveit::py_bindings_tools::roscpp_shutdown() -{ - roscpp_init_or_stop(false); -} - -moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer() -{ - roscpp_init(); -} - -moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(boost::python::list& argv) -{ - roscpp_init(argv); -} - -moveit::py_bindings_tools::ROScppInitializer::ROScppInitializer(const std::string& node_name, boost::python::list& argv) -{ - roscpp_init(node_name, argv); -} diff --git a/moveit_ros/planning_interface/py_bindings_tools/src/wrap_python_roscpp_initializer.cpp b/moveit_ros/planning_interface/py_bindings_tools/src/wrap_python_roscpp_initializer.cpp deleted file mode 100644 index d4cf4d4c85..0000000000 --- a/moveit_ros/planning_interface/py_bindings_tools/src/wrap_python_roscpp_initializer.cpp +++ /dev/null @@ -1,53 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2012, Willow Garage, 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 Willow Garage 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: Ioan Sucan */ - -#include -#include -#include - -namespace bp = boost::python; - -static void wrap_roscpp_initializer() -{ - void (*init_fn)(const std::string&, bp::list&) = &moveit::py_bindings_tools::roscpp_init; - bp::def("roscpp_init", init_fn); - bp::def("roscpp_shutdown", &moveit::py_bindings_tools::roscpp_shutdown); -} - -BOOST_PYTHON_MODULE(_moveit_roscpp_initializer) -{ - wrap_roscpp_initializer(); -} diff --git a/moveit_ros/planning_interface/python/moveit_ros_planning_interface/__init__.py b/moveit_ros/planning_interface/python/moveit_ros_planning_interface/__init__.py deleted file mode 100644 index e69de29bb2..0000000000 diff --git a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt b/moveit_ros/planning_interface/robot_interface/CMakeLists.txt deleted file mode 100644 index 031b350854..0000000000 --- a/moveit_ros/planning_interface/robot_interface/CMakeLists.txt +++ /dev/null @@ -1,15 +0,0 @@ -add_library(moveit_robot_interface_python src/wrap_python_robot_interface.cpp) -target_link_libraries(moveit_robot_interface_python ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} - moveit_common_planning_interface_objects moveit_py_bindings_tools -) -set_target_properties(moveit_robot_interface_python PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -set_target_properties(moveit_robot_interface_python PROPERTIES OUTPUT_NAME _moveit_robot_interface PREFIX "") -set_target_properties(moveit_robot_interface_python PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") -if(WIN32) - set_target_properties(moveit_robot_interface_python PROPERTIES SUFFIX .pyd) -endif() - -install(TARGETS moveit_robot_interface_python - EXPORT moveit_robot_interface_python - DESTINATION ${CATKIN_PACKAGE_PYTHON_DESTINATION} -) diff --git a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp b/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp deleted file mode 100644 index 8005f5d886..0000000000 --- a/moveit_ros/planning_interface/robot_interface/src/wrap_python_robot_interface.cpp +++ /dev/null @@ -1,430 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2013, Willow Garage, 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 Willow Garage 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: Ioan Sucan */ - -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -/** @cond IGNORE */ - -namespace bp = boost::python; -using moveit::py_bindings_tools::GILReleaser; - -namespace moveit -{ -class RobotInterfacePython : protected py_bindings_tools::ROScppInitializer -{ -public: - RobotInterfacePython(const std::string& robot_description, const std::string& ns = "") - : py_bindings_tools::ROScppInitializer() - { - robot_model_ = planning_interface::getSharedRobotModel(robot_description); - if (!robot_model_) - throw std::runtime_error("RobotInterfacePython: invalid robot model"); - current_state_monitor_ = - planning_interface::getSharedStateMonitor(robot_model_, planning_interface::getSharedTF(), ns); - } - - const char* getRobotName() const - { - return robot_model_->getName().c_str(); - } - - bp::list getActiveJointNames() const - { - return py_bindings_tools::listFromString(robot_model_->getActiveJointModelNames()); - } - - bp::list getGroupActiveJointNames(const std::string& group) const - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - return py_bindings_tools::listFromString(jmg->getActiveJointModelNames()); - else - return bp::list(); - } - - bp::list getJointNames() const - { - return py_bindings_tools::listFromString(robot_model_->getJointModelNames()); - } - - bp::list getGroupJointNames(const std::string& group) const - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - return py_bindings_tools::listFromString(jmg->getJointModelNames()); - else - return bp::list(); - } - - bp::list getGroupJointTips(const std::string& group) const - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - { - std::vector tips; - jmg->getEndEffectorTips(tips); - return py_bindings_tools::listFromString(tips); - } - else - return bp::list(); - } - - bp::list getLinkNames() const - { - return py_bindings_tools::listFromString(robot_model_->getLinkModelNames()); - } - - bp::list getGroupLinkNames(const std::string& group) const - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - return py_bindings_tools::listFromString(jmg->getLinkModelNames()); - else - return bp::list(); - } - - bp::list getGroupNames() const - { - return py_bindings_tools::listFromString(robot_model_->getJointModelGroupNames()); - } - - bp::list getJointLimits(const std::string& name) const - { - bp::list result; - const moveit::core::JointModel* jm = robot_model_->getJointModel(name); - if (jm) - { - const std::vector& lim = jm->getVariableBoundsMsg(); - for (const moveit_msgs::msg::JointLimits& joint_limit : lim) - { - bp::list l; - l.append(joint_limit.min_position); - l.append(joint_limit.max_position); - result.append(l); - } - } - return result; - } - - const char* getPlanningFrame() const - { - return robot_model_->getModelFrame().c_str(); - } - - bp::list getLinkPose(const std::string& name) - { - bp::list l; - if (!ensureCurrentState()) - return l; - moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState(); - const moveit::core::LinkModel* lm = state->getLinkModel(name); - if (lm) - { - // getGlobalLinkTransform() returns a valid isometry by contract - const Eigen::Isometry3d& t = state->getGlobalLinkTransform(lm); - std::vector v(7); - v[0] = t.translation().x(); - v[1] = t.translation().y(); - v[2] = t.translation().z(); - Eigen::Quaterniond q(t.linear()); - v[3] = q.x(); - v[4] = q.y(); - v[5] = q.z(); - v[6] = q.w(); - l = py_bindings_tools::listFromDouble(v); - } - return l; - } - - bp::list getDefaultStateNames(const std::string& group) - { - bp::list l; - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (jmg) - { - for (auto& known_state : jmg->getDefaultStateNames()) - { - l.append(known_state); - } - } - return l; - } - - bp::list getCurrentJointValues(const std::string& name) - { - bp::list l; - if (!ensureCurrentState()) - return l; - moveit::core::RobotStatePtr state = current_state_monitor_->getCurrentState(); - const moveit::core::JointModel* jm = state->getJointModel(name); - if (jm) - { - const double* pos = state->getJointPositions(jm); - const unsigned int sz = jm->getVariableCount(); - for (unsigned int i = 0; i < sz; ++i) - l.append(pos[i]); - } - - return l; - } - - bp::dict getJointValues(const std::string& group, const std::string& named_state) - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (!jmg) - return boost::python::dict(); - std::map values; - jmg->getVariableDefaultPositions(named_state, values); - return py_bindings_tools::dictFromType(values); - } - - bool ensureCurrentState(double wait = 1.0) - { - if (!current_state_monitor_) - { - ROS_ERROR("Unable to get current robot state"); - return false; - } - - // if needed, start the monitor and wait up to 1 second for a full robot state - if (!current_state_monitor_->isActive()) - { - GILReleaser gr; - current_state_monitor_->startStateMonitor(); - if (!current_state_monitor_->waitForCompleteState(wait)) - ROS_WARN("Joint values for monitored state are requested but the full state is not known"); - } - return true; - } - - py_bindings_tools::ByteString getCurrentState() - { - if (!ensureCurrentState()) - return py_bindings_tools::ByteString(""); - moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); - moveit_msgs::msg::RobotState msg; - moveit::core::robotStateToRobotStateMsg(*s, msg); - return py_bindings_tools::serializeMsg(msg); - } - - bp::tuple getEndEffectorParentGroup(const std::string& group) - { - // name of the group that is parent to this end-effector group; - // Second: the link this in the parent group that this group attaches to - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (!jmg) - return boost::python::make_tuple("", ""); - std::pair parent_group = jmg->getEndEffectorParentGroup(); - return boost::python::make_tuple(parent_group.first, parent_group.second); - } - - py_bindings_tools::ByteString getRobotMarkersPythonDictList(bp::dict& values, bp::list& links) - { - moveit::core::RobotStatePtr state; - if (ensureCurrentState()) - { - state = current_state_monitor_->getCurrentState(); - } - else - { - state = std::make_shared(robot_model_); - } - - bp::list k = values.keys(); - int l = bp::len(k); - sensor_msgs::JointState joint_state; - joint_state.name.resize(l); - joint_state.position.resize(l); - for (int i = 0; i < l; ++i) - { - joint_state.name[i] = bp::extract(k[i]); - joint_state.position[i] = bp::extract(values[k[i]]); - } - state->setVariableValues(joint_state); - visualization_msgs::MarkerArray msg; - state->getRobotMarkers(msg, py_bindings_tools::stringFromList(links)); - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkersPythonDict(bp::dict& values) - { - bp::list links = py_bindings_tools::listFromString(robot_model_->getLinkModelNames()); - return getRobotMarkersPythonDictList(values, links); - } - - py_bindings_tools::ByteString getRobotMarkersFromMsg(const py_bindings_tools::ByteString& state_str) - { - moveit_msgs::msg::RobotState state_msg; - moveit::core::RobotState state(robot_model_); - py_bindings_tools::deserializeMsg(state_str, state_msg); - moveit::core::robotStateMsgToRobotState(state_msg, state); - - visualization_msgs::MarkerArray msg; - state.getRobotMarkers(msg, state.getRobotModel()->getLinkModelNames()); - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkers() - { - if (!ensureCurrentState()) - return py_bindings_tools::ByteString(); - moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); - visualization_msgs::MarkerArray msg; - s->getRobotMarkers(msg, s->getRobotModel()->getLinkModelNames()); - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkersPythonList(const bp::list& links) - { - if (!ensureCurrentState()) - return py_bindings_tools::ByteString(""); - moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); - visualization_msgs::MarkerArray msg; - s->getRobotMarkers(msg, py_bindings_tools::stringFromList(links)); - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkersGroup(const std::string& group) - { - if (!ensureCurrentState()) - return py_bindings_tools::ByteString(""); - moveit::core::RobotStatePtr s = current_state_monitor_->getCurrentState(); - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - visualization_msgs::MarkerArray msg; - if (jmg) - { - s->getRobotMarkers(msg, jmg->getLinkModelNames()); - } - - return py_bindings_tools::serializeMsg(msg); - } - - py_bindings_tools::ByteString getRobotMarkersGroupPythonDict(const std::string& group, bp::dict& values) - { - const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(group); - if (!jmg) - return py_bindings_tools::ByteString(""); - bp::list links = py_bindings_tools::listFromString(jmg->getLinkModelNames()); - return getRobotMarkersPythonDictList(values, links); - } - - bp::dict getCurrentVariableValues() - { - bp::dict d; - - if (!ensureCurrentState()) - return d; - - const std::map& vars = current_state_monitor_->getCurrentStateValues(); - for (const std::pair& var : vars) - d[var.first] = var.second; - - return d; - } - - const char* getRobotRootLink() const - { - return robot_model_->getRootLinkName().c_str(); - } - - bool hasGroup(const std::string& group) const - { - return robot_model_->hasJointModelGroup(group); - } - -private: - moveit::core::RobotModelConstPtr robot_model_; - planning_scene_monitor::CurrentStateMonitorPtr current_state_monitor_; - ros::NodeHandle nh_; -}; -} // namespace moveit - -static void wrap_robot_interface() -{ - using namespace moveit; - - bp::class_ robot_class("RobotInterface", bp::init>()); - - robot_class.def("get_joint_names", &RobotInterfacePython::getJointNames); - robot_class.def("get_group_joint_names", &RobotInterfacePython::getGroupJointNames); - robot_class.def("get_active_joint_names", &RobotInterfacePython::getActiveJointNames); - robot_class.def("get_group_active_joint_names", &RobotInterfacePython::getGroupActiveJointNames); - robot_class.def("get_group_default_states", &RobotInterfacePython::getDefaultStateNames); - robot_class.def("get_group_joint_tips", &RobotInterfacePython::getGroupJointTips); - robot_class.def("get_group_names", &RobotInterfacePython::getGroupNames); - robot_class.def("get_link_names", &RobotInterfacePython::getLinkNames); - robot_class.def("get_group_link_names", &RobotInterfacePython::getGroupLinkNames); - robot_class.def("get_joint_limits", &RobotInterfacePython::getJointLimits); - robot_class.def("get_link_pose", &RobotInterfacePython::getLinkPose); - robot_class.def("get_planning_frame", &RobotInterfacePython::getPlanningFrame); - robot_class.def("get_current_state", &RobotInterfacePython::getCurrentState); - robot_class.def("get_current_variable_values", &RobotInterfacePython::getCurrentVariableValues); - robot_class.def("get_current_joint_values", &RobotInterfacePython::getCurrentJointValues); - robot_class.def("get_joint_values", &RobotInterfacePython::getJointValues); - robot_class.def("get_robot_root_link", &RobotInterfacePython::getRobotRootLink); - robot_class.def("has_group", &RobotInterfacePython::hasGroup); - robot_class.def("get_robot_name", &RobotInterfacePython::getRobotName); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkers); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonList); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersFromMsg); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDictList); - robot_class.def("get_robot_markers", &RobotInterfacePython::getRobotMarkersPythonDict); - robot_class.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroup); - robot_class.def("get_group_markers", &RobotInterfacePython::getRobotMarkersGroupPythonDict); - robot_class.def("get_parent_group", &RobotInterfacePython::getEndEffectorParentGroup); -} - -BOOST_PYTHON_MODULE(_moveit_robot_interface) -{ - wrap_robot_interface(); -} - -/** @endcond */ diff --git a/moveit_ros/planning_interface/setup.py b/moveit_ros/planning_interface/setup.py deleted file mode 100644 index 4be294ccc8..0000000000 --- a/moveit_ros/planning_interface/setup.py +++ /dev/null @@ -1,9 +0,0 @@ -from setuptools import setup -from catkin_pkg.python_setup import generate_distutils_setup - -d = generate_distutils_setup() -d["packages"] = ["moveit_ros_planning_interface"] -d["scripts"] = [] -d["package_dir"] = {"": "python"} - -setup(**d) diff --git a/moveit_ros/planning_interface/test/CMakeLists.txt b/moveit_ros/planning_interface/test/CMakeLists.txt index 613f506800..1d4532594f 100644 --- a/moveit_ros/planning_interface/test/CMakeLists.txt +++ b/moveit_ros/planning_interface/test/CMakeLists.txt @@ -1,47 +1,32 @@ if(BUILD_TESTING) find_package(moveit_resources REQUIRED) find_package(ament_cmake_gtest REQUIRED) - # TODO(henningkayser): migrate to launch_test - # find_package(rostest REQUIRED) + # TODO(henningkayser): migrate to launch_test find_package(rostest REQUIRED) find_package(eigen_conversions REQUIRED) # add_executable(test_cleanup test_cleanup.cpp) # target_link_libraries(test_cleanup moveit_move_group_interface) ament_add_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) - #ament_target_dependencies(moveit_cpp_test rclcpp) + # ament_target_dependencies(moveit_cpp_test rclcpp) target_link_libraries(moveit_cpp_test moveit_cpp) # TODO(henningkayser): re-enable new tests - # add_rostest_gtest(move_group_interface_cpp_test move_group_interface_cpp_test.test move_group_interface_cpp_test.cpp) - # target_link_libraries(move_group_interface_cpp_test moveit_move_group_interface ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + # add_rostest_gtest(move_group_interface_cpp_test + # move_group_interface_cpp_test.test move_group_interface_cpp_test.cpp) + # target_link_libraries(move_group_interface_cpp_test + # moveit_move_group_interface ${catkin_LIBRARIES} + # ${eigen_conversions_LIBRARIES}) # - # add_rostest_gtest(move_group_pick_place_test move_group_pick_place_test.test move_group_pick_place_test.cpp) - # target_link_libraries(move_group_pick_place_test moveit_move_group_interface ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) + # add_rostest_gtest(move_group_pick_place_test move_group_pick_place_test.test + # move_group_pick_place_test.cpp) + # target_link_libraries(move_group_pick_place_test moveit_move_group_interface + # ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) # - # add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) - # target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) + # add_rostest_gtest(moveit_cpp_test moveit_cpp_test.test moveit_cpp_test.cpp) + # target_link_libraries(moveit_cpp_test moveit_cpp ${catkin_LIBRARIES}) # - # add_rostest_gtest(subframes_test subframes_test.test subframes_test.cpp) - # target_link_libraries(subframes_test moveit_move_group_interface - # ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) - - # add_rostest(python_move_group.test) - # add_rostest(python_move_group_ns.test) - # add_rostest(robot_state_update.test) - # add_rostest(dual_arm_robot_state_update.test) - # add_rostest(cleanup.test) - - - # SET(HELPER_LIB moveit_planning_interface_test_serialize_msg_cpp_helper) - # add_library(${HELPER_LIB} serialize_msg_python_cpp_helpers.cpp) - # set_target_properties(${HELPER_LIB} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") - # target_link_libraries(${HELPER_LIB} ${PYTHON_LIBRARIES} ${catkin_LIBRARIES} ${Boost_LIBRARIES} moveit_py_bindings_tools) - # set_target_properties(${HELPER_LIB} PROPERTIES OUTPUT_NAME "_${HELPER_LIB}" PREFIX "") - # set_target_properties(${HELPER_LIB} PROPERTIES LIBRARY_OUTPUT_DIRECTORY "${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_PYTHON_DESTINATION}") - # if(WIN32) - # set_target_properties(${HELPER_LIB} PROPERTIES SUFFIX .pyd) - # endif(WIN32) - - # catkin_add_nosetests(serialize_msg.py) + # add_rostest_gtest(subframes_test subframes_test.test subframes_test.cpp) + # target_link_libraries(subframes_test moveit_move_group_interface + # ${catkin_LIBRARIES} ${eigen_conversions_LIBRARIES}) endif() diff --git a/moveit_ros/planning_interface/test/cleanup.py b/moveit_ros/planning_interface/test/cleanup.py deleted file mode 100755 index 933d763756..0000000000 --- a/moveit_ros/planning_interface/test/cleanup.py +++ /dev/null @@ -1,42 +0,0 @@ -#!/usr/bin/env python - -from __future__ import print_function - -import unittest -import rostest -import subprocess -import rospkg -import roslib.packages - -PKGNAME = "moveit_ros_planning_interface" -NODENAME = "moveit_cleanup_tests" - - -# As issue #592 is related to a crash during program exit, -# we cannot perform a standard unit test. -# We have to check the return value of the called program. -# As rostest doesn't do this automatically, we do it ourselves -# and call the actual test program here. -class CleanupTest(unittest.TestCase): - def __init__(self, *args, **kwargs): - super(CleanupTest, self).__init__(*args, **kwargs) - self._rospack = rospkg.RosPack() - - def run_cmd(self, cmd, num=5): - failures = 0 - for i in range(num): - if subprocess.call(cmd) != 0: - failures += 1 - self.assertEqual(failures, 0, "%d of %d runs failed" % (failures, num)) - - def test_py(self): - self.run_cmd( - roslib.packages.find_node(PKGNAME, "test_cleanup.py", self._rospack) - ) - - def test_cpp(self): - self.run_cmd(roslib.packages.find_node(PKGNAME, "test_cleanup", self._rospack)) - - -if __name__ == "__main__": - rostest.rosrun(PKGNAME, NODENAME, CleanupTest) diff --git a/moveit_ros/planning_interface/test/cleanup.test b/moveit_ros/planning_interface/test/cleanup.test deleted file mode 100644 index 775ea18666..0000000000 --- a/moveit_ros/planning_interface/test/cleanup.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_ros/planning_interface/test/dual_arm_robot_state_update.py b/moveit_ros/planning_interface/test/dual_arm_robot_state_update.py deleted file mode 100755 index 9486105f9a..0000000000 --- a/moveit_ros/planning_interface/test/dual_arm_robot_state_update.py +++ /dev/null @@ -1,59 +0,0 @@ -#!/usr/bin/env python - -import unittest -import rospy -import rostest -import os - -import moveit_commander - -from moveit_msgs.msg import MoveItErrorCodes, RobotTrajectory - - -class RobotStateUpdateTest(unittest.TestCase): - @classmethod - def setUpClass(self): - self.dual_arm_group = moveit_commander.MoveGroupCommander("dual_arm") - self.panda_1_group = moveit_commander.MoveGroupCommander("panda_1") - self.panda_2_group = moveit_commander.MoveGroupCommander("panda_2") - - @classmethod - def tearDown(self): - pass - - def plan(self, target): - self.dual_arm_group.set_joint_value_target(target) - return self.dual_arm_group.plan() - - def test(self): - panda_1_pose = self.panda_1_group.get_current_pose("panda_1_link8") - panda_1_pose.pose.position.z -= 0.05 - panda_1_pose.pose.position.x += 0.05 - - panda_2_pose = self.panda_2_group.get_current_pose("panda_2_link8") - panda_2_pose.pose.position.z -= 0.05 - panda_2_pose.pose.position.x -= 0.05 - self.dual_arm_group.set_start_state_to_current_state() - self.dual_arm_group.set_pose_target( - panda_1_pose, end_effector_link="panda_1_link8" - ) - self.dual_arm_group.set_pose_target( - panda_2_pose, end_effector_link="panda_2_link8" - ) - - result = self.dual_arm_group.plan() - if isinstance(result, RobotTrajectory): - self.assertTrue(result.joint_trajectory.joint_names) - else: - success, plan, planning_time, error = result - self.assertTrue(error.val == MoveItErrorCodes.SUCCESS) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_robot_state_update" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest) - - # suppress cleanup segfault in ROS < Kinetic - os._exit(0) diff --git a/moveit_ros/planning_interface/test/dual_arm_robot_state_update.test b/moveit_ros/planning_interface/test/dual_arm_robot_state_update.test deleted file mode 100644 index de9379609e..0000000000 --- a/moveit_ros/planning_interface/test/dual_arm_robot_state_update.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp index 429dd9d802..370d2f72df 100644 --- a/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_interface_cpp_test.cpp @@ -36,7 +36,7 @@ /* Author: Tyler Weaver, Boston Cleek */ /* These integration tests are based on the tutorials for using move_group: - * https://ros-planning.github.io/moveit_tutorials/doc/move_group_interface/move_group_interface_tutorial.html + * https://moveit.github.io/moveit_tutorials/doc/move_group_interface/move_group_interface_tutorial.html */ // C++ diff --git a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp index faad657180..b866015e00 100644 --- a/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp +++ b/moveit_ros/planning_interface/test/move_group_pick_place_test.cpp @@ -36,7 +36,7 @@ /* Author: Tyler Weaver */ /* These integration tests are based on the tutorials for using move_group to do a pick and place: - * https://ros-planning.github.io/moveit_tutorials/doc/pick_place/pick_place_tutorial.html + * https://moveit.github.io/moveit_tutorials/doc/pick_place/pick_place_tutorial.html */ // C++ diff --git a/moveit_ros/planning_interface/test/python_move_group.py b/moveit_ros/planning_interface/test/python_move_group.py deleted file mode 100755 index 8b5c9a9c05..0000000000 --- a/moveit_ros/planning_interface/test/python_move_group.py +++ /dev/null @@ -1,112 +0,0 @@ -#!/usr/bin/env python - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_ros_planning_interface._moveit_move_group_interface import ( - MoveGroupInterface, -) -from moveit_msgs.msg import MoveItErrorCodes - - -class PythonMoveGroupTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - - @classmethod - def setUpClass(self): - self.group = MoveGroupInterface( - self.PLANNING_GROUP, "robot_description", rospy.get_namespace() - ) - - @classmethod - def tearDown(self): - pass - - def check_target_setting(self, expect, *args): - if len(args) == 0: - args = [expect] - self.group.set_joint_value_target(*args) - res = self.group.get_joint_value_target() - self.assertTrue( - np.all(np.asarray(res) == np.asarray(expect)), - "Setting failed for %s, values: %s" % (type(args[0]), res), - ) - - def test_target_setting(self): - n = self.group.get_variable_count() - self.check_target_setting([0.1] * n) - self.check_target_setting((0.2,) * n) - self.check_target_setting(np.zeros(n)) - self.check_target_setting( - [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()} - ) - self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test_validation(self): - current = np.asarray(self.group.get_current_joint_values()) - - error_code1, plan1, time = self.plan(current + 0.2) - error_code2, plan2, time = self.plan(current + 0.2) - - # both plans should have succeeded: - error_code = MoveItErrorCodes() - error_code.deserialize(error_code1) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code2) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - # first plan should execute - self.assertTrue(self.group.execute(plan1)) - - # second plan should be invalid now (due to modified start point) and rejected - self.assertFalse(self.group.execute(plan2)) - - # newly planned trajectory should execute again - error_code3, plan3, time = self.plan(current) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code3) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - self.assertTrue(self.group.execute(plan3)) - - def test_get_jacobian_matrix(self): - current = self.group.get_current_joint_values() - result = self.group.get_jacobian_matrix(current) - # Value check by known value at the initial pose - expected = np.array( - [ - [0.0, 0.8, -0.2, 0.0, 0.0, 0.0], - [0.89, 0.0, 0.0, 0.0, 0.0, 0.0], - [0.0, -0.74, 0.74, 0.0, 0.1, 0.0], - [0.0, 0.0, 0.0, -1.0, 0.0, -1.0], - [0.0, 1.0, -1.0, 0.0, -1.0, 0.0], - [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - ] - ) - self.assertTrue(np.allclose(result, expected)) - - result = self.group.get_jacobian_matrix(current, [1.0, 1.0, 1.0]) - expected = np.array( - [ - [1.0, 1.8, -1.2, 0.0, -1.0, 0.0], - [1.89, 0.0, 0.0, 1.0, 0.0, 1.0], - [0.0, -1.74, 1.74, 1.0, 1.1, 1.0], - [0.0, 0.0, 0.0, -1.0, 0.0, -1.0], - [0.0, 1.0, -1.0, 0.0, -1.0, 0.0], - [1.0, 0.0, 0.0, 0.0, 0.0, 0.0], - ] - ) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_python_move_group" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupTest) diff --git a/moveit_ros/planning_interface/test/python_move_group.test b/moveit_ros/planning_interface/test/python_move_group.test deleted file mode 100644 index 4367da03ed..0000000000 --- a/moveit_ros/planning_interface/test/python_move_group.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - - diff --git a/moveit_ros/planning_interface/test/python_move_group_ns.py b/moveit_ros/planning_interface/test/python_move_group_ns.py deleted file mode 100755 index 0f2be65b59..0000000000 --- a/moveit_ros/planning_interface/test/python_move_group_ns.py +++ /dev/null @@ -1,124 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2012, Willow Garage, 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 Willow Garage, 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: William Baker -# -# This test is used to ensure planning with a MoveGroupInterface is -# possible if the robot's move_group node is in a different namespace - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_ros_planning_interface._moveit_move_group_interface import ( - MoveGroupInterface, -) -from moveit_msgs.msg import MoveItErrorCodes - - -class PythonMoveGroupNsTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - PLANNING_NS = "test_ns/" - - @classmethod - def setUpClass(self): - self.group = MoveGroupInterface( - self.PLANNING_GROUP, - "%srobot_description" % self.PLANNING_NS, - self.PLANNING_NS, - ) - - @classmethod - def tearDown(self): - pass - - def check_target_setting(self, expect, *args): - if len(args) == 0: - args = [expect] - self.group.set_joint_value_target(*args) - res = self.group.get_joint_value_target() - self.assertTrue( - np.all(np.asarray(res) == np.asarray(expect)), - "Setting failed for %s, values: %s" % (type(args[0]), res), - ) - - def test_target_setting(self): - n = self.group.get_variable_count() - self.check_target_setting([0.1] * n) - self.check_target_setting((0.2,) * n) - self.check_target_setting(np.zeros(n)) - self.check_target_setting( - [0.3] * n, {name: 0.3 for name in self.group.get_active_joints()} - ) - self.check_target_setting([0.5] + [0.3] * (n - 1), "joint_1", 0.5) - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test_validation(self): - current = np.asarray(self.group.get_current_joint_values()) - - error_code1, plan1, time = self.plan(current + 0.2) - error_code2, plan2, time = self.plan(current + 0.2) - - # both plans should have succeeded: - error_code = MoveItErrorCodes() - error_code.deserialize(error_code1) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code2) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - # first plan should execute - self.assertTrue(self.group.execute(plan1)) - - # second plan should be invalid now (due to modified start point) and rejected - self.assertFalse(self.group.execute(plan2)) - - # newly planned trajectory should execute again - error_code3, plan3, time = self.plan(current) - self.assertTrue(self.group.execute(plan3)) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code3) - self.assertEqual(error_code.val, MoveItErrorCodes.SUCCESS) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_python_move_group" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, PythonMoveGroupNsTest) diff --git a/moveit_ros/planning_interface/test/python_move_group_ns.test b/moveit_ros/planning_interface/test/python_move_group_ns.test deleted file mode 100644 index 082779ac94..0000000000 --- a/moveit_ros/planning_interface/test/python_move_group_ns.test +++ /dev/null @@ -1,9 +0,0 @@ - - - - - - - diff --git a/moveit_ros/planning_interface/test/robot_state_update.py b/moveit_ros/planning_interface/test/robot_state_update.py deleted file mode 100755 index cfd40858f9..0000000000 --- a/moveit_ros/planning_interface/test/robot_state_update.py +++ /dev/null @@ -1,58 +0,0 @@ -#!/usr/bin/env python - -import unittest -import numpy as np -import rospy -import rostest -import os - -from moveit_ros_planning_interface._moveit_move_group_interface import ( - MoveGroupInterface, -) -from moveit_msgs.msg import MoveItErrorCodes - - -class RobotStateUpdateTest(unittest.TestCase): - PLANNING_GROUP = "manipulator" - - @classmethod - def setUpClass(self): - self.group = MoveGroupInterface( - self.PLANNING_GROUP, "robot_description", rospy.get_namespace() - ) - - @classmethod - def tearDown(self): - pass - - def plan(self, target): - self.group.set_joint_value_target(target) - return self.group.plan() - - def test(self): - current = np.asarray(self.group.get_current_joint_values()) - for i in range(30): - target = current + np.random.uniform(-0.5, 0.5, size=current.shape) - # if plan was successfully executed, current state should be reported at target - error_code1, plan, time = self.plan(target) - error_code = MoveItErrorCodes() - error_code.deserialize(error_code1) - if (error_code.val == MoveItErrorCodes.SUCCESS) and self.group.execute( - plan - ): - actual = np.asarray(self.group.get_current_joint_values()) - self.assertTrue(np.allclose(target, actual, atol=1e-4, rtol=0.0)) - # otherwise current state should be still the same - else: - actual = np.asarray(self.group.get_current_joint_values()) - self.assertTrue(np.allclose(current, actual, atol=1e-4, rtol=0.0)) - - -if __name__ == "__main__": - PKGNAME = "moveit_ros_planning_interface" - NODENAME = "moveit_test_robot_state_update" - rospy.init_node(NODENAME) - rostest.rosrun(PKGNAME, NODENAME, RobotStateUpdateTest) - - # suppress cleanup segfault in ROS < Kinetic - os._exit(0) diff --git a/moveit_ros/planning_interface/test/robot_state_update.test b/moveit_ros/planning_interface/test/robot_state_update.test deleted file mode 100644 index f7849d7d7c..0000000000 --- a/moveit_ros/planning_interface/test/robot_state_update.test +++ /dev/null @@ -1,5 +0,0 @@ - - - - diff --git a/moveit_ros/planning_interface/test/serialize_msg.py b/moveit_ros/planning_interface/test/serialize_msg.py deleted file mode 100644 index 469e68d7f9..0000000000 --- a/moveit_ros/planning_interface/test/serialize_msg.py +++ /dev/null @@ -1,116 +0,0 @@ -#!/usr/bin/env python - -# Software License Agreement (BSD License) -# -# Copyright (c) 2020, RWTH Aachen University -# 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 RWTH Aachen University 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: Bjarne von Horn -# - -from moveit_ros_planning_interface._moveit_planning_interface_test_serialize_msg_cpp_helper import ( - ByteStringTestHelper, -) -from geometry_msgs.msg import Vector3 -import unittest - -try: - # Try Python 2.7 behaviour first - from StringIO import StringIO - - py_version_maj = 2 -except ImportError: - # Use Python 3.x behaviour as fallback and choose the non-unicode version - from io import BytesIO as StringIO - - py_version_maj = 3 - - -class PythonMsgSerializeTest(unittest.TestCase): - def setUp(self): - self.helper = ByteStringTestHelper() - - def test_EmbeddedZeros(self): - self.assertTrue(self.helper.compareEmbeddedZeros(b"\xff\xef\x00\x10")) - - def test_ByteStringFromPchar(self): - # ByteString(const char*) constructor - self.assertEqual(self.helper.getBytesPChar(), b"abcdef") - - def test_ByteStringFromStdString(self): - # ByteString(const std::string&) constructor - self.assertEqual(self.helper.getBytesStdString(), b"\xff\xfe\x10\x00\x00") - - def test_ByteStringDefaultCtor(self): - self.assertEqual(self.helper.getDefaultBytes(), b"") - - def test_CxxTupleToPy(self): - # sending a tuple from C++ to Python - ans = self.helper.getTuple() - self.assertIsInstance(ans, tuple) - self.assertEqual(len(ans), 1) - self.assertEqual(ans[0], b"abcdef") - - def test_PyTupleToCxx(self): - # sending a tuple from Python to C++ - self.assertTrue(self.helper.compareTuple((b"mno",))) - - def test_sendMessage(self): - tmp = StringIO() - Vector3(x=1.0, y=-2.0, z=0.25).serialize(tmp) - self.assertTrue(self.helper.compareVector(tmp.getvalue())) - - def test_recieveMessage(self): - tmp = Vector3() - tmp.deserialize(self.helper.getVector()) - self.assertEqual(tmp, Vector3(1.0, -2.0, 0.25)) - - def test_rejectInt(self): - with self.assertRaisesRegexp(Exception, "Python argument types in"): - self.helper.compareEmbeddedZeros(4711) - - def test_rejectIntTuple(self): - with self.assertRaisesRegexp(Exception, "Python argument types in"): - self.helper.compareEmbeddedZeros((4711,)) - - def test_rejectUnicode(self): - with self.assertRaisesRegexp(Exception, "Python argument types in"): - self.helper.compareEmbeddedZeros(u"kdasd") # fmt: skip - - @unittest.skipIf(py_version_maj == 2, "does not trigger with python 2.7") - def test_rejectUnicodeTuple(self): - with self.assertRaisesRegexp( - RuntimeError, "Underlying python object is not a Bytes/String instance" - ): - self.helper.compareVectorTuple((u"kdasd",)) # fmt: skip - - -if __name__ == "__main__": - unittest.main() diff --git a/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp b/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp deleted file mode 100644 index 13c6e446d6..0000000000 --- a/moveit_ros/planning_interface/test/serialize_msg_python_cpp_helpers.cpp +++ /dev/null @@ -1,134 +0,0 @@ -/********************************************************************* - * Software License Agreement (BSD License) - * - * Copyright (c) 2020, RWTH Aachen University. - * 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 RWTH Aachen University 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: Bjarne von Horn */ - -#include -#include -#include -#include -#include - -namespace bp = boost::python; -using moveit::py_bindings_tools::ByteString; - -// Helper class to be exposed to Python -class ByteStringTestHelper -{ - // Helper to test whether a vector of unsigned chars has the same content as a bytes Object - bool doCompare(const std::vector& data, PyObject* obj) - { - const char* py_data = PyBytes_AsString(obj); - if (!py_data) - return false; - Py_ssize_t size = PyBytes_GET_SIZE(obj); - if (size < 0 || std::vector::size_type(size) != data.size()) - return false; - return std::memcmp(py_data, &data[0], size) == 0; - } - -public: - bool compareEmbeddedZeros(const ByteString& s) - { - const std::vector testdata{ 0xff, 0xef, 0x00, 0x10 }; - return doCompare(testdata, s.ptr()); - } - bool compareTuple(const bp::tuple& t) - { - const ByteString s(t[0]); - const std::vector testdata{ 'm', 'n', 'o' }; - return doCompare(testdata, s.ptr()); - } - - ByteString getBytesPChar() - { - return ByteString("abcdef"); - } - ByteString getBytesStdString() - { - std::string s; - s.push_back('\xff'); - s.push_back('\xfe'); - s.push_back('\x10'); - s.push_back('\x00'); - s.push_back('\x00'); - return ByteString(s); - } - ByteString getDefaultBytes() - { - return ByteString(); - } - bp::tuple getTuple() - { - return bp::make_tuple(ByteString("abcdef")); - } - ByteString getVector() - { - geometry_msgs::Vector3 v; - v.x = 1.0; - v.y = -2.0; - v.z = 0.25; - return ByteString(v); - } - bool compareVector(const ByteString& s) - { - geometry_msgs::Vector3 v; - s.deserialize(v); - return v.x == 1.0 && v.y == -2.0 && v.z == 0.25; - } - bool compareVectorTuple(const bp::tuple& t) - { - const ByteString s(t[0]); - return compareVector(s); - } - - static void setup() - { - bp::class_ cls("ByteStringTestHelper"); - cls.def("compareEmbeddedZeros", &ByteStringTestHelper::compareEmbeddedZeros); - cls.def("compareTuple", &ByteStringTestHelper::compareTuple); - cls.def("compareVectorTuple", &ByteStringTestHelper::compareVectorTuple); - cls.def("getBytesPChar", &ByteStringTestHelper::getBytesPChar); - cls.def("getBytesStdString", &ByteStringTestHelper::getBytesStdString); - cls.def("getDefaultBytes", &ByteStringTestHelper::getDefaultBytes); - cls.def("getTuple", &ByteStringTestHelper::getTuple); - cls.def("getVector", &ByteStringTestHelper::getVector); - cls.def("compareVector", &ByteStringTestHelper::compareVector); - } -}; - -BOOST_PYTHON_MODULE(_moveit_planning_interface_test_serialize_msg_cpp_helper) -{ - ByteStringTestHelper::setup(); -} diff --git a/moveit_ros/planning_interface/test/subframes_test.cpp b/moveit_ros/planning_interface/test/subframes_test.cpp index f87eab588d..1f07dd30d8 100644 --- a/moveit_ros/planning_interface/test/subframes_test.cpp +++ b/moveit_ros/planning_interface/test/subframes_test.cpp @@ -36,7 +36,7 @@ /* Author: Felix von Drigalski, Jacob Aas, Tyler Weaver, Boston Cleek */ /* This integration test is based on the tutorial for using subframes - * https://ros-planning.github.io/moveit_tutorials/doc/subframes/subframes_tutorial.html + * https://moveit.github.io/moveit_tutorials/doc/subframes/subframes_tutorial.html */ // C++ diff --git a/moveit_ros/planning_interface/test/test_cleanup.py b/moveit_ros/planning_interface/test/test_cleanup.py deleted file mode 100755 index bb0aad1f72..0000000000 --- a/moveit_ros/planning_interface/test/test_cleanup.py +++ /dev/null @@ -1,8 +0,0 @@ -#!/usr/bin/env python - -import rospy -from moveit_ros_planning_interface._moveit_move_group_interface import ( - MoveGroupInterface, -) - -group = MoveGroupInterface("manipulator", "robot_description", rospy.get_namespace()) diff --git a/moveit_ros/robot_interaction/CHANGELOG.rst b/moveit_ros/robot_interaction/CHANGELOG.rst index 5cdfc4a21f..c6ec3927a8 100644 --- a/moveit_ros/robot_interaction/CHANGELOG.rst +++ b/moveit_ros/robot_interaction/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_ros_robot_interaction ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging in moveit_core (`#2503 `_) diff --git a/moveit_ros/robot_interaction/CMakeLists.txt b/moveit_ros/robot_interaction/CMakeLists.txt index a499f87b90..c9140d2a4f 100644 --- a/moveit_ros/robot_interaction/CMakeLists.txt +++ b/moveit_ros/robot_interaction/CMakeLists.txt @@ -14,29 +14,26 @@ find_package(tf2_geometry_msgs REQUIRED) find_package(tf2_ros REQUIRED) find_package(rclcpp REQUIRED) - -set(THIS_PACKAGE_INCLUDE_DEPENDS - moveit_core - moveit_ros_planning - interactive_markers - tf2_geometry_msgs -) +set(THIS_PACKAGE_INCLUDE_DEPENDS moveit_core moveit_ros_planning + interactive_markers tf2_geometry_msgs) include_directories(include) -add_library(moveit_robot_interaction SHARED - src/interactive_marker_helpers.cpp - src/kinematic_options.cpp - src/kinematic_options_map.cpp - src/locked_robot_state.cpp - src/interaction_handler.cpp - src/robot_interaction.cpp -) +add_library( + moveit_robot_interaction SHARED + src/interactive_marker_helpers.cpp src/kinematic_options.cpp + src/kinematic_options_map.cpp src/locked_robot_state.cpp + src/interaction_handler.cpp src/robot_interaction.cpp) include(GenerateExportHeader) generate_export_header(moveit_robot_interaction) -target_include_directories(moveit_robot_interaction PUBLIC $) -set_target_properties(moveit_robot_interaction PROPERTIES VERSION "${moveit_ros_robot_interaction_VERSION}") -ament_target_dependencies(moveit_robot_interaction ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_include_directories( + moveit_robot_interaction + PUBLIC $) +set_target_properties( + moveit_robot_interaction PROPERTIES VERSION + "${moveit_ros_robot_interaction_VERSION}") +ament_target_dependencies(moveit_robot_interaction + ${THIS_PACKAGE_INCLUDE_DEPENDS}) if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) @@ -50,27 +47,16 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_robot_interaction -) + INCLUDES + DESTINATION include/moveit_ros_robot_interaction) install(DIRECTORY include/ DESTINATION include/moveit_ros_robot_interaction) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_robot_interaction_export.h DESTINATION include/moveit_ros_robot_interaction) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_robot_interaction_export.h + DESTINATION include/moveit_ros_robot_interaction) install(DIRECTORY resources DESTINATION share/moveit_ros_robot_interaction) ament_export_targets(moveit_ros_robot_interactionTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_FOUND TRUE) - set(ament_cmake_cpplint_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() -endif() - ament_package() diff --git a/moveit_ros/robot_interaction/package.xml b/moveit_ros/robot_interaction/package.xml index 0db6067881..7f65b1d5da 100644 --- a/moveit_ros/robot_interaction/package.xml +++ b/moveit_ros/robot_interaction/package.xml @@ -2,7 +2,7 @@ moveit_ros_robot_interaction - 2.9.0 + 2.10.0 Components of MoveIt that offer interaction via interactive markers Henning Kayser Tyler Weaver @@ -12,8 +12,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan @@ -30,8 +30,6 @@ interactive_markers ament_cmake_gtest - ament_lint_auto - ament_lint_common ament_cmake diff --git a/moveit_ros/robot_interaction/src/interaction_handler.cpp b/moveit_ros/robot_interaction/src/interaction_handler.cpp index e1b41782f6..a56ac384cc 100644 --- a/moveit_ros/robot_interaction/src/interaction_handler.cpp +++ b/moveit_ros/robot_interaction/src/interaction_handler.cpp @@ -406,14 +406,15 @@ bool InteractionHandler::transformFeedbackPose( } catch (tf2::TransformException& e) { - RCLCPP_ERROR(moveit::getLogger("InteractionHandler"), "Error transforming from frame '%s' to frame '%s'", - tpose.header.frame_id.c_str(), planning_frame_.c_str()); + RCLCPP_ERROR(moveit::getLogger("moveit.ros.interaction_handler"), + "Error transforming from frame '%s' to frame '%s'", tpose.header.frame_id.c_str(), + planning_frame_.c_str()); return false; } } else { - RCLCPP_ERROR(moveit::getLogger("InteractionHandler"), + RCLCPP_ERROR(moveit::getLogger("moveit.ros.interaction_handler"), "Cannot transform from frame '%s' to frame '%s' (no TF instance provided)", tpose.header.frame_id.c_str(), planning_frame_.c_str()); return false; diff --git a/moveit_ros/robot_interaction/src/kinematic_options.cpp b/moveit_ros/robot_interaction/src/kinematic_options.cpp index 0e13c3c725..de704ae6e9 100644 --- a/moveit_ros/robot_interaction/src/kinematic_options.cpp +++ b/moveit_ros/robot_interaction/src/kinematic_options.cpp @@ -51,7 +51,7 @@ bool robot_interaction::KinematicOptions::setStateFromIK(moveit::core::RobotStat const moveit::core::JointModelGroup* jmg = state.getJointModelGroup(group); if (!jmg) { - RCLCPP_ERROR(moveit::getLogger("KinematicOptions"), "No getJointModelGroup('%s') found", group.c_str()); + RCLCPP_ERROR(moveit::getLogger("moveit.ros.kinematic_options"), "No getJointModelGroup('%s') found", group.c_str()); return false; } bool result = state.setFromIK(jmg, pose, tip, diff --git a/moveit_ros/robot_interaction/src/robot_interaction.cpp b/moveit_ros/robot_interaction/src/robot_interaction.cpp index 75ed047820..d641cd040c 100644 --- a/moveit_ros/robot_interaction/src/robot_interaction.cpp +++ b/moveit_ros/robot_interaction/src/robot_interaction.cpp @@ -65,7 +65,7 @@ RobotInteraction::RobotInteraction(const moveit::core::RobotModelConstPtr& robot const rclcpp::Node::SharedPtr& node, const std::string& ns) : robot_model_(robot_model) , node_(node) - , logger_(moveit::getLogger("moveit_ros_robot_interaction")) + , logger_(moveit::getLogger("moveit.ros.robot_interaction")) , kinematic_options_map_(std::make_shared()) { topic_ = ns.empty() ? INTERACTIVE_MARKER_TOPIC : ns + "/" + INTERACTIVE_MARKER_TOPIC; diff --git a/moveit_ros/tests/CHANGELOG.rst b/moveit_ros/tests/CHANGELOG.rst new file mode 100644 index 0000000000..8bd12d34ee --- /dev/null +++ b/moveit_ros/tests/CHANGELOG.rst @@ -0,0 +1,95 @@ +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +Changelog for package moveit_ros_tests +^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Add parameter api integration test (`#2662 `_) +* Contributors: Robert Haschke, Sebastian Jahr + +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Add parameter api integration test (`#2662 `_) +* Contributors: Robert Haschke, Sebastian Jahr + +2.9.0 (2024-01-09) +------------------ + +2.8.0 (2023-09-10) +------------------ + +2.7.4 (2023-05-18) +------------------ + +2.7.3 (2023-04-24) +------------------ + +2.7.2 (2023-04-18) +------------------ + +2.7.1 (2023-03-23) +------------------ + +2.7.0 (2023-02-09) +------------------ + +2.6.0 (2022-11-10) +------------------ + +2.5.3 (2022-07-28) +------------------ + +2.5.2 (2022-07-18) +------------------ + +2.5.1 (2022-05-31) +------------------ + +2.5.0 (2022-05-26) +------------------ + +2.4.0 (2022-01-20) +------------------ + +2.3.2 (2021-12-29) +------------------ + +2.3.1 (2021-12-23) +------------------ + +2.3.0 (2021-10-08) +------------------ + +2.2.1 (2021-07-12) +------------------ + +2.2.0 (2021-06-30) +------------------ + +2.1.4 (2021-05-31) +------------------ + +2.1.3 (2021-05-22) +------------------ + +2.1.2 (2021-04-22) +------------------ + +2.1.1 (2021-04-13) +------------------ + +2.1.0 (2020-11-24) +------------------ + +2.0.0 (2020-05-13) +------------------ diff --git a/moveit_ros/tests/CMakeLists.txt b/moveit_ros/tests/CMakeLists.txt new file mode 100644 index 0000000000..4f9a83b7f3 --- /dev/null +++ b/moveit_ros/tests/CMakeLists.txt @@ -0,0 +1,19 @@ +cmake_minimum_required(VERSION 3.22) +project(moveit_ros_tests LANGUAGES CXX) + +# Common cmake code applied to all moveit packages +find_package(moveit_common REQUIRED) +moveit_package() + +if(BUILD_TESTING) + find_package(ament_cmake_gtest REQUIRED) + find_package(ament_cmake REQUIRED) + find_package(rclcpp REQUIRED) + find_package(ros_testing REQUIRED) + + include_directories(include) + ament_add_gtest_executable(move_group_api_test src/move_group_api_test.cpp) + ament_target_dependencies(move_group_api_test rclcpp) + add_ros_test(launch/move_group_api.test.py TIMEOUT 30 ARGS + "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}") +endif() diff --git a/moveit_ros/tests/include/parameter_name_list.hpp b/moveit_ros/tests/include/parameter_name_list.hpp new file mode 100644 index 0000000000..90c1806fc7 --- /dev/null +++ b/moveit_ros/tests/include/parameter_name_list.hpp @@ -0,0 +1,495 @@ + +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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: Sebastian Jahr + Description: List of all parameter names used when move_group with OMPL, CHOMP, STOMP and PILZ is launched (01/2024). +*/ +#include + +namespace move_group_test +{ +const std::vector PARAMETER_NAME_LIST = { + "chomp.add_randomness", + "chomp.animate_endeffector", + "chomp.animate_endeffector_segment", + "chomp.animate_path", + "chomp.collision_clearance", + "chomp.collision_threshold", + "chomp.default_workspace_bounds", + "chomp.display_contacts_topic", + "chomp.display_path_topic", + "chomp.enable_failure_recovery", + "chomp.fix_start_state", + "chomp.hmc_annealing_factor", + "chomp.hmc_discretization", + "chomp.hmc_stochasticity", + "chomp.joint_update_limit", + "chomp.learning_rate", + "chomp.max_iterations", + "chomp.max_iterations_after_collision_free", + "chomp.max_recovery_attempts", + "chomp.obstacle_cost_weight", + "chomp.planning_plugins", + "chomp.planning_time_limit", + "chomp.progress_topic", + "chomp.pseudo_inverse_ridge_factor", + "chomp.random_jump_amount", + "chomp.request_adapters", + "chomp.response_adapters", + "chomp.ridge_factor", + "chomp.smoothness_cost_acceleration", + "chomp.smoothness_cost_jerk", + "chomp.smoothness_cost_velocity", + "chomp.smoothness_cost_weight", + "chomp.totg.min_angle_change", + "chomp.totg.path_tolerance", + "chomp.totg.resample_dt", + "chomp.trajectory_initialization_method", + "chomp.use_hamiltonian_monte_carlo", + "chomp.use_pseudo_inverse", + "chomp.use_stochastic_descent", + "default_planning_pipeline", + "moveit_controller_manager", + "moveit_manage_controllers", + "moveit_simple_controller_manager.controller_names", + "moveit_simple_controller_manager.panda_arm_controller.action_ns", + "moveit_simple_controller_manager.panda_arm_controller.default", + "moveit_simple_controller_manager.panda_arm_controller.joints", + "moveit_simple_controller_manager.panda_arm_controller.type", + "moveit_simple_controller_manager.panda_hand_controller.action_ns", + "moveit_simple_controller_manager.panda_hand_controller.default", + "moveit_simple_controller_manager.panda_hand_controller.joints", + "moveit_simple_controller_manager.panda_hand_controller.type", + "ompl.default_workspace_bounds", + "ompl.display_contacts_topic", + "ompl.display_path_topic", + "ompl.fix_start_state", + "ompl.hand.planner_configs", + "ompl.panda_arm.planner_configs", + "ompl.panda_arm_hand.planner_configs", + "ompl.planner_configs.APSConfigDefault.hybridize", + "ompl.planner_configs.APSConfigDefault.max_hybrid_paths", + "ompl.planner_configs.APSConfigDefault.num_planners", + "ompl.planner_configs.APSConfigDefault.planners", + "ompl.planner_configs.APSConfigDefault.shortcut", + "ompl.planner_configs.APSConfigDefault.type", + "ompl.planner_configs.BFMTkConfigDefault.balanced", + "ompl.planner_configs.BFMTkConfigDefault.cache_cc", + "ompl.planner_configs.BFMTkConfigDefault.extended_fmt", + "ompl.planner_configs.BFMTkConfigDefault.heuristics", + "ompl.planner_configs.BFMTkConfigDefault.nearest_k", + "ompl.planner_configs.BFMTkConfigDefault.num_samples", + "ompl.planner_configs.BFMTkConfigDefault.optimality", + "ompl.planner_configs.BFMTkConfigDefault.radius_multiplier", + "ompl.planner_configs.BFMTkConfigDefault.type", + "ompl.planner_configs.BKPIECEkConfigDefault.border_fraction", + "ompl.planner_configs.BKPIECEkConfigDefault.failed_expansion_score_factor", + "ompl.planner_configs.BKPIECEkConfigDefault.min_valid_path_fraction", + "ompl.planner_configs.BKPIECEkConfigDefault.range", + "ompl.planner_configs.BKPIECEkConfigDefault.type", + "ompl.planner_configs.BiESTkConfigDefault.range", + "ompl.planner_configs.BiESTkConfigDefault.type", + "ompl.planner_configs.BiTRRTkConfigDefault.cost_threshold", + "ompl.planner_configs.BiTRRTkConfigDefault.frountier_node_ratio", + "ompl.planner_configs.BiTRRTkConfigDefault.frountier_threshold", + "ompl.planner_configs.BiTRRTkConfigDefault.init_temperature", + "ompl.planner_configs.BiTRRTkConfigDefault.range", + "ompl.planner_configs.BiTRRTkConfigDefault.temp_change_factor", + "ompl.planner_configs.BiTRRTkConfigDefault.type", + "ompl.planner_configs.ESTkConfigDefault.goal_bias", + "ompl.planner_configs.ESTkConfigDefault.range", + "ompl.planner_configs.ESTkConfigDefault.type", + "ompl.planner_configs.FMTkConfigDefault.cache_cc", + "ompl.planner_configs.FMTkConfigDefault.extended_fmt", + "ompl.planner_configs.FMTkConfigDefault.heuristics", + "ompl.planner_configs.FMTkConfigDefault.nearest_k", + "ompl.planner_configs.FMTkConfigDefault.num_samples", + "ompl.planner_configs.FMTkConfigDefault.radius_multiplier", + "ompl.planner_configs.FMTkConfigDefault.type", + "ompl.planner_configs.KPIECEkConfigDefault.border_fraction", + "ompl.planner_configs.KPIECEkConfigDefault.failed_expansion_score_factor", + "ompl.planner_configs.KPIECEkConfigDefault.goal_bias", + "ompl.planner_configs.KPIECEkConfigDefault.min_valid_path_fraction", + "ompl.planner_configs.KPIECEkConfigDefault.range", + "ompl.planner_configs.KPIECEkConfigDefault.type", + "ompl.planner_configs.LBKPIECEkConfigDefault.border_fraction", + "ompl.planner_configs.LBKPIECEkConfigDefault.min_valid_path_fraction", + "ompl.planner_configs.LBKPIECEkConfigDefault.range", + "ompl.planner_configs.LBKPIECEkConfigDefault.type", + "ompl.planner_configs.LBTRRTkConfigDefault.epsilon", + "ompl.planner_configs.LBTRRTkConfigDefault.goal_bias", + "ompl.planner_configs.LBTRRTkConfigDefault.range", + "ompl.planner_configs.LBTRRTkConfigDefault.type", + "ompl.planner_configs.LazyPRMkConfigDefault.range", + "ompl.planner_configs.LazyPRMkConfigDefault.type", + "ompl.planner_configs.LazyPRMstarkConfigDefault.type", + "ompl.planner_configs.PDSTkConfigDefault.type", + "ompl.planner_configs.PRMkConfigDefault.max_nearest_neighbors", + "ompl.planner_configs.PRMkConfigDefault.type", + "ompl.planner_configs.PRMstarkConfigDefault.type", + "ompl.planner_configs.ProjESTkConfigDefault.goal_bias", + "ompl.planner_configs.ProjESTkConfigDefault.range", + "ompl.planner_configs.ProjESTkConfigDefault.type", + "ompl.planner_configs.RRTConnectkConfigDefault.range", + "ompl.planner_configs.RRTConnectkConfigDefault.type", + "ompl.planner_configs.RRTkConfigDefault.goal_bias", + "ompl.planner_configs.RRTkConfigDefault.range", + "ompl.planner_configs.RRTkConfigDefault.type", + "ompl.planner_configs.RRTstarkConfigDefault.delay_collision_checking", + "ompl.planner_configs.RRTstarkConfigDefault.goal_bias", + "ompl.planner_configs.RRTstarkConfigDefault.range", + "ompl.planner_configs.RRTstarkConfigDefault.type", + "ompl.planner_configs.SBLkConfigDefault.range", + "ompl.planner_configs.SBLkConfigDefault.type", + "ompl.planner_configs.SPARSkConfigDefault.dense_delta_fraction", + "ompl.planner_configs.SPARSkConfigDefault.max_failures", + "ompl.planner_configs.SPARSkConfigDefault.sparse_delta_fraction", + "ompl.planner_configs.SPARSkConfigDefault.stretch_factor", + "ompl.planner_configs.SPARSkConfigDefault.type", + "ompl.planner_configs.SPARStwokConfigDefault.dense_delta_fraction", + "ompl.planner_configs.SPARStwokConfigDefault.max_failures", + "ompl.planner_configs.SPARStwokConfigDefault.sparse_delta_fraction", + "ompl.planner_configs.SPARStwokConfigDefault.stretch_factor", + "ompl.planner_configs.SPARStwokConfigDefault.type", + "ompl.planner_configs.STRIDEkConfigDefault.degree", + "ompl.planner_configs.STRIDEkConfigDefault.estimated_dimension", + "ompl.planner_configs.STRIDEkConfigDefault.goal_bias", + "ompl.planner_configs.STRIDEkConfigDefault.max_degree", + "ompl.planner_configs.STRIDEkConfigDefault.max_pts_per_leaf", + "ompl.planner_configs.STRIDEkConfigDefault.min_degree", + "ompl.planner_configs.STRIDEkConfigDefault.min_valid_path_fraction", + "ompl.planner_configs.STRIDEkConfigDefault.range", + "ompl.planner_configs.STRIDEkConfigDefault.type", + "ompl.planner_configs.STRIDEkConfigDefault.use_projected_distance", + "ompl.planner_configs.TRRTkConfigDefault.frountierNodeRatio", + "ompl.planner_configs.TRRTkConfigDefault.frountier_threshold", + "ompl.planner_configs.TRRTkConfigDefault.goal_bias", + "ompl.planner_configs.TRRTkConfigDefault.init_temperature", + "ompl.planner_configs.TRRTkConfigDefault.k_constant", + "ompl.planner_configs.TRRTkConfigDefault.max_states_failed", + "ompl.planner_configs.TRRTkConfigDefault.min_temperature", + "ompl.planner_configs.TRRTkConfigDefault.range", + "ompl.planner_configs.TRRTkConfigDefault.temp_change_factor", + "ompl.planner_configs.TRRTkConfigDefault.type", + "ompl.planner_configs.TrajOptDefault.type", + "ompl.planning_plugins", + "ompl.progress_topic", + "ompl.request_adapters", + "ompl.response_adapters", + "ompl.totg.min_angle_change", + "ompl.totg.path_tolerance", + "ompl.totg.resample_dt", + "pilz_industrial_motion_planner.capabilities", + "pilz_industrial_motion_planner.default_planner_config", + "pilz_industrial_motion_planner.default_workspace_bounds", + "pilz_industrial_motion_planner.fix_start_state", + "pilz_industrial_motion_planner.planning_plugins", + "pilz_industrial_motion_planner.progress_topic", + "pilz_industrial_motion_planner.request_adapters", + "pilz_industrial_motion_planner.response_adapters", + "planning_pipelines", + "publish_geometry_updates", + "publish_planning_scene", + "publish_robot_description", + "publish_robot_description_semantic", + "publish_state_updates", + "publish_transforms_updates", + "robot_description", + "robot_description_kinematics.hand.kinematics_solver", + "robot_description_kinematics.hand.kinematics_solver_search_resolution", + "robot_description_kinematics.hand.kinematics_solver_timeout", + "robot_description_kinematics.panda_arm.epsilon", + "robot_description_kinematics.panda_arm.joints", + "robot_description_kinematics.panda_arm.kinematics_solver", + "robot_description_kinematics.panda_arm.kinematics_solver_search_resolution", + "robot_description_kinematics.panda_arm.kinematics_solver_timeout", + "robot_description_kinematics.panda_arm.max_solver_iterations", + "robot_description_kinematics.panda_arm.orientation_vs_position", + "robot_description_kinematics.panda_arm.position_only_ik", + "robot_description_kinematics.panda_arm_hand.kinematics_solver", + "robot_description_kinematics.panda_arm_hand.kinematics_solver_search_resolution", + "robot_description_kinematics.panda_arm_hand.kinematics_solver_timeout", + "robot_description_planning.cartesian_limits.max_rot_vel", + "robot_description_planning.cartesian_limits.max_trans_acc", + "robot_description_planning.cartesian_limits.max_trans_dec", + "robot_description_planning.cartesian_limits.max_trans_vel", + "robot_description_planning.joint_limits.panda_finger_joint1.angle_wraparound", + "robot_description_planning.joint_limits.panda_finger_joint1.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_finger_joint1.has_deceleration_limits", + "robot_description_planning.joint_limits.panda_finger_joint1.has_effort_limits", + "robot_description_planning.joint_limits.panda_finger_joint1.has_jerk_limits", + "robot_description_planning.joint_limits.panda_finger_joint1.has_position_limits", + "robot_description_planning.joint_limits.panda_finger_joint1.has_soft_limits", + "robot_description_planning.joint_limits.panda_finger_joint1.has_velocity_limits", + "robot_description_planning.joint_limits.panda_finger_joint1.k_position", + "robot_description_planning.joint_limits.panda_finger_joint1.k_velocity", + "robot_description_planning.joint_limits.panda_finger_joint1.max_acceleration", + "robot_description_planning.joint_limits.panda_finger_joint1.max_effort", + "robot_description_planning.joint_limits.panda_finger_joint1.max_jerk", + "robot_description_planning.joint_limits.panda_finger_joint1.max_position", + "robot_description_planning.joint_limits.panda_finger_joint1.max_velocity", + "robot_description_planning.joint_limits.panda_finger_joint1.min_position", + "robot_description_planning.joint_limits.panda_finger_joint1.min_velocity", + "robot_description_planning.joint_limits.panda_finger_joint1.soft_lower_limit", + "robot_description_planning.joint_limits.panda_finger_joint1.soft_upper_limit", + "robot_description_planning.joint_limits.panda_finger_joint2.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_finger_joint2.has_jerk_limits", + "robot_description_planning.joint_limits.panda_finger_joint2.has_velocity_limits", + "robot_description_planning.joint_limits.panda_finger_joint2.max_acceleration", + "robot_description_planning.joint_limits.panda_finger_joint2.max_position", + "robot_description_planning.joint_limits.panda_finger_joint2.max_velocity", + "robot_description_planning.joint_limits.panda_finger_joint2.min_position", + "robot_description_planning.joint_limits.panda_joint1.angle_wraparound", + "robot_description_planning.joint_limits.panda_joint1.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_joint1.has_deceleration_limits", + "robot_description_planning.joint_limits.panda_joint1.has_effort_limits", + "robot_description_planning.joint_limits.panda_joint1.has_jerk_limits", + "robot_description_planning.joint_limits.panda_joint1.has_position_limits", + "robot_description_planning.joint_limits.panda_joint1.has_soft_limits", + "robot_description_planning.joint_limits.panda_joint1.has_velocity_limits", + "robot_description_planning.joint_limits.panda_joint1.k_position", + "robot_description_planning.joint_limits.panda_joint1.k_velocity", + "robot_description_planning.joint_limits.panda_joint1.max_acceleration", + "robot_description_planning.joint_limits.panda_joint1.max_effort", + "robot_description_planning.joint_limits.panda_joint1.max_jerk", + "robot_description_planning.joint_limits.panda_joint1.max_position", + "robot_description_planning.joint_limits.panda_joint1.max_velocity", + "robot_description_planning.joint_limits.panda_joint1.min_position", + "robot_description_planning.joint_limits.panda_joint1.min_velocity", + "robot_description_planning.joint_limits.panda_joint1.soft_lower_limit", + "robot_description_planning.joint_limits.panda_joint1.soft_upper_limit", + "robot_description_planning.joint_limits.panda_joint2.angle_wraparound", + "robot_description_planning.joint_limits.panda_joint2.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_joint2.has_deceleration_limits", + "robot_description_planning.joint_limits.panda_joint2.has_effort_limits", + "robot_description_planning.joint_limits.panda_joint2.has_jerk_limits", + "robot_description_planning.joint_limits.panda_joint2.has_position_limits", + "robot_description_planning.joint_limits.panda_joint2.has_soft_limits", + "robot_description_planning.joint_limits.panda_joint2.has_velocity_limits", + "robot_description_planning.joint_limits.panda_joint2.k_position", + "robot_description_planning.joint_limits.panda_joint2.k_velocity", + "robot_description_planning.joint_limits.panda_joint2.max_acceleration", + "robot_description_planning.joint_limits.panda_joint2.max_effort", + "robot_description_planning.joint_limits.panda_joint2.max_jerk", + "robot_description_planning.joint_limits.panda_joint2.max_position", + "robot_description_planning.joint_limits.panda_joint2.max_velocity", + "robot_description_planning.joint_limits.panda_joint2.min_position", + "robot_description_planning.joint_limits.panda_joint2.min_velocity", + "robot_description_planning.joint_limits.panda_joint2.soft_lower_limit", + "robot_description_planning.joint_limits.panda_joint2.soft_upper_limit", + "robot_description_planning.joint_limits.panda_joint3.angle_wraparound", + "robot_description_planning.joint_limits.panda_joint3.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_joint3.has_deceleration_limits", + "robot_description_planning.joint_limits.panda_joint3.has_effort_limits", + "robot_description_planning.joint_limits.panda_joint3.has_jerk_limits", + "robot_description_planning.joint_limits.panda_joint3.has_position_limits", + "robot_description_planning.joint_limits.panda_joint3.has_soft_limits", + "robot_description_planning.joint_limits.panda_joint3.has_velocity_limits", + "robot_description_planning.joint_limits.panda_joint3.k_position", + "robot_description_planning.joint_limits.panda_joint3.k_velocity", + "robot_description_planning.joint_limits.panda_joint3.max_acceleration", + "robot_description_planning.joint_limits.panda_joint3.max_effort", + "robot_description_planning.joint_limits.panda_joint3.max_jerk", + "robot_description_planning.joint_limits.panda_joint3.max_position", + "robot_description_planning.joint_limits.panda_joint3.max_velocity", + "robot_description_planning.joint_limits.panda_joint3.min_position", + "robot_description_planning.joint_limits.panda_joint3.min_velocity", + "robot_description_planning.joint_limits.panda_joint3.soft_lower_limit", + "robot_description_planning.joint_limits.panda_joint3.soft_upper_limit", + "robot_description_planning.joint_limits.panda_joint4.angle_wraparound", + "robot_description_planning.joint_limits.panda_joint4.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_joint4.has_deceleration_limits", + "robot_description_planning.joint_limits.panda_joint4.has_effort_limits", + "robot_description_planning.joint_limits.panda_joint4.has_jerk_limits", + "robot_description_planning.joint_limits.panda_joint4.has_position_limits", + "robot_description_planning.joint_limits.panda_joint4.has_soft_limits", + "robot_description_planning.joint_limits.panda_joint4.has_velocity_limits", + "robot_description_planning.joint_limits.panda_joint4.k_position", + "robot_description_planning.joint_limits.panda_joint4.k_velocity", + "robot_description_planning.joint_limits.panda_joint4.max_acceleration", + "robot_description_planning.joint_limits.panda_joint4.max_effort", + "robot_description_planning.joint_limits.panda_joint4.max_jerk", + "robot_description_planning.joint_limits.panda_joint4.max_position", + "robot_description_planning.joint_limits.panda_joint4.max_velocity", + "robot_description_planning.joint_limits.panda_joint4.min_position", + "robot_description_planning.joint_limits.panda_joint4.min_velocity", + "robot_description_planning.joint_limits.panda_joint4.soft_lower_limit", + "robot_description_planning.joint_limits.panda_joint4.soft_upper_limit", + "robot_description_planning.joint_limits.panda_joint5.angle_wraparound", + "robot_description_planning.joint_limits.panda_joint5.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_joint5.has_deceleration_limits", + "robot_description_planning.joint_limits.panda_joint5.has_effort_limits", + "robot_description_planning.joint_limits.panda_joint5.has_jerk_limits", + "robot_description_planning.joint_limits.panda_joint5.has_position_limits", + "robot_description_planning.joint_limits.panda_joint5.has_soft_limits", + "robot_description_planning.joint_limits.panda_joint5.has_velocity_limits", + "robot_description_planning.joint_limits.panda_joint5.k_position", + "robot_description_planning.joint_limits.panda_joint5.k_velocity", + "robot_description_planning.joint_limits.panda_joint5.max_acceleration", + "robot_description_planning.joint_limits.panda_joint5.max_effort", + "robot_description_planning.joint_limits.panda_joint5.max_jerk", + "robot_description_planning.joint_limits.panda_joint5.max_position", + "robot_description_planning.joint_limits.panda_joint5.max_velocity", + "robot_description_planning.joint_limits.panda_joint5.min_position", + "robot_description_planning.joint_limits.panda_joint5.min_velocity", + "robot_description_planning.joint_limits.panda_joint5.soft_lower_limit", + "robot_description_planning.joint_limits.panda_joint5.soft_upper_limit", + "robot_description_planning.joint_limits.panda_joint6.angle_wraparound", + "robot_description_planning.joint_limits.panda_joint6.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_joint6.has_deceleration_limits", + "robot_description_planning.joint_limits.panda_joint6.has_effort_limits", + "robot_description_planning.joint_limits.panda_joint6.has_jerk_limits", + "robot_description_planning.joint_limits.panda_joint6.has_position_limits", + "robot_description_planning.joint_limits.panda_joint6.has_soft_limits", + "robot_description_planning.joint_limits.panda_joint6.has_velocity_limits", + "robot_description_planning.joint_limits.panda_joint6.k_position", + "robot_description_planning.joint_limits.panda_joint6.k_velocity", + "robot_description_planning.joint_limits.panda_joint6.max_acceleration", + "robot_description_planning.joint_limits.panda_joint6.max_effort", + "robot_description_planning.joint_limits.panda_joint6.max_jerk", + "robot_description_planning.joint_limits.panda_joint6.max_position", + "robot_description_planning.joint_limits.panda_joint6.max_velocity", + "robot_description_planning.joint_limits.panda_joint6.min_position", + "robot_description_planning.joint_limits.panda_joint6.min_velocity", + "robot_description_planning.joint_limits.panda_joint6.soft_lower_limit", + "robot_description_planning.joint_limits.panda_joint6.soft_upper_limit", + "robot_description_planning.joint_limits.panda_joint7.angle_wraparound", + "robot_description_planning.joint_limits.panda_joint7.has_acceleration_limits", + "robot_description_planning.joint_limits.panda_joint7.has_deceleration_limits", + "robot_description_planning.joint_limits.panda_joint7.has_effort_limits", + "robot_description_planning.joint_limits.panda_joint7.has_jerk_limits", + "robot_description_planning.joint_limits.panda_joint7.has_position_limits", + "robot_description_planning.joint_limits.panda_joint7.has_soft_limits", + "robot_description_planning.joint_limits.panda_joint7.has_velocity_limits", + "robot_description_planning.joint_limits.panda_joint7.k_position", + "robot_description_planning.joint_limits.panda_joint7.k_velocity", + "robot_description_planning.joint_limits.panda_joint7.max_acceleration", + "robot_description_planning.joint_limits.panda_joint7.max_effort", + "robot_description_planning.joint_limits.panda_joint7.max_jerk", + "robot_description_planning.joint_limits.panda_joint7.max_position", + "robot_description_planning.joint_limits.panda_joint7.max_velocity", + "robot_description_planning.joint_limits.panda_joint7.min_position", + "robot_description_planning.joint_limits.panda_joint7.min_velocity", + "robot_description_planning.joint_limits.panda_joint7.soft_lower_limit", + "robot_description_planning.joint_limits.panda_joint7.soft_upper_limit", + "robot_description_planning.joint_limits.virtual_joint.angle_wraparound", + "robot_description_planning.joint_limits.virtual_joint.has_acceleration_limits", + "robot_description_planning.joint_limits.virtual_joint.has_deceleration_limits", + "robot_description_planning.joint_limits.virtual_joint.has_effort_limits", + "robot_description_planning.joint_limits.virtual_joint.has_jerk_limits", + "robot_description_planning.joint_limits.virtual_joint.has_position_limits", + "robot_description_planning.joint_limits.virtual_joint.has_soft_limits", + "robot_description_planning.joint_limits.virtual_joint.has_velocity_limits", + "robot_description_planning.joint_limits.virtual_joint.k_position", + "robot_description_planning.joint_limits.virtual_joint.k_velocity", + "robot_description_planning.joint_limits.virtual_joint.max_acceleration", + "robot_description_planning.joint_limits.virtual_joint.max_effort", + "robot_description_planning.joint_limits.virtual_joint.max_jerk", + "robot_description_planning.joint_limits.virtual_joint.max_position", + "robot_description_planning.joint_limits.virtual_joint.max_velocity", + "robot_description_planning.joint_limits.virtual_joint.min_position", + "robot_description_planning.joint_limits.virtual_joint.min_velocity", + "robot_description_planning.joint_limits.virtual_joint.soft_lower_limit", + "robot_description_planning.joint_limits.virtual_joint.soft_upper_limit", + "robot_description_planning.joint_limits.virtual_joint/rot_w.has_acceleration_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_w.has_jerk_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_w.has_velocity_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_w.max_position", + "robot_description_planning.joint_limits.virtual_joint/rot_w.min_position", + "robot_description_planning.joint_limits.virtual_joint/rot_x.has_acceleration_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_x.has_jerk_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_x.has_velocity_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_x.max_position", + "robot_description_planning.joint_limits.virtual_joint/rot_x.min_position", + "robot_description_planning.joint_limits.virtual_joint/rot_y.has_acceleration_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_y.has_jerk_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_y.has_velocity_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_y.max_position", + "robot_description_planning.joint_limits.virtual_joint/rot_y.min_position", + "robot_description_planning.joint_limits.virtual_joint/rot_z.has_acceleration_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_z.has_jerk_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_z.has_velocity_limits", + "robot_description_planning.joint_limits.virtual_joint/rot_z.max_position", + "robot_description_planning.joint_limits.virtual_joint/rot_z.min_position", + "robot_description_planning.joint_limits.virtual_joint/trans_x.has_acceleration_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_x.has_jerk_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_x.has_velocity_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_x.max_position", + "robot_description_planning.joint_limits.virtual_joint/trans_x.min_position", + "robot_description_planning.joint_limits.virtual_joint/trans_y.has_acceleration_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_y.has_jerk_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_y.has_velocity_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_y.max_position", + "robot_description_planning.joint_limits.virtual_joint/trans_y.min_position", + "robot_description_planning.joint_limits.virtual_joint/trans_z.has_acceleration_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_z.has_jerk_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_z.has_velocity_limits", + "robot_description_planning.joint_limits.virtual_joint/trans_z.max_position", + "robot_description_planning.joint_limits.virtual_joint/trans_z.min_position", + "robot_description_semantic", + "stomp.control_cost_weight", + "stomp.default_workspace_bounds", + "stomp.delta_t", + "stomp.display_contacts_topic", + "stomp.display_path_topic", + "stomp.exponentiated_cost_sensitivity", + "stomp.fix_start_state", + "stomp.max_rollouts", + "stomp.num_iterations", + "stomp.num_iterations_after_valid", + "stomp.num_rollouts", + "stomp.num_timesteps", + "stomp.path_marker_topic", + "stomp.planning_plugins", + "stomp.progress_topic", + "stomp.request_adapters", + "stomp.response_adapters", + "stomp.stomp_moveit.control_cost_weight", + "stomp.stomp_moveit.delta_t", + "stomp.stomp_moveit.exponentiated_cost_sensitivity", + "stomp.stomp_moveit.max_rollouts", + "stomp.stomp_moveit.num_iterations", + "stomp.stomp_moveit.num_iterations_after_valid", + "stomp.stomp_moveit.num_rollouts", + "stomp.stomp_moveit.num_timesteps", + "stomp.totg.min_angle_change", + "stomp.totg.path_tolerance", + "stomp.totg.resample_dt", + "trajectory_execution.allowed_execution_duration_scaling", + "trajectory_execution.allowed_goal_duration_margin", + "trajectory_execution.allowed_start_tolerance", + "use_sim_time", +}; +} // namespace move_group_test diff --git a/moveit_ros/tests/launch/move_group_api.test.py b/moveit_ros/tests/launch/move_group_api.test.py new file mode 100644 index 0000000000..4d66d6f018 --- /dev/null +++ b/moveit_ros/tests/launch/move_group_api.test.py @@ -0,0 +1,137 @@ +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 + + +def generate_test_description(): + moveit_config = ( + MoveItConfigsBuilder("moveit_resources_panda") + .robot_description( + file_path="config/panda.urdf.xacro", + ) + .robot_description_semantic(file_path="config/panda.srdf") + .planning_scene_monitor( + publish_robot_description=True, publish_robot_description_semantic=True + ) + .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml") + .planning_pipelines( + pipelines=["ompl", "chomp", "pilz_industrial_motion_planner", "stomp"] + ) + .to_moveit_configs() + ) + + # Start the actual move_group node/action server + move_group_node = launch_ros.actions.Node( + package="moveit_ros_move_group", + executable="move_group", + output="screen", + parameters=[moveit_config.to_dict()], + arguments=["--ros-args", "--log-level", "info"], + ) + + # 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 = launch_ros.actions.Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[ros2_controllers_path], + remappings=[ + ("/controller_manager/robot_description", "/robot_description"), + ], + output="screen", + ) + + joint_state_broadcaster_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=[ + "joint_state_broadcaster", + "--controller-manager", + "/controller_manager", + ], + output="screen", + ) + + panda_arm_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_arm_controller", "-c", "/controller_manager"], + ) + + panda_hand_controller_spawner = launch_ros.actions.Node( + package="controller_manager", + executable="spawner", + arguments=["panda_hand_controller", "-c", "/controller_manager"], + ) + + # Static TF + static_tf_node = launch_ros.actions.Node( + package="tf2_ros", + executable="static_transform_publisher", + name="static_transform_publisher", + output="log", + arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"], + ) + + # Publish TF + robot_state_publisher = launch_ros.actions.Node( + package="robot_state_publisher", + executable="robot_state_publisher", + name="robot_state_publisher", + output="both", + parameters=[moveit_config.robot_description], + ) + + move_group_gtest = launch_ros.actions.Node( + executable=launch.substitutions.PathJoinSubstitution( + [ + launch.substitutions.LaunchConfiguration("test_binary_dir"), + "move_group_api_test", + ] + ), + 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", + ), + static_tf_node, + robot_state_publisher, + move_group_node, + ros2_control_node, + joint_state_broadcaster_spawner, + panda_arm_controller_spawner, + panda_hand_controller_spawner, + move_group_gtest, + # launch.actions.TimerAction(period=15.0, actions=[move_group_gtest]), + launch_testing.actions.ReadyToTest(), + ] + ), { + "move_group_gtest": move_group_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, move_group_gtest): + self.proc_info.assertWaitForShutdown(move_group_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, move_group_gtest): + launch_testing.asserts.assertExitCodes(proc_info, process=move_group_gtest) diff --git a/moveit_ros/tests/package.xml b/moveit_ros/tests/package.xml new file mode 100644 index 0000000000..22ed52fd25 --- /dev/null +++ b/moveit_ros/tests/package.xml @@ -0,0 +1,40 @@ + + + + moveit_ros_tests + 2.10.0 + Integration tests for moveit_ros + MoveIt Release Team + + BSD-3-Clause + + http://moveit.ros.org + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 + + Sebastian Jahr + + ament_cmake + moveit_common + rclcpp + + ament_cmake_gtest + ament_lint_auto + moveit_configs_utils + moveit_core + moveit_resources_panda_moveit_config + moveit_ros_planning + moveit_ros_planning_interface + moveit_simple_controller_manager + moveit_planners_ompl + moveit_planners_chomp + moveit_planners_stomp + moveit_ros_move_group + pilz_industrial_motion_planner + ros_testing + tf2_ros + + + ament_cmake + + diff --git a/moveit_ros/tests/src/move_group_api_test.cpp b/moveit_ros/tests/src/move_group_api_test.cpp new file mode 100644 index 0000000000..adef43800d --- /dev/null +++ b/moveit_ros/tests/src/move_group_api_test.cpp @@ -0,0 +1,105 @@ +/********************************************************************* + * Software License Agreement (BSD License) + * + * Copyright (c) 2024, 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: Sebastian Jahr + Description: Integration tests for the move_group interface +*/ + +#include +#include + +#include "parameter_name_list.hpp" + +class MoveGroupFixture : public testing::Test +{ +protected: + void SetUp() override + { + rclcpp::NodeOptions node_options; + node_options.automatically_declare_parameters_from_overrides(true); + test_node_ = std::make_shared("move_group_param_test_node", node_options); + } + + void TearDown() override + { + } + + std::shared_ptr test_node_; +}; + +TEST_F(MoveGroupFixture, testParamAPI) +{ + // Create parameter client + auto params_client = std::make_shared(test_node_, "move_group"); + bool reach_param_client = params_client->wait_for_service(std::chrono::seconds(5)); + // This pattern helps with debugging if service is not available. + if (!reach_param_client) + { + RCLCPP_ERROR(test_node_->get_logger(), "Couldn't reach parameter server. Is the move_group up and running?"); + } + ASSERT_TRUE(reach_param_client); + + // GIVEN a running move_group + // WHEN a parameter name from parameter API checked + // THEN that parameter exists + for (const auto& param_name : move_group_test::PARAMETER_NAME_LIST) + { + bool param_exists = false; + if (params_client->wait_for_service(std::chrono::milliseconds(1))) + { + param_exists = params_client->has_parameter(param_name); + if (!param_exists) + { + RCLCPP_ERROR(test_node_->get_logger(), "Parameter %s doesn't exists", param_name.c_str()); + } + } + else + { + RCLCPP_ERROR(test_node_->get_logger(), "Cannot read parameter %s because service couldn't be reached", + param_name.c_str()); + } + EXPECT_TRUE(param_exists); + } +} + +// TODO(sjahr): Add more API tests e.g. from move_group_interface_cpp_test.cpp + +int main(int argc, char** argv) +{ + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +} diff --git a/moveit_ros/visualization/CHANGELOG.rst b/moveit_ros/visualization/CHANGELOG.rst index 82b78629d7..8c78a7cebf 100644 --- a/moveit_ros/visualization/CHANGELOG.rst +++ b/moveit_ros/visualization/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_ros_visualization ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Henning Kayser, Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_ros/visualization/CMakeLists.txt b/moveit_ros/visualization/CMakeLists.txt index bae654856d..27855877a3 100644 --- a/moveit_ros/visualization/CMakeLists.txt +++ b/moveit_ros/visualization/CMakeLists.txt @@ -5,7 +5,8 @@ project(moveit_ros_visualization LANGUAGES CXX) find_package(moveit_common REQUIRED) moveit_package() -# definition needed for boost/math/constants/constants.hpp included by Ogre to compile +# definition needed for boost/math/constants/constants.hpp included by Ogre to +# compile add_definitions(-DBOOST_MATH_DISABLE_FLOAT128) find_package(ament_cmake REQUIRED) @@ -39,7 +40,8 @@ include(ConfigExtras.cmake) # Qt Stuff find_package(Qt5 ${rviz_QT_VERSION} REQUIRED Core Widgets) set(QT_LIBRARIES Qt5::Widgets) -macro(qt_wrap_ui) +# Delegate to QT5 macro +macro(QT_WRAP_UI) qt5_wrap_ui(${ARGN}) endmacro() @@ -49,47 +51,43 @@ set(CMAKE_AUTORCC ON) add_definitions(-DQT_NO_KEYWORDS) set(THIS_PACKAGE_LIBRARIES - moveit_motion_planning_rviz_plugin - moveit_motion_planning_rviz_plugin_core - moveit_planning_scene_rviz_plugin - moveit_planning_scene_rviz_plugin_core - moveit_robot_state_rviz_plugin - moveit_robot_state_rviz_plugin_core - moveit_rviz_plugin_render_tools - moveit_trajectory_rviz_plugin - moveit_trajectory_rviz_plugin_core -) + moveit_motion_planning_rviz_plugin + moveit_motion_planning_rviz_plugin_core + moveit_planning_scene_rviz_plugin + moveit_planning_scene_rviz_plugin_core + moveit_robot_state_rviz_plugin + moveit_robot_state_rviz_plugin_core + moveit_rviz_plugin_render_tools + moveit_trajectory_rviz_plugin + moveit_trajectory_rviz_plugin_core) set(THIS_PACKAGE_INCLUDE_DEPENDS - class_loader - geometric_shapes - interactive_markers - moveit_ros_planning - moveit_ros_planning_interface - moveit_msgs - moveit_core - moveit_ros_warehouse - octomap_msgs - moveit_ros_robot_interaction - object_recognition_msgs - pluginlib - rclcpp - rclcpp_action - rclpy - rviz2 - tf2_eigen - Eigen3 - rviz_ogre_vendor - rviz_common - rviz_default_plugins -) - -include_directories(rviz_plugin_render_tools/include - robot_state_rviz_plugin/include - planning_scene_rviz_plugin/include - motion_planning_rviz_plugin/include - trajectory_rviz_plugin/include -) + class_loader + geometric_shapes + interactive_markers + moveit_ros_planning + moveit_ros_planning_interface + moveit_msgs + moveit_core + moveit_ros_warehouse + octomap_msgs + moveit_ros_robot_interaction + object_recognition_msgs + pluginlib + rclcpp + rclcpp_action + rclpy + rviz2 + tf2_eigen + Eigen3 + rviz_ogre_vendor + rviz_common + rviz_default_plugins) + +include_directories( + rviz_plugin_render_tools/include robot_state_rviz_plugin/include + planning_scene_rviz_plugin/include motion_planning_rviz_plugin/include + trajectory_rviz_plugin/include) add_subdirectory(rviz_plugin_render_tools) add_subdirectory(robot_state_rviz_plugin) @@ -99,10 +97,14 @@ add_subdirectory(trajectory_rviz_plugin) install(DIRECTORY icons DESTINATION share/moveit_ros_visualization) -pluginlib_export_plugin_description_file(rviz_common motion_planning_rviz_plugin_description.xml) -pluginlib_export_plugin_description_file(rviz_common trajectory_rviz_plugin_description.xml) -pluginlib_export_plugin_description_file(rviz_common planning_scene_rviz_plugin_description.xml) -pluginlib_export_plugin_description_file(rviz_common robot_state_rviz_plugin_description.xml) +pluginlib_export_plugin_description_file( + rviz_common motion_planning_rviz_plugin_description.xml) +pluginlib_export_plugin_description_file(rviz_common + trajectory_rviz_plugin_description.xml) +pluginlib_export_plugin_description_file( + rviz_common planning_scene_rviz_plugin_description.xml) +pluginlib_export_plugin_description_file( + rviz_common robot_state_rviz_plugin_description.xml) install( TARGETS ${THIS_PACKAGE_LIBRARIES} @@ -110,25 +112,12 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_visualization -) + INCLUDES + DESTINATION include/moveit_ros_visualization) ament_export_targets(moveit_ros_visualizationTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_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() -endif() - ament_package(CONFIG_EXTRAS ConfigExtras.cmake) include_directories("${OGRE_PREFIX_DIR}/include") diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt index 3e1dbca643..dc4f94ac8f 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/CMakeLists.txt @@ -1,56 +1,57 @@ set(HEADERS - include/moveit/motion_planning_rviz_plugin/motion_planning_display.h - include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h - include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h - include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h - include/moveit/motion_planning_rviz_plugin/interactive_marker_display.h -) -qt5_wrap_ui(UIC_FILES - src/ui/motion_planning_rviz_plugin_frame.ui - src/ui/motion_planning_rviz_plugin_frame_joints.ui -) + include/moveit/motion_planning_rviz_plugin/motion_planning_display.h + include/moveit/motion_planning_rviz_plugin/motion_planning_frame.h + include/moveit/motion_planning_rviz_plugin/motion_planning_frame_joints_widget.h + include/moveit/motion_planning_rviz_plugin/motion_planning_param_widget.h + include/moveit/motion_planning_rviz_plugin/interactive_marker_display.h) +qt5_wrap_ui(UIC_FILES src/ui/motion_planning_rviz_plugin_frame.ui + src/ui/motion_planning_rviz_plugin_frame_joints.ui) include_directories(${CMAKE_CURRENT_BINARY_DIR}) # Plugin Source set(SOURCE_FILES - src/motion_planning_frame.cpp - src/motion_planning_frame_context.cpp - src/motion_planning_frame_planning.cpp - src/motion_planning_frame_objects.cpp - src/motion_planning_frame_scenes.cpp - src/motion_planning_frame_states.cpp - src/motion_planning_frame_joints_widget.cpp - src/motion_planning_display.cpp - src/motion_planning_frame_manipulation.cpp - src/motion_planning_param_widget.cpp - src/interactive_marker_display.cpp - src/icons/icons.qrc -) + src/motion_planning_frame.cpp + src/motion_planning_frame_context.cpp + src/motion_planning_frame_planning.cpp + src/motion_planning_frame_objects.cpp + src/motion_planning_frame_scenes.cpp + src/motion_planning_frame_states.cpp + src/motion_planning_frame_joints_widget.cpp + src/motion_planning_display.cpp + src/motion_planning_frame_manipulation.cpp + src/motion_planning_param_widget.cpp + src/interactive_marker_display.cpp + src/icons/icons.qrc) -add_library(moveit_motion_planning_rviz_plugin_core SHARED ${SOURCE_FILES} ${HEADERS} ${UIC_FILES}) -set_target_properties(moveit_motion_planning_rviz_plugin_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_motion_planning_rviz_plugin_core moveit_rviz_plugin_render_tools moveit_planning_scene_rviz_plugin) -ament_target_dependencies(moveit_motion_planning_rviz_plugin_core +add_library(moveit_motion_planning_rviz_plugin_core SHARED + ${SOURCE_FILES} ${HEADERS} ${UIC_FILES}) +set_target_properties(moveit_motion_planning_rviz_plugin_core + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries( + moveit_motion_planning_rviz_plugin_core moveit_rviz_plugin_render_tools + moveit_planning_scene_rviz_plugin) +ament_target_dependencies( + moveit_motion_planning_rviz_plugin_core moveit_ros_robot_interaction moveit_ros_planning_interface moveit_ros_warehouse rviz2 rviz_ogre_vendor Qt5 - pluginlib -) -target_include_directories(moveit_motion_planning_rviz_plugin_core PRIVATE "${OGRE_PREFIX_DIR}/include") + pluginlib) +target_include_directories(moveit_motion_planning_rviz_plugin_core + PRIVATE "${OGRE_PREFIX_DIR}/include") add_library(moveit_motion_planning_rviz_plugin SHARED src/plugin_init.cpp) -set_target_properties(moveit_motion_planning_rviz_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_motion_planning_rviz_plugin moveit_motion_planning_rviz_plugin_core) -ament_target_dependencies(moveit_motion_planning_rviz_plugin - moveit_ros_robot_interaction - moveit_ros_warehouse - pluginlib - rviz_ogre_vendor -) -target_include_directories(moveit_motion_planning_rviz_plugin PRIVATE "${OGRE_PREFIX_DIR}/include") +set_target_properties(moveit_motion_planning_rviz_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_motion_planning_rviz_plugin + moveit_motion_planning_rviz_plugin_core) +ament_target_dependencies( + moveit_motion_planning_rviz_plugin moveit_ros_robot_interaction + moveit_ros_warehouse pluginlib rviz_ogre_vendor) +target_include_directories(moveit_motion_planning_rviz_plugin + PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include/moveit_ros_visualization) diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp index cd9fdfd205..110fe48915 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_display.cpp @@ -1037,7 +1037,10 @@ void MotionPlanningDisplay::changePlanningGroup(const std::string& group) planning_group_property_->setStdString(group); } else - RCLCPP_ERROR(moveit::getLogger("MotionPlanningDisplay"), "Group [%s] not found in the robot model.", group.c_str()); + { + RCLCPP_ERROR(moveit::getLogger("moveit.ros.motion_planning_display"), "Group [%s] not found in the robot model.", + group.c_str()); + } } void MotionPlanningDisplay::changedPlanningGroup() diff --git a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp index 229dcb9bc6..72850cf9e2 100644 --- a/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp +++ b/moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp @@ -68,7 +68,7 @@ MotionPlanningFrame::MotionPlanningFrame(MotionPlanningDisplay* pdisplay, rviz_c , planning_display_(pdisplay) , context_(context) , ui_(new Ui::MotionPlanningUI()) - , logger_(moveit::getLogger("motion_planning_frame")) + , logger_(moveit::getLogger("moveit.ros.motion_planning_frame")) , first_time_(true) { auto ros_node_abstraction = context_->getRosNodeAbstraction().lock(); diff --git a/moveit_ros/visualization/package.xml b/moveit_ros/visualization/package.xml index 68a61c04b5..02dd337091 100644 --- a/moveit_ros/visualization/package.xml +++ b/moveit_ros/visualization/package.xml @@ -2,7 +2,7 @@ moveit_ros_visualization - 2.9.0 + 2.10.0 Components of MoveIt that offer visualization Henning Kayser Tyler Weaver @@ -12,8 +12,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan Dave Coleman @@ -42,9 +42,6 @@ rviz2 tf2_eigen - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt index 5968abebfc..71f84afb5d 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/CMakeLists.txt @@ -1,31 +1,40 @@ -add_library(moveit_planning_scene_rviz_plugin_core SHARED - src/planning_scene_display.cpp - src/background_processing.cpp +add_library( + moveit_planning_scene_rviz_plugin_core SHARED + src/planning_scene_display.cpp src/background_processing.cpp include/moveit/planning_scene_rviz_plugin/planning_scene_display.h) include(GenerateExportHeader) generate_export_header(moveit_planning_scene_rviz_plugin_core) -target_include_directories(moveit_planning_scene_rviz_plugin_core PUBLIC $) -set_target_properties(moveit_planning_scene_rviz_plugin_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_planning_scene_rviz_plugin_core moveit_rviz_plugin_render_tools) -ament_target_dependencies(moveit_planning_scene_rviz_plugin_core - rclcpp - rviz2 - moveit_ros_planning_interface - moveit_ros_planning - moveit_msgs - pluginlib - rviz_ogre_vendor -) -target_include_directories(moveit_planning_scene_rviz_plugin_core PRIVATE "${OGRE_PREFIX_DIR}/include") +target_include_directories( + moveit_planning_scene_rviz_plugin_core + PUBLIC $) +set_target_properties(moveit_planning_scene_rviz_plugin_core + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_planning_scene_rviz_plugin_core + moveit_rviz_plugin_render_tools) +ament_target_dependencies( + moveit_planning_scene_rviz_plugin_core + rclcpp + rviz2 + moveit_ros_planning_interface + moveit_ros_planning + moveit_msgs + pluginlib + rviz_ogre_vendor) +target_include_directories(moveit_planning_scene_rviz_plugin_core + PRIVATE "${OGRE_PREFIX_DIR}/include") add_library(moveit_planning_scene_rviz_plugin SHARED src/plugin_init.cpp) -set_target_properties(moveit_planning_scene_rviz_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_planning_scene_rviz_plugin moveit_planning_scene_rviz_plugin_core Qt5::Widgets) -ament_target_dependencies(moveit_planning_scene_rviz_plugin - pluginlib - rviz_ogre_vendor -) -target_include_directories(moveit_planning_scene_rviz_plugin PRIVATE "${OGRE_PREFIX_DIR}/include") +set_target_properties(moveit_planning_scene_rviz_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_planning_scene_rviz_plugin + moveit_planning_scene_rviz_plugin_core Qt5::Widgets) +ament_target_dependencies(moveit_planning_scene_rviz_plugin pluginlib + rviz_ogre_vendor) +target_include_directories(moveit_planning_scene_rviz_plugin + PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include/moveit_ros_visualization) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_rviz_plugin_core_export.h DESTINATION include/moveit_ros_visualization) +install( + FILES + ${CMAKE_CURRENT_BINARY_DIR}/moveit_planning_scene_rviz_plugin_core_export.h + DESTINATION include/moveit_ros_visualization) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp index 4104e8c707..4ec97d5741 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/background_processing.cpp @@ -79,14 +79,14 @@ void BackgroundProcessing::processingThread() action_lock_.unlock(); try { - RCLCPP_DEBUG(moveit::getLogger("BackgroundProcessing"), "Begin executing '%s'", action_name.c_str()); + RCLCPP_DEBUG(moveit::getLogger("moveit.ros.background_processing"), "Begin executing '%s'", action_name.c_str()); fn(); - RCLCPP_DEBUG(moveit::getLogger("BackgroundProcessing"), "Done executing '%s'", action_name.c_str()); + RCLCPP_DEBUG(moveit::getLogger("moveit.ros.background_processing"), "Done executing '%s'", action_name.c_str()); } catch (std::exception& ex) { - RCLCPP_ERROR(moveit::getLogger("BackgroundProcessing"), "Exception caught while processing action '%s': %s", - action_name.c_str(), ex.what()); + RCLCPP_ERROR(moveit::getLogger("moveit.ros.background_processing"), + "Exception caught while processing action '%s': %s", action_name.c_str(), ex.what()); } processing_ = false; if (queue_change_event_) diff --git a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp index 16abd1515b..00d84685a5 100644 --- a/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp +++ b/moveit_ros/visualization/planning_scene_rviz_plugin/src/planning_scene_display.cpp @@ -67,7 +67,7 @@ PlanningSceneDisplay::PlanningSceneDisplay(bool listen_to_planning_scene, bool s : Display() , planning_scene_needs_render_(true) , current_scene_time_(0.0f) - , logger_(moveit::getLogger("planning_scene_display")) + , logger_(moveit::getLogger("moveit.ros.planning_scene_display")) { move_group_ns_property_ = new rviz_common::properties::StringProperty("Move Group Namespace", "", "The name of the ROS namespace in " diff --git a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt index 7c5491f345..73b5751a1e 100644 --- a/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/robot_state_rviz_plugin/CMakeLists.txt @@ -1,30 +1,31 @@ -add_library(moveit_robot_state_rviz_plugin_core SHARED +add_library( + moveit_robot_state_rviz_plugin_core SHARED src/robot_state_display.cpp include/moveit/robot_state_rviz_plugin/robot_state_display.h) -set_target_properties(moveit_robot_state_rviz_plugin_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +set_target_properties(moveit_robot_state_rviz_plugin_core + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") target_link_libraries(moveit_robot_state_rviz_plugin_core - moveit_rviz_plugin_render_tools -) -ament_target_dependencies(moveit_robot_state_rviz_plugin_core - rclcpp - rviz2 - moveit_ros_planning - moveit_msgs - pluginlib - Boost - rviz_ogre_vendor -) -target_include_directories(moveit_robot_state_rviz_plugin_core PRIVATE "${OGRE_PREFIX_DIR}/include") + moveit_rviz_plugin_render_tools) +ament_target_dependencies( + moveit_robot_state_rviz_plugin_core + rclcpp + rviz2 + moveit_ros_planning + moveit_msgs + pluginlib + Boost + rviz_ogre_vendor) +target_include_directories(moveit_robot_state_rviz_plugin_core + PRIVATE "${OGRE_PREFIX_DIR}/include") add_library(moveit_robot_state_rviz_plugin SHARED src/plugin_init.cpp) -set_target_properties(moveit_robot_state_rviz_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_robot_state_rviz_plugin moveit_robot_state_rviz_plugin_core) -ament_target_dependencies(moveit_robot_state_rviz_plugin - rclcpp - pluginlib - Boost - rviz_ogre_vendor -) -target_include_directories(moveit_robot_state_rviz_plugin PRIVATE "${OGRE_PREFIX_DIR}/include") +set_target_properties(moveit_robot_state_rviz_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_robot_state_rviz_plugin + moveit_robot_state_rviz_plugin_core) +ament_target_dependencies(moveit_robot_state_rviz_plugin rclcpp pluginlib Boost + rviz_ogre_vendor) +target_include_directories(moveit_robot_state_rviz_plugin + PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include/moveit_ros_visualization) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt index b1216716df..65d2ce41a4 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt +++ b/moveit_ros/visualization/rviz_plugin_render_tools/CMakeLists.txt @@ -1,17 +1,17 @@ set(HEADERS - include/moveit/rviz_plugin_render_tools/octomap_render.h - include/moveit/rviz_plugin_render_tools/planning_link_updater.h - include/moveit/rviz_plugin_render_tools/planning_scene_render.h - include/moveit/rviz_plugin_render_tools/render_shapes.h - include/moveit/rviz_plugin_render_tools/robot_state_visualization.h - include/moveit/rviz_plugin_render_tools/trajectory_visualization.h - include/moveit/rviz_plugin_render_tools/trajectory_panel.h - include/ogre_helpers/mesh_shape.hpp -) + include/moveit/rviz_plugin_render_tools/octomap_render.h + include/moveit/rviz_plugin_render_tools/planning_link_updater.h + include/moveit/rviz_plugin_render_tools/planning_scene_render.h + include/moveit/rviz_plugin_render_tools/render_shapes.h + include/moveit/rviz_plugin_render_tools/robot_state_visualization.h + include/moveit/rviz_plugin_render_tools/trajectory_visualization.h + include/moveit/rviz_plugin_render_tools/trajectory_panel.h + include/ogre_helpers/mesh_shape.hpp) include_directories(${CMAKE_CURRENT_BINARY_DIR}) -add_library(moveit_rviz_plugin_render_tools SHARED +add_library( + moveit_rviz_plugin_render_tools SHARED src/render_shapes.cpp src/robot_state_visualization.cpp src/planning_scene_render.cpp @@ -20,23 +20,22 @@ add_library(moveit_rviz_plugin_render_tools SHARED src/trajectory_visualization.cpp src/trajectory_panel.cpp src/mesh_shape.cpp - ${HEADERS} -) -set_target_properties(moveit_rviz_plugin_render_tools PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") + ${HEADERS}) +set_target_properties(moveit_rviz_plugin_render_tools + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_rviz_plugin_render_tools - Qt5::Widgets -) +target_link_libraries(moveit_rviz_plugin_render_tools Qt5::Widgets) -ament_target_dependencies(moveit_rviz_plugin_render_tools - rclcpp - moveit_core - Boost - octomap_msgs - rviz_ogre_vendor - rviz_common - rviz_default_plugins -) -target_include_directories(moveit_rviz_plugin_render_tools PRIVATE "${OGRE_PREFIX_DIR}/include") +ament_target_dependencies( + moveit_rviz_plugin_render_tools + rclcpp + moveit_core + Boost + octomap_msgs + rviz_ogre_vendor + rviz_common + rviz_default_plugins) +target_include_directories(moveit_rviz_plugin_render_tools + PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include/moveit_ros_visualization) diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp index 5a36c4d082..393d11bc9e 100755 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/octomap_render.cpp @@ -159,7 +159,6 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& unsigned int render_mode_mask = static_cast(octree_voxel_rendering); - size_t point_count = 0; { int step_size = 1 << (octree->getTreeDepth() - octree_depth_); // for pruning of occluded voxels @@ -242,8 +241,6 @@ void OcTreeRender::octreeDecoding(const std::shared_ptr& // push to point vectors unsigned int depth = it.getDepth(); point_buf[depth - 1].push_back(new_point); - - ++point_count; } } } diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp index 41b29d94dc..25c8ca8b09 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/robot_state_visualization.cpp @@ -48,7 +48,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("rviz_plugin_render_tools.robot_state_visualization"); + return moveit::getLogger("moveit.ros.rviz_plugin_render_tools.robot_state_visualization"); } } // namespace diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp index 5ce7b776ae..aec921e06e 100644 --- a/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp +++ b/moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp @@ -72,7 +72,7 @@ TrajectoryVisualization::TrajectoryVisualization(rviz_common::properties::Proper , widget_(widget) , trajectory_slider_panel_(nullptr) , trajectory_slider_dock_panel_(nullptr) - , logger_(moveit::getLogger("trajectory_visualization")) + , logger_(moveit::getLogger("moveit.ros.trajectory_visualization")) { trajectory_topic_property_ = new rviz_common::properties::RosTopicProperty( "Trajectory Topic", "/display_planned_path", diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt index 90816b1909..63cdcf6316 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt +++ b/moveit_ros/visualization/trajectory_rviz_plugin/CMakeLists.txt @@ -1,42 +1,37 @@ # Header files that need Qt Moc pre-processing for use with Qt signals, etc: -set(HEADERS - include/moveit/trajectory_rviz_plugin/trajectory_display.h -) +set(HEADERS include/moveit/trajectory_rviz_plugin/trajectory_display.h) include_directories(${CMAKE_CURRENT_BINARY_DIR}) # Trajectory Display -add_library(moveit_trajectory_rviz_plugin_core SHARED - src/trajectory_display.cpp - ${HEADERS} -) -set_target_properties(moveit_trajectory_rviz_plugin_core PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_trajectory_rviz_plugin_core - moveit_rviz_plugin_render_tools - moveit_planning_scene_rviz_plugin_core - rviz_ogre_vendor::OgreMain -) +add_library(moveit_trajectory_rviz_plugin_core SHARED src/trajectory_display.cpp + ${HEADERS}) +set_target_properties(moveit_trajectory_rviz_plugin_core + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries( + moveit_trajectory_rviz_plugin_core moveit_rviz_plugin_render_tools + moveit_planning_scene_rviz_plugin_core rviz_ogre_vendor::OgreMain) -ament_target_dependencies(moveit_trajectory_rviz_plugin_core - rclcpp - rviz2 - moveit_msgs - pluginlib - Boost - rviz_ogre_vendor -) -target_include_directories(moveit_trajectory_rviz_plugin_core PRIVATE "${OGRE_PREFIX_DIR}/include") +ament_target_dependencies( + moveit_trajectory_rviz_plugin_core + rclcpp + rviz2 + moveit_msgs + pluginlib + Boost + rviz_ogre_vendor) +target_include_directories(moveit_trajectory_rviz_plugin_core + PRIVATE "${OGRE_PREFIX_DIR}/include") # Plugin Initializer add_library(moveit_trajectory_rviz_plugin SHARED src/plugin_init.cpp) -set_target_properties(moveit_trajectory_rviz_plugin PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") -target_link_libraries(moveit_trajectory_rviz_plugin moveit_trajectory_rviz_plugin_core) -ament_target_dependencies(moveit_trajectory_rviz_plugin - rclcpp - pluginlib - Boost - rviz_ogre_vendor -) -target_include_directories(moveit_trajectory_rviz_plugin PRIVATE "${OGRE_PREFIX_DIR}/include") +set_target_properties(moveit_trajectory_rviz_plugin + PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") +target_link_libraries(moveit_trajectory_rviz_plugin + moveit_trajectory_rviz_plugin_core) +ament_target_dependencies(moveit_trajectory_rviz_plugin rclcpp pluginlib Boost + rviz_ogre_vendor) +target_include_directories(moveit_trajectory_rviz_plugin + PRIVATE "${OGRE_PREFIX_DIR}/include") install(DIRECTORY include/ DESTINATION include/moveit_ros_visualization) diff --git a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp index 36dc26edaa..da6d73de0c 100644 --- a/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp +++ b/moveit_ros/visualization/trajectory_rviz_plugin/src/trajectory_display.cpp @@ -45,7 +45,7 @@ namespace moveit_rviz_plugin { -TrajectoryDisplay::TrajectoryDisplay() : Display(), logger_(moveit::getLogger("trajectory_display")) +TrajectoryDisplay::TrajectoryDisplay() : Display(), logger_(moveit::getLogger("moveit.ros.trajectory_display")) { // The robot description property is only needed when using the trajectory playback standalone (not within motion diff --git a/moveit_ros/warehouse/CHANGELOG.rst b/moveit_ros/warehouse/CHANGELOG.rst index 97c3089c07..b1f2a11e8d 100644 --- a/moveit_ros/warehouse/CHANGELOG.rst +++ b/moveit_ros/warehouse/CHANGELOG.rst @@ -2,6 +2,19 @@ Changelog for package moveit_ros_warehouse ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging in moveit_core (`#2503 `_) diff --git a/moveit_ros/warehouse/CMakeLists.txt b/moveit_ros/warehouse/CMakeLists.txt index b15a4aa2b1..825f6b11c7 100644 --- a/moveit_ros/warehouse/CMakeLists.txt +++ b/moveit_ros/warehouse/CMakeLists.txt @@ -20,70 +20,72 @@ include(ConfigExtras.cmake) include_directories(include) set(THIS_PACKAGE_INCLUDE_DEPENDS - fmt - Boost - moveit_core - rclcpp - warehouse_ros - moveit_ros_planning - tf2_eigen - tf2_ros -) + fmt + Boost + moveit_core + rclcpp + warehouse_ros + moveit_ros_planning + tf2_eigen + tf2_ros) # Libraries -add_library(moveit_warehouse SHARED +add_library( + moveit_warehouse SHARED src/moveit_message_storage.cpp src/planning_scene_storage.cpp src/planning_scene_world_storage.cpp src/constraints_storage.cpp src/trajectory_constraints_storage.cpp src/state_storage.cpp - src/warehouse_connector.cpp -) + src/warehouse_connector.cpp) include(GenerateExportHeader) generate_export_header(moveit_warehouse) -target_include_directories(moveit_warehouse PUBLIC $) -set_target_properties(moveit_warehouse PROPERTIES VERSION "${moveit_ros_warehouse_VERSION}") +target_include_directories( + moveit_warehouse PUBLIC $) +set_target_properties(moveit_warehouse + PROPERTIES VERSION "${moveit_ros_warehouse_VERSION}") ament_target_dependencies(moveit_warehouse ${THIS_PACKAGE_INCLUDE_DEPENDS}) - # Executables add_executable(moveit_warehouse_broadcast src/broadcast.cpp) -ament_target_dependencies(moveit_warehouse_broadcast ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(moveit_warehouse_broadcast + ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_warehouse_broadcast moveit_warehouse) add_executable(moveit_save_to_warehouse src/save_to_warehouse.cpp) -ament_target_dependencies(moveit_save_to_warehouse ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(moveit_save_to_warehouse + ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_save_to_warehouse moveit_warehouse fmt) add_executable(moveit_warehouse_import_from_text src/import_from_text.cpp) -ament_target_dependencies(moveit_warehouse_import_from_text ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(moveit_warehouse_import_from_text + ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_warehouse_import_from_text moveit_warehouse) add_executable(moveit_warehouse_save_as_text src/save_as_text.cpp) -ament_target_dependencies(moveit_warehouse_save_as_text ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(moveit_warehouse_save_as_text + ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_warehouse_save_as_text moveit_warehouse) add_executable(moveit_init_demo_warehouse src/initialize_demo_db.cpp) -ament_target_dependencies(moveit_init_demo_warehouse ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(moveit_init_demo_warehouse + ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_init_demo_warehouse moveit_warehouse) add_executable(moveit_warehouse_services src/warehouse_services.cpp) -ament_target_dependencies(moveit_warehouse_services ${THIS_PACKAGE_INCLUDE_DEPENDS}) +ament_target_dependencies(moveit_warehouse_services + ${THIS_PACKAGE_INCLUDE_DEPENDS}) target_link_libraries(moveit_warehouse_services moveit_warehouse) install( - TARGETS - moveit_save_to_warehouse - moveit_warehouse_broadcast - moveit_warehouse_import_from_text - moveit_warehouse_save_as_text - moveit_init_demo_warehouse - moveit_warehouse_services - RUNTIME DESTINATION lib/moveit_ros_warehouse -) + TARGETS moveit_save_to_warehouse moveit_warehouse_broadcast + moveit_warehouse_import_from_text moveit_warehouse_save_as_text + moveit_init_demo_warehouse moveit_warehouse_services + RUNTIME DESTINATION lib/moveit_ros_warehouse) install(DIRECTORY include/ DESTINATION include/moveit_ros_warehouse) -install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_warehouse_export.h DESTINATION include/moveit_ros_warehouse) +install(FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_warehouse_export.h + DESTINATION include/moveit_ros_warehouse) install( TARGETS moveit_warehouse @@ -91,23 +93,10 @@ install( LIBRARY DESTINATION lib ARCHIVE DESTINATION lib RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_ros_warehouse -) + INCLUDES + DESTINATION include/moveit_ros_warehouse) ament_export_targets(moveit_ros_warehouseTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - - # These don't pass yet, disable them for now - set(ament_cmake_copyright_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() -endif() - ament_package(CONFIG_EXTRAS ConfigExtras.cmake) diff --git a/moveit_ros/warehouse/ConfigExtras.cmake b/moveit_ros/warehouse/ConfigExtras.cmake index b80fc2b035..22f3754b57 100644 --- a/moveit_ros/warehouse/ConfigExtras.cmake +++ b/moveit_ros/warehouse/ConfigExtras.cmake @@ -1,11 +1,11 @@ # Extras module needed for dependencies to find boost components find_package( - Boost REQUIRED + Boost + REQUIRED thread system filesystem regex date_time - program_options -) + program_options) diff --git a/moveit_ros/warehouse/package.xml b/moveit_ros/warehouse/package.xml index 6d0d6ebbb9..cfa7c01280 100644 --- a/moveit_ros/warehouse/package.xml +++ b/moveit_ros/warehouse/package.xml @@ -2,7 +2,7 @@ moveit_ros_warehouse - 2.9.0 + 2.10.0 Components of MoveIt connecting to MongoDB Henning Kayser Tyler Weaver @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 Ioan Sucan @@ -27,9 +27,6 @@ tf2_ros fmt - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_ros/warehouse/src/constraints_storage.cpp b/moveit_ros/warehouse/src/constraints_storage.cpp index 0fc4e7342e..9ec2b63b13 100644 --- a/moveit_ros/warehouse/src/constraints_storage.cpp +++ b/moveit_ros/warehouse/src/constraints_storage.cpp @@ -49,7 +49,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::ConstraintsStorage::ConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_constraints_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit.ros.warehouse_constraints_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/import_from_text.cpp b/moveit_ros/warehouse/src/import_from_text.cpp index fe2aec4223..6fa876220b 100644 --- a/moveit_ros/warehouse/src/import_from_text.cpp +++ b/moveit_ros/warehouse/src/import_from_text.cpp @@ -51,7 +51,7 @@ static const std::string ROBOT_DESCRIPTION = "robot_description"; rclcpp::Logger getLogger() { - return moveit::getLogger("import_from_text"); + return moveit::getLogger("moveit.ros.import_from_text"); } void parseStart(std::istream& in, planning_scene_monitor::PlanningSceneMonitor* psm, diff --git a/moveit_ros/warehouse/src/planning_scene_storage.cpp b/moveit_ros/warehouse/src/planning_scene_storage.cpp index b85749a850..5e861bd6eb 100644 --- a/moveit_ros/warehouse/src/planning_scene_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_storage.cpp @@ -49,7 +49,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::PlanningSceneStorage::PlanningSceneStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_planning_scene_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit.ros.warehouse_planning_scene_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp index 524bea290d..f245d9b9f1 100644 --- a/moveit_ros/warehouse/src/planning_scene_world_storage.cpp +++ b/moveit_ros/warehouse/src/planning_scene_world_storage.cpp @@ -46,7 +46,8 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::PlanningSceneWorldStorage::PlanningSceneWorldStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_planning_scene_world_storage")) + : MoveItMessageStorage(std::move(conn)) + , logger_(moveit::getLogger("moveit.ros.warehouse_planning_scene_world_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/save_to_warehouse.cpp b/moveit_ros/warehouse/src/save_to_warehouse.cpp index 8d78327224..2e478afa38 100644 --- a/moveit_ros/warehouse/src/save_to_warehouse.cpp +++ b/moveit_ros/warehouse/src/save_to_warehouse.cpp @@ -62,7 +62,7 @@ rclcpp::Logger getLogger() { - return moveit::getLogger("save_to_warehouse"); + return moveit::getLogger("moveit.ros.save_to_warehouse"); } static const std::string ROBOT_DESCRIPTION = "robot_description"; diff --git a/moveit_ros/warehouse/src/state_storage.cpp b/moveit_ros/warehouse/src/state_storage.cpp index 9661d65aa0..965aba9ccf 100644 --- a/moveit_ros/warehouse/src/state_storage.cpp +++ b/moveit_ros/warehouse/src/state_storage.cpp @@ -48,7 +48,7 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::RobotStateStorage::RobotStateStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_robot_state_storage")) + : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit.ros.warehouse_robot_state_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp index 1551adcc64..b60c9233f8 100644 --- a/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp +++ b/moveit_ros/warehouse/src/trajectory_constraints_storage.cpp @@ -49,7 +49,8 @@ using warehouse_ros::Metadata; using warehouse_ros::Query; moveit_warehouse::TrajectoryConstraintsStorage::TrajectoryConstraintsStorage(warehouse_ros::DatabaseConnection::Ptr conn) - : MoveItMessageStorage(std::move(conn)), logger_(moveit::getLogger("moveit_warehouse_trajectory_constraints_storage")) + : MoveItMessageStorage(std::move(conn)) + , logger_(moveit::getLogger("moveit.ros.warehouse_trajectory_constraints_storage")) { createCollections(); } diff --git a/moveit_ros/warehouse/src/warehouse_connector.cpp b/moveit_ros/warehouse/src/warehouse_connector.cpp index 84193dce8a..d297652840 100644 --- a/moveit_ros/warehouse/src/warehouse_connector.cpp +++ b/moveit_ros/warehouse/src/warehouse_connector.cpp @@ -46,7 +46,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("warehouse_connector"); + return moveit::getLogger("moveit.ros.warehouse_connector"); } } // namespace diff --git a/moveit_ros/warehouse/src/warehouse_services.cpp b/moveit_ros/warehouse/src/warehouse_services.cpp index 64c5a3afea..cae2de5823 100644 --- a/moveit_ros/warehouse/src/warehouse_services.cpp +++ b/moveit_ros/warehouse/src/warehouse_services.cpp @@ -54,7 +54,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("warehouse_services"); + return moveit::getLogger("moveit.ros.warehouse_services"); } } // namespace diff --git a/moveit_runtime/CHANGELOG.rst b/moveit_runtime/CHANGELOG.rst index 44763724b5..cfebbed03f 100644 --- a/moveit_runtime/CHANGELOG.rst +++ b/moveit_runtime/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_runtime ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit_runtime/CMakeLists.txt b/moveit_runtime/CMakeLists.txt index 5a6a82c6aa..9a22687573 100644 --- a/moveit_runtime/CMakeLists.txt +++ b/moveit_runtime/CMakeLists.txt @@ -2,9 +2,4 @@ cmake_minimum_required(VERSION 3.22) project(moveit_runtime LANGUAGES NONE) find_package(ament_cmake REQUIRED) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/moveit_runtime/package.xml b/moveit_runtime/package.xml index 761d32dda3..284fa34b3b 100644 --- a/moveit_runtime/package.xml +++ b/moveit_runtime/package.xml @@ -2,7 +2,7 @@ moveit_runtime - 2.9.0 + 2.10.0 moveit_runtime meta package contains MoveIt packages that are essential for its runtime (e.g. running MoveIt on robots). Henning Kayser Tyler Weaver @@ -24,9 +24,6 @@ moveit_ros_planning_interface moveit_ros_warehouse - ament_lint_auto - ament_lint_common - ament_cmake diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst index 114d6518df..80e1b4e7ce 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_app_plugins/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_setup_app_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt index ae1c402067..fb10aca607 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_app_plugins/CMakeLists.txt @@ -17,36 +17,27 @@ find_package(rclcpp REQUIRED) set(CMAKE_AUTOMOC ON) add_definitions(-DQT_NO_KEYWORDS) -qt5_wrap_cpp(MOC_FILES - include/moveit_setup_app_plugins/launches_widget.hpp - include/moveit_setup_app_plugins/perception_widget.hpp -) +qt5_wrap_cpp(MOC_FILES include/moveit_setup_app_plugins/launches_widget.hpp + include/moveit_setup_app_plugins/perception_widget.hpp) -add_library(moveit_setup_app_plugins - src/launches.cpp - src/launches_widget.cpp - src/launches_config.cpp - src/perception_config.cpp - src/perception.cpp - src/perception_widget.cpp - ${MOC_FILES} -) -target_include_directories(moveit_setup_app_plugins PUBLIC - $ - $ -) -ament_target_dependencies(moveit_setup_app_plugins - ament_index_cpp - moveit_ros_visualization - moveit_setup_framework - pluginlib - rclcpp -) +add_library( + moveit_setup_app_plugins + src/launches.cpp + src/launches_widget.cpp + src/launches_config.cpp + src/perception_config.cpp + src/perception.cpp + src/perception_widget.cpp + ${MOC_FILES}) +target_include_directories( + moveit_setup_app_plugins + PUBLIC $ + $) +ament_target_dependencies( + moveit_setup_app_plugins ament_index_cpp moveit_ros_visualization + moveit_setup_framework pluginlib rclcpp) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_perception test/test_perception.cpp) target_link_libraries(test_perception moveit_setup_app_plugins) @@ -54,21 +45,20 @@ endif() install(DIRECTORY templates DESTINATION share/moveit_setup_app_plugins) -install(TARGETS moveit_setup_app_plugins - EXPORT moveit_setup_app_pluginsTargets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_setup_app_plugins -) +install( + TARGETS moveit_setup_app_plugins + EXPORT moveit_setup_app_pluginsTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_setup_app_plugins) install(FILES moveit_setup_framework_plugins.xml - DESTINATION share/moveit_setup_app_plugins -) -install(DIRECTORY include/ - DESTINATION include/moveit_setup_app_plugins -) + DESTINATION share/moveit_setup_app_plugins) +install(DIRECTORY include/ DESTINATION include/moveit_setup_app_plugins) ament_export_targets(moveit_setup_app_pluginsTargets HAS_LIBRARY_TARGET) -pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) +pluginlib_export_plugin_description_file(moveit_setup_framework + moveit_setup_framework_plugins.xml) ament_package() diff --git a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml index 32d4d85961..b8c0a99c7e 100644 --- a/moveit_setup_assistant/moveit_setup_app_plugins/package.xml +++ b/moveit_setup_assistant/moveit_setup_app_plugins/package.xml @@ -2,7 +2,7 @@ moveit_setup_app_plugins - 2.9.0 + 2.10.0 Various specialty plugins for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause @@ -17,11 +17,7 @@ pluginlib rclcpp - ament_clang_format ament_cmake_gtest - ament_cmake_lint_cmake - ament_cmake_xmllint - ament_lint_auto ament_cmake diff --git a/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst index aebe136c1b..6cc4daffe4 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_assistant/CHANGELOG.rst @@ -2,6 +2,18 @@ Changelog for package moveit_setup_assistant ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Add dependency on moveit_configs_utils to moveit_setup_assistant (`#2832 `_) +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Tyler Weaver, hacker1024 + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt index 14e119e3d8..e7c69dcc92 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_assistant/CMakeLists.txt @@ -17,85 +17,59 @@ find_package(Qt5Widgets REQUIRED) find_package(rclcpp REQUIRED) set(THIS_PACKAGE_INCLUDE_DEPENDS - ament_index_cpp - moveit_setup_framework - moveit_setup_srdf_plugins - pluginlib - Qt5Core - Qt5Widgets - rclcpp -) + ament_index_cpp + moveit_setup_framework + moveit_setup_srdf_plugins + pluginlib + Qt5Core + Qt5Widgets + rclcpp) # Header files that need Qt Moc pre-processing for use with Qt signals, etc: -set(HEADERS - include/moveit_setup_assistant/navigation_widget.hpp - include/moveit_setup_assistant/setup_assistant_widget.hpp -) +set(HEADERS include/moveit_setup_assistant/navigation_widget.hpp + include/moveit_setup_assistant/setup_assistant_widget.hpp) # Instruct CMake to run moc automatically when needed. set(CMAKE_AUTOMOC ON) add_definitions(-DQT_NO_KEYWORDS) -qt5_wrap_cpp(MOC_FILES - ${HEADERS} -) +qt5_wrap_cpp(MOC_FILES ${HEADERS}) -add_executable(moveit_setup_assistant - src/main.cpp - src/setup_assistant_widget.cpp - src/navigation_widget.cpp - ${MOC_FILES} -) -target_include_directories(moveit_setup_assistant PUBLIC - $ - $ -) -ament_target_dependencies( +add_executable( + moveit_setup_assistant src/main.cpp src/setup_assistant_widget.cpp + src/navigation_widget.cpp ${MOC_FILES}) +target_include_directories( moveit_setup_assistant - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) -target_link_libraries(moveit_setup_assistant - ${Boost_LIBRARIES} -) + PUBLIC $ + $) +ament_target_dependencies(moveit_setup_assistant + ${THIS_PACKAGE_INCLUDE_DEPENDS}) +target_link_libraries(moveit_setup_assistant ${Boost_LIBRARIES}) add_executable(moveit_setup_assistant_updater src/collisions_updater.cpp) -target_include_directories(moveit_setup_assistant_updater PUBLIC - $ - $) - -ament_target_dependencies( +target_include_directories( moveit_setup_assistant_updater - ${THIS_PACKAGE_INCLUDE_DEPENDS} -) + PUBLIC $ + $) -set_target_properties( - moveit_setup_assistant_updater - PROPERTIES OUTPUT_NAME collisions_updater - PREFIX "" -) +ament_target_dependencies(moveit_setup_assistant_updater + ${THIS_PACKAGE_INCLUDE_DEPENDS}) + +set_target_properties(moveit_setup_assistant_updater + PROPERTIES OUTPUT_NAME collisions_updater PREFIX "") install( - TARGETS - moveit_setup_assistant - moveit_setup_assistant_updater + TARGETS moveit_setup_assistant moveit_setup_assistant_updater EXPORT moveit_setup_assistantTargets ARCHIVE DESTINATION lib LIBRARY DESTINATION lib RUNTIME DESTINATION lib/moveit_setup_assistant - INCLUDES DESTINATION include/moveit_setup_assistant -) -install(DIRECTORY include/ - DESTINATION include/moveit_setup_assistant -) + INCLUDES + DESTINATION include/moveit_setup_assistant) +install(DIRECTORY include/ DESTINATION include/moveit_setup_assistant) ament_export_include_directories(include) install(DIRECTORY launch DESTINATION share/moveit_setup_assistant) install(DIRECTORY resources DESTINATION share/moveit_setup_assistant) ament_export_targets(moveit_setup_assistantTargets HAS_LIBRARY_TARGET) ament_export_dependencies(${THIS_PACKAGE_INCLUDE_DEPENDS}) -# Unit tests -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - ament_package() diff --git a/moveit_setup_assistant/moveit_setup_assistant/package.xml b/moveit_setup_assistant/moveit_setup_assistant/package.xml index d6ac13985e..568f05a51e 100644 --- a/moveit_setup_assistant/moveit_setup_assistant/package.xml +++ b/moveit_setup_assistant/moveit_setup_assistant/package.xml @@ -2,7 +2,7 @@ moveit_setup_assistant - 2.9.0 + 2.10.0 Generates a configuration package that makes it easy to use MoveIt Henning Kayser Tyler Weaver @@ -13,8 +13,8 @@ BSD-3-Clause http://moveit.ros.org - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 David V. Lu!! Dave Coleman @@ -27,16 +27,13 @@ rclcpp moveit_setup_framework moveit_setup_srdf_plugins + moveit_configs_utils moveit_setup_controllers moveit_setup_core_plugins moveit_setup_app_plugins - + - ament_lint_auto - ament_clang_format - ament_cmake_lint_cmake - ament_cmake_xmllint moveit_resources_panda_moveit_config ament_cmake_gtest diff --git a/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst index c0cb6578c4..24d4f3b6f4 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_controllers/CHANGELOG.rst @@ -2,6 +2,12 @@ Changelog for package moveit_setup_controllers ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Add allow_nonzero_velocity_at_trajectory_end parameter to exported ros2_controllers config file (`#2751 `_) +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Sebastian Castro, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt index 273ebad171..3e661098ba 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_controllers/CMakeLists.txt @@ -16,42 +16,34 @@ find_package(rclcpp REQUIRED) set(CMAKE_AUTOMOC ON) add_definitions(-DQT_NO_KEYWORDS) -qt5_wrap_cpp(MOC_FILES - include/moveit_setup_controllers/controller_edit_widget.hpp - include/moveit_setup_controllers/controllers_widget.hpp - include/moveit_setup_controllers/urdf_modifications_widget.hpp -) +qt5_wrap_cpp( + MOC_FILES include/moveit_setup_controllers/controller_edit_widget.hpp + include/moveit_setup_controllers/controllers_widget.hpp + include/moveit_setup_controllers/urdf_modifications_widget.hpp) -add_library(moveit_setup_controllers - src/control_xacro_config.cpp - src/controller_edit_widget.cpp - src/controllers.cpp - src/controllers_widget.cpp - src/controllers_config.cpp - src/modified_urdf_config.cpp - src/moveit_controllers_config.cpp - src/moveit_controllers_widget.cpp - src/ros2_controllers_config.cpp - src/ros2_controllers_widget.cpp - src/urdf_modifications.cpp - src/urdf_modifications_widget.cpp - ${MOC_FILES} -) -target_include_directories(moveit_setup_controllers PUBLIC - $ - $ -) -ament_target_dependencies(moveit_setup_controllers - ament_index_cpp - moveit_setup_framework - pluginlib - rclcpp -) +add_library( + moveit_setup_controllers + src/control_xacro_config.cpp + src/controller_edit_widget.cpp + src/controllers.cpp + src/controllers_widget.cpp + src/controllers_config.cpp + src/modified_urdf_config.cpp + src/moveit_controllers_config.cpp + src/moveit_controllers_widget.cpp + src/ros2_controllers_config.cpp + src/ros2_controllers_widget.cpp + src/urdf_modifications.cpp + src/urdf_modifications_widget.cpp + ${MOC_FILES}) +target_include_directories( + moveit_setup_controllers + PUBLIC $ + $) +ament_target_dependencies(moveit_setup_controllers ament_index_cpp + moveit_setup_framework pluginlib rclcpp) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_controllers test/test_controllers.cpp) target_link_libraries(test_controllers moveit_setup_controllers) @@ -59,24 +51,21 @@ endif() install(DIRECTORY templates DESTINATION share/moveit_setup_controllers) -install(TARGETS moveit_setup_controllers - EXPORT moveit_setup_controllersTargets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_setup_controllers -) +install( + TARGETS moveit_setup_controllers + EXPORT moveit_setup_controllersTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_setup_controllers) install(FILES moveit_setup_framework_plugins.xml - DESTINATION share/moveit_setup_controllers -) -install(DIRECTORY include/ - DESTINATION include/moveit_setup_controllers -) -install(DIRECTORY launch - DESTINATION share/moveit_setup_controllers -) + DESTINATION share/moveit_setup_controllers) +install(DIRECTORY include/ DESTINATION include/moveit_setup_controllers) +install(DIRECTORY launch DESTINATION share/moveit_setup_controllers) ament_export_targets(moveit_setup_controllersTargets HAS_LIBRARY_TARGET) -pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) +pluginlib_export_plugin_description_file(moveit_setup_framework + moveit_setup_framework_plugins.xml) ament_package() diff --git a/moveit_setup_assistant/moveit_setup_controllers/package.xml b/moveit_setup_assistant/moveit_setup_controllers/package.xml index b3d757eaff..3750ab43e7 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/package.xml +++ b/moveit_setup_assistant/moveit_setup_controllers/package.xml @@ -2,7 +2,7 @@ moveit_setup_controllers - 2.9.0 + 2.10.0 MoveIt Setup Steps for ROS 2 Control David V. Lu!! BSD-3-Clause @@ -15,11 +15,7 @@ pluginlib rclcpp - ament_clang_format ament_cmake_gtest - ament_cmake_lint_cmake - ament_cmake_xmllint - ament_lint_auto moveit_configs_utils moveit_resources_fanuc_moveit_config moveit_resources_panda_moveit_config diff --git a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp index 97f1907211..b21ec11f1c 100644 --- a/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp +++ b/moveit_setup_assistant/moveit_setup_controllers/src/ros2_controllers_config.cpp @@ -206,6 +206,7 @@ bool ROS2ControllersConfig::GeneratedControllersConfig::writeYaml(YAML::Emitter& const ControlInterfaces interfaces = parent_.getControlInterfaces(ci.joints_); emitter << YAML::Key << "command_interfaces" << YAML::Value << interfaces.command_interfaces; emitter << YAML::Key << "state_interfaces" << YAML::Value << interfaces.state_interfaces; + emitter << YAML::Key << "allow_nonzero_velocity_at_trajectory_end" << YAML::Value << true; } } emitter << YAML::EndMap; diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst index 857f51d475..86200f7e22 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_core_plugins/CHANGELOG.rst @@ -2,6 +2,11 @@ Changelog for package moveit_setup_core_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Merge branch 'main' into dependabot/github_actions/SonarSource/sonarcloud-github-c-cpp-2 diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_core_plugins/CMakeLists.txt index 6440723f35..2610a37ea4 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_core_plugins/CMakeLists.txt @@ -19,55 +19,47 @@ find_package(urdf REQUIRED) set(CMAKE_AUTOMOC ON) add_definitions(-DQT_NO_KEYWORDS) -qt5_wrap_cpp(MOC_FILES - include/moveit_setup_core_plugins/start_screen_widget.hpp - include/moveit_setup_core_plugins/configuration_files_widget.hpp - include/moveit_setup_core_plugins/author_information_widget.hpp -) +qt5_wrap_cpp( + MOC_FILES include/moveit_setup_core_plugins/start_screen_widget.hpp + include/moveit_setup_core_plugins/configuration_files_widget.hpp + include/moveit_setup_core_plugins/author_information_widget.hpp) -add_library(${PROJECT_NAME} - src/start_screen.cpp - src/start_screen_widget.cpp - src/configuration_files.cpp - src/configuration_files_widget.cpp - src/author_information.cpp - src/author_information_widget.cpp - ${MOC_FILES} -) -target_include_directories(${PROJECT_NAME} PUBLIC - $ - $ -) -ament_target_dependencies(${PROJECT_NAME} +add_library( + ${PROJECT_NAME} + src/start_screen.cpp + src/start_screen_widget.cpp + src/configuration_files.cpp + src/configuration_files_widget.cpp + src/author_information.cpp + src/author_information_widget.cpp + ${MOC_FILES}) +target_include_directories( + ${PROJECT_NAME} PUBLIC $ + $) +ament_target_dependencies( + ${PROJECT_NAME} ament_index_cpp moveit_ros_visualization moveit_setup_framework pluginlib rclcpp srdfdom - urdf -) + urdf) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() - -install(TARGETS moveit_setup_core_plugins - EXPORT moveit_setup_core_pluginsTargets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_setup_core_plugins -) +install( + TARGETS moveit_setup_core_plugins + EXPORT moveit_setup_core_pluginsTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_setup_core_plugins) install(FILES moveit_setup_framework_plugins.xml - DESTINATION share/${PROJECT_NAME} -) -install(DIRECTORY include/ - DESTINATION include/moveit_setup_core_plugins -) + DESTINATION share/${PROJECT_NAME}) +install(DIRECTORY include/ DESTINATION include/moveit_setup_core_plugins) ament_export_targets(moveit_setup_core_pluginsTargets HAS_LIBRARY_TARGET) -pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) +pluginlib_export_plugin_description_file(moveit_setup_framework + moveit_setup_framework_plugins.xml) ament_package() diff --git a/moveit_setup_assistant/moveit_setup_core_plugins/package.xml b/moveit_setup_assistant/moveit_setup_core_plugins/package.xml index dc33da3159..ab74738daf 100644 --- a/moveit_setup_assistant/moveit_setup_core_plugins/package.xml +++ b/moveit_setup_assistant/moveit_setup_core_plugins/package.xml @@ -2,7 +2,7 @@ moveit_setup_core_plugins - 2.9.0 + 2.10.0 Core (meta) plugins for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause @@ -18,11 +18,6 @@ srdfdom urdf - ament_lint_auto - ament_clang_format - ament_cmake_lint_cmake - ament_cmake_xmllint - ament_cmake diff --git a/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst index f35fa66657..e9a38a5aa2 100644 --- a/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_framework/CHANGELOG.rst @@ -2,6 +2,20 @@ Changelog for package moveit_setup_framework ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Apply clang-tidy fixes +* Migrate ros-planning org to moveit (`#2847 `_) + * Rename github.com/ros-planning -> github.com/moveit + * Rename ros-planning.github.io -> moveit.github.io + * Rename ros-planning organization in docker and CI workflow files + - ghcr.io/ros-planning -> ghcr.io/moveit + - github.repository == 'moveit/*'' +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt index 466994c486..3e3d417fbc 100644 --- a/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_framework/CMakeLists.txt @@ -23,31 +23,32 @@ find_package(urdf REQUIRED) set(CMAKE_INCLUDE_CURRENT_DIR ON) -qt5_wrap_cpp(MOC_FILES - include/moveit_setup_framework/qt/helper_widgets.hpp - include/moveit_setup_framework/qt/setup_step_widget.hpp - include/moveit_setup_framework/qt/rviz_panel.hpp - include/moveit_setup_framework/qt/double_list_widget.hpp -) +qt5_wrap_cpp( + MOC_FILES + include/moveit_setup_framework/qt/helper_widgets.hpp + include/moveit_setup_framework/qt/setup_step_widget.hpp + include/moveit_setup_framework/qt/rviz_panel.hpp + include/moveit_setup_framework/qt/double_list_widget.hpp) -add_library(moveit_setup_framework - src/helper_widgets.cpp - src/double_list_widget.cpp - src/urdf_config.cpp - src/package_settings_config.cpp - src/srdf_config.cpp - src/rviz_panel.cpp - src/data_warehouse.cpp - src/templates.cpp - src/utilities.cpp - src/xml_syntax_highlighter.cpp - ${MOC_FILES} -) -target_include_directories(moveit_setup_framework PUBLIC - $ - $ -) -ament_target_dependencies(moveit_setup_framework +add_library( + moveit_setup_framework + src/helper_widgets.cpp + src/double_list_widget.cpp + src/urdf_config.cpp + src/package_settings_config.cpp + src/srdf_config.cpp + src/rviz_panel.cpp + src/data_warehouse.cpp + src/templates.cpp + src/utilities.cpp + src/xml_syntax_highlighter.cpp + ${MOC_FILES}) +target_include_directories( + moveit_setup_framework + PUBLIC $ + $) +ament_target_dependencies( + moveit_setup_framework ament_index_cpp moveit_core moveit_ros_planning @@ -59,30 +60,23 @@ ament_target_dependencies(moveit_setup_framework rviz_common rviz_rendering srdfdom - urdf -) + urdf) -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() -install(DIRECTORY include/ - DESTINATION include/moveit_setup_framework -) +install(DIRECTORY include/ DESTINATION include/moveit_setup_framework) install(FILES moveit_setup_framework_plugins.xml - DESTINATION share/moveit_setup_framework -) + DESTINATION share/moveit_setup_framework) install(DIRECTORY templates DESTINATION share/moveit_setup_framework) ament_export_targets(moveit_setup_frameworkTargets HAS_LIBRARY_TARGET) -install(TARGETS moveit_setup_framework - EXPORT moveit_setup_frameworkTargets - LIBRARY DESTINATION lib - ARCHIVE DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_setup_framework -) +install( + TARGETS moveit_setup_framework + EXPORT moveit_setup_frameworkTargets + LIBRARY DESTINATION lib + ARCHIVE DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_setup_framework) ament_export_dependencies(fmt) ament_export_dependencies(rclcpp) @@ -94,6 +88,7 @@ ament_export_dependencies(moveit_ros_planning) ament_export_dependencies(moveit_ros_visualization) ament_export_dependencies(rviz_common) ament_export_dependencies(rviz_rendering) -pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) +pluginlib_export_plugin_description_file(moveit_setup_framework + moveit_setup_framework_plugins.xml) ament_package() diff --git a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp index 4b3fae3440..c5ad29cbf0 100644 --- a/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp +++ b/moveit_setup_assistant/moveit_setup_framework/include/moveit_setup_framework/data/srdf_config.hpp @@ -118,6 +118,7 @@ class SRDFConfig : public SetupConfig std::vector getGroupNames() const { std::vector group_names; + group_names.reserve(srdf_.groups_.size()); for (const srdf::Model::Group& group : srdf_.groups_) { group_names.push_back(group.name_); diff --git a/moveit_setup_assistant/moveit_setup_framework/package.xml b/moveit_setup_assistant/moveit_setup_framework/package.xml index 250389be34..6afe337eb4 100644 --- a/moveit_setup_assistant/moveit_setup_framework/package.xml +++ b/moveit_setup_assistant/moveit_setup_framework/package.xml @@ -2,7 +2,7 @@ moveit_setup_framework - 2.9.0 + 2.10.0 C++ Interface for defining setup steps for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause @@ -22,11 +22,6 @@ urdf fmt - ament_lint_auto - ament_clang_format - ament_cmake_lint_cmake - ament_cmake_xmllint - ament_cmake diff --git a/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp index 3343b90cf9..d48388f9e4 100644 --- a/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp +++ b/moveit_setup_assistant/moveit_setup_framework/src/templates.cpp @@ -45,7 +45,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("moveit_setup.templates"); + return moveit::getLogger("moveit.setup_assistant.setup.templates"); } } // namespace std::vector TemplatedGeneratedFile::variables; diff --git a/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt index f8caee0dee..d724e5a68d 100644 --- a/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_framework/templates/CMakeLists.txt @@ -5,7 +5,9 @@ find_package(ament_cmake REQUIRED) ament_package() -install(DIRECTORY launch DESTINATION share/${PROJECT_NAME} +install( + DIRECTORY launch + DESTINATION share/${PROJECT_NAME} PATTERN "setup_assistant.launch" EXCLUDE) install(DIRECTORY config DESTINATION share/${PROJECT_NAME}) install(FILES .setup_assistant DESTINATION share/${PROJECT_NAME}) diff --git a/moveit_setup_assistant/moveit_setup_framework/templates/package.xml.template b/moveit_setup_assistant/moveit_setup_framework/templates/package.xml.template index b27a9fa44f..dc647a10ce 100644 --- a/moveit_setup_assistant/moveit_setup_framework/templates/package.xml.template +++ b/moveit_setup_assistant/moveit_setup_framework/templates/package.xml.template @@ -11,8 +11,8 @@ BSD-3-Clause http://moveit.ros.org/ - https://github.com/ros-planning/moveit2/issues - https://github.com/ros-planning/moveit2 + https://github.com/moveit/moveit2/issues + https://github.com/moveit/moveit2 [AUTHOR_NAME] diff --git a/moveit_setup_assistant/moveit_setup_simulation/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_simulation/CMakeLists.txt index 60bee63f27..127b1e408f 100644 --- a/moveit_setup_assistant/moveit_setup_simulation/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_simulation/CMakeLists.txt @@ -16,48 +16,33 @@ find_package(rclcpp REQUIRED) set(CMAKE_AUTOMOC ON) add_definitions(-DQT_NO_KEYWORDS) -qt5_wrap_cpp(MOC_FILES - include/moveit_setup_simulation/simulation_widget.hpp -) - -add_library(moveit_setup_simulation - src/simulation.cpp - src/simulation_widget.cpp - ${MOC_FILES} -) -target_include_directories(moveit_setup_simulation PUBLIC - $ - $ -) -ament_target_dependencies(moveit_setup_simulation - ament_index_cpp - moveit_setup_framework - pluginlib - rclcpp -) - -if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() -endif() +qt5_wrap_cpp(MOC_FILES include/moveit_setup_simulation/simulation_widget.hpp) + +add_library(moveit_setup_simulation src/simulation.cpp + src/simulation_widget.cpp ${MOC_FILES}) +target_include_directories( + moveit_setup_simulation + PUBLIC $ + $) +ament_target_dependencies(moveit_setup_simulation ament_index_cpp + moveit_setup_framework pluginlib rclcpp) install(DIRECTORY templates DESTINATION share/moveit_setup_simulation) -install(TARGETS moveit_setup_simulation - EXPORT moveit_setup_simulationTargets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_setup_simulation -) +install( + TARGETS moveit_setup_simulation + EXPORT moveit_setup_simulationTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_setup_simulation) install(FILES moveit_setup_framework_plugins.xml - DESTINATION share/moveit_setup_simulation -) -install(DIRECTORY include/ - DESTINATION include/moveit_setup_simulation -) + DESTINATION share/moveit_setup_simulation) +install(DIRECTORY include/ DESTINATION include/moveit_setup_simulation) ament_export_targets(moveit_setup_simulationTargets HAS_LIBRARY_TARGET) -pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) +pluginlib_export_plugin_description_file(moveit_setup_framework + moveit_setup_framework_plugins.xml) ament_package() diff --git a/moveit_setup_assistant/moveit_setup_simulation/package.xml b/moveit_setup_assistant/moveit_setup_simulation/package.xml index c71a8214ad..f50d698895 100644 --- a/moveit_setup_assistant/moveit_setup_simulation/package.xml +++ b/moveit_setup_assistant/moveit_setup_simulation/package.xml @@ -15,11 +15,6 @@ pluginlib rclcpp - ament_lint_auto - ament_clang_format - ament_cmake_lint_cmake - ament_cmake_xmllint - ament_cmake diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst b/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst index 429b72e3ee..5d6d002716 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/CHANGELOG.rst @@ -2,6 +2,14 @@ Changelog for package moveit_setup_srdf_plugins ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ +2.10.0 (2024-06-13) +------------------- +* Apply clang-tidy fixes +* Unify log names (`#2720 `_) + Co-authored-by: Abishalini Sivaraman +* CMake format and lint in pre-commit (`#2683 `_) +* Contributors: Robert Haschke, Sebastian Jahr, Tyler Weaver + 2.9.0 (2024-01-09) ------------------ * Node logging for the rest of MoveIt (`#2599 `_) diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt b/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt index ebf0600df8..6e34b6346b 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/CMakeLists.txt @@ -10,75 +10,69 @@ find_package(ament_cmake_ros REQUIRED) find_package(moveit_setup_framework REQUIRED) find_package(pluginlib REQUIRED) -qt5_wrap_cpp(MOC_FILES - include/moveit_setup_srdf_plugins/collision_linear_model.hpp - include/moveit_setup_srdf_plugins/collision_matrix_model.hpp - include/moveit_setup_srdf_plugins/default_collisions_widget.hpp - include/moveit_setup_srdf_plugins/end_effectors_widget.hpp - include/moveit_setup_srdf_plugins/group_edit_widget.hpp - include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp - include/moveit_setup_srdf_plugins/passive_joints_widget.hpp - include/moveit_setup_srdf_plugins/planning_groups_widget.hpp - include/moveit_setup_srdf_plugins/robot_poses_widget.hpp - include/moveit_setup_srdf_plugins/rotated_header_view.hpp - include/moveit_setup_srdf_plugins/virtual_joints_widget.hpp -) +qt5_wrap_cpp( + MOC_FILES + include/moveit_setup_srdf_plugins/collision_linear_model.hpp + include/moveit_setup_srdf_plugins/collision_matrix_model.hpp + include/moveit_setup_srdf_plugins/default_collisions_widget.hpp + include/moveit_setup_srdf_plugins/end_effectors_widget.hpp + include/moveit_setup_srdf_plugins/group_edit_widget.hpp + include/moveit_setup_srdf_plugins/kinematic_chain_widget.hpp + include/moveit_setup_srdf_plugins/passive_joints_widget.hpp + include/moveit_setup_srdf_plugins/planning_groups_widget.hpp + include/moveit_setup_srdf_plugins/robot_poses_widget.hpp + include/moveit_setup_srdf_plugins/rotated_header_view.hpp + include/moveit_setup_srdf_plugins/virtual_joints_widget.hpp) -add_library(moveit_setup_srdf_plugins - src/collision_linear_model.cpp - src/collision_matrix_model.cpp - src/compute_default_collisions.cpp - src/default_collisions.cpp - src/default_collisions_widget.cpp - src/end_effectors.cpp - src/end_effectors_widget.cpp - src/group_edit_widget.cpp - src/group_meta_config.cpp - src/kinematic_chain_widget.cpp - src/passive_joints.cpp - src/passive_joints_widget.cpp - src/planning_groups.cpp - src/planning_groups_widget.cpp - src/robot_poses.cpp - src/robot_poses_widget.cpp - src/rotated_header_view.cpp - src/virtual_joints.cpp - src/virtual_joints_widget.cpp - ${MOC_FILES} -) -target_include_directories(moveit_setup_srdf_plugins PUBLIC - $ - $ -) -ament_target_dependencies(moveit_setup_srdf_plugins - moveit_setup_framework - pluginlib -) +add_library( + moveit_setup_srdf_plugins + src/collision_linear_model.cpp + src/collision_matrix_model.cpp + src/compute_default_collisions.cpp + src/default_collisions.cpp + src/default_collisions_widget.cpp + src/end_effectors.cpp + src/end_effectors_widget.cpp + src/group_edit_widget.cpp + src/group_meta_config.cpp + src/kinematic_chain_widget.cpp + src/passive_joints.cpp + src/passive_joints_widget.cpp + src/planning_groups.cpp + src/planning_groups_widget.cpp + src/robot_poses.cpp + src/robot_poses_widget.cpp + src/rotated_header_view.cpp + src/virtual_joints.cpp + src/virtual_joints_widget.cpp + ${MOC_FILES}) +target_include_directories( + moveit_setup_srdf_plugins + PUBLIC $ + $) +ament_target_dependencies(moveit_setup_srdf_plugins moveit_setup_framework + pluginlib) if(BUILD_TESTING) - find_package(ament_lint_auto REQUIRED) - ament_lint_auto_find_test_dependencies() - find_package(ament_cmake_gtest REQUIRED) ament_add_gtest(test_srdf test/test_srdf.cpp) target_link_libraries(test_srdf moveit_setup_srdf_plugins) endif() -install(TARGETS moveit_setup_srdf_plugins - EXPORT moveit_setup_srdf_pluginsTargets - ARCHIVE DESTINATION lib - LIBRARY DESTINATION lib - RUNTIME DESTINATION bin - INCLUDES DESTINATION include/moveit_setup_srdf_plugins -) +install( + TARGETS moveit_setup_srdf_plugins + EXPORT moveit_setup_srdf_pluginsTargets + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin + INCLUDES + DESTINATION include/moveit_setup_srdf_plugins) install(FILES moveit_setup_framework_plugins.xml - DESTINATION share/moveit_setup_srdf_plugins -) -install(DIRECTORY include/ - DESTINATION include/moveit_setup_srdf_plugins -) + DESTINATION share/moveit_setup_srdf_plugins) +install(DIRECTORY include/ DESTINATION include/moveit_setup_srdf_plugins) ament_export_targets(moveit_setup_srdf_pluginsTargets HAS_LIBRARY_TARGET) -pluginlib_export_plugin_description_file(moveit_setup_framework moveit_setup_framework_plugins.xml) +pluginlib_export_plugin_description_file(moveit_setup_framework + moveit_setup_framework_plugins.xml) ament_package() diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml b/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml index b44e952f3e..67afec1f4f 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/package.xml @@ -2,7 +2,7 @@ moveit_setup_srdf_plugins - 2.9.0 + 2.10.0 SRDF-based plugins for MoveIt Setup Assistant David V. Lu!! BSD-3-Clause @@ -13,11 +13,7 @@ moveit_setup_framework pluginlib - ament_clang_format ament_cmake_gtest - ament_cmake_lint_cmake - ament_cmake_xmllint - ament_lint_auto moveit_resources_fanuc_description diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp index e6564a876c..9fb5b65d1b 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/collision_linear_model.cpp @@ -260,8 +260,8 @@ void SortFilterProxyModel::setShowAll(bool show_all) bool SortFilterProxyModel::filterAcceptsRow(int source_row, const QModelIndex& source_parent) const { CollisionLinearModel* m = qobject_cast(sourceModel()); - if (!(show_all_ || m->reason(source_row) <= ALWAYS || - m->data(m->index(source_row, 2), Qt::CheckStateRole) == Qt::Checked)) + if (!show_all_ && m->reason(source_row) > ALWAYS && + m->data(m->index(source_row, 2), Qt::CheckStateRole) != Qt::Checked) return false; // not accepted due to check state const QRegExp regexp = filterRegExp(); diff --git a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp index 7e72724d84..b45620a885 100644 --- a/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp +++ b/moveit_setup_assistant/moveit_setup_srdf_plugins/src/compute_default_collisions.cpp @@ -50,7 +50,7 @@ namespace { rclcpp::Logger getLogger() { - return moveit::getLogger("collision_updater"); + return moveit::getLogger("moveit.setup_assistant.collision_updater"); } } // namespace // ******************************************************************************************