Imported upstream version '1.1.15' of 'upstream'

ros-gbp · Sep 9, 2024 · fc53b67 · fc53b67
1 parent d4c0886
commit fc53b67
Show file tree

Hide file tree

Showing 105 changed files with 806 additions and 346 deletions.
diff --git a/.github/workflows/docker.yaml b/.github/workflows/docker.yaml
@@ -46,7 +46,7 @@ jobs:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       - name: Build and Push
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
         with:
           file: .docker/${{ github.job }}/Dockerfile
@@ -90,7 +90,7 @@ jobs:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       - name: Build and Push
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
         with:
           file: .docker/${{ github.job }}/Dockerfile
@@ -136,7 +136,7 @@ jobs:
           username: ${{ secrets.DOCKERHUB_USERNAME }}
           password: ${{ secrets.DOCKERHUB_TOKEN }}
       - name: Build and Push
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         if: ${{ github.event_name == 'workflow_dispatch' || github.event_name != 'schedule' || steps.apt.outputs.no_cache }}
         with:
           file: .docker/${{ github.job }}/Dockerfile
@@ -174,7 +174,7 @@ jobs:
       - name: "Remove .dockerignore"
         run: rm .dockerignore  # enforce full source context
       - name: Build and Push
-        uses: docker/build-push-action@v5
+        uses: docker/build-push-action@v6
         with:
           context: .
           file: .docker/${{ github.job }}/Dockerfile

diff --git a/README.md b/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](http://moveit.ros.org). For the ROS 2 repository see [MoveIt 2](https://github.com/moveit/moveit2). For the commercially supported version see [MoveIt Pro](http://picknik.ai/pro).
 

diff --git a/moveit/CHANGELOG.rst b/moveit/CHANGELOG.rst
@@ -2,6 +2,9 @@
 Changelog for package moveit
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.15 (2024-09-09)
+-------------------
+
 1.1.14 (2024-05-27)
 -------------------
 

diff --git a/moveit/package.xml b/moveit/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="2">
   <name>moveit</name>
-  <version>1.1.14</version>
+  <version>1.1.15</version>
   <description>Meta package that contains all essential package of MoveIt. Until Summer 2016 MoveIt had been developed over multiple repositories, where developers' usability and maintenance effort was non-trivial. See <a href = "http://discourse.ros.org/t/migration-to-one-github-repo-for-moveit/266/34">the detailed discussion for the merge of several repositories</a>.</description>
   <maintainer email="[email protected]">Dave Coleman</maintainer>
   <maintainer email="[email protected]">Michael Ferguson</maintainer>

diff --git a/moveit_commander/CHANGELOG.rst b/moveit_commander/CHANGELOG.rst
@@ -2,6 +2,11 @@
 Changelog for package moveit_commander
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.15 (2024-09-09)
+-------------------
+* New implementation for computeCartesianPath (`#3618 <https://github.com/ros-planning/moveit/issues/3618>`_)
+* Contributors: Robert Haschke
+
 1.1.14 (2024-05-27)
 -------------------
 * Fixed order of return values in doc string of compute_cartesian_path() (`#3574 <https://github.com/ros-planning/moveit/issues/3574>`_)

diff --git a/moveit_commander/demos/plan_with_constraints.py b/moveit_commander/demos/plan_with_constraints.py
@@ -71,5 +71,5 @@
     wpose.position.y += 0.05
     waypoints.append(wpose)
 
-    plan, fraction = a.compute_cartesian_path(waypoints, 0.01, 0.0, path_constraints=c)
+    plan, fraction = a.compute_cartesian_path(waypoints, 0.01, True, path_constraints=c)
     print("Plan success percent: ", fraction)
diff --git a/moveit_commander/package.xml b/moveit_commander/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_commander</name>
-  <version>1.1.14</version>
+  <version>1.1.15</version>
   <description>Python interfaces to MoveIt</description>
 
   <author email="[email protected]">Ioan Sucan</author>

diff --git a/moveit_commander/src/moveit_commander/move_group.py b/moveit_commander/src/moveit_commander/move_group.py
@@ -648,7 +648,6 @@ def compute_cartesian_path(
         self,
         waypoints,
         eef_step,
-        jump_threshold,
         avoid_collisions=True,
         path_constraints=None,
     ):
@@ -670,15 +669,13 @@ def compute_cartesian_path(
             (ser_path, fraction) = self._g.compute_cartesian_path(
                 [conversions.pose_to_list(p) for p in waypoints],
                 eef_step,
-                jump_threshold,
                 avoid_collisions,
                 constraints_str,
             )
         else:
             (ser_path, fraction) = self._g.compute_cartesian_path(
                 [conversions.pose_to_list(p) for p in waypoints],
                 eef_step,
-                jump_threshold,
                 avoid_collisions,
             )
 

diff --git a/moveit_commander/test/python_time_parameterization.py b/moveit_commander/test/python_time_parameterization.py
@@ -60,7 +60,7 @@ def plan(self):
         goal_pose = self.group.get_current_pose().pose
         goal_pose.position.z -= 0.1
         (plan, fraction) = self.group.compute_cartesian_path(
-            [start_pose, goal_pose], 0.005, 0.0
+            [start_pose, goal_pose], 0.005
         )
         self.assertEqual(fraction, 1.0, "Cartesian path plan failed")
         return plan

diff --git a/moveit_core/CHANGELOG.rst b/moveit_core/CHANGELOG.rst
@@ -2,6 +2,15 @@
 Changelog for package moveit_core
 ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 
+1.1.15 (2024-09-09)
+-------------------
+* Disable ruckig's webclient support in MoveIt build (`#3636 <https://github.com/ros-planning/moveit/issues/3636>`_)
+* New implementation for computeCartesianPath (`#3618 <https://github.com/ros-planning/moveit/issues/3618>`_)
+* Optimize MOVE_SHAPE operations for FCL (`#3601 <https://github.com/ros-planning/moveit/issues/3601>`_)
+* Provide violated bounds for a JointConstraint
+* PSM: Correctly handle full planning scene message (`#3610 <https://github.com/ros-planning/moveit/issues/3610>`_)
+* Contributors: Captain Yoshi, Michael Görner, Robert Haschke
+
 1.1.14 (2024-05-27)
 -------------------
 * Allow moving of all shapes of an object in one go (`#3599 <https://github.com/ros-planning/moveit/issues/3599>`_)

diff --git a/moveit_core/CMakeLists.txt b/moveit_core/CMakeLists.txt
@@ -50,6 +50,13 @@ find_package(ruckig REQUIRED)
 # in include_directories below because the target is correctly imported here.
 get_target_property(ruckig_INCLUDE_DIRS ruckig::ruckig INTERFACE_INCLUDE_DIRECTORIES)
 set(ruckig_LIBRARIES "ruckig::ruckig")
+# Remove the WITH_CLOUD_CLIENT definition because we don't use it and it causes build issues in some configurations
+# See https://github.com/moveit/moveit/pull/3636
+get_target_property(ruckig_defs ruckig::ruckig INTERFACE_COMPILE_DEFINITIONS)
+if(ruckig_defs)
+  list(REMOVE_ITEM ruckig_defs "WITH_CLOUD_CLIENT")
+  set_target_properties(ruckig::ruckig PROPERTIES INTERFACE_COMPILE_DEFINITIONS "${ruckig_defs}")
+endif()
 
 find_package(urdfdom REQUIRED)
 find_package(urdfdom_headers REQUIRED)

diff --git a/...core/collision_detection/include/moveit/collision_detection/test_collision_common_panda.h b/...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);
 }

diff --git a/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp b/moveit_core/collision_detection_fcl/src/collision_env_fcl.cpp
@@ -434,6 +434,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())
+    {
+      ROS_ERROR_NAMED(LOGNAME, "Cannot move shapes of unknown FCL object: '%s'", obj->id_.c_str());
+      return;
+    }
+
+    if (obj->global_shape_poses_.size() != it->second.collision_objects_.size())
+    {
+      ROS_ERROR_NAMED(LOGNAME,
+                      "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_);

diff --git a/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp b/moveit_core/kinematic_constraints/src/kinematic_constraint.cpp
@@ -221,18 +221,18 @@ bool JointConstraint::configure(const moveit_msgs::JointConstraint& jc)
         joint_position_ = bounds.min_position_;
         joint_tolerance_above_ = std::numeric_limits<double>::epsilon();
         ROS_WARN_NAMED("kinematic_constraints",
-                       "Joint %s is constrained to be below the minimum bounds. "
+                       "Joint %s is constrained to be below the minimum bound %g. "
                        "Assuming minimum bounds instead.",
-                       jc.joint_name.c_str());
+                       jc.joint_name.c_str(), bounds.min_position_);
       }
       else if (bounds.max_position_ < joint_position_ - joint_tolerance_below_)
       {
         joint_position_ = bounds.max_position_;
         joint_tolerance_below_ = std::numeric_limits<double>::epsilon();
         ROS_WARN_NAMED("kinematic_constraints",
-                       "Joint %s is constrained to be above the maximum bounds. "
+                       "Joint %s is constrained to be above the maximum bounds %g. "
                        "Assuming maximum bounds instead.",
-                       jc.joint_name.c_str());
+                       jc.joint_name.c_str(), bounds.max_position_);
       }
     }
 

diff --git a/moveit_core/package.xml b/moveit_core/package.xml
@@ -1,7 +1,7 @@
 <?xml version="1.0"?>
 <package format="3">
   <name>moveit_core</name>
-  <version>1.1.14</version>
+  <version>1.1.15</version>
   <description>Core libraries used by MoveIt</description>
   <author email="[email protected]">Ioan Sucan</author>
   <author email="[email protected]">Sachin Chitta</author>

diff --git a/moveit_core/planning_scene/src/planning_scene.cpp b/moveit_core/planning_scene/src/planning_scene.cpp
@@ -1318,6 +1318,7 @@ bool PlanningScene::setPlanningSceneDiffMsg(const moveit_msgs::PlanningScene& sc
 
 bool PlanningScene::setPlanningSceneMsg(const moveit_msgs::PlanningScene& scene_msg)
 {
+  assert(scene_msg.is_diff == false);
   ROS_DEBUG_NAMED(LOGNAME, "Setting new planning scene: '%s'", scene_msg.name.c_str());
   name_ = scene_msg.name;