Merge branch 'main' into pr/cleanup-col-distance-field-test

moveit_core/collision_detection_bullet/src/collision_detector_bullet_plugin_loader.cpp

@@ -41,7 +41,7 @@ namespace collision_detection
 {
 bool CollisionDetectorBtPluginLoader::initialize(const planning_scene::PlanningScenePtr& scene, bool exclusive) const
 {
-  scene->setActiveCollisionDetector(CollisionDetectorAllocatorBullet::create(), exclusive);
+  scene->allocateCollisionDetector(CollisionDetectorAllocatorBullet::create(), exclusive);
   return true;
 }
 }  // namespace collision_detection

moveit_core/collision_detection_fcl/src/collision_detector_fcl_plugin_loader.cpp

@@ -41,7 +41,7 @@ namespace collision_detection
 {
 bool CollisionDetectorFCLPluginLoader::initialize(const planning_scene::PlanningScenePtr& scene) const
 {
-  scene->setCollisionDetectorType(CollisionDetectorAllocatorFCL::create());
+  scene->allocateCollisionDetector(CollisionDetectorAllocatorFCL::create());
   return true;
 }
 }  // namespace collision_detection

moveit_core/planning_scene/include/moveit/planning_scene/planning_scene.h

@@ -912,7 +912,7 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
   /** \brief Clone a planning scene. Even if the scene \e scene depends on a parent, the cloned scene will not. */
   static PlanningScenePtr clone(const PlanningSceneConstPtr& scene);
 
-  /** \brief Replace previous collision detector with a new collision detector type (or create new, if none previously).
+  /** \brief Allocate a new collision detector and replace the previous one if there was any.
    *
    * The collision detector type is specified with (a shared pointer to) an
    * allocator which is a subclass of CollisionDetectorAllocator.  This
@@ -922,10 +922,13 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
    * A new PlanningScene uses an FCL collision detector by default.
    *
    * example: to add FCL collision detection (normally not necessary) call
-   *   planning_scene->setCollisionDetectorType(collision_detection::CollisionDetectorAllocatorFCL::create());
+   *   planning_scene->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
    *
    * */
-  void setCollisionDetectorType(const collision_detection::CollisionDetectorAllocatorPtr& allocator);
+  void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator)
+  {
+    allocateCollisionDetector(allocator, nullptr /* no parent_detector */);
+  }
 
 private:
   /* Private constructor used by the diff() methods. */
@@ -949,6 +952,10 @@ class PlanningScene : private boost::noncopyable, public std::enable_shared_from
 
   MOVEIT_STRUCT_FORWARD(CollisionDetector)
 
+  /* Construct a new CollisionDector from allocator, copy-construct environments from parent_detector if not null */
+  void allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
+                                 const CollisionDetectorPtr& parent_detector);
+
   /* \brief A set of compatible collision detectors */
   struct CollisionDetector
   {

moveit_core/planning_scene/src/planning_scene.cpp

@@ -161,7 +161,7 @@ void PlanningScene::initialize()
   for (const srdf::Model::DisabledCollision& it : dc)
     acm_->setEntry(it.link1_, it.link2_, true);
 
-  setCollisionDetectorType(collision_detection::CollisionDetectorAllocatorFCL::create());
+  allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
 }
 
 /* return NULL on failure */
@@ -193,7 +193,7 @@ PlanningScene::PlanningScene(const PlanningSceneConstPtr& parent) : parent_(pare
   // record changes to the world
   world_diff_.reset(new collision_detection::WorldDiff(world_));
 
-  setCollisionDetectorType(parent_->collision_detector_->alloc_);
+  allocateCollisionDetector(parent_->collision_detector_->alloc_, parent_->collision_detector_);
   collision_detector_->copyPadding(*parent_->collision_detector_);
 }
 
@@ -223,36 +223,37 @@ void PlanningScene::CollisionDetector::copyPadding(const PlanningScene::Collisio
   cenv_->setLinkScale(src.getCollisionEnv()->getLinkScale());
 }
 
-void PlanningScene::setCollisionDetectorType(const collision_detection::CollisionDetectorAllocatorPtr& allocator)
+void PlanningScene::allocateCollisionDetector(const collision_detection::CollisionDetectorAllocatorPtr& allocator,
+                                              const CollisionDetectorPtr& parent_detector)
 {
-  // Temporary copy of the previous (if any), to copy padding
-  CollisionDetector prev_coll_detector;
-  bool have_previous_coll_detector = false;
-  if (collision_detector_)
-  {
-    have_previous_coll_detector = true;
-    prev_coll_detector = *collision_detector_;
-  }
-
-  // TODO(andyz): uncomment this for a small speed boost when another collision detector type is available in MoveIt2
-  // For now, it is useful in the switchCollisionDetectorType() unit test
-  //  const std::string& name = allocator->getName();
-  //  if (name == getCollisionDetectorName())  // already using this collision detector
-  //    return;
+  // Temporarily keep pointer to the previous (if any) collision detector to copy padding from
+  CollisionDetectorPtr prev_coll_detector = collision_detector_;
 
+  // Construct a fresh CollisionDetector and store allocator
   collision_detector_.reset(new CollisionDetector());
-
   collision_detector_->alloc_ = allocator;
-  collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel());
-  collision_detector_->cenv_const_ = collision_detector_->cenv_;
-  collision_detector_->cenv_unpadded_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel());
-  collision_detector_->cenv_unpadded_const_ = collision_detector_->cenv_unpadded_;
 
-  // Copy padding from the previous collision detector
-  if (have_previous_coll_detector)
+  // If parent_detector is specified, copy-construct collision environments (copies link shapes and attached objects)
+  // Otherwise, construct new collision environment from world and robot model
+  if (parent_detector)
   {
-    collision_detector_->copyPadding(prev_coll_detector);
+    collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(parent_detector->cenv_, world_);
+    collision_detector_->cenv_unpadded_ =
+        collision_detector_->alloc_->allocateEnv(parent_detector->cenv_unpadded_, world_);
   }
+  else
+  {
+    collision_detector_->cenv_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel());
+    collision_detector_->cenv_unpadded_ = collision_detector_->alloc_->allocateEnv(world_, getRobotModel());
+
+    // Copy padding to collision_detector_->cenv_
+    if (prev_coll_detector)
+      collision_detector_->copyPadding(*prev_coll_detector);
+  }
+
+  // Assign const pointers
+  collision_detector_->cenv_const_ = collision_detector_->cenv_;
+  collision_detector_->cenv_unpadded_const_ = collision_detector_->cenv_unpadded_;
 }
 
 const collision_detection::CollisionEnvConstPtr&
@@ -295,11 +296,8 @@ void PlanningScene::clearDiffs()
   if (current_world_object_update_callback_)
     current_world_object_update_observer_handle_ = world_->addObserver(current_world_object_update_callback_);
 
-  if (parent_)
-  {
-    collision_detector_->copyPadding(*parent_->collision_detector_);
-  }
-  collision_detector_->cenv_const_ = collision_detector_->cenv_;
+  // Reset collision detector to the the parent's version
+  allocateCollisionDetector(parent_->collision_detector_->alloc_, parent_->collision_detector_);
 
   scene_transforms_.reset();
   robot_state_.reset();

moveit_core/planning_scene/test/test_planning_scene.cpp

@@ -224,7 +224,7 @@ TEST(PlanningScene, switchCollisionDetectorType)
     EXPECT_FALSE(ps->isStateValid(current_state, "left_arm"));
   }
 
-  ps->setCollisionDetectorType(collision_detection::CollisionDetectorAllocatorFCL::create());
+  ps->allocateCollisionDetector(collision_detection::CollisionDetectorAllocatorFCL::create());
   if (ps->isStateColliding(current_state, "left_arm"))
   {
     EXPECT_FALSE(ps->isStateValid(current_state, "left_arm"));

moveit_ros/moveit_servo/CMakeLists.txt

@@ -189,46 +189,44 @@ if(BUILD_TESTING)
   ament_add_gtest(test_low_pass_filter test/test_low_pass_filter.cpp)
   target_link_libraries(test_low_pass_filter ${SERVO_LIB_NAME})
 
-  # This test is flaky, disabling until it is fixed
-  # Unit test for ServoCalcs
+  # TODO(andyz): re-enable integration tests when they are less flakey.
+  # The issue is that the test completes successfully but a results file is not generated.
+
+  # # Unit test for ServoCalcs
   # ament_add_gtest_executable(
   #   unit_test_servo_calcs
   #   test/unit_test_servo_calcs.cpp
   # )
   # target_link_libraries(unit_test_servo_calcs ${SERVO_LIB_NAME})
   # ament_target_dependencies(unit_test_servo_calcs ${THIS_PACKAGE_INCLUDE_DEPENDS} Boost)
   # add_ros_test(test/launch/unit_test_servo_calcs.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-  # end Unit test for ServoCalcs
-
-  # Servo integration launch test
-  ament_add_gtest_executable(test_servo_integration
-      test/test_servo_interface.cpp
-      test/servo_launch_test_common.hpp
-  )
-  target_link_libraries(test_servo_integration ${SERVO_PARAM_LIB_NAME})
-  ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  add_ros_test(test/launch/test_servo_integration.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-  # end servo integration launch test
-
-  # Servo collision checking integration test
-  ament_add_gtest_executable(test_servo_collision
-        test/test_servo_collision.cpp
-        test/servo_launch_test_common.hpp
-  )
-  target_link_libraries(test_servo_collision ${SERVO_PARAM_LIB_NAME})
-  ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  add_ros_test(test/launch/test_servo_collision.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-  # end servo collision checking integration test
-
-  # Servo singularity checking integration test
-  ament_add_gtest_executable(test_servo_singularity
-        test/test_servo_singularity.cpp
-        test/servo_launch_test_common.hpp
-  )
-  target_link_libraries(test_servo_singularity ${SERVO_PARAM_LIB_NAME})
-  ament_target_dependencies(test_servo_singularity ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  add_ros_test(test/launch/test_servo_singularity.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-  # end servo singularity checking integration test
+
+  # # Servo integration launch test
+  # ament_add_gtest_executable(test_servo_integration
+  #     test/test_servo_interface.cpp
+  #     test/servo_launch_test_common.hpp
+  # )
+  # target_link_libraries(test_servo_integration ${SERVO_PARAM_LIB_NAME})
+  # ament_target_dependencies(test_servo_integration ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  # add_ros_test(test/launch/test_servo_integration.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+  # # Servo collision checking integration test
+  # ament_add_gtest_executable(test_servo_collision
+  #       test/test_servo_collision.cpp
+  #       test/servo_launch_test_common.hpp
+  # )
+  # target_link_libraries(test_servo_collision ${SERVO_PARAM_LIB_NAME})
+  # ament_target_dependencies(test_servo_collision ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  # add_ros_test(test/launch/test_servo_collision.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+  # # Servo singularity checking integration test
+  # ament_add_gtest_executable(test_servo_singularity
+  #       test/test_servo_singularity.cpp
+  #       test/servo_launch_test_common.hpp
+  # )
+  # target_link_libraries(test_servo_singularity ${SERVO_PARAM_LIB_NAME})
+  # ament_target_dependencies(test_servo_singularity ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  # add_ros_test(test/launch/test_servo_singularity.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 
   # TODO(henningkayser): Port tests to ROS2
   # servo_cpp_interface
@@ -241,24 +239,21 @@ if(BUILD_TESTING)
   #   ${catkin_LIBRARIES}
   # )
 
-  # Flaky, disabling until it is fixed
-  # pose_tracking
-  ament_add_gtest_executable(test_servo_pose_tracking
-        test/pose_tracking_test.cpp
-  )
-  ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS})
-  target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING})
-  add_ros_test(test/launch/test_servo_pose_tracking.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
-
-  # # low_latency
-  # add_rostest_gtest(servo_low_latency_test
-  # test/servo_low_latency_test.test
-  # test/servo_low_latency_test.cpp
+  # # pose_tracking
+  # ament_add_gtest_executable(test_servo_pose_tracking
+  #       test/pose_tracking_test.cpp
   # )
-  # target_link_libraries(servo_low_latency_test
-  # ${SERVO_LIB_NAME}
-  # ${catkin_LIBRARIES}
+  # ament_target_dependencies(test_servo_pose_tracking ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  # target_link_libraries(test_servo_pose_tracking ${POSE_TRACKING})
+  # add_ros_test(test/launch/test_servo_pose_tracking.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
+
+  # # basic_servo_tests
+  # ament_add_gtest_executable(basic_servo_tests
+  #       test/basic_servo_tests.cpp
   # )
+  # ament_target_dependencies(basic_servo_tests ${THIS_PACKAGE_INCLUDE_DEPENDS})
+  # target_link_libraries(basic_servo_tests ${POSE_TRACKING})
+  # add_ros_test(test/launch/test_basic_servo.test.py ARGS "test_binary_dir:=${CMAKE_CURRENT_BINARY_DIR}")
 
 endif()
 

moveit_ros/moveit_servo/include/moveit_servo/servo.h

@@ -38,8 +38,10 @@
 
 #pragma once
 
+// System
 #include <memory>
 
+// Moveit2
 #include <moveit_servo/collision_check.h>
 #include <moveit_servo/servo_parameters.h>
 #include <moveit_servo/servo_calcs.h>
@@ -86,6 +88,9 @@ class Servo
   /** \brief Get the parameters used by servo node. */
   const ServoParameters::SharedConstPtr& getParameters() const;
 
+  // Give test access to private/protected methods
+  friend class ServoFixture;
+
 private:
   // Pointer to the collision environment
   planning_scene_monitor::PlanningSceneMonitorPtr planning_scene_monitor_;

moveit_ros/moveit_servo/include/moveit_servo/servo_calcs.h

@@ -347,5 +347,7 @@ class ServoCalcs
   // dynamic parameters
   std::string robot_link_command_frame_;
   rcl_interfaces::msg::SetParametersResult robotLinkCommandFrameCallback(const rclcpp::Parameter& parameter);
+
+  friend class ServoFixture;
 };
 }  // namespace moveit_servo

moveit_ros/moveit_servo/src/servo_calcs.cpp

@@ -755,58 +755,26 @@ double ServoCalcs::velocityScalingFactorForSingularity(const Eigen::VectorXd& co
 
 void ServoCalcs::enforceVelLimits(Eigen::ArrayXd& delta_theta)
 {
+  // Convert to joint angle velocities for checking and applying joint specific velocity limits.
   Eigen::ArrayXd velocity = delta_theta / parameters_->publish_period;
 
-  std::size_t joint_delta_index = 0;
-  // Track the smallest velocity scaling factor required, across all joints
-  double velocity_limit_scaling_factor = 1;
-
-  for (auto joint : joint_model_group_->getActiveJointModels())
+  std::size_t joint_delta_index{ 0 };
+  double velocity_scaling_factor{ 1.0 };
+  for (const moveit::core::JointModel* joint : joint_model_group_->getActiveJointModels())
   {
-    // Some joints do not have bounds defined
-    const auto bound = joint->getVariableBounds(joint->getName());
-
-    if (bound.velocity_bounded_)
+    const auto& bounds = joint->getVariableBounds(joint->getName());
+    if (bounds.velocity_bounded_ && velocity(joint_delta_index) != 0.0)
     {
-      velocity(joint_delta_index) = delta_theta(joint_delta_index) / parameters_->publish_period;
-
-      bool clip_velocity = false;
-      double velocity_limit = 0.0;
-      if (velocity(joint_delta_index) < bound.min_velocity_)
-      {
-        clip_velocity = true;
-        velocity_limit = bound.min_velocity_;
-      }
-      else if (velocity(joint_delta_index) > bound.max_velocity_)
-      {
-        clip_velocity = true;
-        velocity_limit = bound.max_velocity_;
-      }
-
-      // Apply velocity bounds
-      if (clip_velocity)
-      {
-        const double scaling_factor =
-            fabs(velocity_limit * parameters_->publish_period) / fabs(delta_theta(joint_delta_index));
-
-        // Store the scaling factor if it's the smallest yet
-        if (scaling_factor < velocity_limit_scaling_factor)
-          velocity_limit_scaling_factor = scaling_factor;
-      }
+      const double unbounded_velocity = velocity(joint_delta_index);
+      // Clamp each joint velocity to a joint specific [min_velocity, max_velocity] range.
+      const auto bounded_velocity = std::min(std::max(unbounded_velocity, bounds.min_velocity_), bounds.max_velocity_);
+      velocity_scaling_factor = std::min(velocity_scaling_factor, bounded_velocity / unbounded_velocity);
     }
     ++joint_delta_index;
   }
 
-  // Apply the velocity scaling to all joints
-  if (velocity_limit_scaling_factor < 1)
-  {
-    for (joint_delta_index = 0; joint_delta_index < joint_model_group_->getActiveJointModels().size();
-         ++joint_delta_index)
-    {
-      delta_theta(joint_delta_index) = velocity_limit_scaling_factor * delta_theta(joint_delta_index);
-      velocity(joint_delta_index) = velocity_limit_scaling_factor * velocity(joint_delta_index);
-    }
-  }
+  // Convert back to joint angle increments.
+  delta_theta = velocity_scaling_factor * velocity * parameters_->publish_period;
 }
 
 bool ServoCalcs::enforcePositionLimits(trajectory_msgs::msg::JointTrajectory& joint_trajectory) const