Skip to content

Commit dcaef8d

Browse files
gmajrobotecgmajrobotec
committed
RJD-1509 Filter nearest NPCs to calculate distance
1 parent dc503c6 commit dcaef8d

File tree

11 files changed

+69
-28
lines changed

11 files changed

+69
-28
lines changed

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/action_node.hpp‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -86,6 +86,7 @@ class ActionNode : public BT::ActionNodeBase
8686
BT::InputPort<std::shared_ptr<traffic_simulator::CanonicalizedEntityStatus>>("canonicalized_entity_status"),
8787
BT::InputPort<std::shared_ptr<traffic_simulator::TrafficLightsBase>>("traffic_lights"),
8888
BT::InputPort<traffic_simulator::behavior::Request>("request"),
89+
BT::InputPort<std::shared_ptr<DistancesMap>>("distances_map"),
8990
BT::OutputPort<std::optional<traffic_simulator_msgs::msg::Obstacle>>("obstacle"),
9091
BT::OutputPort<traffic_simulator_msgs::msg::WaypointsArray>("waypoints"),
9192
BT::OutputPort<traffic_simulator::behavior::Request>("request"),
@@ -116,6 +117,7 @@ class ActionNode : public BT::ActionNodeBase
116117
std::optional<double> target_speed;
117118
EntityStatusDict other_entity_status;
118119
lanelet::Ids route_lanelets;
120+
std::shared_ptr<DistancesMap> distances_map;
119121

120122
auto getDistanceToTargetEntity(
121123
const math::geometry::CatmullRomSplineInterface & spline,

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/pedestrian/behavior_tree.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -70,6 +70,7 @@ class PedestrianBehaviorTree : public BehaviorPluginBase
7070
DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr<traffic_simulator::TrafficLightsBase>)
7171
DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters)
7272
DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray)
73+
DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr<DistancesMap>)
7374
// clang-format on
7475

7576
#undef DEFINE_GETTER_SETTER

‎simulation/behavior_tree_plugin/include/behavior_tree_plugin/vehicle/behavior_tree.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -74,6 +74,7 @@ class VehicleBehaviorTree : public BehaviorPluginBase
7474
DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr<traffic_simulator::TrafficLightsBase>)
7575
DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters)
7676
DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray)
77+
DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr<DistancesMap>)
7778
// clang-format on
7879
#undef DEFINE_GETTER_SETTER
7980

‎simulation/behavior_tree_plugin/src/action_node.cpp‎

Lines changed: 23 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -91,6 +91,9 @@ auto ActionNode::getBlackBoardValues() -> void
9191
if (!getInput<lanelet::Ids>("route_lanelets", route_lanelets)) {
9292
THROW_SIMULATION_ERROR("failed to get input route_lanelets in ActionNode");
9393
}
94+
if (!getInput<std::shared_ptr<DistancesMap>>("distances_map", distances_map)) {
95+
THROW_SIMULATION_ERROR("failed to get input distances_map in ActionNode");
96+
}
9497
}
9598

9699
auto ActionNode::getHorizon() const -> double
@@ -237,10 +240,21 @@ auto ActionNode::getDistanceToFrontEntity(
237240
auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterface & spline) const
238241
-> std::optional<std::string>
239242
{
240-
std::vector<double> distances;
241-
std::vector<std::string> entities;
242-
for (const auto & [name, status] : other_entity_status) {
243-
const auto distance = getDistanceToTargetEntity(spline, status);
243+
if (distances_map == nullptr) {
244+
return std::nullopt;
245+
}
246+
std::map<double, std::string> rough_distances;
247+
for (const auto & [name_pair, distance] : *distances_map) {
248+
if (
249+
name_pair.first == canonicalized_entity_status->getName() && distance < spline.getLength()) {
250+
rough_distances.emplace(distance, name_pair.second);
251+
} else if (
252+
name_pair.second == canonicalized_entity_status->getName() && distance < spline.getLength()) {
253+
rough_distances.emplace(distance, name_pair.first);
254+
}
255+
}
256+
257+
for (const auto & [rough_distance, name] : rough_distances) {
244258
const auto quat = math::geometry::getRotation(
245259
canonicalized_entity_status->getMapPose().orientation,
246260
other_entity_status.at(name).getMapPose().orientation);
@@ -250,21 +264,14 @@ auto ActionNode::getFrontEntityName(const math::geometry::CatmullRomSplineInterf
250264
if (
251265
std::fabs(math::geometry::convertQuaternionToEulerAngle(quat).z) <=
252266
boost::math::constants::half_pi<double>()) {
253-
if (distance && distance.value() < 40) {
254-
entities.emplace_back(name);
255-
distances.emplace_back(distance.value());
267+
const auto distance = getDistanceToTargetEntity(spline, other_entity_status.at(name));
268+
269+
if (distance && distance.value() < spline.getLength()) {
270+
return name;
256271
}
257272
}
258273
}
259-
if (entities.size() != distances.size()) {
260-
THROW_SIMULATION_ERROR("size of entities and distances vector does not match.");
261-
}
262-
if (distances.empty()) {
263-
return std::nullopt;
264-
}
265-
std::vector<double>::iterator iter = std::min_element(distances.begin(), distances.end());
266-
size_t index = std::distance(distances.begin(), iter);
267-
return entities[index];
274+
return std::nullopt;
268275
}
269276

270277
auto ActionNode::getDistanceToTargetEntityOnCrosswalk(

‎simulation/do_nothing_plugin/include/do_nothing_plugin/plugin.hpp‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@ public: \
5858
DEFINE_GETTER_SETTER(TrafficLights, std::shared_ptr<traffic_simulator::TrafficLightsBase>)
5959
DEFINE_GETTER_SETTER(VehicleParameters, traffic_simulator_msgs::msg::VehicleParameters)
6060
DEFINE_GETTER_SETTER(Waypoints, traffic_simulator_msgs::msg::WaypointsArray)
61+
DEFINE_GETTER_SETTER(DistancesMap, std::shared_ptr<DistancesMap>)
6162
// clang-format on
6263
#undef DEFINE_GETTER_SETTER
6364

‎simulation/traffic_simulator/include/traffic_simulator/behavior/behavior_plugin_base.hpp‎

Lines changed: 10 additions & 7 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,8 @@ namespace entity_behavior
3636
using EntityStatusDict =
3737
std::unordered_map<std::string, traffic_simulator::CanonicalizedEntityStatus>;
3838

39+
using DistancesMap = std::unordered_map<std::pair<std::string, std::string>, double>;
40+
3941
class BehaviorPluginBase
4042
{
4143
public:
@@ -44,13 +46,13 @@ class BehaviorPluginBase
4446
virtual auto update(const double current_time, const double step_time) -> void = 0;
4547
virtual const std::string & getCurrentAction() const = 0;
4648

47-
#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \
48-
virtual TYPE get##NAME() = 0; \
49-
virtual void set##NAME(const TYPE & value) = 0; \
50-
auto get##NAME##Key() const->const std::string & \
51-
{ \
52-
static const std::string key = KEY; \
53-
return key; \
49+
#define DEFINE_GETTER_SETTER(NAME, KEY, TYPE) \
50+
virtual TYPE get##NAME() = 0; \
51+
virtual void set##NAME(const TYPE & value) = 0; \
52+
auto get##NAME##Key() const -> const std::string & \
53+
{ \
54+
static const std::string key = KEY; \
55+
return key; \
5456
}
5557

5658
// clang-format off
@@ -74,6 +76,7 @@ class BehaviorPluginBase
7476
DEFINE_GETTER_SETTER(TrafficLights, "traffic_lights", std::shared_ptr<traffic_simulator::TrafficLightsBase>)
7577
DEFINE_GETTER_SETTER(VehicleParameters, "vehicle_parameters", traffic_simulator_msgs::msg::VehicleParameters)
7678
DEFINE_GETTER_SETTER(Waypoints, "waypoints", traffic_simulator_msgs::msg::WaypointsArray)
79+
DEFINE_GETTER_SETTER(DistancesMap, "distances_map", std::shared_ptr<DistancesMap>)
7780
// clang-format on
7881
#undef DEFINE_GETTER_SETTER
7982
};

‎simulation/traffic_simulator/include/traffic_simulator/entity/entity_base.hpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -47,6 +47,7 @@ namespace traffic_simulator
4747
{
4848
namespace entity
4949
{
50+
using DistancesMap = std::unordered_map<std::pair<std::string, std::string>, double>;
5051
class EntityBase : public std::enable_shared_from_this<EntityBase>
5152
{
5253
public:
@@ -312,6 +313,8 @@ class EntityBase : public std::enable_shared_from_this<EntityBase>
312313

313314
bool verbose;
314315

316+
void setDistances(const std::shared_ptr<DistancesMap> & distances);
317+
315318
protected:
316319
std::shared_ptr<CanonicalizedEntityStatus> status_;
317320

@@ -333,6 +336,8 @@ class EntityBase : public std::enable_shared_from_this<EntityBase>
333336
std::unique_ptr<traffic_simulator::longitudinal_speed_planning::LongitudinalSpeedPlanner>
334337
speed_planner_;
335338

339+
std::shared_ptr<DistancesMap> distances_map_;
340+
336341
private:
337342
virtual auto requestSpeedChangeWithConstantAcceleration(
338343
const double target_speed, const speed_change::Transition, const double acceleration,

‎simulation/traffic_simulator/include/traffic_simulator/entity/entity_manager.hpp‎

Lines changed: 3 additions & 2 deletions
Original file line numberDiff line numberDiff line change
@@ -93,8 +93,9 @@ class EntityManager
9393
// update
9494
auto update(const double current_time, const double step_time) -> void;
9595

96-
auto updateNpcLogic(const std::string & name, const double current_time, const double step_time)
97-
-> const CanonicalizedEntityStatus &;
96+
auto updateNpcLogic(
97+
const std::string & name, const double current_time, const double step_time,
98+
const std::shared_ptr<DistancesMap> distances) -> const CanonicalizedEntityStatus &;
9899

99100
auto updateHdmapMarker() const -> void;
100101

‎simulation/traffic_simulator/src/entity/entity_base.cpp‎

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -851,5 +851,10 @@ auto EntityBase::requestSynchronize(
851851
return false;
852852
}
853853

854+
void EntityBase::setDistances(const std::shared_ptr<DistancesMap> & distances)
855+
{
856+
distances_map_ = distances;
857+
}
858+
854859
} // namespace entity
855860
} // namespace traffic_simulator

‎simulation/traffic_simulator/src/entity/entity_manager.cpp‎

Lines changed: 17 additions & 3 deletions
Original file line numberDiff line numberDiff line change
@@ -12,6 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15+
#include <geometry/distance.hpp>
1516
#include <geometry/vector3/operator.hpp>
1617
#include <std_msgs/msg/header.hpp>
1718
#include <traffic_simulator/entity/entity_manager.hpp>
@@ -75,8 +76,20 @@ auto EntityManager::update(const double current_time, const double step_time) ->
7576
entity_ptr->setOtherStatus(all_status);
7677
}
7778
all_status.clear();
79+
80+
std::shared_ptr<DistancesMap> distances = std::make_shared<DistancesMap>();
81+
for (auto && [name_from, entity_from] : entities_) {
82+
for (auto && [name_to, entity_to] : entities_) {
83+
if (const auto pair = std::minmax(name_from, name_to);
84+
distances->find(pair) == distances->end() && name_from != name_to) {
85+
distances->emplace(
86+
pair, math::geometry::getDistance(entity_from->getMapPose(), entity_to->getMapPose()));
87+
}
88+
}
89+
}
90+
7891
for (const auto & [name, entity_ptr] : entities_) {
79-
all_status.try_emplace(name, updateNpcLogic(name, current_time, step_time));
92+
all_status.try_emplace(name, updateNpcLogic(name, current_time, step_time, distances));
8093
}
8194
for (const auto & [name, entity_ptr] : entities_) {
8295
entity_ptr->setOtherStatus(all_status);
@@ -108,15 +121,16 @@ auto EntityManager::update(const double current_time, const double step_time) ->
108121
}
109122

110123
auto EntityManager::updateNpcLogic(
111-
const std::string & name, const double current_time, const double step_time)
112-
-> const CanonicalizedEntityStatus &
124+
const std::string & name, const double current_time, const double step_time,
125+
const std::shared_ptr<DistancesMap> distances) -> const CanonicalizedEntityStatus &
113126
{
114127
if (configuration_.verbose) {
115128
std::cout << "update " << name << " behavior" << std::endl;
116129
}
117130
auto & entity = getEntity(name);
118131
// Update npc completely if logic has started, otherwise update Autoware only - if it is Ego
119132
if (npc_logic_started_) {
133+
entity.setDistances(distances);
120134
entity.onUpdate(current_time, step_time);
121135
} else if (entity.is<entity::EgoEntity>()) {
122136
getEgoEntity(name).updateFieldOperatorApplication();

0 commit comments

Comments
 (0)