Only append to start_modes in prepare_switch (#1344)

Before the main control mode (position/velocity) was always overwriting
the complete list for the joint. That worked fine when that was the only
control mode for a joint or if it was the first in the list. But that is
not guaranteed.
Since we create the list as an empty vector beforehand, anyway, we can
simply append to that list in any case. The list will be checked for
compatibility at a later point, anyway.

Author: urfeex

ur_robot_driver/src/hardware_interface.cpp

@@ -1099,12 +1099,6 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
     }
   }
 
-  if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(),
-                   [&](const std::vector<std::string>& other) { return other == start_modes_[0]; })) {
-    RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same.");
-    return hardware_interface::return_type::ERROR;
-  }
-
   // Starting interfaces
   // If a joint has been reserved already, raise an error.
   // Modes that are not directly mapped to a single joint such as force_mode reserve all joints.
@@ -1119,7 +1113,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
                                      "requested.");
           return hardware_interface::return_type::ERROR;
         }
-        start_modes_[i] = { hardware_interface::HW_IF_POSITION };
+        start_modes_[i].push_back(hardware_interface::HW_IF_POSITION);
       } else if (key == info_.joints[i].name + "/" + hardware_interface::HW_IF_VELOCITY) {
         if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
               return item == hardware_interface::HW_IF_POSITION || item == PASSTHROUGH_GPIO ||
@@ -1129,7 +1123,7 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
                                      "requested.");
           return hardware_interface::return_type::ERROR;
         }
-        start_modes_[i] = { hardware_interface::HW_IF_VELOCITY };
+        start_modes_[i].push_back(hardware_interface::HW_IF_VELOCITY);
       } else if (key == tf_prefix + FORCE_MODE_GPIO + "/type") {
         if (std::any_of(start_modes_[i].begin(), start_modes_[i].end(), [&](const std::string& item) {
               return item == hardware_interface::HW_IF_POSITION || item == hardware_interface::HW_IF_VELOCITY;
@@ -1169,6 +1163,12 @@ hardware_interface::return_type URPositionHardwareInterface::prepare_command_mod
     }
   }
 
+  if (!std::all_of(start_modes_.begin() + 1, start_modes_.end(),
+                   [&](const std::vector<std::string>& other) { return other == start_modes_[0]; })) {
+    RCLCPP_ERROR(get_logger(), "Start modes of all joints have to be the same.");
+    return hardware_interface::return_type::ERROR;
+  }
+
   // Stopping interfaces
   // add stop interface per joint in tmp var for later check
   for (const auto& key : stop_interfaces) {