@@ -29,11 +29,11 @@ namespace {
2929SCENARIO (" Failed Detect Conflict" )
3030{
3131 rmf_traffic::agv::VehicleTraits traits{
32- {0.5 , 2.0 }, {0.75 , 1.5 },
33- rmf_traffic::Profile{
34- rmf_traffic::geometry::make_final_convex (
35- rmf_traffic::geometry::Circle (0.2 ))
36- }
32+ {0.5 , 2.0 }, {0.75 , 1.5 },
33+ rmf_traffic::Profile{
34+ rmf_traffic::geometry::make_final_convex (
35+ rmf_traffic::geometry::Circle (0.2 ))
36+ }
3737 };
3838
3939 auto graph = rmf_traffic::agv::Graph ();
@@ -44,10 +44,12 @@ SCENARIO("Failed Detect Conflict")
4444 graph.add_waypoint (" test" , Eigen::Vector2d (16.097900000000003 , -12.8518 ));
4545 graph.add_waypoint (" test" , Eigen::Vector2d (13.2777 , -12.980850000000002 ));
4646 graph.add_waypoint (" test" , Eigen::Vector2d (13.325 , -8.2798 ));
47- graph.add_waypoint (" test" , Eigen::Vector2d (117.30075000000001 , -12.863300000000002 ));
47+ graph.add_waypoint (" test" ,
48+ Eigen::Vector2d (117.30075000000001 , -12.863300000000002 ));
4849 graph.add_waypoint (" test" , Eigen::Vector2d (120.03815 , -12.8944 ));
4950 graph.add_waypoint (" test" , Eigen::Vector2d (120.05175 , -8.3695 ));
50- graph.add_waypoint (" test" , Eigen::Vector2d (13.675250000000002 , -28.884750000000004 ));
51+ graph.add_waypoint (" test" ,
52+ Eigen::Vector2d (13.675250000000002 , -28.884750000000004 ));
5153 graph.add_waypoint (" test" , Eigen::Vector2d (66.48685 , -28.959050000000005 ));
5254 graph.add_waypoint (" test" , Eigen::Vector2d (119.97360000000002 , -28.97215 ));
5355 graph.add_waypoint (" test" , Eigen::Vector2d (18.28865 , -8.2941 ));
@@ -58,75 +60,77 @@ SCENARIO("Failed Detect Conflict")
5860 graph.add_waypoint (" test" , Eigen::Vector2d (52.278800000000004 , -8.3107 ));
5961 graph.add_waypoint (" test" , Eigen::Vector2d (63.325 , -8.3315 ));
6062
61- graph.add_lane ( 0 , 1 );
62- graph.add_lane ( 1 , 0 );
63- graph.add_lane ( 1 , 2 );
64- graph.add_lane ( 2 , 1 );
65- graph.add_lane ( 3 , 4 );
66- graph.add_lane ( 4 , 3 );
67- graph.add_lane ( 4 , 5 );
68- graph.add_lane ( 5 , 4 );
69- graph.add_lane ( 6 , 7 );
70- graph.add_lane ( 7 , 6 );
71- graph.add_lane ( 7 , 8 );
72- graph.add_lane ( 8 , 7 );
73- graph.add_lane ( 4 , 9 );
74- graph.add_lane ( 9 , 4 );
75- graph.add_lane ( 1 , 10 );
76- graph.add_lane ( 10 , 1 );
77- graph.add_lane ( 0 , 8 );
78- graph.add_lane ( 8 , 0 );
79- graph.add_lane ( 7 , 11 );
80- graph.add_lane ( 11 , 7 );
81- graph.add_lane ( 5 , 12 );
82- graph.add_lane ( 12 , 13 );
83- graph.add_lane ( 13 , 14 );
84- graph.add_lane ( 14 , 15 );
85- graph.add_lane ( 15 , 16 );
86- graph.add_lane ( 16 , 17 );
87- graph.add_lane ( 17 , 18 );
88- graph.add_lane ( 18 , 0 );
89- graph.add_lane ( 0 , 18 );
90- graph.add_lane ( 18 , 17 );
91- graph.add_lane ( 17 , 16 );
92- graph.add_lane ( 16 , 15 );
93- graph.add_lane ( 15 , 14 );
94- graph.add_lane ( 14 , 13 );
95- graph.add_lane ( 13 , 12 );
96- graph.add_lane ( 12 , 5 );
63+ graph.add_lane (0 , 1 );
64+ graph.add_lane (1 , 0 );
65+ graph.add_lane (1 , 2 );
66+ graph.add_lane (2 , 1 );
67+ graph.add_lane (3 , 4 );
68+ graph.add_lane (4 , 3 );
69+ graph.add_lane (4 , 5 );
70+ graph.add_lane (5 , 4 );
71+ graph.add_lane (6 , 7 );
72+ graph.add_lane (7 , 6 );
73+ graph.add_lane (7 , 8 );
74+ graph.add_lane (8 , 7 );
75+ graph.add_lane (4 , 9 );
76+ graph.add_lane (9 , 4 );
77+ graph.add_lane (1 , 10 );
78+ graph.add_lane (10 , 1 );
79+ graph.add_lane (0 , 8 );
80+ graph.add_lane (8 , 0 );
81+ graph.add_lane (7 , 11 );
82+ graph.add_lane (11 , 7 );
83+ graph.add_lane (5 , 12 );
84+ graph.add_lane (12 , 13 );
85+ graph.add_lane (13 , 14 );
86+ graph.add_lane (14 , 15 );
87+ graph.add_lane (15 , 16 );
88+ graph.add_lane (16 , 17 );
89+ graph.add_lane (17 , 18 );
90+ graph.add_lane (18 , 0 );
91+ graph.add_lane (0 , 18 );
92+ graph.add_lane (18 , 17 );
93+ graph.add_lane (17 , 16 );
94+ graph.add_lane (16 , 15 );
95+ graph.add_lane (15 , 14 );
96+ graph.add_lane (14 , 13 );
97+ graph.add_lane (13 , 12 );
98+ graph.add_lane (12 , 5 );
9799
98100 const auto start_time = std::chrono::steady_clock::now ();
99101
100102 const auto database = std::make_shared<rmf_traffic::schedule::Database>();
101103
102104 rmf_traffic::agv::Planner planner_obstacle = rmf_traffic::agv::Planner{
103- {graph, traits},
104- {nullptr }
105+ {graph, traits},
106+ {nullptr }
105107 };
106108
107109 const auto N = database->participant_ids ().size ();
108110
109111 auto new_obstacle = rmf_traffic::schedule::make_participant (
110- rmf_traffic::schedule::ParticipantDescription{
111- " obstacle_" + std::to_string (N),
112- " obstacles" ,
113- rmf_traffic::schedule::ParticipantDescription::Rx::Unresponsive,
114- planner_obstacle.get_configuration ().vehicle_traits ().profile ()
115- }, database);
112+ rmf_traffic::schedule::ParticipantDescription{
113+ " obstacle_" + std::to_string (N),
114+ " obstacles" ,
115+ rmf_traffic::schedule::ParticipantDescription::Rx::Unresponsive,
116+ planner_obstacle.get_configuration ().vehicle_traits ().profile ()
117+ }, database);
116118
117- auto plan_obstacle = planner_obstacle.plan ({rmf_traffic::time::apply_offset (start_time, 106.362 ), 1 , 90.0 }, {5 });
119+ auto plan_obstacle =
120+ planner_obstacle.plan ({rmf_traffic::time::apply_offset (start_time,
121+ 106.362 ), 1 , 90.0 }, {5 });
118122
119123 new_obstacle.set (plan_obstacle->get_itinerary ());
120124
121125 const auto obstacle_validator =
122- rmf_traffic::agv::ScheduleRouteValidator::make (
123- database, std::numeric_limits<std::size_t >::max (), traits.profile ());
126+ rmf_traffic::agv::ScheduleRouteValidator::make (
127+ database, std::numeric_limits<std::size_t >::max (), traits.profile ());
124128
125129 rmf_traffic::agv::Planner::Options options (obstacle_validator);
126130 options.saturation_limit (100000 );
127131 rmf_traffic::agv::Planner planner = rmf_traffic::agv::Planner{
128- {graph, traits},
129- options
132+ {graph, traits},
133+ options
130134 };
131135 auto plan = planner.plan ({start_time, 5 , 180.0 }, {2 });
132136
0 commit comments