Merge branch 'main' into forward-ports

.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

.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"']

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/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

@@ -654,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)
       {
@@ -1260,7 +1267,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;
@@ -1723,16 +1730,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,
@@ -1854,6 +1861,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.",
@@ -1867,6 +1875,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;
   }
 

moveit_core/planning_scene/test/test_planning_scene.cpp

@@ -44,6 +44,8 @@
 #include <sstream>
 #include <string>
 #include <tf2_eigen/tf2_eigen.hpp>
+#include <octomap_msgs/conversions.h>
+#include <octomap/octomap.h>
 
 #include <moveit/collision_detection/collision_common.h>
 #include <moveit/collision_detection/collision_plugin_cache.h>
@@ -89,6 +91,59 @@ TEST(PlanningScene, LoadRestore)
   EXPECT_EQ(ps.getRobotModel()->getName(), ps_msg.robot_model_name);
 }
 
+TEST(PlanningScene, LoadOctomap)
+{
+  urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");
+  srdf::ModelSharedPtr srdf_model(new srdf::Model());
+  planning_scene::PlanningScene ps(urdf_model, srdf_model);
+
+  {  // check octomap before doing any operations on it
+    octomap_msgs::msg::OctomapWithPose msg;
+    ps.getOctomapMsg(msg);
+    EXPECT_TRUE(msg.octomap.id.empty());
+    EXPECT_TRUE(msg.octomap.data.empty());
+  }
+
+  {  // fill PlanningScene's octomap
+    octomap::OcTree octomap(0.1);
+    octomap::point3d origin(0, 0, 0);
+    octomap::point3d end(0, 1, 2);
+    octomap.insertRay(origin, end);
+
+    // populate PlanningScene with octomap
+    moveit_msgs::msg::PlanningScene msg;
+    msg.is_diff = true;
+    octomap_msgs::fullMapToMsg(octomap, msg.world.octomap.octomap);
+    ps.setPlanningSceneDiffMsg(msg);
+
+    // validate octomap message
+    octomap_msgs::msg::OctomapWithPose octomap_msg;
+    ps.getOctomapMsg(octomap_msg);
+    EXPECT_EQ(octomap_msg.octomap.id, "OcTree");
+    EXPECT_EQ(octomap_msg.octomap.data.size(), msg.world.octomap.octomap.data.size());
+  }
+
+  {  // verify that a PlanningScene msg with an empty octomap id does not modify the octomap
+    // create planning scene
+    moveit_msgs::msg::PlanningScene msg;
+    msg.is_diff = true;
+    ps.setPlanningSceneDiffMsg(msg);
+
+    octomap_msgs::msg::OctomapWithPose octomap_msg;
+    ps.getOctomapMsg(octomap_msg);
+    EXPECT_EQ(octomap_msg.octomap.id, "OcTree");
+    EXPECT_FALSE(octomap_msg.octomap.data.empty());
+  }
+
+  {  // check that a non-empty octomap id, but empty octomap will clear the octomap
+    moveit_msgs::msg::PlanningScene msg;
+    msg.is_diff = true;
+    msg.world.octomap.octomap.id = "xxx";
+    ps.setPlanningSceneDiffMsg(msg);
+    EXPECT_FALSE(static_cast<bool>(ps.getWorld()->getObject(planning_scene::PlanningScene::OCTOMAP_NS)));
+  }
+}
+
 TEST(PlanningScene, LoadRestoreDiff)
 {
   urdf::ModelInterfaceSharedPtr urdf_model = moveit::core::loadModelInterface("pr2");

moveit_planners/pilz_industrial_motion_planner/src/trajectory_blender_transition_window.cpp

@@ -274,7 +274,7 @@ bool pilz_industrial_motion_planner::TrajectoryBlenderTransitionWindow::searchIn
   // (last point of the first trajectory, first point of the second trajectory)
   Eigen::Isometry3d circ_pose = req.first_trajectory->getLastWayPoint().getFrameTransform(req.link_name);
 
-  // Searh for intersection points according to distance
+  // Search for intersection points according to distance
   if (!linearSearchIntersectionPoint(req.link_name, circ_pose.translation(), req.blend_radius, req.first_trajectory,
                                      true, first_interse_index))
   {

moveit_ros/planning/moveit_cpp/include/moveit/moveit_cpp/moveit_cpp.h

@@ -57,27 +57,28 @@ class MoveItCpp
   /// Specification of options to use when constructing the MoveItCpp class
   struct PlanningSceneMonitorOptions
   {
+    PlanningSceneMonitorOptions()
+      : joint_state_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC)
+      , attached_collision_object_topic(
+            planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC)
+      , monitored_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC)
+      , publish_planning_scene_topic(planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC)
+    {
+    }
+
     void load(const rclcpp::Node::SharedPtr& node)
     {
       const std::string ns = "planning_scene_monitor_options";
       node->get_parameter_or(ns + ".name", name, std::string("planning_scene_monitor"));
       node->get_parameter_or(ns + ".robot_description", robot_description, std::string("robot_description"));
-      node->get_parameter_or(ns + ".joint_state_topic", joint_state_topic,
-                             planning_scene_monitor::PlanningSceneMonitor::DEFAULT_JOINT_STATES_TOPIC);
-      node->get_parameter_or(ns + ".attached_collision_object_topic", attached_collision_object_topic,
-                             planning_scene_monitor::PlanningSceneMonitor::DEFAULT_ATTACHED_COLLISION_OBJECT_TOPIC);
-      node->get_parameter_or(ns + ".monitored_planning_scene_topic", monitored_planning_scene_topic,
-                             planning_scene_monitor::PlanningSceneMonitor::MONITORED_PLANNING_SCENE_TOPIC);
-      node->get_parameter_or(ns + ".publish_planning_scene_topic", publish_planning_scene_topic,
-                             planning_scene_monitor::PlanningSceneMonitor::DEFAULT_PLANNING_SCENE_TOPIC);
       node->get_parameter_or(ns + ".wait_for_initial_state_timeout", wait_for_initial_state_timeout, 0.0);
     }
     std::string name;
     std::string robot_description;
-    std::string joint_state_topic;
-    std::string attached_collision_object_topic;
-    std::string monitored_planning_scene_topic;
-    std::string publish_planning_scene_topic;
+    const std::string joint_state_topic;
+    const std::string attached_collision_object_topic;
+    const std::string monitored_planning_scene_topic;
+    const std::string publish_planning_scene_topic;
     double wait_for_initial_state_timeout;
   };
 

moveit_ros/planning/planning_scene_monitor/src/planning_scene_monitor.cpp

@@ -866,7 +866,7 @@ void PlanningSceneMonitor::excludeRobotLinksFromOctree()
   bool warned = false;
   for (const moveit::core::LinkModel* link : links)
   {
-    std::vector<shapes::ShapeConstPtr> shapes = link->getShapes();  // copy shared ptrs on purpuse
+    std::vector<shapes::ShapeConstPtr> shapes = link->getShapes();  // copy shared ptrs on purpose
     for (std::size_t j = 0; j < shapes.size(); ++j)
     {
       // merge mesh vertices up to 0.1 mm apart

moveit_ros/planning_interface/move_group_interface/include/moveit/move_group_interface/move_group_interface.h

@@ -103,7 +103,7 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
 
   MOVEIT_STRUCT_FORWARD(Plan);
 
-  /// The representation of a motion plan (as ROS messasges)
+  /// The representation of a motion plan (as ROS messages)
   struct Plan
   {
     /// The full starting state used for planning
@@ -236,13 +236,19 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
       If the value is greater than 1, it is set to 1.0. */
   void setMaxVelocityScalingFactor(double max_velocity_scaling_factor);
 
+  /** \brief Get the max velocity scaling factor set by setMaxVelocityScalingFactor(). */
+  double getMaxVelocityScalingFactor() const;
+
   /** \brief Set a scaling factor for optionally reducing the maximum joint acceleration.
       Allowed values are in (0,1]. The maximum joint acceleration specified
       in the robot model is multiplied by the factor. If the value is 0, it is set to
       the default value, which is defined in joint_limits.yaml of the moveit_config.
       If the value is greater than 1, it is set to 1.0. */
   void setMaxAccelerationScalingFactor(double max_acceleration_scaling_factor);
 
+  /** \brief Get the max acceleration scaling factor set by setMaxAccelerationScalingFactor(). */
+  double getMaxAccelerationScalingFactor() const;
+
   /** \brief Get the number of seconds set by setPlanningTime() */
   double getPlanningTime() const;
 
@@ -796,6 +802,14 @@ class MOVEIT_MOVE_GROUP_INTERFACE_EXPORT MoveGroupInterface
   /** \brief How often is the system allowed to move the camera to update environment model when looking */
   void setLookAroundAttempts(int32_t attempts);
 
+  /** \brief Build a RobotState message for use with plan() or computeCartesianPath()
+   *  If the move_group has a custom set start state, this method will use that as the robot state.
+   *
+   *  Otherwise, the robot state will be with `is_diff` set to true, causing it to be an offset from the current state
+   *  of the robot at time of the state's use.
+   */
+  void constructRobotState(moveit_msgs::msg::RobotState& state);
+
   /** \brief Build the MotionPlanRequest that would be sent to the move_group action with plan() or move() and store it
       in \e request */
   void constructMotionPlanRequest(moveit_msgs::msg::MotionPlanRequest& request);