Merge branch 'main' into feature/adds-attached-body-regression-tests

.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 tyler@picknik.ai

.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: >

moveit_core/CMakeLists.txt

@@ -69,13 +69,7 @@ add_subdirectory(utils)
 add_subdirectory(version)
 
 install(
-  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
+  TARGETS moveit_collision_detection
           moveit_collision_detection_bullet
           moveit_collision_detection_fcl
           moveit_collision_distance_field
@@ -92,8 +86,6 @@ install(
           moveit_robot_model
           moveit_robot_state
           moveit_robot_trajectory
-          moveit_ruckig_filter
-          moveit_ruckig_filter_parameters
           moveit_smoothing_base
           moveit_test_utils
           moveit_trajectory_processing
@@ -104,6 +96,20 @@ install(
   ARCHIVE DESTINATION lib
   RUNTIME DESTINATION bin)
 
+install(
+  TARGETS collision_detector_bullet_plugin
+          collision_detector_fcl_plugin
+          moveit_acceleration_filter
+          moveit_acceleration_filter_parameters
+          moveit_butterworth_filter
+          moveit_butterworth_filter_parameters
+          moveit_ruckig_filter
+          moveit_ruckig_filter_parameters
+  EXPORT moveit_core_pluginTargets
+  LIBRARY DESTINATION lib
+  ARCHIVE DESTINATION lib
+  RUNTIME DESTINATION bin)
+
 ament_export_targets(moveit_coreTargets HAS_LIBRARY_TARGET)
 ament_export_dependencies(
   angles

moveit_core/distance_field/include/moveit/distance_field/distance_field.hpp

@@ -166,7 +166,8 @@ class DistanceField
    * @param [in] pose The pose of the shape.
    * @param [out] points The points determined for this shape.
    */
-  bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose, EigenSTL::vector_Vector3d* points);
+  bool getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose,
+                      EigenSTL::vector_Vector3d* points) const;
 
   /**
    * \brief Adds the set of points corresponding to the shape at the
@@ -583,7 +584,7 @@ class DistanceField
    * @param [in] octree The octree to find points for.
    * @param [out] points The points determined for this octree.
    */
-  void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points);
+  void getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points) const;
 
   /**
    * \brief Helper function that sets the point value and color given

moveit_core/distance_field/src/distance_field.cpp

@@ -206,7 +206,7 @@ void DistanceField::getGradientMarkers(double min_distance, double max_distance,
 }
 
 bool DistanceField::getShapePoints(const shapes::Shape* shape, const Eigen::Isometry3d& pose,
-                                   EigenSTL::vector_Vector3d* points)
+                                   EigenSTL::vector_Vector3d* points) const
 {
   if (shape->type == shapes::OCTREE)
   {
@@ -237,7 +237,7 @@ void DistanceField::addShapeToField(const shapes::Shape* shape, const Eigen::Iso
   addPointsToField(point_vec);
 }
 
-void DistanceField::getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points)
+void DistanceField::getOcTreePoints(const octomap::OcTree* octree, EigenSTL::vector_Vector3d* points) const
 {
   // lower extent
   double min_x, min_y, min_z;

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/acceleration_filter.hpp

@@ -78,7 +78,7 @@ c --------x--- v   |
 #include <moveit/robot_model/robot_model.hpp>
 #include <moveit/robot_state/robot_state.hpp>
 #include <moveit/utils/logger.hpp>
-#include <moveit_acceleration_filter_parameters.hpp>
+#include <moveit_core/moveit_acceleration_filter_parameters.hpp>
 
 #include <osqp.h>
 #include <types.h>

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.hpp

@@ -41,9 +41,9 @@
 
 #include <cstddef>
 
-#include <moveit_butterworth_filter_parameters.hpp>
 #include <moveit/robot_model/robot_model.hpp>
 #include <moveit/online_signal_smoothing/smoothing_base_class.hpp>
+#include <moveit_core/moveit_butterworth_filter_parameters.hpp>
 
 namespace online_signal_smoothing
 {

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/ruckig_filter.hpp

@@ -42,7 +42,7 @@ Description: Applies jerk/acceleration/velocity limits to online motion commands
 
 #include <moveit/robot_model/robot_model.hpp>
 #include <moveit/online_signal_smoothing/smoothing_base_class.hpp>
-#include <moveit_ruckig_filter_parameters.hpp>
+#include <moveit_core/moveit_ruckig_filter_parameters.hpp>
 
 #include <ruckig/ruckig.hpp>
 

moveit_core/planning_scene/src/planning_scene.cpp

@@ -374,6 +374,11 @@ void PlanningScene::pushDiffs(const PlanningScenePtr& scene)
         scene->world_->removeObject(it.first);
         scene->removeObjectColor(it.first);
         scene->removeObjectType(it.first);
+        // if object is attached, it should not be removed from the ACM
+        if (!scene->getCurrentState().hasAttachedBody(it.first))
+        {
+          scene->getAllowedCollisionMatrixNonConst().removeEntry(it.first);
+        }
       }
       else
       {
@@ -1465,6 +1470,7 @@ void PlanningScene::removeAllCollisionObjects()
       world_->removeObject(object_id);
       removeObjectColor(object_id);
       removeObjectType(object_id);
+      getAllowedCollisionMatrixNonConst().removeEntry(object_id);
     }
   }
 }
@@ -1939,6 +1945,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,80 @@ 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 }));
+  };
+
+  // Helper function to attach the object to the robot
+  auto attach_object = [&] {
+    const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::ADD, true);
+    ps->usePlanningSceneMsg(ps1);
+    EXPECT_EQ(getAttachedCollisionObjectsNames(*ps), (std::set<std::string>{ object_name }));
+  };
+
+  // Helper function to detach the object from the robot
+  auto detach_object = [&] {
+    const auto ps1 = createPlanningSceneDiff(*ps, object_name, moveit_msgs::msg::CollisionObject::REMOVE, true);
+    ps->usePlanningSceneMsg(ps1);
+    EXPECT_EQ(getAttachedCollisionObjectsNames(*ps), (std::set<std::string>{}));
+  };
+
+  // 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());
+  // Attach and detach the object from the robot to make sure that collision are still allowed
+  attach_object();
+  EXPECT_FALSE(check_collision());
+  detach_object();
+  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_core/robot_state/include/moveit/robot_state/cartesian_interpolator.hpp

@@ -115,15 +115,15 @@ class CartesianInterpolator
       if (value < 0.0 || value > 1.0)
         throw std::runtime_error("Percentage values must be between 0 and 1, inclusive");
     }
-    operator double()
+    operator double() const
     {
       return value;
     }
-    double operator*()
+    double operator*() const
     {
       return value;
     }
-    Percentage operator*(const Percentage& p)
+    Percentage operator*(const Percentage& p) const
     {
       Percentage res(value * p.value);
       return res;
@@ -136,15 +136,15 @@ class CartesianInterpolator
     Distance(double meters) : meters(meters)
     {
     }
-    operator double()
+    operator double() const
     {
       return meters;
     }
-    double operator*()
+    double operator*() const
     {
       return meters;
     }
-    Distance operator*(const Percentage& p)
+    Distance operator*(const Percentage& p) const
     {
       Distance res(meters * p.value);
       return res;

moveit_core/robot_state/src/robot_state.cpp

@@ -2039,8 +2039,8 @@ bool RobotState::setFromIKSubgroups(const JointModelGroup* jmg, const EigenSTL::
   {
     if (consistency_limits[i].size() != sub_groups[i]->getVariableCount())
     {
-      RCLCPP_ERROR(getLogger(), "Number of joints in consistency_limits is %zu but it should be should be %u", i,
-                   sub_groups[i]->getVariableCount());
+      RCLCPP_ERROR(getLogger(), "Number of joints in consistency_limits[%zu] is %lu but it should be should be %u", i,
+                   consistency_limits[i].size(), sub_groups[i]->getVariableCount());
       return false;
     }
   }

moveit_core/robot_state/test/test_aabb.cpp

@@ -42,7 +42,12 @@
 #include <string>
 #include <gtest/gtest.h>
 #include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
+// TODO: Remove conditional include when released to all active distros.
+#if __has_include(<tf2/LinearMath/Vector3.hpp>)
+#include <tf2/LinearMath/Vector3.hpp>
+#else
 #include <tf2/LinearMath/Vector3.h>
+#endif
 #include <moveit/utils/robot_model_test_utils.hpp>
 
 // To visualize bbox of the PR2, set this to 1.

moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.hpp

@@ -158,20 +158,20 @@ class Trajectory
   };
 
   bool getNextSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration,
-                             double& after_acceleration);
+                             double& after_acceleration) const;
   bool getNextAccelerationSwitchingPoint(double path_pos, TrajectoryStep& next_switching_point,
-                                         double& before_acceleration, double& after_acceleration);
+                                         double& before_acceleration, double& after_acceleration) const;
   bool getNextVelocitySwitchingPoint(double path_pos, TrajectoryStep& next_switching_point, double& before_acceleration,
-                                     double& after_acceleration);
+                                     double& after_acceleration) const;
   bool integrateForward(std::list<TrajectoryStep>& trajectory, double acceleration);
   void integrateBackward(std::list<TrajectoryStep>& start_trajectory, double path_pos, double path_vel,
                          double acceleration);
-  double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max);
-  double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max);
+  double getMinMaxPathAcceleration(double path_position, double path_velocity, bool max) const;
+  double getMinMaxPhaseSlope(double path_position, double path_velocity, bool max) const;
   double getAccelerationMaxPathVelocity(double path_pos) const;
   double getVelocityMaxPathVelocity(double path_pos) const;
-  double getAccelerationMaxPathVelocityDeriv(double path_pos);
-  double getVelocityMaxPathVelocityDeriv(double path_pos);
+  double getAccelerationMaxPathVelocityDeriv(double path_pos) const;
+  double getVelocityMaxPathVelocityDeriv(double path_pos) const;
 
   std::list<TrajectoryStep>::const_iterator getTrajectorySegment(double time) const;