feat: added an option to set all the parameters directly via services

Signed-off-by: Kenzo Lobos-Tsunekawa <[email protected]>

nebula_messages/continental_srvs/srv/ContinentalArs548SetSensorMounting.srv

@@ -1,3 +1,9 @@
+bool autoconfigure_extrinsics 1
+float32 longitudinal
+float32 lateral
+float32 vertical
+float32 yaw
+float32 pitch
 bool plug_orientation # 0 = PLUG_RIGHT. 1 = PLUG_LEFT
 ---
 bool success

nebula_messages/continental_srvs/srv/ContinentalArs548SetVehicleParameters.srv

@@ -1,3 +1,7 @@
+float32 vehicle_length -1.0
+float32 vehicle_width -1.0
+float32 vehicle_height -1.0
+float32 vehicle_wheelbase -1.0
 ---
 bool success
 string message # status messages

nebula_ros/src/continental/continental_ars548_hw_interface_ros_wrapper.cpp

@@ -412,29 +412,46 @@ void ContinentalARS548HwInterfaceRosWrapper::SetSensorMountingRequestCallback(
   auto tf_buffer = std::make_unique<tf2_ros::Buffer>(this->get_clock());
   auto tf_listener = std::make_unique<tf2_ros::TransformListener>(*tf_buffer);
 
-  geometry_msgs::msg::TransformStamped base_to_sensor_tf;
-  try {
-    base_to_sensor_tf = tf_buffer->lookupTransform(
-      sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0),
-      rclcpp::Duration::from_seconds(0.5));
-  } catch (tf2::TransformException & ex) {
-    RCLCPP_ERROR(
-      this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)",
-      sensor_configuration_.frame_id.c_str(), ex.what());
-    response->success = false;
-    response->message = ex.what();
-    return;
+  float longitudinal = request->longitudinal;
+  float lateral = request->lateral;
+  float vertical = request->vertical;
+  float yaw = request->yaw;
+  float pitch = request->pitch;
+
+  if (request->autoconfigure_extrinsics) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "autoconfigure_extrinsics was set to true, so the mounting position will be derived from the "
+      "tfs");
+
+    geometry_msgs::msg::TransformStamped base_to_sensor_tf;
+    try {
+      base_to_sensor_tf = tf_buffer->lookupTransform(
+        sensor_configuration_.base_frame, sensor_configuration_.frame_id, rclcpp::Time(0),
+        rclcpp::Duration::from_seconds(0.5));
+    } catch (tf2::TransformException & ex) {
+      RCLCPP_ERROR(
+        this->get_logger(), "Could not obtain the transform from the base frame to %s (%s)",
+        sensor_configuration_.frame_id.c_str(), ex.what());
+      response->success = false;
+      response->message = ex.what();
+      return;
+    }
+
+    const auto & quat = base_to_sensor_tf.transform.rotation;
+    geometry_msgs::msg::Vector3 rpy;
+    tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z);
+
+    longitudinal = base_to_sensor_tf.transform.translation.x -
+                   sensor_configuration_.configuration_vehicle_wheelbase;
+    lateral = base_to_sensor_tf.transform.translation.y;
+    vertical = base_to_sensor_tf.transform.translation.z;
+    yaw = rpy.z;
+    pitch = rpy.y;
   }
 
-  const auto & quat = base_to_sensor_tf.transform.rotation;
-  geometry_msgs::msg::Vector3 rpy;
-  tf2::Matrix3x3(tf2::Quaternion(quat.x, quat.y, quat.z, quat.w)).getRPY(rpy.x, rpy.y, rpy.z);
-
   auto result = hw_interface_.SetSensorMounting(
-    base_to_sensor_tf.transform.translation.x -
-      sensor_configuration_.configuration_vehicle_wheelbase,
-    base_to_sensor_tf.transform.translation.y, base_to_sensor_tf.transform.translation.z, rpy.z,
-    rpy.y, request->plug_orientation);
+    longitudinal, lateral, vertical, yaw, pitch, request->plug_orientation);
 
   response->success = result == Status::OK;
   response->message = (std::stringstream() << result).str();
@@ -447,12 +464,46 @@ void ContinentalARS548HwInterfaceRosWrapper::SetVehicleParametersRequestCallback
   const std::shared_ptr<continental_srvs::srv::ContinentalArs548SetVehicleParameters::Response>
     response)
 {
+  float vehicle_length = request->vehicle_length;
+  float vehicle_width = request->vehicle_width;
+  float vehicle_height = request->vehicle_height;
+  float vehicle_wheelbase = request->vehicle_wheelbase;
+
+  if (vehicle_length < 0.f) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Service vehicle_length is invalid. Falling back to configuration value (%.2f)",
+      sensor_configuration_.configuration_vehicle_length);
+    vehicle_length = sensor_configuration_.configuration_vehicle_length;
+  }
+
+  if (vehicle_width < 0.f) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Service vehicle_width is invalid. Falling back to configuration value (%.2f)",
+      sensor_configuration_.configuration_vehicle_width);
+    vehicle_width = sensor_configuration_.configuration_vehicle_width;
+  }
+
+  if (vehicle_height < 0.f) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Service vehicle_height is invalid. Falling back to configuration value (%.2f)",
+      sensor_configuration_.configuration_vehicle_height);
+    vehicle_height = sensor_configuration_.configuration_vehicle_height;
+  }
+
+  if (vehicle_wheelbase < 0.f) {
+    RCLCPP_INFO(
+      this->get_logger(),
+      "Service vehicle_wheelbase is invalid. Falling back to configuration value (%.2f)",
+      sensor_configuration_.configuration_vehicle_wheelbase);
+    vehicle_wheelbase = sensor_configuration_.configuration_vehicle_wheelbase;
+  }
+
   std::scoped_lock lock(mtx_config_);
   auto result = hw_interface_.SetVehicleParameters(
-    sensor_configuration_.configuration_vehicle_length,
-    sensor_configuration_.configuration_vehicle_width,
-    sensor_configuration_.configuration_vehicle_height,
-    sensor_configuration_.configuration_vehicle_wheelbase);
+    vehicle_length, vehicle_width, vehicle_height, vehicle_wheelbase);
 
   response->success = result == Status::OK;
   response->message = (std::stringstream() << result).str();