Merge moveit/moveit@424a5b7

MIGRATION.md

@@ -37,6 +37,8 @@ API changes in MoveIt releases
 - 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`).
+- `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.
 
 ## ROS Melodic
 

moveit2.repos

@@ -15,3 +15,7 @@ repositories:
     type: git
     url: https://github.com/ompl/ompl.git
     version: main
+  srdfdom:
+    type: git
+    url: https://github.com/ros-planning/srdfdom.git
+    version: ros2

moveit_commander/package.xml

@@ -2,7 +2,7 @@
 <?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.6</version>
+  <version>1.1.7</version>
   <description>Python interfaces to MoveIt</description>
   <maintainer email="me@v4hn.de">Michael Görner</maintainer>
   <maintainer email="rhaschke@techfak.uni-bielefeld.de">Robert Haschke</maintainer>

moveit_core/collision_detection/include/moveit/collision_detection/collision_matrix.h

@@ -87,43 +87,45 @@ class AllowedCollisionMatrix
    *  will be ignored. */
   AllowedCollisionMatrix(const std::vector<std::string>& names, bool allowed = false);
 
+  /** @brief Construct from an SRDF representation */
+  AllowedCollisionMatrix(const srdf::Model& srdf);
+
   /** @brief Construct the structure from a message representation */
   AllowedCollisionMatrix(const moveit_msgs::msg::AllowedCollisionMatrix& msg);
 
   /** @brief Copy constructor */
   AllowedCollisionMatrix(const AllowedCollisionMatrix& acm) = default;
 
-  /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the
-   * collision matrix.
-   * Return false if the entry is not found.
+  /** @brief Get the type of the allowed collision between two elements.
+   *  Return true if the entry is included in the collision matrix. Return false if the entry is not found.
    *  @param name1 name of first element
    *  @param name2 name of second element
    *  @param allowed_collision_type The allowed collision type will be filled here */
   bool getEntry(const std::string& name1, const std::string& name2,
                 AllowedCollision::Type& allowed_collision_type) const;
 
-  /** @brief Get the allowed collision predicate between two elements. Return true if a predicate for entry is included
-   * in the collision matrix
-   * (if the type is AllowedCollision::CONDITIONAL). Return false if the entry is not found.
+  /** @brief Get the allowed collision predicate between two elements.
+   *  Return true if a predicate for this entry is available in the collision matrix.
+   *  Return false if the entry is not found.
    *  @param name1 name of first element
    *  @param name2 name of second element
    *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled
    * here */
   bool getEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
 
-  /** @brief Check if the allowed collision matrix has an entry at all for an element. Returns true if the element is
-   * included.
+  /** @brief Check if the allowed collision matrix has an entry at all for an element.
+   *  Returns true if the element is included.
    *  @param name name of the element */
   bool hasEntry(const std::string& name) const;
 
-  /** @brief Check if the allowed collision matrix has an entry for a pair of elements. Returns true if the pair is
-   * included.
+  /** @brief Check if the allowed collision matrix has an entry for a pair of elements.
+   *  Returns true if the pair is included.
    *  @param name1 name of first element
    *  @param name2 name of second element*/
   bool hasEntry(const std::string& name1, const std::string& name2) const;
 
-  /** @brief Remove an entry corresponding to a pair of elements. Nothing happens if the pair does not exist in the
-   * collision matrix.
+  /** @brief Remove an entry corresponding to a pair of elements.
+   *  Nothing happens if the pair does not exist in the collision matrix.
    *  @param name1 name of first element
    *  @param name2 name of second element*/
   void removeEntry(const std::string& name1, const std::string& name2);
@@ -137,26 +139,25 @@ class AllowedCollisionMatrix
    *  @param name2 name of second element
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(const std::string& name1, const std::string& name2, bool allowed);
 
-  /** @brief Set an entry corresponding to a pair of elements. This sets the type of the entry to
-   * AllowedCollision::CONDITIONAL.
+  /** @brief Set an entry corresponding to a pair of elements.
+   *
+   *  This sets the type of the entry to AllowedCollision::CONDITIONAL.
    *  @param name1 name of first element
    *  @param name2 name of second element
-   *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is
-   * expected here */
+   *  @param fn A callback function that is used to decide if collisions are allowed between the two elements */
   void setEntry(const std::string& name1, const std::string& name2, DecideContactFn& fn);
 
-  /** @brief Set the entries corresponding to a name. With each of the the known names in the collision matrix, form a
-   * pair using the name
-   * specified as argument to this function and set the entry as indicated by \e allowed.
+  /** @brief Set the entries corresponding to a name.
+   *  With each of the *known names* in the collision matrix, form a pair using the name
+   *  specified as argument to this function and set the entry as indicated by \e allowed.
+   *  As the set of known names might change in future, consider using setDefaultEntry() instead!
    *  @param name the object name
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(const std::string& name, bool allowed);
 
   /** @brief Set multiple entries. Pairs of names are formed using \e name and \e other_names
@@ -165,24 +166,21 @@ class AllowedCollisionMatrix
    *  matrix entries will be set for all such pairs.
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(const std::string& name, const std::vector<std::string>& other_names, bool allowed);
 
   /** @brief Set an entry corresponding to all possible pairs between two sets of elements
    *  @param names1 First set of names
    *  @param names2 Second set of names
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(const std::vector<std::string>& names1, const std::vector<std::string>& names2, bool allowed);
 
   /** @brief Set an entry corresponding to all known pairs
    *  @param allowed If false, indicates that collisions between two elements must be checked for and no collisions
    *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between two elements are ok and an
-   * explicit collision
-   *  computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setEntry(bool allowed);
 
   /** @brief Get all the names known to the collision matrix */
@@ -200,65 +198,42 @@ class AllowedCollisionMatrix
     return entries_.size();
   }
 
-  /** @brief Set the default value for entries that include \e name. If such a default value is set, queries to
-   * getAllowedCollision() that include
-   *  \e name will return this value instead, @b even if a pair that includes \e name was previously specified with
-   * setEntry().
+  /** @brief Set the default value for entries that include \e name but are not set explicitly with setEntry().
    *  @param name The name of the element for which to set the default value
    *  @param allowed If false, indicates that collisions between \e name and any other element must be checked for and
-   * no collisions
-   *  will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and any other
-   * element are ok and
-   *  an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
+   *  no collisions will be ignored (AllowedCollision::NEVER). If true, indicates that collisions between \e name and
+   *  any other element are ok and an explicit collision computation is not necessary (AllowedCollision::ALWAYS).*/
   void setDefaultEntry(const std::string& name, bool allowed);
 
-  /** @brief Set the default value for entries that include \e name to be AllowedCollision::CONDITIONAL and specify the
-   * allowed contact predicate to be \e fn.
-   *  If this function is called, queries to getAllowedCollision() that include \e name will return this value instead,
-   * @b even if a pair that includes \e name
-   *  was previously specified with setEntry().
+  /** @brief Set the default value for entries that include \e name but are not set explicitly with setEntry().
    *  @param name The name of the element for which to set the default value
    *  @param fn A callback function that is used to decide if collisions are allowed between \e name and some other
    * element is expected here. */
   void setDefaultEntry(const std::string& name, DecideContactFn& fn);
 
-  /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a
-   * default value was
-   *  found for the specified element. Return false otherwise.
+  /** @brief Get the type of the allowed collision to be considered by default for an element.
+   *  Return true if a default value was found for the specified element. Return false otherwise.
    *  @param name name of the element
    *  @param allowed_collision The default allowed collision type will be filled here */
   bool getDefaultEntry(const std::string& name, AllowedCollision::Type& allowed_collision) const;
 
-  /** @brief Get the type of the allowed collision between to be considered by default for an element. Return true if a
-   * default value was
-   *  found for the specified element. Return false otherwise.
+  /** @brief Get the type of the allowed collision between to be considered by default for an element.
+   *  Return true if a default value was found for the specified element. Return false otherwise.
    *  @param name name of the element
-   *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled
-   * here. */
+   *  @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */
   bool getDefaultEntry(const std::string& name, DecideContactFn& fn) const;
 
-  /** @brief Get the allowed collision predicate between two elements. Return true if a predicate for entry is included
-   * in the collision matrix
-   *  (if the type is AllowedCollision::CONDITIONAL) or if one was computed from defaults. Return false if the entry is
-   * not found.
-   *  Default values take precedence. If a default value is specified  for either \e name1 or \e name2, that value is
-   * returned instead (if both
-   *  elements have default values specified, both predicates must be satisfied). If no default values are specified,
-   * getEntry() is used to look
-   *  for the pair (\e name1, \e name2).
+  /** @brief Get the allowed collision predicate between two elements.
+   *  Return true if a predicate for entry is included in the collision matrix (if the type is AllowedCollision::CONDITIONAL)
+   *  or if one was computed from defaults. Return false if the entry is not found.
    *  @param name1 name of first element
    *  @param name2 name of second element
-   *  @param fn A callback function that is used to decide if collisions are allowed between the two elements is filled
-   * here */
+   *  @param fn Return the callback function that is used to decide if collisions are allowed between the two elements. */
   bool getAllowedCollision(const std::string& name1, const std::string& name2, DecideContactFn& fn) const;
 
-  /** @brief Get the type of the allowed collision between two elements. Return true if the entry is included in the
-   * collision matrix or if
-   *  specified defaults were found. Return false if the entry is not found. Default values take precedence. If a
-   * default value is specified
-   *  for either \e name1 or \e name2, that value is returned instead (if both elements have default values specified,
-   * AllowedCollision::NEVER takes
-   *  precedence). If no default values are specified, getEntry() is used to look for the pair (\e name1, \e name2).
+  /** @brief Get the type of the allowed collision between two elements.
+   *  Return true if the entry is included in the collision matrix or if specified defaults were found.
+   *  Return false if the entry is not found.
    *  @param name1 name of first element
    *  @param name2 name of second element
    *  @param allowed_collision The allowed collision type will be filled here */
@@ -269,6 +244,9 @@ class AllowedCollisionMatrix
   void print(std::ostream& out) const;
 
 private:
+  bool getDefaultEntry(const std::string& name1, const std::string& name2,
+                       AllowedCollision::Type& allowed_collision) const;
+
   std::map<std::string, std::map<std::string, AllowedCollision::Type> > entries_;
   std::map<std::string, std::map<std::string, DecideContactFn> > allowed_contacts_;
 

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

@@ -78,15 +78,7 @@ class CollisionDetectorPandaTest : public testing::Test
     robot_model_ = moveit::core::loadTestingRobotModel("panda");
     robot_model_ok_ = static_cast<bool>(robot_model_);
 
-    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);
-
-    // allow collisions for pairs that have been disabled
-    const std::vector<srdf::Model::DisabledCollision>& dc = robot_model_->getSRDF()->getDisabledCollisionPairs();
-    for (const srdf::Model::DisabledCollision& it : dc)
-      acm_->setEntry(it.link1_, it.link2_, true);
+    acm_ = std::make_shared<collision_detection::AllowedCollisionMatrix>(*robot_model_->getSRDF());
 
     cenv_ = value_->allocateEnv(robot_model_);
 

moveit_core/collision_detection/include/moveit/collision_detection/test_collision_common_pr2.h

@@ -87,6 +87,12 @@ class CollisionDetectorTest : public ::testing::Test
   std::string kinect_dae_resource_;
 };
 
+#ifdef NDEBUG
+#define EXPECT_TIME_LT(EXPR, VAL) EXPECT_LT(EXPR, VAL)
+#else  // Don't perform timing checks in Debug mode (but evaluate expression)
+#define EXPECT_TIME_LT(EXPR, VAL) (void)(EXPR)
+#endif
+
 TYPED_TEST_CASE_P(CollisionDetectorTest);
 
 TYPED_TEST_P(CollisionDetectorTest, InitOK)
@@ -363,7 +369,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester)
   double second_check =
       std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - before).count();
 
-  EXPECT_LT(fabs(first_check - second_check), .05);
+  EXPECT_TIME_LT(fabs(first_check - second_check), .05);
 
   std::vector<shapes::ShapeConstPtr> shapes;
   shapes.resize(1);
@@ -384,7 +390,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester)
   second_check = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - before).count();
 
   // the first check is going to take a while, as data must be constructed
-  EXPECT_LT(second_check, .1);
+  EXPECT_TIME_LT(second_check, .1);
 
   collision_detection::CollisionEnvPtr other_new_cenv = this->value_->allocateEnv(this->cenv_, this->cenv_->getWorld());
   before = std::chrono::system_clock::now();
@@ -394,7 +400,7 @@ TYPED_TEST_P(CollisionDetectorTest, DiffSceneTester)
   new_cenv->checkSelfCollision(req, res, robot_state);
   second_check = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - before).count();
 
-  EXPECT_LT(fabs(first_check - second_check), .05);
+  EXPECT_TIME_LT(fabs(first_check - second_check), .05);
 }
 
 TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached)
@@ -422,7 +428,7 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached)
   double second_check =
       std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - before).count();
 
-  EXPECT_LT(second_check, .05);
+  EXPECT_TIME_LT(second_check, .05);
 
   collision_detection::CollisionEnv::ObjectConstPtr object = this->cenv_->getWorld()->getObject("kinect");
   this->cenv_->getWorld()->removeObject("kinect");
@@ -461,8 +467,8 @@ TYPED_TEST_P(CollisionDetectorTest, ConvertObjectToAttached)
   this->cenv_->checkSelfCollision(req, res, robot_state2, *this->acm_);
   second_check = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - before).count();
 
-  EXPECT_LT(first_check, .05);
-  EXPECT_LT(fabs(first_check - second_check), .1);
+  EXPECT_TIME_LT(first_check, .05);
+  EXPECT_TIME_LT(fabs(first_check - second_check), .1);
 }
 
 TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed)
@@ -478,7 +484,7 @@ TYPED_TEST_P(CollisionDetectorTest, TestCollisionMapAdditionSpeed)
   this->cenv_->getWorld()->addToObject("map", shapes, poses);
   double t = std::chrono::duration_cast<std::chrono::seconds>(std::chrono::system_clock::now() - start).count();
   // TODO (j-petit): investigate why bullet collision checking is considerably slower here
-  EXPECT_GE(5.0, t);
+  EXPECT_TIME_LT(t, 5.0);
   // this is not really a failure; it is just that slow;
   // looking into doing collision checking with a voxel grid.
   RCLCPP_INFO(rclcpp::get_logger("moveit.core.collision_detection.bullet"), "Adding boxes took %g", t);