[MSA] Merge main into feature/msa (Part III)

MIGRATION.md

@@ -39,6 +39,8 @@ API changes in MoveIt releases
 - 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.
+- End-effector markers in rviz are shown only if the eef's parent group is active _and_ the parent link is part of that group. Before, these conditions were _OR_-connected.
+  You might need to define additional end-effectors.
 
 ## ROS Melodic
 

moveit_commander/src/moveit_commander/move_group.py

@@ -603,7 +603,7 @@ def go(self, joints=None, wait=True):
         elif joints is not None:
             try:
                 self.set_joint_value_target(self.get_remembered_joint_values()[joints])
-            except TypeError:
+            except (KeyError, TypeError):
                 self.set_joint_value_target(joints)
         if wait:
             return self._g.move()

moveit_commander/test/python_moveit_commander.py

@@ -43,12 +43,21 @@
 import os
 
 from moveit_msgs.msg import RobotState
+from sensor_msgs.msg import JointState
 
 from moveit_commander import RobotCommander, PlanningSceneInterface
 
 
 class PythonMoveitCommanderTest(unittest.TestCase):
     PLANNING_GROUP = "manipulator"
+    JOINT_NAMES = [
+        "joint_1",
+        "joint_2",
+        "joint_3",
+        "joint_4",
+        "joint_5",
+        "joint_6",
+    ]
 
     @classmethod
     def setUpClass(self):
@@ -67,14 +76,7 @@ def test_enforce_bounds_empty_state(self):
     def test_enforce_bounds(self):
         in_msg = RobotState()
         in_msg.joint_state.header.frame_id = "base_link"
-        in_msg.joint_state.name = [
-            "joint_1",
-            "joint_2",
-            "joint_3",
-            "joint_4",
-            "joint_5",
-            "joint_6",
-        ]
+        in_msg.joint_state.name = self.JOINT_NAMES
         in_msg.joint_state.position = [0] * 6
         in_msg.joint_state.position[0] = 1000
 
@@ -87,14 +89,7 @@ def test_get_current_state(self):
         expected_state = RobotState()
         expected_state.joint_state.header.frame_id = "base_link"
         expected_state.multi_dof_joint_state.header.frame_id = "base_link"
-        expected_state.joint_state.name = [
-            "joint_1",
-            "joint_2",
-            "joint_3",
-            "joint_4",
-            "joint_5",
-            "joint_6",
-        ]
+        expected_state.joint_state.name = self.JOINT_NAMES
         expected_state.joint_state.position = [0] * 6
         self.assertEqual(self.group.get_current_state(), expected_state)
 
@@ -141,6 +136,25 @@ def test_validation(self):
         self.assertTrue(success3)
         self.assertTrue(self.group.execute(plan3))
 
+    def test_gogogo(self):
+        current_joints = np.asarray(self.group.get_current_joint_values())
+
+        self.group.set_joint_value_target(current_joints)
+        self.assertTrue(self.group.go(True))
+
+        self.assertTrue(self.group.go(current_joints))
+        self.assertTrue(self.group.go(list(current_joints)))
+        self.assertTrue(self.group.go(tuple(current_joints)))
+        self.assertTrue(
+            self.group.go(JointState(name=self.JOINT_NAMES, position=current_joints))
+        )
+
+        self.group.remember_joint_values("current")
+        self.assertTrue(self.group.go("current"))
+
+        current_pose = self.group.get_current_pose()
+        self.assertTrue(self.group.go(current_pose))
+
     def test_planning_scene_interface(self):
         planning_scene = PlanningSceneInterface()
 

moveit_core/collision_detection/src/collision_matrix.cpp

@@ -344,10 +344,14 @@ void AllowedCollisionMatrix::getAllEntryNames(std::vector<std::string>& names) c
 {
   names.clear();
   for (const auto& entry : entries_)
-    if (!names.empty() && names.back() == entry.first)
-      continue;
-    else
-      names.push_back(entry.first);
+    names.push_back(entry.first);
+
+  for (const auto& item : default_entries_)
+  {
+    auto it = std::lower_bound(names.begin(), names.end(), item.first);
+    if (it != names.end() && *it != item.first)
+      names.insert(it, item.first);
+  }
 }
 
 void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix& msg) const
@@ -358,7 +362,6 @@ void AllowedCollisionMatrix::getMessage(moveit_msgs::msg::AllowedCollisionMatrix
   msg.default_entry_values.clear();
 
   getAllEntryNames(msg.entry_names);
-  std::sort(msg.entry_names.begin(), msg.entry_names.end());
 
   msg.entry_values.resize(msg.entry_names.size());
   for (std::size_t i = 0; i < msg.entry_names.size(); ++i)
@@ -389,7 +392,6 @@ void AllowedCollisionMatrix::print(std::ostream& out) const
 {
   std::vector<std::string> names;
   getAllEntryNames(names);
-  std::sort(names.begin(), names.end());
 
   std::size_t spacing = 4;
   for (auto& name : names)

moveit_core/collision_distance_field/src/collision_distance_field_types.cpp

@@ -51,17 +51,26 @@ std::vector<CollisionSphere> determineCollisionSpheres(const bodies::Body* body,
 {
   std::vector<CollisionSphere> css;
 
-  bodies::BoundingCylinder cyl;
-  body->computeBoundingCylinder(cyl);
-  unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0));
-  double spacing = cyl.length / ((num_points * 1.0) - 1.0);
-  relative_transform = cyl.pose;
-
-  for (unsigned int i = 1; i < num_points - 1; ++i)
+  if (body->getType() == shapes::ShapeType::SPHERE)
   {
-    CollisionSphere cs(relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius);
+    collision_detection::CollisionSphere cs(body->getPose().translation(), body->getDimensions()[0]);
     css.push_back(cs);
   }
+  else
+  {
+    bodies::BoundingCylinder cyl;
+    body->computeBoundingCylinder(cyl);
+    unsigned int num_points = ceil(cyl.length / (cyl.radius / 2.0));
+    double spacing = cyl.length / ((num_points * 1.0) - 1.0);
+    relative_transform = cyl.pose;
+
+    for (unsigned int i = 1; i < num_points - 1; i++)
+    {
+      collision_detection::CollisionSphere cs(
+          relative_transform * Eigen::Vector3d(0, 0, (-cyl.length / 2.0) + i * spacing), cyl.radius);
+      css.push_back(cs);
+    }
+  }
 
   return css;
 }

moveit_core/trajectory_processing/include/moveit/trajectory_processing/iterative_time_parameterization.h

@@ -44,6 +44,7 @@ namespace trajectory_processing
 {
 /// \brief This class  modifies the timestamps of a trajectory to respect
 /// velocity and acceleration constraints.
+MOVEIT_CLASS_FORWARD(IterativeParabolicTimeParameterization);
 class IterativeParabolicTimeParameterization : public TimeParameterization
 {
 public:

moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_optimal_trajectory_generation.h

@@ -160,6 +160,7 @@ class Trajectory
   mutable std::list<TrajectoryStep>::const_iterator cached_trajectory_segment_;
 };
 
+MOVEIT_CLASS_FORWARD(TimeOptimalTrajectoryGeneration);
 class TimeOptimalTrajectoryGeneration : public TimeParameterization
 {
 public:

moveit_core/trajectory_processing/include/moveit/trajectory_processing/time_parameterization.h

@@ -7,6 +7,7 @@ namespace trajectory_processing
 /**
  * @brief Base class for trajectory parameterization algorithms
  */
+MOVEIT_CLASS_FORWARD(TimeParameterization);
 class TimeParameterization
 {
 public:

moveit_ros/benchmarks/scripts/moveit_benchmark_statistics.py

@@ -112,6 +112,9 @@ def readRequiredMultilineValue(filevar):
 def readBenchmarkLog(dbname, filenames):
     """Parse benchmark log files and store the parsed data in a sqlite3 database."""
 
+    def isInvalidValue(value):
+        return len(value) == 0 or value in ["nan", "-nan", "inf", "-inf"]
+
     conn = sqlite3.connect(dbname)
     c = conn.cursor()
     c.execute("PRAGMA FOREIGN_KEYS = ON")
@@ -313,7 +316,7 @@ def readBenchmarkLog(dbname, filenames):
             runIds = []
             for j in range(numRuns):
                 runValues = [
-                    None if len(x) == 0 or x == "nan" or x == "inf" else x
+                    None if isInvalidValue(x) else x
                     for x in logfile.readline().split("; ")[:-1]
                 ]
                 values = tuple([experimentId, plannerId] + runValues)
@@ -362,7 +365,7 @@ def readBenchmarkLog(dbname, filenames):
                         values = tuple(
                             [runIds[j]]
                             + [
-                                None if len(x) == 0 or x == "nan" or x == "inf" else x
+                                None if isInvalidValue(x) else x
                                 for x in dataSample.split(",")[:-1]
                             ]
                         )

moveit_ros/move_group/include/moveit/move_group/move_group_context.h

@@ -37,6 +37,7 @@
 #pragma once
 
 #include <rclcpp/rclcpp.hpp>
+#include <string>
 #include <moveit/macros/class_forward.h>
 
 namespace moveit_cpp

moveit_ros/move_group/src/move_group.cpp

@@ -236,16 +236,16 @@ int main(int argc, char** argv)
   std::string default_planning_pipeline;
   if (nh->get_parameter("default_planning_pipeline", default_planning_pipeline))
   {
-    // Ignore default_planning_pipeline if there is no known entry in pipeline_names
+    // Ignore default_planning_pipeline if there is no matching entry in pipeline_names
     if (std::find(pipeline_names.begin(), pipeline_names.end(), default_planning_pipeline) == pipeline_names.end())
     {
       RCLCPP_WARN(LOGGER,
-                  "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~/planning_pipelines",
+                  "MoveGroup launched with ~default_planning_pipeline '%s' not configured in ~planning_pipelines",
                   default_planning_pipeline.c_str());
       default_planning_pipeline = "";  // reset invalid pipeline id
     }
   }
-  else
+  else if (pipeline_names.size() > 1)  // only warn if there are multiple pipelines to choose from
   {
     // Handle deprecated move_group.launch
     RCLCPP_WARN(LOGGER,

moveit_ros/robot_interaction/src/robot_interaction.cpp

@@ -294,7 +294,7 @@ void RobotInteraction::decideActiveEndEffectors(const std::string& group, Intera
   auto add_active_end_effectors_for_single_group = [&](const moveit::core::JointModelGroup* single_group) {
     bool found_eef{ false };
     for (const srdf::Model::EndEffector& eef : eefs)
-      if ((single_group->hasLinkModel(eef.parent_link_) || single_group->getName() == eef.parent_group_) &&
+      if (single_group->hasLinkModel(eef.parent_link_) && single_group->getName() == eef.parent_group_ &&
           single_group->canSetStateFromIK(eef.parent_link_))
       {
         // We found an end-effector whose parent is the group.

moveit_ros/visualization/motion_planning_rviz_plugin/src/motion_planning_frame.cpp

@@ -619,6 +619,13 @@ void MotionPlanningFrame::initFromMoveGroupNS()
   planning_scene_world_publisher_ =
       node_->create_publisher<moveit_msgs::msg::PlanningSceneWorld>("planning_scene_world", 1);
 
+  // Declare parameter for default planning pipeline
+  if (!node_->has_parameter(planning_display_->getMoveGroupNS() + "default_planning_pipeline"))
+    node_->declare_parameter<std::string>(planning_display_->getMoveGroupNS() + "default_planning_pipeline", "");
+
+  // Query default planning pipeline id
+  node_->get_parameter(planning_display_->getMoveGroupNS() + "default_planning_pipeline", default_planning_pipeline_);
+
   // Set initial velocity and acceleration scaling factors from ROS parameters
   double factor;
   node_->get_parameter_or("robot_description_planning.default_velocity_scaling_factor", factor, 0.1);
@@ -638,10 +645,6 @@ void MotionPlanningFrame::initFromMoveGroupNS()
   {
     ui_->database_port->setValue(port);
   }
-
-  // Query default planning pipeline id
-  node_->get_parameter(planning_display_->getMoveGroupNS() + "/move_group/default_planning_pipeline",
-                       default_planning_pipeline_);
 }
 
 void MotionPlanningFrame::disable()

moveit_ros/visualization/rviz_plugin_render_tools/include/moveit/rviz_plugin_render_tools/trajectory_visualization.h

@@ -87,7 +87,7 @@ class TrajectoryVisualization : public QObject
 
   ~TrajectoryVisualization() override;
 
-  virtual void update(float wall_dt, float ros_dt);
+  virtual void update(float wall_dt, float sim_dt);
   virtual void reset();
 
   void onInitialize(const rclcpp::Node::SharedPtr& node, Ogre::SceneNode* scene_node,
@@ -172,6 +172,7 @@ private Q_SLOTS:
   rviz_common::properties::RosTopicProperty* trajectory_topic_property_;
   rviz_common::properties::FloatProperty* robot_path_alpha_property_;
   rviz_common::properties::BoolProperty* loop_display_property_;
+  rviz_common::properties::BoolProperty* use_sim_time_property_;
   rviz_common::properties::BoolProperty* trail_display_property_;
   rviz_common::properties::BoolProperty* interrupt_display_property_;
   rviz_common::properties::ColorProperty* robot_color_property_;

moveit_ros/visualization/rviz_plugin_render_tools/src/trajectory_visualization.cpp

@@ -108,6 +108,10 @@ TrajectoryVisualization::TrajectoryVisualization(rviz_common::properties::Proper
   state_display_time_property_->addOptionStd("0.1s");
   state_display_time_property_->addOptionStd("0.5s");
 
+  use_sim_time_property_ = new rviz_common::properties::BoolProperty(
+      "Use Sim Time", false, "Indicates whether simulation time or wall-time is used for state display timing.", widget,
+      nullptr, this);
+
   loop_display_property_ = new rviz_common::properties::BoolProperty("Loop Animation", false,
                                                                      "Indicates whether the last received path "
                                                                      "is to be animated in a loop",
@@ -410,7 +414,7 @@ void TrajectoryVisualization::dropTrajectory()
   drop_displaying_trajectory_ = true;
 }
 
-void TrajectoryVisualization::update(float wall_dt, float /*ros_dt*/)
+void TrajectoryVisualization::update(float wall_dt, float sim_dt)
 {
   if (drop_displaying_trajectory_)
   {
@@ -460,7 +464,14 @@ void TrajectoryVisualization::update(float wall_dt, float /*ros_dt*/)
   {
     int previous_state = current_state_;
     int waypoint_count = displaying_trajectory_message_->getWayPointCount();
-    current_state_time_ += wall_dt;
+    if (use_sim_time_property_->getBool())
+    {
+      current_state_time_ += sim_dt;
+    }
+    else
+    {
+      current_state_time_ += wall_dt;
+    }
     float tm = getStateDisplayTime();
 
     if (trajectory_slider_panel_ && trajectory_slider_panel_->isVisible() && trajectory_slider_panel_->isPaused())