Merge branch 'main' into pr-delete_untagged_images

.clang-tidy

@@ -9,6 +9,8 @@ Checks:          '-*,
                   modernize-use-default,
                   modernize-use-override,
                   modernize-loop-convert,
+                  modernize-make-shared,
+                  modernize-make-unique,
                   readability-named-parameter,
                   readability-redundant-smartptr-get,
                   readability-redundant-string-cstr,

.github/mergify.yml

@@ -0,0 +1,26 @@
+pull_request_rules:
+  - name: backport to foxy at reviewers discretion
+    conditions:
+      - base=main
+      - "label=backport-foxy"
+    actions:
+      backport:
+        branches:
+          - foxy
+
+  - name: ask to resolve conflict
+    conditions:
+      - conflict
+      - author!=mergify
+    actions:
+        comment:
+          message: This pull request is in conflict. Could you fix it @{{author}}?
+
+  - name: development targets main branch
+    conditions:
+      - base!=main
+      - author!=mergify
+    actions:
+        comment:
+          message: |
+            Please target the `main` branch for development, we will backport the changes to {{base}} for you if approved and if they don't break API.

.github/workflows/ci.yaml

@@ -27,6 +27,8 @@ jobs:
       CXXFLAGS: "-Wall -Wextra -Wwrite-strings -Wunreachable-code -Wpointer-arith -Wredundant-decls -Wno-deprecated-copy"
       DOCKER_IMAGE: ghcr.io/ros-planning/moveit2:${{ matrix.env.IMAGE }}
       UPSTREAM_WORKSPACE: moveit2.repos $(f="moveit2_$(sed 's/-.*$//' <<< "${{ matrix.env.IMAGE }}").repos"; test -r $f && echo $f)
+      # TODO(#885): Fix circular test dependency with moveit_resources and remove it from the target workspace
+      TARGET_WORKSPACE: $TARGET_REPO_PATH github:ros-planning/moveit_resources#ros2
       # Pull any updates to the upstream workspace (after restoring it from cache)
       AFTER_SETUP_UPSTREAM_WORKSPACE: vcs pull $BASEDIR/upstream_ws/src
       AFTER_SETUP_DOWNSTREAM_WORKSPACE: vcs pull $BASEDIR/downstream_ws/src

MIGRATION.md

@@ -4,6 +4,13 @@ API changes in MoveIt releases
 
 ## ROS Rolling
 - ServoServer was renamed to ServoNode
+- `CollisionObject` messages are now defined with a `Pose`, and shapes and subframes are defined relative to the object's pose. This makes it easier to place objects with subframes and multiple shapes in the scene. This causes several changes:
+    - `getFrameTransform()` now returns this pose instead of the first shape's pose.
+    - The Rviz plugin's manipulation tab now uses the object's pose instead of the shape pose to evaluate if object's are in the region of interest.
+    - Planning scene geometry text files (`.scene`) have changed format. Add a line `0 0 0 0 0 0 1` under each line with an asterisk to upgrade old files if required.
+- add API for passing RNG to setToRandomPositionsNearBy
+- Static member variable interface of the CollisionDetectorAllocatorTemplate for the string NAME was replaced with a virtual method `getName`.
+- Enhance `RDFLoader` to load from string parameter OR string topic (and add the ability to publish a string topic).
 
 ## ROS Noetic
 - RobotModel no longer overrides empty URDF collision geometry by matching the visual geometry of the link.
@@ -24,6 +31,7 @@ API changes in MoveIt releases
 - The joint states of `passive` joints must be published in ROS and the CurrentStateMonitor will now wait for them as well. Their semantics dictate that they cannot be actively controlled, but they must be known to use the full robot state in collision checks. (https://github.com/ros-planning/moveit/pull/2663)
 - Removed deprecated header `moveit/macros/deprecation.h`. Use `[[deprecated]]` instead.
 - All uses of `MOVEIT_CLASS_FORWARD` et. al. must now be followed by a semicolon for consistency (and to get -pedantic builds to pass for the codebase).
+- In case you start RViz in a namespace, the default topic for the trajectory visualization display now uses the relative instead of the absolute namespace (i.e. `<ns>/move_group/display_planned_path` instead of `/move_group/display_planned_path`).
 
 ## ROS Melodic
 

moveit/package.xml

@@ -1,14 +1,13 @@
 <?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>moveit</name>
   <version>2.3.0</version>
   <description>Meta package that contains all essential packages of MoveIt 2</description>
-  <maintainer email="dave@picknik.ai">Dave Coleman</maintainer>
-  <maintainer email="mferguson@fetchrobotics.com">Michael Ferguson</maintainer>
+  <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
+  <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
   <maintainer email="me@v4hn.de">Michael Görner</maintainer>
   <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
-  <maintainer email="imcmahon01@gmail.com">Ian McMahon</maintainer>
-  <maintainer email="130s@2000.jukuin.keio.ac.jp">Isaac I. Y. Saito</maintainer>
 
   <license>BSD</license>
 
@@ -18,6 +17,7 @@
 
   <author email="isucan@google.com">Ioan Sucan</author>
   <author email="robot.moveit@gmail.com">Sachin Chitta</author>
+  <author email="dave@picknik.ai">Dave Coleman</author>
 
   <buildtool_depend>ament_cmake</buildtool_depend>
   <!--

moveit2.repos

@@ -3,10 +3,15 @@ repositories:
     type: git
     url: https://github.com/ros-planning/geometric_shapes
     version: ros2
-  moveit_resources:
+  moveit_msgs:
     type: git
-    url: https://github.com/ros-planning/moveit_resources
+    url: https://github.com/ros-planning/moveit_msgs
     version: ros2
+  # TODO(#885): Re-enable when circular dependency is fixed
+  # moveit_resources:
+  #   type: git
+  #   url: https://github.com/ros-planning/moveit_resources
+  #   version: ros2
   warehouse_ros:
     type: git
     url: https://github.com/ros-planning/warehouse_ros

moveit_commander/package.xml

@@ -1,11 +1,9 @@
 <?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>moveit_commander</name>
-  <version>1.1.5</version>
+  <version>1.1.6</version>
   <description>Python interfaces to MoveIt</description>
-
-  <author email="isucan@google.com">Ioan Sucan</author>
-
   <maintainer email="me@v4hn.de">Michael Görner</maintainer>
   <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>
@@ -16,6 +14,8 @@
   <url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
   <url type="repository">https://github.com/ros-planning/moveit2</url>
 
+  <author email="isucan@google.com">Ioan Sucan</author>
+
   <buildtool_depend>catkin</buildtool_depend>
   <buildtool_depend condition="$ROS_PYTHON_VERSION == 2">python-catkin-pkg</buildtool_depend>
   <buildtool_depend condition="$ROS_PYTHON_VERSION == 3">python3-catkin-pkg</buildtool_depend>

moveit_commander/src/moveit_commander/move_group.py

@@ -497,22 +497,22 @@ def clear_path_constraints(self):
         self._g.clear_path_constraints()
 
     def get_trajectory_constraints(self):
-        """ Get the actual trajectory constraints in form of a moveit_msgs.msgs.Constraints """
-        c = Constraints()
+        """ Get the actual trajectory constraints in form of a moveit_msgs.msgs.TrajectoryConstraints """
+        c = TrajectoryConstraints()
         c_str = self._g.get_trajectory_constraints()
         conversions.msg_from_string(c, c_str)
         return c
 
     def set_trajectory_constraints(self, value):
-        """ Specify the trajectory constraints to be used """
+        """ Specify the trajectory constraints to be used (setting from database is not implemented yet)"""
         if value is None:
             self.clear_trajectory_constraints()
         else:
             if type(value) is TrajectoryConstraints:
                 self._g.set_trajectory_constraints_from_msg(
                     conversions.msg_to_string(value)
                 )
-            elif not self._g.set_trajectory_constraints(value):
+            else:
                 raise MoveItCommanderException(
                     "Unable to set trajectory constraints " + value
                 )

moveit_commander/src/moveit_commander/robot.py

@@ -52,7 +52,7 @@ def variable_count(self):
             """
             @return number of the list that _Joint__get_joint_limits
                     methods returns.
-            @see: http://docs.ros.org/indigo/api/moveit_core/html/classmoveit_1_1core_1_1JointModel.html#details
+            @see: http://docs.ros.org/en/latest/api/moveit_core/html/cpp/classmoveit_1_1core_1_1JointModel.html#details
                   for more about variable.
             """
             return len(self.__get_joint_limits())

moveit_common/cmake/moveit_package.cmake

@@ -32,6 +32,9 @@ macro(moveit_package)
   find_package(ament_cmake REQUIRED)
   ament_package_xml()
 
+  # Enable backward_ros on every moveit package
+  find_package(backward_ros QUIET)
+
   if(NOT "${CMAKE_CXX_STANDARD}")
     set(CMAKE_CXX_STANDARD 17)
   endif()

moveit_common/package.xml

@@ -1,17 +1,25 @@
 <?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
 <package format="3">
   <name>moveit_common</name>
   <version>2.3.0</version>
   <description>Common support functionality used throughout MoveIt</description>
+  <maintainer email="henningkayser@picknik.ai">Henning Kayser</maintainer>
+  <maintainer email="tyler@picknik.ai">Tyler Weaver</maintainer>
   <maintainer email="moveit_releasers@googlegroups.com">MoveIt Release Team</maintainer>
 
   <license>BSD</license>
+
   <url type="website">http://moveit.ros.org</url>
   <url type="bugtracker">https://github.com/ros-planning/moveit2/issues</url>
   <url type="repository">https://github.com/ros-planning/moveit2</url>
 
+  <author email="lior.lustgarten@microsoft.com">Lior Lustgarten</author>
+
   <buildtool_depend>ament_cmake</buildtool_depend>
 
+  <depend>backward_ros</depend>
+
   <test_depend>ament_lint_auto</test_depend>
   <test_depend>ament_lint_common</test_depend>
 

moveit_core/collision_detection/include/moveit/collision_detection/collision_plugin.h

@@ -54,10 +54,13 @@ MOVEIT_CLASS_FORWARD(CollisionPlugin);  // Defines CollisionPluginPtr, ConstPtr,
  *     public collision_detection::CollisionDetectorAllocatorTemplate<MyCollisionEnv, MyCollisionDetectorAllocator>
  *   {
  *     public:
+ *       const std::string& getName() const override {
+ *         static const std::string NAME = "my_checker";
+ *         return NAME;
+ *       }
  *       static const std::string NAME_;
  *   };
  *
- *   const std::string MyCollisionDetectorAllocator::NAME_("my_checker");
  *   }
  *
  *   namespace collision_detection

moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h

@@ -74,11 +74,11 @@ class CollisionDetectorPandaTest : public testing::Test
 protected:
   void SetUp() override
   {
-    value_.reset(new CollisionAllocatorType);
+    value_ = std::make_shared<CollisionAllocatorType>();
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    acm_.reset(new collision_detection::AllowedCollisionMatrix());
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>();
     // Use default collision operations in the SRDF to setup the acm
     const std::vector<std::string>& collision_links = robot_model_->getLinkModelNamesWithCollisionGeometry();
     acm_->setEntry(collision_links, collision_links, false);
@@ -90,7 +90,7 @@ class CollisionDetectorPandaTest : public testing::Test
 
     cenv_ = value_->allocateEnv(robot_model_);
 
-    robot_state_.reset(new moveit::core::RobotState(robot_model_));
+    robot_state_ = std::make_shared<moveit::core::RobotState>(robot_model_);
     setToHome(*robot_state_);
   }
 
@@ -153,7 +153,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1)
 
   Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
   pos1.translation().z() = 0.3;
-  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos1);
+  this->cenv_->getWorld()->addToObject("box", pos1, shape_ptr, Eigen::Isometry3d::Identity());
 
   this->cenv_->checkSelfCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_FALSE(res.collision);
@@ -186,7 +186,8 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_2)
 
   Eigen::Isometry3d pos1 = Eigen::Isometry3d::Identity();
   pos1.translation().z() = 0.3;
-  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos1);
+  this->cenv_->getWorld()->addToObject("box", pos1, shape_ptr, Eigen::Isometry3d::Identity());
+
   this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_TRUE(res.collision);
   ASSERT_GE(res.contact_count, 3u);
@@ -213,7 +214,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, PaddingTest)
   pos.translation().x() = 0.43;
   pos.translation().y() = 0;
   pos.translation().z() = 0.55;
-  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos);
+  this->cenv_->getWorld()->addToObject("box", pos, shape_ptr, Eigen::Isometry3d::Identity());
 
   this->cenv_->setLinkPadding("panda_hand", 0.08);
   this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
@@ -250,7 +251,7 @@ TYPED_TEST_P(CollisionDetectorPandaTest, DistanceWorld)
   pos.translation().x() = 0.43;
   pos.translation().y() = 0;
   pos.translation().z() = 0.55;
-  this->cenv_->getWorld()->addToObject("box", shape_ptr, pos);
+  this->cenv_->getWorld()->addToObject("box", pos, shape_ptr, Eigen::Isometry3d::Identity());
 
   this->cenv_->setLinkPadding("panda_hand", 0.0);
   this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
@@ -287,9 +288,9 @@ TYPED_TEST_P(DistanceCheckPandaTest, DistanceSingle)
     rng.quaternion(quat);
     pose.linear() = Eigen::Quaterniond(quat[0], quat[1], quat[2], quat[3]).toRotationMatrix();
 
-    this->cenv_->getWorld()->addToObject("collection", shape, pose);
+    this->cenv_->getWorld()->addToObject("collection", Eigen::Isometry3d::Identity(), shape, pose);
     this->cenv_->getWorld()->removeObject("object");
-    this->cenv_->getWorld()->addToObject("object", shape, pose);
+    this->cenv_->getWorld()->addToObject("object", pose, shape, Eigen::Isometry3d::Identity());
 
     this->cenv_->distanceRobot(req, res, *this->robot_state_);
     auto& distances1 = res.distances[std::pair<std::string, std::string>("collection", "panda_hand")];
@@ -336,13 +337,13 @@ TYPED_TEST_P(DistanceFullPandaTest, DistancePoints)
   this->cenv_->distanceRobot(req, res, *this->robot_state_);
 
   // Checks a particular point is inside the box
-  auto checkInBox = [&](const Eigen::Vector3d& p) {
-    Eigen::Vector3d inBox = pos.inverse() * p;
+  auto check_in_box = [&](const Eigen::Vector3d& p) {
+    Eigen::Vector3d in_box = pos.inverse() * p;
 
-    constexpr double EPS = 1e-5;
-    EXPECT_LE(std::abs(inBox.x()), shape->size[0] + EPS);
-    EXPECT_LE(std::abs(inBox.y()), shape->size[1] + EPS);
-    EXPECT_LE(std::abs(inBox.z()), shape->size[2] + EPS);
+    constexpr double eps = 1e-5;
+    EXPECT_LE(std::abs(in_box.x()), shape->size[0] + eps);
+    EXPECT_LE(std::abs(in_box.y()), shape->size[1] + eps);
+    EXPECT_LE(std::abs(in_box.z()), shape->size[2] + eps);
   };
 
   // Check that all points reported on "box" are actually on the box and not
@@ -352,9 +353,9 @@ TYPED_TEST_P(DistanceFullPandaTest, DistancePoints)
     for (auto& pair : distance.second)
     {
       if (pair.link_names[0] == "box")
-        checkInBox(pair.nearest_points[0]);
+        check_in_box(pair.nearest_points[0]);
       else if (pair.link_names[1] == "box")
-        checkInBox(pair.nearest_points[1]);
+        check_in_box(pair.nearest_points[1]);
       else
         ADD_FAILURE() << "Unrecognized link names";
     }