Merge branch 'main' into pr-add_param_api_unittest

Author: sjahr

moveit_core/robot_model/src/joint_model_group.cpp

@@ -229,6 +229,14 @@ JointModelGroup::JointModelGroup(const std::string& group_name, const srdf::Mode
   {
     link_model_map_[link_model->getName()] = link_model;
     link_model_name_vector_.push_back(link_model->getName());
+    // if this is the first link of the group with a valid parent and includes geometry (for example `base_link`) it should included
+    if (link_model_with_geometry_vector_.empty() && link_model->getParentLinkModel() &&
+        !link_model->getParentLinkModel()->getShapes().empty())
+    {
+      link_model_with_geometry_vector_.push_back(link_model->getParentLinkModel());
+      link_model_with_geometry_name_vector_.push_back(link_model->getParentLinkModel()->getName());
+    }
+    // all child links with collision geometry should also be included
     if (!link_model->getShapes().empty())
     {
       link_model_with_geometry_vector_.push_back(link_model);

moveit_ros/moveit_servo/src/servo_node.cpp

@@ -75,19 +75,18 @@ ServoNode::ServoNode(const rclcpp::NodeOptions& options)
                        "in the launch file");
   }
 
-  // Check if a realtime kernel is available
-  if (realtime_tools::has_realtime_kernel())
+  // Configure SCHED_FIFO and priority
+  if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority))
   {
-    if (realtime_tools::configure_sched_fifo(servo_params_.thread_priority))
-    {
-      RCLCPP_INFO_STREAM(node_->get_logger(), "Realtime kernel available, higher thread priority has been set.");
-    }
-    else
-    {
-      RCLCPP_WARN_STREAM(node_->get_logger(), "Could not enable FIFO RT scheduling policy.");
-    }
+    RCLCPP_INFO_STREAM(node_->get_logger(), "Enabled SCHED_FIFO and higher thread priority.");
   }
   else
+  {
+    RCLCPP_WARN_STREAM(node_->get_logger(), "Could not enable FIFO RT scheduling policy. Continuing with the default.");
+  }
+
+  // Check if a realtime kernel is available
+  if (!realtime_tools::has_realtime_kernel())
   {
     RCLCPP_WARN_STREAM(node_->get_logger(), "Realtime kernel is recommended for better performance.");
   }