Merge branch 'main' into pr-ompl_require_box_constraints

.dockerignore

@@ -17,11 +17,9 @@
 !moveit_planners/ompl/package.xml
 !moveit_planners/chomp/chomp_motion_planner/package.xml
 !moveit_planners/chomp/chomp_interface/package.xml
-!moveit_planners/chomp/chomp_optimizer_adapter/package.xml
 !moveit_planners/pilz_industrial_motion_planner_testutils/package.xml
 !moveit_planners/pilz_industrial_motion_planner/package.xml
 !moveit_planners/moveit_planners/package.xml
-!moveit_planners/trajopt/package.xml
 !moveit_runtime/package.xml
 !moveit/package.xml
 !moveit_ros/warehouse/package.xml

.github/CODEOWNERS.disabled

@@ -91,8 +91,6 @@
 
 /moveit_planners/ompl/ @BryceStevenWilley @mamoll
 /moveit_planners/chomp/chomp_interface/ @raghavendersahdev @knorth55 @bmagyar
-/moveit_planners/chomp/chomp_optimizer_adapter/ @raghavendersahdev @knorth55 @bmagyar
 /moveit_planners/chomp/chomp_motion_planner/ @raghavendersahdev @knorth55 @bmagyar
-/moveit_planners/trajopt @ommmid @mamoll
 /moveit_planners/pilz_industrial_motion_planner @jschleicher @ct2034
 /moveit_planners/pilz_industrial_motion_planner_testutils/ @jschleicher @ct2034

.github/workflows/ci.yaml

@@ -141,7 +141,7 @@ jobs:
  run: |
  testspace "[ ${{ matrix.env.IMAGE }} ]${{ env.BASEDIR }}/target_ws/**/test_results/**/*.xml"
  - name: Upload test artifacts (on failure)
- uses: actions/upload-artifact@v3
+ uses: actions/upload-artifact@v4
  if: failure() && (steps.ici.outputs.run_target_test || steps.ici.outputs.target_test_results)
  with:
  name: test-results-${{ matrix.env.IMAGE }}

.github/workflows/format.yaml

@@ -16,7 +16,7 @@ jobs:
  runs-on: ubuntu-22.04
  steps:
  - uses: actions/checkout@v4
- - uses: actions/setup-python@v4
+ - uses: actions/setup-python@v5
  with:
  python-version: '3.10'
  - name: Install clang-format-14

.github/workflows/stale.yaml

@@ -12,7 +12,7 @@ jobs:
  issues: write
  pull-requests: write
  steps:
- - uses: actions/stale@v8
+ - uses: actions/stale@v9
  with:
  stale-issue-label: 'stale'
  stale-pr-label: 'stale'

.github/workflows/tutorial_docker.yaml

@@ -50,7 +50,7 @@ jobs:
  path: .ccache
  key: docker-tutorial-ccache-${{ matrix.ROS_DISTRO }}-${{ hashFiles( '.docker/tutorial-source/Dockerfile' ) }}
  - name: inject ccache into docker
- uses: reproducible-containers/[email protected].2
+ uses: reproducible-containers/[email protected].3
  with:
  cache-source: .ccache
  cache-target: /root/.ccache/

MIGRATION.md

@@ -3,6 +3,14 @@
 API changes in MoveIt releases
 
 ## ROS Rolling
+- [12/2023] LMA kinematics plugin is removed to remove the maintenance work because better alternatives exist like KDL or TracIK
+- [10/2023] The planning pipeline now has a vector of planner plugins rather than a single one. Please update the planner plugin parameter e.g. like this:
+```diff
+- planning_plugin: ompl_interface/OMPLPlanner
++ planning_plugins:
++ - ompl_interface/OMPLPlanner
+```
+
 - [10/2023] Planning request adapters are now separated into PlanRequest (preprocessing) and PlanResponse (postprocessing) adapters. The adapters are configured with ROS parameter vectors (vector order corresponds to execution order). Please update your pipeline configurations for example like this:
 ```diff
 - request_adapters: >-

moveit_configs_utils/default_configs/chomp_planning.yaml

@@ -1,4 +1,5 @@
-planning_plugin: chomp_interface/CHOMPPlanner
+planning_plugins:
+ - chomp_interface/CHOMPPlanner
 enable_failure_recovery: true
 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 request_adapters:

moveit_configs_utils/default_configs/ompl_planning.yaml

@@ -1,4 +1,5 @@
-planning_plugin: ompl_interface/OMPLPlanner
+planning_plugins:
+ - ompl_interface/OMPLPlanner
 # The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
 request_adapters:
  - default_planning_request_adapters/ResolveConstraintFrames

moveit_configs_utils/default_configs/pilz_industrial_motion_planner_planning.yaml

@@ -1,6 +1,14 @@
-planning_plugin: pilz_industrial_motion_planner/CommandPlanner
-# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+planning_plugins:
+ - pilz_industrial_motion_planner/CommandPlanner
 default_planner_config: PTP
+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/ValidateSolution
+ - default_planning_response_adapters/DisplayMotionPath
 capabilities: >-
  pilz_industrial_motion_planner/MoveGroupSequenceAction
  pilz_industrial_motion_planner/MoveGroupSequenceService

moveit_configs_utils/default_configs/stomp_planning.yaml

@@ -1,5 +1,5 @@
-planning_plugin: stomp_moveit/StompPlanner
-# The order of the elements in the adapter corresponds to the order they are processed by the motion planning pipeline.
+planning_plugins:
+ - stomp_moveit/StompPlanner
 request_adapters:
  - default_planning_request_adapters/ResolveConstraintFrames
  - default_planning_request_adapters/ValidateWorkspaceBounds

moveit_core/collision_detection/include/moveit/collision_detection/collision_common.h

@@ -141,41 +141,7 @@ struct CostSource
  }
 };
 
-/** \brief Representation of a collision checking result */
-struct CollisionResult
-{
- using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
-
- EIGEN_MAKE_ALIGNED_OPERATOR_NEW
-
- /** \brief Clear a previously stored result */
- void clear()
- {
- collision = false;
- distance = std::numeric_limits<double>::max();
- contact_count = 0;
- contacts.clear();
- cost_sources.clear();
- }
-
- /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */
- void print() const;
-
- /** \brief True if collision was found, false otherwise */
- bool collision = false;
-
- /** \brief Closest distance between two bodies */
- double distance = std::numeric_limits<double>::max();
-
- /** \brief Number of contacts returned */
- std::size_t contact_count = 0;
-
- /** \brief A map returning the pairs of body ids in contact, plus their contact details */
- ContactMap contacts;
-
- /** \brief These are the individual cost sources when costs are computed */
- std::set<CostSource> cost_sources;
-};
+struct CollisionResult;
 
 /** \brief Representation of a collision checking request */
 struct CollisionRequest
@@ -187,6 +153,9 @@ struct CollisionRequest
  /** \brief If true, compute proximity distance */
  bool distance = false;
 
+ /** \brief If true, return detailed distance information. Distance must be set to true as well */
+ bool detailed_distance = false;
+
  /** \brief If true, a collision cost is computed */
  bool cost = false;
 
@@ -354,4 +323,43 @@ struct DistanceResult
  distances.clear();
  }
 };
+
+/** \brief Representation of a collision checking result */
+struct CollisionResult
+{
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+
+ /** \brief Clear a previously stored result */
+ void clear()
+ {
+ collision = false;
+ distance = std::numeric_limits<double>::max();
+ distance_result.clear();
+ contact_count = 0;
+ contacts.clear();
+ cost_sources.clear();
+ }
+
+ /** \brief Throttled warning printing the first collision pair, if any. All collisions are logged at DEBUG level */
+ void print() const;
+
+ /** \brief True if collision was found, false otherwise */
+ bool collision = false;
+
+ /** \brief Closest distance between two bodies */
+ double distance = std::numeric_limits<double>::max();
+
+ /** \brief Distance data for each link */
+ DistanceResult distance_result;
+
+ /** \brief Number of contacts returned */
+ std::size_t contact_count = 0;
+
+ /** \brief A map returning the pairs of body ids in contact, plus their contact details */
+ using ContactMap = std::map<std::pair<std::string, std::string>, std::vector<Contact> >;
+ ContactMap contacts;
+
+ /** \brief These are the individual cost sources when costs are computed */
+ std::set<CostSource> cost_sources;
+};
 } // namespace collision_detection

moveit_core/collision_detection_bullet/test/test_bullet_continuous_collision_checking.cpp

@@ -47,13 +47,17 @@
 
 #include <moveit/collision_detection_bullet/collision_env_bullet.h>
 #include <moveit/collision_detection_bullet/bullet_integration/basic_types.h>
+#include <moveit/utils/logger.hpp>
 
 #include <urdf_parser/urdf_parser.h>
 #include <geometric_shapes/shape_operations.h>
 
 namespace cb = collision_detection_bullet;
 
-static const rclcpp::Logger TEST_LOGGER = rclcpp::get_logger("collision_detection.bullet_test");
+rclcpp::Logger getLogger()
+{
+ return moveit::getLogger("collision_detection.bullet_test");
+}
 
 /** \brief Brings the panda robot in user defined home position */
 inline void setToHome(moveit::core::RobotState& panda_state)
@@ -254,7 +258,7 @@ TEST_F(BulletCollisionDetectionTester, DISABLED_ContinuousCollisionSelf)
  ASSERT_FALSE(res.collision);
  res.clear();
 
- RCLCPP_INFO(TEST_LOGGER, "Continuous to continuous collisions are not supported yet, therefore fail here.");
+ RCLCPP_INFO(getLogger(), "Continuous to continuous collisions are not supported yet, therefore fail here.");
  ASSERT_TRUE(res.collision);
  res.clear();
 }

moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp

@@ -290,6 +290,10 @@ void CollisionEnvFCL::checkSelfCollisionHelper(const CollisionRequest& req, Coll
  dreq.enableGroup(getRobotModel());
  distanceSelf(dreq, dres, state);
  res.distance = dres.minimum_distance.distance;
+ if (req.detailed_distance)
+ {
+ res.distance_result = dres;
+ }
  }
 }
 
@@ -343,6 +347,10 @@ void CollisionEnvFCL::checkRobotCollisionHelper(const CollisionRequest& req, Col
  dreq.enableGroup(getRobotModel());
  distanceRobot(dreq, dres, state);
  res.distance = dres.minimum_distance.distance;
+ if (req.detailed_distance)
+ {
+ res.distance_result = dres;
+ }
  }
 }
 

moveit_core/collision_detection_fcl/test/test_fcl_env.cpp

@@ -41,6 +41,7 @@
 #include <moveit/robot_model/robot_model.h>
 #include <moveit/robot_state/robot_state.h>
 #include <moveit/utils/robot_model_test_utils.h>
+#include <moveit/utils/logger.hpp>
 
 #include <moveit/collision_detection_fcl/collision_common.h>
 #include <moveit/collision_detection_fcl/collision_env_fcl.h>
@@ -63,7 +64,10 @@ inline void setToHome(moveit::core::RobotState& panda_state)
  panda_state.update();
 }
 
-static const rclcpp::Logger LOGGER = rclcpp::get_logger("moveit_collision_detection_fcl.test.test_fcl_env");
+rclcpp::Logger getLogger()
+{
+ return moveit::getLogger("moveit_collision_detection_fcl.test.test_fcl_env");
+}
 
 class CollisionDetectionEnvTest : public testing::Test
 {
@@ -262,7 +266,7 @@ TEST_F(CollisionDetectionEnvTest, DISABLED_ContinuousCollisionSelf)
  res.clear();
 
  // c_env_->checkSelfCollision(req, res, state1, state2, *acm_);
- RCLCPP_INFO(LOGGER, "Continuous to continuous collisions are not supported yet, therefore fail here.");
+ RCLCPP_INFO(getLogger(), "Continuous to continuous collisions are not supported yet, therefore fail here.");
  ASSERT_TRUE(res.collision);
  res.clear();
 }

moveit_core/collision_distance_field/src/collision_distance_field_types.cpp

@@ -42,13 +42,19 @@
 #include <rclcpp/logger.hpp>
 #include <rclcpp/time.hpp>
 #include <memory>
+#include <moveit/utils/logger.hpp>
 
 static const double EPSILON{ 0.0001 };
 
 namespace collision_detection
 {
-static const rclcpp::Logger LOGGER =
- rclcpp::get_logger("moveit_collision_distance_field.collision_distance_field_types");
+namespace
+{
+rclcpp::Logger getLogger()
+{
+ return moveit::getLogger("collision_distance_field_types");
+}
+} // namespace
 
 std::vector<CollisionSphere> determineCollisionSpheres(const bodies::Body* body, Eigen::Isometry3d& relative_transform)
 {
@@ -158,7 +164,7 @@ bool getCollisionSphereGradients(const distance_field::DistanceField* distance_f
  double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
  if (!in_bounds && grad.norm() > EPSILON)
  {
- RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
+ RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds %lf, %lf, %lf", p.x(), p.y(), p.z());
  return true;
  }
 
@@ -216,7 +222,7 @@ bool getCollisionSphereCollision(const distance_field::DistanceField* distance_f
 
  if (!in_bounds && grad.norm() > 0)
  {
- RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds");
+ RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds");
  return true;
  }
 
@@ -243,7 +249,7 @@ bool getCollisionSphereCollision(const distance_field::DistanceField* distance_f
  double dist = distance_field->getDistanceGradient(p.x(), p.y(), p.z(), grad.x(), grad.y(), grad.z(), in_bounds);
  if (!in_bounds && (grad.norm() > 0))
  {
- RCLCPP_DEBUG(LOGGER, "Collision sphere point is out of bounds");
+ RCLCPP_DEBUG(getLogger(), "Collision sphere point is out of bounds");
  return true;
  }
  if (maximum_value > dist && (sphere_list[i].radius_ - dist > tolerance))
@@ -324,8 +330,8 @@ void BodyDecomposition::init(const std::vector<shapes::ShapeConstPtr>& shapes, c
  }
  bodies::mergeBoundingSpheres(bounding_spheres, relative_bounding_sphere_);
 
- RCLCPP_DEBUG(LOGGER, "BodyDecomposition generated %zu collision spheres out of %zu shapes", collision_spheres_.size(),
- shapes.size());
+ RCLCPP_DEBUG(getLogger(), "BodyDecomposition generated %zu collision spheres out of %zu shapes",
+ collision_spheres_.size(), shapes.size());
 }
 
 BodyDecomposition::~BodyDecomposition()
@@ -450,7 +456,7 @@ void getProximityGradientMarkers(const std::string& frame_id, const std::string&
  rclcpp::Clock ros_clock;
  if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
  {
- RCLCPP_WARN(LOGGER, "Size mismatch between gradients %u and decompositions %u",
+ RCLCPP_WARN(getLogger(), "Size mismatch between gradients %u and decompositions %u",
  static_cast<unsigned int>(gradients.size()),
  static_cast<unsigned int>((posed_decompositions.size() + posed_vector_decompositions.size())));
  return;
@@ -484,12 +490,12 @@ void getProximityGradientMarkers(const std::string& frame_id, const std::string&
  }
  else
  {
- RCLCPP_DEBUG(LOGGER, "Negative length for %u %d %lf", i, arrow_mark.id, gradients[i].gradients[j].norm());
+ RCLCPP_DEBUG(getLogger(), "Negative length for %u %d %lf", i, arrow_mark.id, gradients[i].gradients[j].norm());
  }
  }
  else
  {
- RCLCPP_DEBUG(LOGGER, "Negative dist %lf for %u %d", gradients[i].distances[j], i, arrow_mark.id);
+ RCLCPP_DEBUG(getLogger(), "Negative dist %lf for %u %d", gradients[i].distances[j], i, arrow_mark.id);
  }
  arrow_mark.points.resize(2);
  if (i < posed_decompositions.size())
@@ -551,7 +557,7 @@ void getCollisionMarkers(const std::string& frame_id, const std::string& ns, con
  rclcpp::Clock ros_clock;
  if (gradients.size() != posed_decompositions.size() + posed_vector_decompositions.size())
  {
- RCLCPP_WARN(LOGGER, "Size mismatch between gradients %zu and decompositions %zu", gradients.size(),
+ RCLCPP_WARN(getLogger(), "Size mismatch between gradients %zu and decompositions %zu", gradients.size(),
  posed_decompositions.size() + posed_vector_decompositions.size());
  return;
  }