diff --git a/common/math/geometry/src/spline/catmull_rom_spline.cpp b/common/math/geometry/src/spline/catmull_rom_spline.cpp index d9cd6025150..e91bff28596 100644 --- a/common/math/geometry/src/spline/catmull_rom_spline.cpp +++ b/common/math/geometry/src/spline/catmull_rom_spline.cpp @@ -470,8 +470,8 @@ auto CatmullRomSpline::getSquaredDistanceIn2D( } return line_segments_[0].getSquaredDistanceIn2D(point, s, true); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getSquaredDistanceIn2D(point, index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getSquaredDistanceIn2D(point, s_value, true); } } @@ -508,8 +508,8 @@ auto CatmullRomSpline::getSquaredDistanceVector( } return line_segments_[0].getSquaredDistanceVector(point, s, true); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getSquaredDistanceVector(point, index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getSquaredDistanceVector(point, s_value, true); } } @@ -542,8 +542,8 @@ auto CatmullRomSpline::getPoint(const double s) const -> geometry_msgs::msg::Poi } return line_segments_[0].getPoint(s, true); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getPoint(index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getPoint(s_value, true); } } @@ -597,8 +597,8 @@ auto CatmullRomSpline::getNormalVector(const double s) const -> geometry_msgs::m "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getNormalVector(index_and_s.second, true); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getNormalVector(s_value, true); } } @@ -634,6 +634,14 @@ auto CatmullRomSpline::getTangentVector(const double s) const -> geometry_msgs:: "This message is not originally intended to be displayed, if you see it, please " "contact the developer of traffic_simulator."); default: + /** + * @note The current implementation uses `index_and_s` instead of structured binding + * (`const auto [index, s_value] = getCurveIndexAndS(s)`) because some tests fail + * when using structured binding. The root cause of these test failures is under investigation. + */ + // const auto [index, s_value] = getCurveIndexAndS(s); + // return curves_[index].getTangentVector(s_value, true); + const auto index_and_s = getCurveIndexAndS(s); return curves_[index_and_s.first].getTangentVector(index_and_s.second, true); } @@ -665,8 +673,8 @@ auto CatmullRomSpline::getPose(const double s, const bool fill_pitch) const } return line_segments_[0].getPose(s, true, fill_pitch); default: - const auto index_and_s = getCurveIndexAndS(s); - return curves_[index_and_s.first].getPose(index_and_s.second, true, fill_pitch); + const auto [index, s_value] = getCurveIndexAndS(s); + return curves_[index].getPose(s_value, true, fill_pitch); } } diff --git a/rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp b/rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp index 93e8ef1a4f0..2c9cf6dd306 100644 --- a/rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp +++ b/rviz_plugins/openscenario_visualization/src/openscenario_visualization_condition_groups_plugin/openscenario_visualization_condition_groups_plugin.cpp @@ -171,7 +171,6 @@ void VisualizationConditionGroupsDisplay::processMessage(const Context::ConstSha QPainter painter(&hud); painter.setRenderHint(QPainter::Antialiasing, true); - // QColor text_color = property_text_color_->getColor(); QColor text_color(property_text_color_->getColor()); text_color.setAlpha(255); painter.setPen(QPen(text_color, 2, Qt::SolidLine)); diff --git a/simulation/traffic_simulator/src/visualization/visualization_component.cpp b/simulation/traffic_simulator/src/visualization/visualization_component.cpp index 2af92b13bfe..10c1bc5d968 100644 --- a/simulation/traffic_simulator/src/visualization/visualization_component.cpp +++ b/simulation/traffic_simulator/src/visualization/visualization_component.cpp @@ -74,14 +74,14 @@ void VisualizationComponent::entityStatusCallback( entity_name_lists.emplace_back(data.status.name); } std::vector erase_names; - for (const auto & marker : markers_) { - auto itr = std::find(entity_name_lists.begin(), entity_name_lists.end(), marker.first); + for (const auto & [key, _] : markers_) { + auto itr = std::find(entity_name_lists.begin(), entity_name_lists.end(), key); if (itr == entity_name_lists.end()) { - auto delete_marker = generateDeleteMarker(marker.first); + auto delete_marker = generateDeleteMarker(key); std::copy( delete_marker.markers.begin(), delete_marker.markers.end(), std::back_inserter(current_marker.markers)); - erase_names.emplace_back(marker.first); + erase_names.emplace_back(key); } } for (const auto & name : erase_names) { @@ -336,7 +336,6 @@ const visualization_msgs::msg::MarkerArray VisualizationComponent::generateMarke arrow.id = 2; arrow.action = visualization_msgs::msg::Marker::ADD; - // constexpr double arrow_size = 0.3; double arrow_size = 0.4 * status.bounding_box.dimensions.y; constexpr double arrow_ratio = 1.0; geometry_msgs::msg::Point pf, pl, pr;