Merge branch 'humble' into 2024-07-15_revert_breaking_capabilities_ch…

…ange
moveit · Aug 19, 2024 · 40c7abc · 40c7abc
2 parents 920dcb2 + d583c04
commit 40c7abc
Show file tree

Hide file tree

Showing 5 changed files with 15 additions and 8 deletions.
diff --git a/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp b/moveit_planners/pilz_industrial_motion_planner/src/trajectory_generator_lin.cpp
@@ -34,6 +34,8 @@
 
 #include <pilz_industrial_motion_planner/trajectory_generator_lin.h>
 
+#include <pilz_industrial_motion_planner/tip_frame_getter.h>
+
 #include <cassert>
 #include <sstream>
 #include <time.h>
@@ -75,7 +77,7 @@ void TrajectoryGeneratorLIN::extractMotionPlanInfo(const planning_scene::Plannin
   // goal given in joint space
   if (!req.goal_constraints.front().joint_constraints.empty())
   {
-    info.link_name = robot_model_->getJointModelGroup(req.group_name)->getSolverInstance()->getTipFrame();
+    info.link_name = getSolverTipFrame(robot_model_->getJointModelGroup(req.group_name));
 
     if (req.goal_constraints.front().joint_constraints.size() !=
         robot_model_->getJointModelGroup(req.group_name)->getActiveJointModelNames().size())

diff --git a/...ners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp b/...ners/pilz_industrial_motion_planner/test/unit_tests/src/unittest_trajectory_functions.cpp
@@ -220,6 +220,10 @@ TEST_F(TrajectoryFunctionsTestFlangeAndGripper, testIKSolver)
   const moveit::core::JointModelGroup* jmg = robot_model_->getJointModelGroup(planning_group_);
   const kinematics::KinematicsBaseConstPtr& solver = jmg->getSolverInstance();
 
+  if (!solver)
+  {
+    throw("No IK solver configured for group '" + planning_group_ + "'");
+  }
   // robot state
   moveit::core::RobotState rstate(robot_model_);
 

diff --git a/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp b/moveit_plugins/moveit_ros_control_interface/src/controller_manager_plugin.cpp
@@ -418,7 +418,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
       ControllersMap::iterator c = managed_controllers_.find(it);
       if (c != managed_controllers_.end())
       {  // controller belongs to this manager
-        request->stop_controllers.push_back(c->second.name);
+        request->deactivate_controllers.push_back(c->second.name);
         claimed_resources.right.erase(c->second.name);  // remove resources
       }
     }
@@ -430,16 +430,16 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
       ControllersMap::iterator c = managed_controllers_.find(it);
       if (c != managed_controllers_.end())
       {  // controller belongs to this manager
-        request->start_controllers.push_back(c->second.name);
+        request->activate_controllers.push_back(c->second.name);
         for (const auto& required_resource : c->second.required_command_interfaces)
         {
           resources_bimap::right_const_iterator res = claimed_resources.right.find(required_resource);
           if (res != claimed_resources.right.end())
           {  // resource is claimed
-            if (std::find(request->stop_controllers.begin(), request->stop_controllers.end(), res->second) ==
-                request->stop_controllers.end())
+            if (std::find(request->deactivate_controllers.begin(), request->deactivate_controllers.end(),
+                          res->second) == request->deactivate_controllers.end())
             {
-              request->stop_controllers.push_back(res->second);  // add claiming controller to stop list
+              request->deactivate_controllers.push_back(res->second);  // add claiming controller to stop list
             }
             claimed_resources.left.erase(res->second);  // remove claimed resources
           }
@@ -451,7 +451,7 @@ class Ros2ControlManager : public moveit_controller_manager::MoveItControllerMan
     // successfully activated or deactivated.
     request->strictness = controller_manager_msgs::srv::SwitchController::Request::STRICT;
 
-    if (!request->start_controllers.empty() || !request->stop_controllers.empty())
+    if (!request->activate_controllers.empty() || !request->deactivate_controllers.empty())
     {  // something to switch?
       auto result_future = switch_controller_service_->async_send_request(request);
       if (result_future.wait_for(std::chrono::duration<double>(SERVICE_CALL_TIMEOUT)) == std::future_status::timeout)

diff --git a/...troller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h b/...troller_manager/include/moveit_simple_controller_manager/action_based_controller_handle.h
@@ -145,7 +145,7 @@ class ActionBasedControllerHandle : public ActionBasedControllerHandleBase
         do
         {
           status = result_future.wait_for(50ms);
-          if ((status == std::future_status::timeout) and ((node_->now() - start) > timeout))
+          if ((status == std::future_status::timeout) && ((node_->now() - start) > timeout))
           {
             RCLCPP_WARN(LOGGER, "waitForExecution timed out");
             return false;

diff --git a/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp b/moveit_ros/visualization/rviz_plugin_render_tools/src/planning_scene_render.cpp
@@ -109,6 +109,7 @@ void PlanningSceneRender::renderPlanningScene(const planning_scene::PlanningScen
       color.r = c.r;
       color.g = c.g;
       color.b = c.b;
+      color.a = c.a;
       alpha = c.a;
     }
     for (std::size_t j = 0; j < object->shapes_.size(); ++j)