Merge remote-tracking branch 'upstream/main'

Author: jeoseo

.clang-tidy

@@ -11,6 +11,7 @@ Checks:          '-*,
                   modernize-loop-convert,
                   modernize-make-shared,
                   modernize-make-unique,
+                  modernize-avoid-bind,
                   misc-unused-parameters,
                   readability-named-parameter,
                   readability-redundant-smartptr-get,

.docker/ci/Dockerfile

@@ -28,13 +28,11 @@ RUN \
         clang clang-format-12 clang-tidy clang-tools \
         ccache && \
     #
-    # Fetch all dependencies from moveit2.repos
-    # As of 5/2/2022, permissions need to be set explicitly.
-    # See https://github.com/actions/checkout/issues/760.
-    git config --global --add safe.directory /home/runner/work/moveit2/moveit2/.work/upstream_ws/src/geometric_shapes && \
-    git config --global --add safe.directory /home/runner/work/moveit2/moveit2/.work/upstream_ws/src/moveit_resources && \
-    git config --global --add safe.directory /home/runner/work/moveit2/moveit2/.work/upstream_ws/src/srdfdom && \
+    # Globally disable git security
+    # https://github.blog/2022-04-12-git-security-vulnerability-announced
+    git config --global --add safe.directory "*" && \
     #
+    # Fetch all dependencies from moveit2.repos
     vcs import src < src/moveit2/moveit2.repos && \
     if [ -r src/moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import src < src/moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \
     #

.docker/source/Dockerfile

@@ -19,14 +19,7 @@ COPY . src/moveit2
 RUN --mount=type=cache,target=/root/.ccache/ \
     # Enable ccache
     PATH=/usr/lib/ccache:$PATH && \
-    #
-    # Fetch all dependencies from moveit2.repos
-    # As of 5/2/2022, permissions need to be set explicitly.
-    # See https://github.com/actions/checkout/issues/760.
-    git config --global --add safe.directory /home/runner/work/moveit2/moveit2/.work/upstream_ws/src/geometric_shapes && \
-    git config --global --add safe.directory /home/runner/work/moveit2/moveit2/.work/upstream_ws/src/moveit_resources && \
-    git config --global --add safe.directory /home/runner/work/moveit2/moveit2/.work/upstream_ws/src/srdfdom && \
-    #
+    # Fetch required upstream sources for building
     vcs import src < src/moveit2/moveit2.repos && \
     if [ -r src/moveit2/moveit2_${ROS_DISTRO}.repos ] ; then vcs import src < src/moveit2/moveit2_${ROS_DISTRO}.repos ; fi && \
     #

MIGRATION.md

@@ -39,6 +39,8 @@ API changes in MoveIt releases
 - 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`).
 - `RobotState::attachBody()` now takes a unique_ptr instead of an owning raw pointer.
 - Moved the class `MoveItErrorCode` from both `moveit_ros_planning` and `moveit_ros_planning_interface` to `moveit_core`. The class now is in namespace `moveit::core`, access via `moveit::planning_interface` or `moveit_cpp::PlanningComponent` is deprecated.
+- End-effector markers in rviz are shown only if the eef's parent group is active _and_ the parent link is part of that group. Before, these conditions were _OR_-connected.
+  You might need to define additional end-effectors.
 
 ## ROS Melodic
 

moveit_commander/src/moveit_commander/move_group.py

@@ -603,7 +603,7 @@ def go(self, joints=None, wait=True):
         elif joints is not None:
             try:
                 self.set_joint_value_target(self.get_remembered_joint_values()[joints])
-            except TypeError:
+            except (KeyError, TypeError):
                 self.set_joint_value_target(joints)
         if wait:
             return self._g.move()

moveit_commander/test/python_moveit_commander.py

@@ -43,12 +43,21 @@
 import os
 
 from moveit_msgs.msg import RobotState
+from sensor_msgs.msg import JointState
 
 from moveit_commander import RobotCommander, PlanningSceneInterface
 
 
 class PythonMoveitCommanderTest(unittest.TestCase):
     PLANNING_GROUP = "manipulator"
+    JOINT_NAMES = [
+        "joint_1",
+        "joint_2",
+        "joint_3",
+        "joint_4",
+        "joint_5",
+        "joint_6",
+    ]
 
     @classmethod
     def setUpClass(self):
@@ -67,14 +76,7 @@ def test_enforce_bounds_empty_state(self):
     def test_enforce_bounds(self):
         in_msg = RobotState()
         in_msg.joint_state.header.frame_id = "base_link"
-        in_msg.joint_state.name = [
-            "joint_1",
-            "joint_2",
-            "joint_3",
-            "joint_4",
-            "joint_5",
-            "joint_6",
-        ]
+        in_msg.joint_state.name = self.JOINT_NAMES
         in_msg.joint_state.position = [0] * 6
         in_msg.joint_state.position[0] = 1000
 
@@ -87,14 +89,7 @@ def test_get_current_state(self):
         expected_state = RobotState()
         expected_state.joint_state.header.frame_id = "base_link"
         expected_state.multi_dof_joint_state.header.frame_id = "base_link"
-        expected_state.joint_state.name = [
-            "joint_1",
-            "joint_2",
-            "joint_3",
-            "joint_4",
-            "joint_5",
-            "joint_6",
-        ]
+        expected_state.joint_state.name = self.JOINT_NAMES
         expected_state.joint_state.position = [0] * 6
         self.assertEqual(self.group.get_current_state(), expected_state)
 
@@ -141,6 +136,25 @@ def test_validation(self):
         self.assertTrue(success3)
         self.assertTrue(self.group.execute(plan3))
 
+    def test_gogogo(self):
+        current_joints = np.asarray(self.group.get_current_joint_values())
+
+        self.group.set_joint_value_target(current_joints)
+        self.assertTrue(self.group.go(True))
+
+        self.assertTrue(self.group.go(current_joints))
+        self.assertTrue(self.group.go(list(current_joints)))
+        self.assertTrue(self.group.go(tuple(current_joints)))
+        self.assertTrue(
+            self.group.go(JointState(name=self.JOINT_NAMES, position=current_joints))
+        )
+
+        self.group.remember_joint_values("current")
+        self.assertTrue(self.group.go("current"))
+
+        current_pose = self.group.get_current_pose()
+        self.assertTrue(self.group.go(current_pose))
+
     def test_planning_scene_interface(self):
         planning_scene = PlanningSceneInterface()
 

moveit_configs_utils/moveit_configs_utils/moveit_configs_builder.py

@@ -240,8 +240,13 @@ class MoveItConfigsBuilder(ParameterBuilder):
     __config_dir_path = Path("config")
 
     # Look-up for robot_name_moveit_config package
-    def __init__(self, robot_name: str, robot_description="robot_description"):
-        super().__init__(robot_name + "_moveit_config")
+    def __init__(
+        self,
+        robot_name: str,
+        robot_description="robot_description",
+        package_name: Optional[str] = None,
+    ):
+        super().__init__(package_name or (robot_name + "_moveit_config"))
         self.__moveit_configs.package_path = self._package_path
         self.__robot_name = robot_name
         setup_assistant_file = self._package_path / ".setup_assistant"
@@ -479,7 +484,7 @@ def planning_pipelines(
         # Special rule to add ompl planner_configs
         if "ompl" in self.__moveit_configs.planning_pipelines:
             ompl_config = self.__moveit_configs.planning_pipelines["ompl"]
-            if "planner_configs" not in ompl_config.get("move_group", {}):
+            if "planner_configs" not in ompl_config:
                 ompl_config.update(load_yaml(default_folder / "ompl_defaults.yaml"))
 
         return self

moveit_core/CMakeLists.txt

@@ -230,7 +230,6 @@ if(BUILD_TESTING)
 
   # These don't pass yet, disable them for now
   set(ament_cmake_copyright_FOUND TRUE)
-  set(ament_cmake_cppcheck_FOUND TRUE)
   set(ament_cmake_cpplint_FOUND TRUE)
   set(ament_cmake_flake8_FOUND TRUE)
   set(ament_cmake_uncrustify_FOUND TRUE)

moveit_core/collision_detection/src/collision_matrix.cpp

@@ -295,7 +295,7 @@ bool AllowedCollisionMatrix::getAllowedCollision(const std::string& name1, const
     else if (!found1 && found2)
       fn = fn2;
     else if (found1 && found2)
-      fn = std::bind(&andDecideContact, fn1, fn2, std::placeholders::_1);
+      fn = [fn1, fn2](Contact& contact) { return andDecideContact(fn1, fn2, contact); };
     else
       return false;
   }
@@ -344,10 +344,14 @@ void AllowedCollisionMatrix::getAllEntryNames(std::vector<std::string>& names) c
 {
   names.clear();
   for (const auto& entry : entries_)
-    if (!names.empty() && names.back() == entry.first)
-      continue;
-    else
-      names.push_back(entry.first);
+    names.push_back(entry.first);
+
+  for (const auto& item : default_entries_)
+  {
+    auto it = std::lower_bound(names.begin(), names.end(), item.first);
+    if (it != names.end() && *it != item.first)
+      names.insert(it, item.first);
+  }
 }
 
 void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix& msg) const
@@ -358,7 +362,6 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix
   msg.default_entry_values.clear();
 
   getAllEntryNames(msg.entry_names);
-  std::sort(msg.entry_names.begin(), msg.entry_names.end());
 
   msg.entry_values.resize(msg.entry_names.size());
   for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
@@ -389,7 +392,6 @@ void AllowedCollisionMatrix::print(std::ostream& out) const
 {
   std::vector<std::string> names;
   getAllEntryNames(names);
-  std::sort(names.begin(), names.end());
 
   std::size_t spacing = 4;
   for (auto& name : names)

moveit_core/collision_detection/src/collision_octomap_filter.cpp

@@ -247,11 +247,8 @@ bool sampleCloud(const octomap::point3d_list& cloud, const double& spacing, cons
   double a = 0, b = 0, c = 0, r2 = 0, r4 = 0, r6 = 0, a1 = 0, b1 = 0, c1 = 0, a2 = 0, b2 = 0, c2 = 0;
   bool wyvill = true;
 
-  octomap::point3d_list::const_iterator it;
-  for (it = cloud.begin(); it != cloud.end(); ++it)
+  for (const octomath::Vector3& v : cloud)
   {
-    octomath::Vector3 v = (*it);
-
     if (wyvill)
     {
       r2 = r * r;

moveit_core/collision_detection/src/world_diff.cpp

@@ -53,7 +53,7 @@ WorldDiff::WorldDiff()
 WorldDiff::WorldDiff(const WorldPtr& world) : world_(world)
 {
   observer_handle_ =
-      world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2));
+      world->addObserver([this](const World::ObjectConstPtr& object, World::Action action) { notify(object, action); });
 }
 
 WorldDiff::WorldDiff(WorldDiff& other)
@@ -64,8 +64,8 @@ WorldDiff::WorldDiff(WorldDiff& other)
     changes_ = other.changes_;
 
     WorldWeakPtr(world).swap(world_);
-    observer_handle_ =
-        world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2));
+    observer_handle_ = world->addObserver(
+        [this](const World::ObjectConstPtr& object, World::Action action) { notify(object, action); });
   }
 }
 
@@ -90,7 +90,7 @@ void WorldDiff::reset(const WorldPtr& world)
 
   WorldWeakPtr(world).swap(world_);
   observer_handle_ =
-      world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2));
+      world->addObserver([this](const World::ObjectConstPtr& object, World::Action action) { notify(object, action); });
 }
 
 void WorldDiff::setWorld(const WorldPtr& world)
@@ -105,7 +105,7 @@ void WorldDiff::setWorld(const WorldPtr& world)
   WorldWeakPtr(world).swap(world_);
 
   observer_handle_ =
-      world->addObserver(std::bind(&WorldDiff::notify, this, std::placeholders::_1, std::placeholders::_2));
+      world->addObserver([this](const World::ObjectConstPtr& object, World::Action action) { notify(object, action); });
   world->notifyObserverAllObjects(observer_handle_, World::CREATE | World::ADD_SHAPE);
 }
 

moveit_core/collision_detection/test/test_world.cpp

@@ -222,12 +222,12 @@ struct TestAction
 };
 
 /* notification callback */
-static void TrackChangesNotify(TestAction* ta, const collision_detection::World::ObjectConstPtr& obj,
+static void TrackChangesNotify(TestAction& ta, const collision_detection::World::ObjectConstPtr& obj,
                                collision_detection::World::Action action)
 {
-  ta->obj_ = *obj;
-  ta->action_ = action;
-  ta->cnt_++;
+  ta.obj_ = *obj;
+  ta.action_ = action;
+  ta.cnt_++;
 }
 
 TEST(World, TrackChanges)
@@ -236,7 +236,10 @@ TEST(World, TrackChanges)
 
   TestAction ta;
   collision_detection::World::ObserverHandle observer_ta;
-  observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2));
+  observer_ta = world.addObserver(
+      [&ta](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) {
+        return TrackChangesNotify(ta, object, action);
+      });
 
   // Create some shapes
   shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);
@@ -267,7 +270,10 @@ TEST(World, TrackChanges)
 
   TestAction ta2;
   collision_detection::World::ObserverHandle observer_ta2;
-  observer_ta2 = world.addObserver(std::bind(TrackChangesNotify, &ta2, std::placeholders::_1, std::placeholders::_2));
+  observer_ta2 = world.addObserver(
+      [&ta2](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) {
+        return TrackChangesNotify(ta2, object, action);
+      });
 
   world.addToObject("obj2", cyl, Eigen::Isometry3d::Identity());
 
@@ -305,7 +311,10 @@ TEST(World, TrackChanges)
 
   TestAction ta3;
   collision_detection::World::ObserverHandle observer_ta3;
-  observer_ta3 = world.addObserver(std::bind(TrackChangesNotify, &ta3, std::placeholders::_1, std::placeholders::_2));
+  observer_ta3 = world.addObserver(
+      [&ta3](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) {
+        return TrackChangesNotify(ta3, object, action);
+      });
 
   bool rm_good = world.removeShapeFromObject("obj2", cyl);
   EXPECT_TRUE(rm_good);
@@ -371,7 +380,10 @@ TEST(World, ObjectPoseAndSubframes)
 
   TestAction ta;
   collision_detection::World::ObserverHandle observer_ta;
-  observer_ta = world.addObserver(std::bind(TrackChangesNotify, &ta, std::placeholders::_1, std::placeholders::_2));
+  observer_ta = world.addObserver(
+      [&ta](const collision_detection::World::ObjectConstPtr& object, collision_detection::World::Action action) {
+        return TrackChangesNotify(ta, object, action);
+      });
 
   // Create shapes
   shapes::ShapePtr ball = std::make_shared<shapes::Sphere>(1.0);

moveit_core/collision_detection_bullet/src/collision_env_bullet.cpp

@@ -52,7 +52,7 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m
 {
   // request notifications about changes to new world
   observer_handle_ = getWorld()->addObserver(
-      std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
+      [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
 
   for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : robot_model_->getURDF()->links_)
   {
@@ -66,7 +66,7 @@ CollisionEnvBullet::CollisionEnvBullet(const moveit::core::RobotModelConstPtr& m
 {
   // request notifications about changes to new world
   observer_handle_ = getWorld()->addObserver(
-      std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
+      [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
 
   for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : robot_model_->getURDF()->links_)
   {
@@ -81,7 +81,7 @@ CollisionEnvBullet::CollisionEnvBullet(const CollisionEnvBullet& other, const Wo
 {
   // request notifications about changes to new world
   observer_handle_ = getWorld()->addObserver(
-      std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
+      [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
 
   for (const std::pair<const std::string, urdf::LinkSharedPtr>& link : other.robot_model_->getURDF()->links_)
   {
@@ -305,7 +305,7 @@ void CollisionEnvBullet::setWorld(const WorldPtr& world)
 
   // request notifications about changes to new world
   observer_handle_ = getWorld()->addObserver(
-      std::bind(&CollisionEnvBullet::notifyObjectChange, this, std::placeholders::_1, std::placeholders::_2));
+      [this](const World::ObjectConstPtr& object, World::Action action) { notifyObjectChange(object, action); });
 
   // get notifications any objects already in the new world
   getWorld()->notifyObserverAllObjects(observer_handle_, World::CREATE);