Merge branch 'main' into computeCartesianPath

.codespell_words

@@ -1,19 +1,19 @@
+aas
 ang
 ans
 atleast
 ba
+brin
 dof
 dur
 iff
 keyserver
 nto
 ot
+padd
 parameterizes
 planed
+sinic
 tork
 uint
 whis
-sinic
-padd
-brin
-aas

.docker/tutorial-source/Dockerfile

@@ -1,12 +1,16 @@
 # syntax = docker/dockerfile:1.3
 
 # ghcr.io/moveit/moveit2:main-${ROS_DISTRO}-tutorial-source
-# Source build of the repos file from the tutorail site
+# Source build of the repos file from the tutorial site
 
 ARG ROS_DISTRO=rolling
+ARG GZ_VERSION=harmonic
+
 FROM moveit/moveit2:${ROS_DISTRO}-ci
 LABEL maintainer Tyler Weaver [email protected]
 
+ARG GZ_VERSION
+
 # Export ROS_UNDERLAY for downstream docker containers
 ENV ROS_UNDERLAY /root/ws_moveit/install
 WORKDIR $ROS_UNDERLAY/..
@@ -17,6 +21,11 @@ COPY . src/moveit2
 # Commands are combined in single RUN statement with "apt/lists" folder removal to reduce image size
 # https://docs.docker.com/develop/develop-images/dockerfile_best-practices/#minimize-the-number-of-layers
 RUN --mount=type=cache,target=/root/.ccache/,sharing=locked \
+        # Install Gazebo, which is needed by some dependencies.
+        sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable `lsb_release -cs` main" > /etc/apt/sources.list.d/gazebo-stable.list' && \
+        wget http://packages.osrfoundation.org/gazebo.key -O - | sudo apt-key add - && \
+        sudo apt update && \
+        sudo apt-get install -y "gz-${GZ_VERSION}" && \
         # Enable ccache
         PATH=/usr/lib/ccache:$PATH && \
         # Checkout the tutorial repo

.github/workflows/sonar.yaml

@@ -110,7 +110,7 @@ jobs:
         run: cat ${{ github.workspace }}/.work/target_ws/coverage.info
       - name: Install sonar-scanner
         if: steps.ici.outputs.target_test_results == '0'
-        uses: SonarSource/sonarcloud-github-c-cpp@v2
+        uses: SonarSource/sonarcloud-github-c-cpp@v3
       - name: Run sonar-scanner
         if: steps.ici.outputs.target_test_results == '0'
         env:

.github/workflows/tutorial_docker.yaml

@@ -16,7 +16,7 @@ jobs:
     strategy:
       fail-fast: false
       matrix:
-        ROS_DISTRO: [rolling, humble, iron]
+        ROS_DISTRO: [rolling, humble, iron, jazzy]
     runs-on: ubuntu-latest
     permissions:
       packages: write

.pre-commit-config.yaml

@@ -47,7 +47,7 @@ repos:
         files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$
         args: ['-fallback-style=none', '-i']
   - repo: https://github.com/codespell-project/codespell
-    rev: v2.2.6
+    rev: v2.3.0
     hooks:
       - id: codespell
         args: ['--write-changes', '--ignore-words=.codespell_words', '--skip="*.eps"']

README.md

@@ -1,4 +1,4 @@
-<img src="https://moveit.ros.org/assets/logo/moveit_logo-black.png" alt="MoveIt Logo" width="200"/>
+<img src="http://moveit.ros.org/assets/logo/moveit_logo-black.png" alt="MoveIt Logo" width="200"/>
 
 The [MoveIt Motion Planning Framework for ROS 2](http://moveit.ros.org). For the ROS 1 repository see [MoveIt 1](https://github.com/moveit/moveit). For the commercially supported version see [MoveIt Pro](http://picknik.ai/pro).
 

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

@@ -155,11 +155,24 @@ TYPED_TEST_P(CollisionDetectorPandaTest, RobotWorldCollision_1)
   ASSERT_TRUE(res.collision);
   res.clear();
 
+  pos1.translation().z() = 0.25;
+  this->cenv_->getWorld()->moveObject("box", pos1);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
+  ASSERT_FALSE(res.collision);
+  res.clear();
+
+  pos1.translation().z() = 0.05;
   this->cenv_->getWorld()->moveObject("box", pos1);
   this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
   ASSERT_TRUE(res.collision);
   res.clear();
 
+  pos1.translation().z() = 0.25;
+  this->cenv_->getWorld()->moveObject("box", pos1);
+  this->cenv_->checkRobotCollision(req, res, *this->robot_state_, *this->acm_);
+  ASSERT_FALSE(res.collision);
+  res.clear();
+
   this->cenv_->getWorld()->moveObject("box", pos1);
   ASSERT_FALSE(res.collision);
 }

moveit_core/collision_detection/include/moveit/collision_detection/world.h

@@ -217,6 +217,9 @@ class World
   bool moveShapeInObject(const std::string& object_id, const shapes::ShapeConstPtr& shape,
                          const Eigen::Isometry3d& shape_pose);
 
+  /** \brief Update the pose of all shapes in an object. Shape size is verified. Returns true on success. */
+  bool moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses);
+
   /** \brief Move the object pose (thus moving all shapes and subframes in the object)
    * according to the given transform specified in world frame.
    * The transform is relative to and changes the object pose. It does not replace it.

moveit_core/collision_detection/src/world.cpp

@@ -259,6 +259,26 @@ bool World::moveShapeInObject(const std::string& object_id, const shapes::ShapeC
   return false;
 }
 
+bool World::moveShapesInObject(const std::string& object_id, const EigenSTL::vector_Isometry3d& shape_poses)
+{
+  auto it = objects_.find(object_id);
+  if (it != objects_.end())
+  {
+    if (shape_poses.size() == it->second->shapes_.size())
+    {
+      for (std::size_t i = 0; i < shape_poses.size(); ++i)
+      {
+        ASSERT_ISOMETRY(shape_poses[i])  // unsanitized input, could contain a non-isometry
+        it->second->shape_poses_[i] = shape_poses[i];
+        it->second->global_shape_poses_[i] = it->second->pose_ * shape_poses[i];
+      }
+      notify(it->second, MOVE_SHAPE);
+      return true;
+    }
+  }
+  return false;
+}
+
 bool World::moveObject(const std::string& object_id, const Eigen::Isometry3d& transform)
 {
   const auto it = objects_.find(object_id);

moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp

@@ -450,6 +450,38 @@ void CollisionEnvFCL::notifyObjectChange(const ObjectConstPtr& obj, World::Actio
     }
     cleanCollisionGeometryCache();
   }
+  else if (action == World::MOVE_SHAPE)
+  {
+    auto it = fcl_objs_.find(obj->id_);
+    if (it == fcl_objs_.end())
+    {
+      RCLCPP_ERROR(getLogger(), "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str());
+      return;
+    }
+
+    if (obj->global_shape_poses_.size() != it->second.collision_objects_.size())
+    {
+      RCLCPP_ERROR(getLogger(),
+                   "Cannot move shapes, shape size mismatch between FCL object and world object: '%s'. Respectively "
+                   "%zu and %zu.",
+                   obj->id_.c_str(), it->second.collision_objects_.size(), it->second.collision_objects_.size());
+      return;
+    }
+
+    for (std::size_t i = 0; i < it->second.collision_objects_.size(); ++i)
+    {
+      it->second.collision_objects_[i]->setTransform(transform2fcl(obj->global_shape_poses_[i]));
+
+      // compute AABB, order matters
+      it->second.collision_geometry_[i]->collision_geometry_->computeLocalAABB();
+      it->second.collision_objects_[i]->computeAABB();
+    }
+
+    // update AABB in the FCL broadphase manager tree
+    // see https://github.com/moveit/moveit/pull/3601 for benchmarks
+    it->second.unregisterFrom(manager_.get());
+    it->second.registerTo(manager_.get());
+  }
   else
   {
     updateFCLObject(obj->id_);

moveit_core/distance_field/include/moveit/distance_field/find_internal_points.h

@@ -48,7 +48,7 @@ namespace distance_field
  *
  * @param [in] body The body to discretize
  * @param [in] resolution The resolution at which to test
- * @param [out] points The points internal to the body are appended to thiss
+ * @param [out] points The points internal to the body are appended to this
  *                   vector.
  */
 void findInternalPointsConvex(const bodies::Body& body, double resolution, EigenSTL::vector_Vector3d& points);

moveit_core/online_signal_smoothing/include/moveit/online_signal_smoothing/butterworth_filter.h

@@ -60,7 +60,7 @@ namespace online_signal_smoothing
  * It comes from finding the bilinear transform equivalent of the analog transfer function and
  * further applying the inverse z-transform.
  * The parameter "low_pass_filter_coeff" equals (2*pi / tan(omega_d * T))
- * where omega_d is the cutoff frequency and T is the samping period in sec.
+ * where omega_d is the cutoff frequency and T is the sampling period in sec.
  */
 class ButterworthFilter
 {
@@ -69,7 +69,7 @@ class ButterworthFilter
    * Constructor.
    * @param low_pass_filter_coeff Larger filter_coeff-> more smoothing of commands, but more lag.
    * low_pass_filter_coeff = (2*pi / tan(omega_d * T))
-   * where omega_d is the cutoff frequency and T is the samping period in sec.
+   * where omega_d is the cutoff frequency and T is the sampling period in sec.
    */
   ButterworthFilter(double low_pass_filter_coeff);
   ButterworthFilter() = delete;

moveit_core/planning_scene/src/planning_scene.cpp

@@ -210,10 +210,14 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare
 
   robot_model_ = parent_->robot_model_;
 
+  setStateFeasibilityPredicate(parent->getStateFeasibilityPredicate());
+  setMotionFeasibilityPredicate(parent->getMotionFeasibilityPredicate());
+
   // maintain a separate world.  Copy on write ensures that most of the object
   // info is shared until it is modified.
   world_ = std::make_shared<collision_detection::World>(*parent_->world_);
   world_const_ = world_;
+  setCollisionObjectUpdateCallback(parent_->current_world_object_update_callback_);
 
   // record changes to the world
   world_diff_ = std::make_shared<collision_detection::WorldDiff>(world_);
@@ -650,7 +654,14 @@ void PlanningScene::getPlanningSceneDiffMsg(moveit_msgs::msg::PlanningScene& sce
     {
       if (it.first == OCTOMAP_NS)
       {
-        do_omap = true;
+        if (it.second == collision_detection::World::DESTROY)
+        {
+          scene_msg.world.octomap.octomap.id = "cleared";  // indicate cleared octomap
+        }
+        else
+        {
+          do_omap = true;
+        }
       }
       else if (it.second == collision_detection::World::DESTROY)
       {
@@ -1203,7 +1214,6 @@ void PlanningScene::decoupleParent()
         (object_types_.value())[it->first] = it->second;
     }
   }
-
   parent_.reset();
 }
 
@@ -1254,7 +1264,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::msg::PlanningScen
     result &= processCollisionObjectMsg(collision_object);
 
   // if an octomap was specified, replace the one we have with that one
-  if (!scene_msg.world.octomap.octomap.data.empty())
+  if (!scene_msg.world.octomap.octomap.id.empty())
     processOctomapMsg(scene_msg.world.octomap);
 
   return result;
@@ -1717,16 +1727,16 @@ bool PlanningScene::shapesAndPosesFromCollisionObjectMessage(const moveit_msgs::
   shapes.reserve(num_shapes);
   shape_poses.reserve(num_shapes);
 
-  utilities::poseMsgToEigen(object.pose, object_pose);
-
   bool switch_object_pose_and_shape_pose = false;
-  if (num_shapes == 1)
+  if (num_shapes == 1 && moveit::core::isEmpty(object.pose))
   {
-    if (moveit::core::isEmpty(object.pose))
-    {
-      switch_object_pose_and_shape_pose = true;  // If the object pose is not set but the shape pose is,
-                                                 // use the shape's pose as the object pose.
-    }
+    // If the object pose is not set but the shape pose is, use the shape's pose as the object pose.
+    switch_object_pose_and_shape_pose = true;
+    object_pose.setIdentity();
+  }
+  else
+  {
+    utilities::poseMsgToEigen(object.pose, object_pose);
   }
 
   auto append = [&object_pose, &shapes, &shape_poses,
@@ -1848,6 +1858,7 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision
 {
   if (world_->hasObject(object.id))
   {
+    // update object pose
     if (!object.primitives.empty() || !object.meshes.empty() || !object.planes.empty())
     {
       RCLCPP_WARN(getLogger(), "Move operation for object '%s' ignores the geometry specified in the message.",
@@ -1861,6 +1872,47 @@ bool PlanningScene::processCollisionObjectMove(const moveit_msgs::msg::Collision
 
     const Eigen::Isometry3d object_frame_transform = world_to_object_header_transform * header_to_pose_transform;
     world_->setObjectPose(object.id, object_frame_transform);
+
+    // update shape poses
+    if (!object.primitive_poses.empty() || !object.mesh_poses.empty() || !object.plane_poses.empty())
+    {
+      auto world_object = world_->getObject(object.id);  // object exists, checked earlier
+
+      std::size_t shape_size = object.primitive_poses.size() + object.mesh_poses.size() + object.plane_poses.size();
+      if (shape_size != world_object->shape_poses_.size())
+      {
+        RCLCPP_ERROR(getLogger(),
+                     "Move operation for object '%s' must have same number of geometry poses. Cannot move.",
+                     object.id.c_str());
+        return false;
+      }
+
+      // order matters -> primitive, mesh and plane
+      EigenSTL::vector_Isometry3d shape_poses;
+      for (const auto& shape_pose : object.primitive_poses)
+      {
+        shape_poses.emplace_back();
+        utilities::poseMsgToEigen(shape_pose, shape_poses.back());
+      }
+      for (const auto& shape_pose : object.mesh_poses)
+      {
+        shape_poses.emplace_back();
+        utilities::poseMsgToEigen(shape_pose, shape_poses.back());
+      }
+      for (const auto& shape_pose : object.plane_poses)
+      {
+        shape_poses.emplace_back();
+        utilities::poseMsgToEigen(shape_pose, shape_poses.back());
+      }
+
+      if (!world_->moveShapesInObject(object.id, shape_poses))
+      {
+        RCLCPP_ERROR(getLogger(), "Move operation for object '%s' internal world error. Cannot move.",
+                     object.id.c_str());
+        return false;
+      }
+    }
+
     return true;
   }