Merge branch 'moveit:main' into patch-3

Author: mosfet80

.codespell_words

@@ -1,6 +1,7 @@
 aas
 ang
 ans
+AppendT
 atleast
 ba
 brin

.docker/tutorial-source/Dockerfile

@@ -4,7 +4,7 @@
 # Source build of the repos file from the tutorial site
 
 ARG ROS_DISTRO=rolling
-ARG GZ_VERSION=harmonic
+ARG GZ_VERSION=ionic
 
 FROM moveit/moveit2:${ROS_DISTRO}-ci
 LABEL maintainer Tyler Weaver [email protected]

.github/workflows/ci.yaml

@@ -32,7 +32,7 @@ jobs:
       # TODO(andyz): When this clang-tidy issue is fixed, remove -Wno-unknown-warning-option
       # https://stackoverflow.com/a/41673702
       CXXFLAGS: >-
-        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option -Wno-cpp
+        -Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-unknown-warning-option
       CLANG_TIDY_ARGS: --fix --fix-errors --format-style=file
       DOCKER_IMAGE: moveit/moveit2:${{ matrix.env.IMAGE }}
       UPSTREAM_WORKSPACE: >

README.md

@@ -8,7 +8,7 @@ The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the
 
 [![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)
+[![Code Coverage](https://codecov.io/gh/moveit/moveit2/branch/main/graph/badge.svg?token=QC1O0VzGpm)](https://codecov.io/gh/moveit/moveit2)
 
 ## Getting Started
 

moveit_core/planning_scene/src/planning_scene.cpp

@@ -374,6 +374,7 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
         scene->world_->removeObject(it.first);
         scene->removeObjectColor(it.first);
         scene->removeObjectType(it.first);
+        scene->getAllowedCollisionMatrixNonConst().removeEntry(it.first);
       }
       else
       {
@@ -1465,6 +1466,7 @@ void PlanningScene::removeAllCollisionObjects()
       world_->removeObject(object_id);
       removeObjectColor(object_id);
       removeObjectType(object_id);
+      getAllowedCollisionMatrixNonConst().removeEntry(object_id);
     }
   }
 }
@@ -1939,6 +1941,7 @@ bool PlanningScene::processCollisionObjectRemove(const moveit_msgs::msg::Collisi
 
     removeObjectColor(object.id);
     removeObjectType(object.id);
+    getAllowedCollisionMatrixNonConst().removeEntry(object.id);
   }
   return true;
 }

moveit_core/planning_scene/test/test_planning_scene.cpp

@@ -590,6 +590,61 @@ TEST(PlanningScene, RobotStateDiffBug)
   }
 }
 
+TEST(PlanningScene, UpdateACMAfterObjectRemoval)
+{
+  auto robot_model = moveit::core::loadTestingRobotModel("panda");
+  auto ps = std::make_shared<planning_scene::PlanningScene>(robot_model);
+
+  const auto object_name = "object";
+  collision_detection::CollisionRequest collision_request;
+  collision_request.group_name = "hand";
+  collision_request.verbose = true;
+
+  // Helper function to add an object to the planning scene
+  auto add_object = [&] {
+    const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::ADD);
+    ps->usePlanningSceneMsg(ps1);
+    EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set<std::string>{ object_name }));
+  };
+
+  // Modify the allowed collision matrix and make sure it is updated
+  auto modify_acm = [&] {
+    collision_detection::AllowedCollisionMatrix& acm = ps->getAllowedCollisionMatrixNonConst();
+    acm.setEntry(object_name, ps->getRobotModel()->getJointModelGroup("hand")->getLinkModelNamesWithCollisionGeometry(),
+                 true);
+    EXPECT_TRUE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
+  };
+
+  // Check collision
+  auto check_collision = [&] {
+    collision_detection::CollisionResult res;
+    ps->checkCollision(collision_request, res);
+    return res.collision;
+  };
+
+  // Test removing a collision object using a diff
+  add_object();
+  EXPECT_TRUE(check_collision());
+  modify_acm();
+  EXPECT_FALSE(check_collision());
+  {
+    const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::REMOVE);
+    ps->usePlanningSceneMsg(ps1);
+    EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set<std::string>{}));
+    EXPECT_FALSE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
+  }
+
+  // Test removing all objects
+  add_object();
+  // This should report a collision since it's a completely new object
+  EXPECT_TRUE(check_collision());
+  modify_acm();
+  EXPECT_FALSE(check_collision());
+  ps->removeAllCollisionObjects();
+  EXPECT_EQ(getCollisionObjectsNames(*ps), (std::set<std::string>{}));
+  EXPECT_FALSE(ps->getAllowedCollisionMatrix().hasEntry(object_name));
+}
+
 #ifndef INSTANTIATE_TEST_SUITE_P  // prior to gtest 1.10
 #define INSTANTIATE_TEST_SUITE_P(...) INSTANTIATE_TEST_CASE_P(__VA_ARGS__)
 #endif

moveit_core/robot_model/include/moveit/robot_model/joint_model_group.hpp

@@ -588,8 +588,10 @@ class JointModelGroup
 
   /**
    * @brief Get the lower and upper position limits of all active variables in the group.
-   *
-   * @return std::pair<Eigen::VectorXd, Eigen::VectorXd> Containing the lower and upper joint limits for all active variables.
+   * @details In the case of variable without position bounds (e.g. continuous joints), the lower and upper limits are
+   * set to infinity.
+   * @return std::pair<Eigen::VectorXd, Eigen::VectorXd> Containing the lower and upper joint limits for all active
+   * variables.
    */
   [[nodiscard]] std::pair<Eigen::VectorXd, Eigen::VectorXd> getLowerAndUpperLimits() const;
 

moveit_core/robot_model/src/joint_model_group.cpp

@@ -842,8 +842,10 @@ std::pair<Eigen::VectorXd, Eigen::VectorXd> JointModelGroup::getLowerAndUpperLim
   {
     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_;
+      lower_limits[variable_index] =
+          variable_bounds.position_bounded_ ? variable_bounds.min_position_ : -std::numeric_limits<double>::infinity();
+      upper_limits[variable_index] =
+          variable_bounds.position_bounded_ ? variable_bounds.max_position_ : std::numeric_limits<double>::infinity();
       variable_index++;
     }
   }

moveit_ros/trajectory_cache/CMakeLists.txt

@@ -28,9 +28,62 @@ set(TRAJECTORY_CACHE_DEPENDENCIES
     trajectory_msgs
     warehouse_ros)
 
+set(TRAJECTORY_CACHE_LIBRARIES
+    moveit_ros_trajectory_cache_utils_lib
+    moveit_ros_trajectory_cache_features_lib
+    moveit_ros_trajectory_cache_cache_insert_policies_lib
+    moveit_ros_trajectory_cache_lib)
+
+# Utils library
+add_library(moveit_ros_trajectory_cache_utils_lib SHARED src/utils/utils.cpp)
+generate_export_header(moveit_ros_trajectory_cache_utils_lib)
+target_include_directories(
+  moveit_ros_trajectory_cache_utils_lib
+  PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
+ament_target_dependencies(moveit_ros_trajectory_cache_utils_lib
+                          ${TRAJECTORY_CACHE_DEPENDENCIES})
+
+# Features library
+add_library(
+  moveit_ros_trajectory_cache_features_lib SHARED
+  src/features/motion_plan_request_features.cpp
+  src/features/get_cartesian_path_request_features.cpp)
+generate_export_header(moveit_ros_trajectory_cache_features_lib)
+target_link_libraries(moveit_ros_trajectory_cache_features_lib
+                      moveit_ros_trajectory_cache_utils_lib)
+target_include_directories(
+  moveit_ros_trajectory_cache_features_lib
+  PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
+ament_target_dependencies(moveit_ros_trajectory_cache_features_lib
+                          ${TRAJECTORY_CACHE_DEPENDENCIES})
+
+# Cache insert policies library
+add_library(
+  moveit_ros_trajectory_cache_cache_insert_policies_lib SHARED
+  src/cache_insert_policies/always_insert_never_prune_policy.cpp
+  src/cache_insert_policies/best_seen_execution_time_policy.cpp)
+generate_export_header(moveit_ros_trajectory_cache_cache_insert_policies_lib)
+target_link_libraries(
+  moveit_ros_trajectory_cache_cache_insert_policies_lib
+  moveit_ros_trajectory_cache_features_lib
+  moveit_ros_trajectory_cache_utils_lib)
+target_include_directories(
+  moveit_ros_trajectory_cache_cache_insert_policies_lib
+  PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
+         $<INSTALL_INTERFACE:include/moveit_ros_trajectory_cache>)
+ament_target_dependencies(moveit_ros_trajectory_cache_cache_insert_policies_lib
+                          ${TRAJECTORY_CACHE_DEPENDENCIES})
+
 # Trajectory cache library
 add_library(moveit_ros_trajectory_cache_lib SHARED src/trajectory_cache.cpp)
 generate_export_header(moveit_ros_trajectory_cache_lib)
+target_link_libraries(
+  moveit_ros_trajectory_cache_lib
+  moveit_ros_trajectory_cache_cache_insert_policies_lib
+  moveit_ros_trajectory_cache_features_lib
+  moveit_ros_trajectory_cache_utils_lib)
 target_include_directories(
   moveit_ros_trajectory_cache_lib
   PUBLIC $<BUILD_INTERFACE:${PROJECT_SOURCE_DIR}/include>
@@ -39,7 +92,7 @@ ament_target_dependencies(moveit_ros_trajectory_cache_lib
                           ${TRAJECTORY_CACHE_DEPENDENCIES})
 
 install(
-  TARGETS moveit_ros_trajectory_cache_lib
+  TARGETS ${TRAJECTORY_CACHE_LIBRARIES}
   EXPORT moveit_ros_trajectory_cacheTargets
   LIBRARY DESTINATION lib
   ARCHIVE DESTINATION lib
@@ -48,29 +101,14 @@ install(
   DESTINATION include/moveit_ros_trajectory_cache)
 
 install(DIRECTORY include/ DESTINATION include/moveit_ros_trajectory_cache)
-install(
-  FILES ${CMAKE_CURRENT_BINARY_DIR}/moveit_ros_trajectory_cache_lib_export.h
-  DESTINATION include/moveit_ros_trajectory_cache)
-
-if(BUILD_TESTING)
-  find_package(ament_cmake_pytest REQUIRED)
-  find_package(launch_testing_ament_cmake REQUIRED)
-  find_package(rmf_utils REQUIRED)
-  find_package(warehouse_ros_sqlite REQUIRED)
-
-  # This test executable is run by the pytest_test, since a node is required for
-  # testing move groups
-  add_executable(test_trajectory_cache test/test_trajectory_cache.cpp)
-  target_link_libraries(test_trajectory_cache moveit_ros_trajectory_cache_lib)
-  ament_target_dependencies(test_trajectory_cache warehouse_ros_sqlite)
-
-  install(TARGETS test_trajectory_cache RUNTIME DESTINATION lib/${PROJECT_NAME})
 
-  ament_add_pytest_test(
-    test_trajectory_cache_py "test/test_trajectory_cache.py" WORKING_DIRECTORY
-    "${CMAKE_CURRENT_BINARY_DIR}")
+# Install export headers for each library
+foreach(lib_target ${TRAJECTORY_CACHE_LIBRARIES})
+  install(FILES ${CMAKE_CURRENT_BINARY_DIR}/${lib_target}_export.h
+          DESTINATION include/moveit_ros_trajectory_cache)
+endforeach()
 
-endif()
+add_subdirectory(test)
 
 ament_export_targets(moveit_ros_trajectory_cacheTargets HAS_LIBRARY_TARGET)
 ament_export_dependencies(${TRAJECTORY_CACHE_DEPENDENCIES})