Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

make Entity lane-changeable everywhere #1468

Merged
merged 10 commits into from
Dec 10, 2024
+32 −19
Merged

make Entity lane-changeable everywhere #1468

Show file tree
Hide file tree
Changes from 9 commits
Commits
Show all changes
10 commits
Select commit Hold shift + click to select a range
b105401
chore: use 'solid' as a subtype of linestrings between road and road …
HansRobo Dec 3, 2024
4de123f
feat: allow lane-change everywhere in `VEHICLE_WITH_ROAD_SHOULDER` ro…
HansRobo Dec 3, 2024
2037d18
Merge remote-tracking branch 'origin/master' into feature/lane-change…
HansRobo Dec 3, 2024
615baba
chore: fix linelint error
HansRobo Dec 3, 2024
e6f8c24
fix:　use lanelet2 functions correctly in HdMapUtils::getLeftLaneletId…
HansRobo Dec 5, 2024
681460f
replace true with false for include_opposite_direction option
HansRobo Dec 5, 2024
ab7ab29
Merge remote-tracking branch 'origin/master' into feature/lane-change…
HansRobo Dec 5, 2024
d5c36d7
fix: fit test constants to latest algorithm
HansRobo Dec 5, 2024
9e4fdaf
Merge branch 'master' into feature/lane-change-everywhere
HansRobo Dec 9, 2024
2ebc41a
fix:　disable default argument value for include_oppsite_direction in …
HansRobo Dec 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
6 changes: 3 additions & 3 deletions map/kashiwanoha_map/map/road_shoulder_added/lanelet2_map.osm
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3477,7 +3477,7 @@
<nd ref="30442"/>
<nd ref="14512"/>
<tag k="type" v="line_thin"/>
<tag k="subtype" v="dashed"/>
<tag k="subtype" v="solid"/>
</way>
<way id="34506">
<nd ref="30444"/>
Expand All @@ -3489,7 +3489,7 @@
<nd ref="13864"/>
<nd ref="30442"/>
<tag k="type" v="line_thin"/>
<tag k="subtype" v="dashed"/>
<tag k="subtype" v="solid"/>
</way>
<way id="34509">
<nd ref="13866"/>
Expand All @@ -3501,7 +3501,7 @@
<nd ref="13369"/>
<nd ref="13864"/>
<tag k="type" v="line_thin"/>
<tag k="subtype" v="dashed"/>
<tag k="subtype" v="solid"/>
</way>
<way id="34512">
<nd ref="13371"/>
Expand Down
2 changes: 1 addition & 1 deletion mock/cpp_mock_scenarios/src/measurement/get_distance_in_lane_coordinate_distance.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -85,7 +85,7 @@ class GetDistanceInLaneCoordinateScenario : public cpp_mock_scenarios::CppScenar
to_entity && to_entity->laneMatchingSucceed()) {
return traffic_simulator::distance::longitudinalDistance(
from_entity->getCanonicalizedLaneletPose().value(),
to_entity->getCanonicalizedLaneletPose().value(), false, true,
to_entity->getCanonicalizedLaneletPose().value(), false, false,
traffic_simulator::RoutingConfiguration(), api_.getHdmapUtils());
}
}
Expand Down
6 changes: 3 additions & 3 deletions simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/hdmap_utils.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -204,7 +204,7 @@ class HdMapUtils

auto getLeftLaneletIds(
const lanelet::Id, const traffic_simulator::RoutingGraphType,
const bool include_opposite_direction = true) const -> lanelet::Ids;
const bool include_opposite_direction = false) const -> lanelet::Ids;
HansRobo marked this conversation as resolved.
Show resolved Hide resolved

auto getLongitudinalDistance(
const traffic_simulator_msgs::msg::LaneletPose & from_pose,
Expand Down Expand Up @@ -269,7 +269,7 @@ class HdMapUtils

auto getRightLaneletIds(
const lanelet::Id, const traffic_simulator::RoutingGraphType,
const bool include_opposite_direction = true) const -> lanelet::Ids;
const bool include_opposite_direction = false) const -> lanelet::Ids;

auto getRightOfWayLaneletIds(const lanelet::Ids &) const
-> std::unordered_map<lanelet::Id, lanelet::Ids>;
Expand Down Expand Up @@ -367,7 +367,7 @@ class HdMapUtils

auto toLaneletPoses(
const geometry_msgs::msg::Pose &, const lanelet::Id, const double matching_distance = 5.0,
const bool include_opposite_direction = true,
const bool include_opposite_direction = false,
const traffic_simulator::RoutingGraphType type =
traffic_simulator::RoutingConfiguration().routing_graph_type) const
-> std::vector<traffic_simulator_msgs::msg::LaneletPose>;
Expand Down
3 changes: 3 additions & 0 deletions simulation/traffic_simulator/include/traffic_simulator/hdmap_utils/traffic_rules.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -66,6 +66,9 @@ class GermanRoadShoulderPassableVehicle : public lanelet::traffic_rules::GermanV
return startsWith(this->participant(), participant);
});
}

lanelet::traffic_rules::LaneChangeType laneChangeType(
const lanelet::ConstLineString3d &, bool) const override;
};
} // namespace hdmap_utils
#endif // TRAFFIC_SIMULATOR__HDMAP_UTILS__TRAFFIC_RULES_HPP_
4 changes: 2 additions & 2 deletions simulation/traffic_simulator/src/entity/entity_base.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -719,7 +719,7 @@ auto EntityBase::requestSynchronize(
lane_changeable_routing_configuration.allow_lane_change = true;

const auto entity_distance = longitudinalDistance(
entity_lanelet_pose.value(), entity_target, true, true,
entity_lanelet_pose.value(), entity_target, true, false,
lane_changeable_routing_configuration, hdmap_utils_ptr_);
if (!entity_distance.has_value()) {
THROW_SEMANTIC_ERROR(
Expand All @@ -735,7 +735,7 @@ auto EntityBase::requestSynchronize(

const auto target_entity_distance = longitudinalDistance(
CanonicalizedLaneletPose(target_entity_lanelet_pose, hdmap_utils_ptr_), target_sync_pose,
true, true, lane_changeable_routing_configuration, hdmap_utils_ptr_);
true, false, lane_changeable_routing_configuration, hdmap_utils_ptr_);
if (!target_entity_distance.has_value() || target_entity_distance.value() < 0.0) {
RCLCPP_WARN_ONCE(
rclcpp::get_logger("traffic_simulator"),
Expand Down
13 changes: 7 additions & 6 deletions simulation/traffic_simulator/src/hdmap_utils/hdmap_utils.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -1183,11 +1183,11 @@ auto HdMapUtils::getLeftLaneletIds(
const bool include_opposite_direction) const -> lanelet::Ids
{
if (include_opposite_direction) {
throw common::Error(
"HdMapUtils::getLeftLaneletIds with include_opposite_direction=true is not implemented yet.");
} else {
return getLaneletIds(
routing_graphs_->routing_graph(type)->lefts(lanelet_map_ptr_->laneletLayer.get(lanelet_id)));
} else {
return getLaneletIds(routing_graphs_->routing_graph(type)->adjacentLefts(
lanelet_map_ptr_->laneletLayer.get(lanelet_id)));
}
}

Expand All @@ -1196,11 +1196,12 @@ auto HdMapUtils::getRightLaneletIds(
const bool include_opposite_direction) const -> lanelet::Ids
{
if (include_opposite_direction) {
throw common::Error(
"HdMapUtils::getRightLaneletIds with include_opposite_direction=true is not implemented "
"yet.");
} else {
return getLaneletIds(
routing_graphs_->routing_graph(type)->rights(lanelet_map_ptr_->laneletLayer.get(lanelet_id)));
} else {
return getLaneletIds(routing_graphs_->routing_graph(type)->adjacentRights(
lanelet_map_ptr_->laneletLayer.get(lanelet_id)));
}
}

Expand Down
8 changes: 8 additions & 0 deletions simulation/traffic_simulator/src/hdmap_utils/traffic_rules.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -19,3 +19,11 @@
lanelet::traffic_rules::RegisterTrafficRules<hdmap_utils::GermanRoadShoulderPassableVehicle>
germanRoadShoulderPassableVehicleRules(
hdmap_utils::Locations::RoadShoulderPassableGermany, lanelet::Participants::Vehicle);

lanelet::traffic_rules::LaneChangeType
hdmap_utils::GermanRoadShoulderPassableVehicle::laneChangeType(
const lanelet::ConstLineString3d &, bool) const
{
/// @note allow lane-changes everywhere even if prohibited by lanelet2 map, because lane-change settings are not for entities but only for Autoware.
return lanelet::traffic_rules::LaneChangeType::Both;
}
4 changes: 2 additions & 2 deletions simulation/traffic_simulator/test/src/utils/test_distance.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -524,7 +524,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch
pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration,
hdmap_utils_ptr);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(result.value(), 103.0, 1.0);
EXPECT_DOUBLE_EQ(result.value(), 97.648110014340688);
}
{
const auto pose_from = traffic_simulator::toCanonicalizedLaneletPose(
Expand All @@ -540,7 +540,7 @@ TEST_F(distanceTest_IntersectionMap, longitudinalDistance_adjacent_noOpposite_ch
pose_from.value(), pose_to.value(), true, false, lane_changeable_routing_configuration,
hdmap_utils_ptr);
ASSERT_TRUE(result.has_value());
EXPECT_NEAR(result.value(), 131.0, 1.0);
EXPECT_DOUBLE_EQ(result.value(), 127.99532311325152);
}
}

Expand Down
Loading