Carma-platform v4.2.0
CARMA Platform is built on robot operating system (ROS) and utilizes open source software (OSS) that enables Cooperative Driving Automation (CDA) features to allow Automated Driving Systems to interact and cooperate with infrastructure and other vehicles through communication.
cooperative_lanechange_node.cpp
Go to the documentation of this file.
1/*
2 * Copyright (C) 2019-2022 LEIDOS.
3 *
4 * Licensed under the Apache License, Version 2.0 (the "License"); you may not
5 * use this file except in compliance with the License. You may obtain a copy of
6 * the License at
7 *
8 * http://www.apache.org/licenses/LICENSE-2.0
9 *
10 * Unless required by applicable law or agreed to in writing, software
11 * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
12 * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. See the
13 * License for the specific language governing permissions and limitations under
14 * the License.
15 */
17
19{
20 namespace std_ph = std::placeholders;
21
23 : carma_guidance_plugins::TacticalPlugin(options)
24 {
25 // Create initial config
26 config_ = Config();
27
28 // Declare parameters
29 config_.trajectory_time_length = declare_parameter<double>("trajectory_time_length", config_.trajectory_time_length);
30 config_.control_plugin_name = declare_parameter<std::string>("control_plugin_name", config_.control_plugin_name);
31 config_.minimum_speed = declare_parameter<double>("minimum_speed", config_.minimum_speed);
32 config_.max_accel = declare_parameter<double>("max_accel", config_.max_accel);
33 config_.minimum_lookahead_distance = declare_parameter<double>("minimum_lookahead_distance", config_.minimum_lookahead_distance);
34 config_.maximum_lookahead_distance = declare_parameter<double>("maximum_lookahead_distance", config_.maximum_lookahead_distance);
35 config_.minimum_lookahead_speed = declare_parameter<double>("minimum_lookahead_speed", config_.minimum_lookahead_speed);
36 config_.maximum_lookahead_speed = declare_parameter<double>("maximum_lookahead_speed", config_.maximum_lookahead_speed);
37 config_.lateral_accel_limit = declare_parameter<double>("lateral_accel_limit", config_.lateral_accel_limit);
38 config_.speed_moving_average_window_size = declare_parameter<int>("speed_moving_average_window_size", config_.speed_moving_average_window_size);
39 config_.curvature_moving_average_window_size = declare_parameter<int>("curvature_moving_average_window_size", config_.curvature_moving_average_window_size);
40 config_.curvature_calc_lookahead_count = declare_parameter<int>("curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count);
41 config_.downsample_ratio = declare_parameter<int>("downsample_ratio", config_.downsample_ratio);
42 config_.destination_range = declare_parameter<double>("destination_range", config_.destination_range);
43 config_.lanechange_time_out = declare_parameter<double>("lanechange_time_out", config_.lanechange_time_out);
44 config_.min_timestep = declare_parameter<double>("min_timestep", config_.min_timestep);
45 config_.starting_downtrack_range = declare_parameter<double>("starting_downtrack_range", config_.starting_downtrack_range);
46 config_.starting_fraction = declare_parameter<double>("starting_fraction", config_.starting_fraction);
47 config_.mid_fraction = declare_parameter<double>("mid_fraction", config_.mid_fraction);
48 config_.min_desired_gap = declare_parameter<double>("min_desired_gap", config_.min_desired_gap);
49 config_.desired_time_gap = declare_parameter<double>("desired_time_gap", config_.desired_time_gap);
50 config_.turn_downsample_ratio = declare_parameter<int>("turn_downsample_ratio", config_.turn_downsample_ratio);
51 config_.curve_resample_step_size = declare_parameter<double>("curve_resample_step_size", config_.curve_resample_step_size);
52 config_.back_distance = declare_parameter<double>("back_distance", config_.back_distance);
53 config_.buffer_ending_downtrack = declare_parameter<double>("buffer_ending_downtrack", config_.buffer_ending_downtrack);
54 config_.vehicle_id = declare_parameter<std::string>("vehicle_id", config_.vehicle_id);
55 }
56
57 rcl_interfaces::msg::SetParametersResult CooperativeLaneChangePlugin::parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters)
58 {
59 auto error = update_params<std::string>(
60 {{"control_plugin_name", config_.control_plugin_name},
61 {"vehicle_id", config_.vehicle_id}}, parameters);
62
63 auto error_2 = update_params<double>(
64 {{"trajectory_time_length", config_.trajectory_time_length},
65 {"minimum_speed", config_.minimum_speed},
66 {"max_accel", config_.max_accel},
67 {"minimum_lookahead_distance", config_.minimum_lookahead_distance},
68 {"maximum_lookahead_distance", config_.maximum_lookahead_distance},
69 {"minimum_lookahead_speed", config_.minimum_lookahead_speed},
70 {"maximum_lookahead_speed", config_.maximum_lookahead_speed},
71 {"lateral_accel_limit", config_.lateral_accel_limit},
72 {"destination_range", config_.destination_range},
73 {"lanechange_time_out", config_.lanechange_time_out},
74 {"min_timestep", config_.min_timestep},
75 {"starting_downtrack_range", config_.starting_downtrack_range},
76 {"starting_fraction", config_.starting_fraction},
77 {"mid_fraction", config_.mid_fraction},
78 {"min_desired_gap", config_.min_desired_gap},
79 {"curve_resample_step_size", config_.curve_resample_step_size},
80 {"back_distance", config_.back_distance},
81 {"buffer_ending_downtrack", config_.buffer_ending_downtrack},
82 {"desired_time_gap", config_.desired_time_gap}}, parameters);
83
84 auto error_3 = update_params<int>(
85 {{"speed_moving_average_window_size", config_.speed_moving_average_window_size},
86 {"curvature_moving_average_window_size", config_.curvature_moving_average_window_size},
87 {"curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count},
88 {"downsample_ratio", config_.downsample_ratio},
89 {"turn_downsample_ratio", config_.turn_downsample_ratio}}, parameters);
90
91 rcl_interfaces::msg::SetParametersResult result;
92
93 result.successful = !error && !error_2 && !error_3;
94
95 return result;
96 }
97
98 carma_ros2_utils::CallbackReturn CooperativeLaneChangePlugin::on_configure_plugin()
99 {
100 RCLCPP_INFO_STREAM(get_logger(), "CooperativeLaneChangePlugin trying to configure");
101
102 // Reset config
103 config_ = Config();
104
105 // Load parameters
106 get_parameter<double>("trajectory_time_length", config_.trajectory_time_length);
107 get_parameter<std::string>("control_plugin_name", config_.control_plugin_name);
108 get_parameter<double>("minimum_speed", config_.minimum_speed);
109 get_parameter<double>("max_accel", config_.max_accel);
110 get_parameter<double>("minimum_lookahead_distance", config_.minimum_lookahead_distance);
111 get_parameter<double>("maximum_lookahead_distance", config_.maximum_lookahead_distance);
112 get_parameter<double>("minimum_lookahead_speed", config_.minimum_lookahead_speed);
113 get_parameter<double>("maximum_lookahead_speed", config_.maximum_lookahead_speed);
114 get_parameter<double>("lateral_accel_limit", config_.lateral_accel_limit);
115 get_parameter<int>("speed_moving_average_window_size", config_.speed_moving_average_window_size);
116 get_parameter<int>("curvature_moving_average_window_size", config_.curvature_moving_average_window_size);
117 get_parameter<int>("curvature_calc_lookahead_count", config_.curvature_calc_lookahead_count);
118 get_parameter<int>("downsample_ratio", config_.downsample_ratio);
119 get_parameter<double>("destination_range", config_.destination_range);
120 get_parameter<double>("lanechange_time_out", config_.lanechange_time_out);
121 get_parameter<double>("min_timestep", config_.min_timestep);
122 get_parameter<double>("starting_downtrack_range", config_.starting_downtrack_range);
123 get_parameter<double>("starting_fraction", config_.starting_fraction);
124 get_parameter<double>("mid_fraction", config_.mid_fraction);
125 get_parameter<double>("min_desired_gap", config_.min_desired_gap);
126 get_parameter<double>("desired_time_gap", config_.desired_time_gap);
127 get_parameter<int>("turn_downsample_ratio", config_.turn_downsample_ratio);
128 get_parameter<double>("curve_resample_step_size", config_.curve_resample_step_size);
129 get_parameter<double>("back_distance", config_.back_distance);
130 get_parameter<double>("buffer_ending_downtrack", config_.buffer_ending_downtrack);
131 get_parameter<std::string>("vehicle_id", config_.vehicle_id);
132
133 // Register runtime parameter update callback
134 add_on_set_parameters_callback(std::bind(&CooperativeLaneChangePlugin::parameter_update_callback, this, std_ph::_1));
135
136 RCLCPP_INFO_STREAM(get_logger(), "Loaded params: " << config_);
137
138 // Setup subscribers
139 pose_sub_ = create_subscription<geometry_msgs::msg::PoseStamped>("current_pose", 1,
140 std::bind(&CooperativeLaneChangePlugin::pose_cb, this, std_ph::_1));
141 twist_sub_ = create_subscription<geometry_msgs::msg::TwistStamped>("current_velocity", 1,
142 std::bind(&CooperativeLaneChangePlugin::twist_cb, this, std_ph::_1));
143 incoming_mobility_response_sub_ = create_subscription<carma_v2x_msgs::msg::MobilityResponse>("incoming_mobility_response", 1,
144 std::bind(&CooperativeLaneChangePlugin::mobilityresponse_cb, this, std_ph::_1));
145 georeference_sub_ = create_subscription<std_msgs::msg::String>("georeference", 1,
146 std::bind(&CooperativeLaneChangePlugin::georeference_cb, this, std_ph::_1));
147 bsm_sub_ = create_subscription<carma_v2x_msgs::msg::BSM>("bsm_outbound", 1,
148 std::bind(&CooperativeLaneChangePlugin::bsm_cb, this, std_ph::_1));
149
150 // Setup publishers
151 outgoing_mobility_request_pub_ = create_publisher<carma_v2x_msgs::msg::MobilityRequest>("outgoing_mobility_request", 5); // Rate from yield plugin
152 lanechange_status_pub_ = create_publisher<carma_planning_msgs::msg::LaneChangeStatus>("cooperative_lane_change_status", 10);
153
154 // Initialize World Model
156
157 // Return success if everything initialized successfully
158 return CallbackReturn::SUCCESS;
159 }
160
161 void CooperativeLaneChangePlugin::mobilityresponse_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg){
162 //@SONAR_STOP@
163 if (clc_called_ && clc_request_id_ == msg->m_header.plan_id)
164 {
165 carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
166 if(msg->is_accepted)
167 {
169 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::ACCEPTANCE_RECEIVED;
170 lc_status_msg.description = "Received lane merge acceptance";
171 }
172 else
173 {
175 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::REJECTION_RECEIVED;
176 lc_status_msg.description = "Received lane merge rejection";
177 }
178 lanechange_status_pub_->publish(lc_status_msg);
179 //@SONAR_START@
180 }
181 else
182 {
183 RCLCPP_DEBUG_STREAM(get_logger(), "received mobility response is not related to CLC");
184 }
185
186 }
187
188 double CooperativeLaneChangePlugin::find_current_gap(long veh2_lanelet_id, double veh2_downtrack, carma_planning_msgs::msg::VehicleState& ego_state) const
189 {
190 //find downtrack distance between ego and lag vehicle
191 RCLCPP_DEBUG_STREAM(get_logger(), "entered find_current_gap");
192 double current_gap = 0.0;
193 lanelet::BasicPoint2d ego_pos(ego_state.x_pos_global, ego_state.y_pos_global);
194 //double ego_current_downtrack = wm_->routeTrackPos(ego_pos).downtrack;
195
196 lanelet::LaneletMapConstPtr const_map(wm_->getMap());
197 lanelet::ConstLanelet veh2_lanelet = const_map->laneletLayer.get(veh2_lanelet_id);
198 RCLCPP_DEBUG_STREAM(get_logger(), "veh2_lanelet id " << veh2_lanelet.id());
199
200 auto current_lanelets = lanelet::geometry::findNearest(const_map->laneletLayer, ego_pos, 10);
201 if(current_lanelets.size() == 0)
202 {
203 RCLCPP_WARN_STREAM(get_logger(), "Cannot find any lanelet in map!");
204 return true;
205 }
206 lanelet::ConstLanelet current_lanelet = current_lanelets[0].second;
207 RCLCPP_DEBUG_STREAM(get_logger(), "current llt id " << current_lanelet.id());
208
209 //Create temporary route between the two vehicles
210 lanelet::ConstLanelet start_lanelet = veh2_lanelet;
211 lanelet::ConstLanelet end_lanelet = current_lanelet;
212
213 auto map_graph = wm_->getMapRoutingGraph();
214 RCLCPP_DEBUG_STREAM(get_logger(), "Graph created");
215
216 auto temp_route = map_graph->getRoute(start_lanelet, end_lanelet);
217 RCLCPP_DEBUG_STREAM(get_logger(), "Route created");
218
219 //Throw exception if there is no shortest path from veh2 to subject vehicle
220 lanelet::routing::LaneletPath shortest_path2;
221 if(temp_route)
222 {
223 shortest_path2 = temp_route.get().shortestPath();
224 }
225 else{
226 RCLCPP_ERROR_STREAM(get_logger(), "No path exists from roadway object to subject");
227 throw std::invalid_argument("No path exists from roadway object to subject");
228 }
229
230 RCLCPP_DEBUG_STREAM(get_logger(), "Shorted path created size: " << shortest_path2.size());
231 for (auto llt : shortest_path2)
232 {
233 RCLCPP_DEBUG_STREAM(get_logger(), "llt id route: " << llt.id());
234 }
235
236 //To find downtrack- creating temporary route from veh2 to veh1(ego vehicle)
237 double veh1_current_downtrack = wm_->routeTrackPos(ego_pos).downtrack;
238 RCLCPP_DEBUG_STREAM(get_logger(), "ego_current_downtrack:" << veh1_current_downtrack);
239
240 current_gap = veh1_current_downtrack - veh2_downtrack;
241 RCLCPP_DEBUG_STREAM(get_logger(), "Finding current gap");
242 RCLCPP_DEBUG_STREAM(get_logger(), "Veh1 current downtrack: " << veh1_current_downtrack << " veh2 downtrack: " << veh2_downtrack);
243
244 return current_gap;
245 }
246
247 void CooperativeLaneChangePlugin::pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg)
248 {
249 pose_msg_ = *msg;
250 }
251
252 void CooperativeLaneChangePlugin::twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg)
253 {
254 current_speed_ = msg->twist.linear.x;
255 }
256
257 void CooperativeLaneChangePlugin::bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg)
258 {
259 bsm_core_ = msg->core_data;
260 }
261
263 std::shared_ptr<rmw_request_id_t>,
264 carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
265 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp)
266 {
267 // Set boolean flag if this is the first time this service has been called
268 if (!clc_called_)
269 {
270 clc_called_ = true;
271 }
272
273 // Only plan the trajectory for the requested LANE_CHANGE maneuver
274 std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
275 if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type != carma_planning_msgs::msg::Maneuver::LANE_CHANGE)
276 {
277 throw std::invalid_argument ("Cooperative Lane Change Plugin doesn't support this maneuver type");
278 }
279 maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]);
280
281 // Currently only checking for first lane change maneuver message
282 long target_lanelet_id = stol(maneuver_plan[0].lane_change_maneuver.ending_lane_id);
283 double target_downtrack = maneuver_plan[0].lane_change_maneuver.end_dist;
284
285 // Get subject vehicle info
286 lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global);
287 double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;
288
289 RCLCPP_DEBUG_STREAM(get_logger(), "target_lanelet_id: " << target_lanelet_id);
290 RCLCPP_DEBUG_STREAM(get_logger(), "target_downtrack: " << target_downtrack);
291 RCLCPP_DEBUG_STREAM(get_logger(), "current_downtrack: " << current_downtrack);
292 RCLCPP_DEBUG_STREAM(get_logger(), "Starting CLC downtrack: " << maneuver_plan[0].lane_change_maneuver.start_dist);
293
294 if(current_downtrack < maneuver_plan[0].lane_change_maneuver.start_dist - config_.starting_downtrack_range){
295 RCLCPP_DEBUG_STREAM(get_logger(), "Lane change trajectory will not be planned. current_downtrack is more than " << config_.starting_downtrack_range << " meters before starting CLC downtrack");
296 return;
297 }
298 auto current_lanelets = lanelet::geometry::findNearest(wm_->getMap()->laneletLayer, veh_pos, 10);
299 long current_lanelet_id = current_lanelets[0].second.id();
300 if(current_lanelet_id == target_lanelet_id && current_downtrack >= target_downtrack - config_.destination_range){
301 carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
302 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::PLANNING_SUCCESS;
303 //No description as per UI documentation
304 lanechange_status_pub_->publish(lc_status_msg);
305 }
306
307 long veh2_lanelet_id = 0;
308 double veh2_downtrack = 0.0, veh2_speed = 0.0;
309 bool foundRoadwayObject = false;
310 bool negotiate = true;
311 std::vector<carma_perception_msgs::msg::RoadwayObstacle> rwol = wm_->getRoadwayObjects();
312 //Assuming only one connected vehicle in list
313 for(int i = 0; i < rwol.size(); i++){
314 if(rwol[i].connected_vehicle_type.type == carma_perception_msgs::msg::ConnectedVehicleType::NOT_CONNECTED){
315 veh2_lanelet_id = rwol[0].lanelet_id;
316 veh2_downtrack = rwol[0].down_track; //Returns downtrack
317 veh2_speed = rwol[0].object.velocity.twist.linear.x;
318 foundRoadwayObject = true;
319 break;
320 }
321 }
322 if(foundRoadwayObject){
323 RCLCPP_DEBUG_STREAM(get_logger(), "Found Roadway object");
324 //get current_gap
325 RCLCPP_DEBUG_STREAM(get_logger(), "veh2_lanelet_id: " << veh2_lanelet_id << ", veh2_downtrack: " << veh2_downtrack);
326
327 double current_gap = find_current_gap(veh2_lanelet_id, veh2_downtrack, req->vehicle_state);
328 RCLCPP_DEBUG_STREAM(get_logger(), "Current gap: " << current_gap);
329
330 //get desired gap - desired time gap (default 3s)* relative velocity
331 double relative_velocity = current_speed_ - veh2_speed;
332 RCLCPP_DEBUG_STREAM(get_logger(), "Relative velocity: " << relative_velocity);
333 double desired_gap = config_.desired_time_gap * relative_velocity;
334 RCLCPP_DEBUG_STREAM(get_logger(), "Desired gap: " << desired_gap);
335
336 if(desired_gap < config_.min_desired_gap){
337 desired_gap = config_.min_desired_gap;
338 }
339 // TODO - this condition needs to be re-enabled after testing
340 // if(current_gap > desired_gap){
341 // negotiate = false; //No need for negotiation
342 // }
343
344 }
345 else{
346 RCLCPP_DEBUG_STREAM(get_logger(), "No roadway object");
347 negotiate = false;
348 }
349
350 //plan lanechange without filling in response
351 RCLCPP_DEBUG_STREAM(get_logger(), "Planning lane change trajectory");
352
353 std::string maneuver_id = maneuver_plan[0].lane_change_maneuver.parameters.maneuver_id;
354 if (original_lc_maneuver_values_.find(maneuver_id) == original_lc_maneuver_values_.end()) {
355 // If this lane change maneuver ID is being received for this first time, store its original start_dist and starting_lane_id locally
356 RCLCPP_DEBUG_STREAM(get_logger(), "Received maneuver id " << maneuver_id << " for the first time");
357 RCLCPP_DEBUG_STREAM(get_logger(), "Original start dist is " << maneuver_plan[0].lane_change_maneuver.start_dist);
358 RCLCPP_DEBUG_STREAM(get_logger(), "Original starting_lane_id is " << maneuver_plan[0].lane_change_maneuver.starting_lane_id);
359
360 // Create LaneChangeManeuverOriginalValues object for this lane change maneuver and add it to original_lc_maneuver_values_
361 LaneChangeManeuverOriginalValues original_lc_values;
362 original_lc_values.maneuver_id = maneuver_id;
363 original_lc_values.original_starting_lane_id = maneuver_plan[0].lane_change_maneuver.starting_lane_id;
364 original_lc_values.original_start_dist = maneuver_plan[0].lane_change_maneuver.start_dist;
365
366 original_lc_maneuver_values_[maneuver_id] = original_lc_values;
367 }
368 else {
369 // If the vehicle has just started this lane change, store its initial velocity locally; this velocity will be maintained throughout the lane change
370 if (current_downtrack >= (original_lc_maneuver_values_[maneuver_id]).original_start_dist && !(original_lc_maneuver_values_[maneuver_id]).has_started) {
371 original_lc_maneuver_values_[maneuver_id].has_started = true;
372 original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms = std::max(req->vehicle_state.longitudinal_vel, config_.minimum_speed);
373
374 RCLCPP_DEBUG_STREAM(get_logger(), "Lane change maneuver " << maneuver_id << " has started, maintaining speed (in m/s): " <<
375 original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms << " throughout lane change");
376 }
377 }
378
379 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> planned_trajectory_points = plan_lanechange(req);
380
381 if(negotiate){
382 RCLCPP_DEBUG_STREAM(get_logger(), "Negotiating");
383 //send mobility request
384 //Planning for first lane change maneuver
385 carma_v2x_msgs::msg::MobilityRequest request = create_mobility_request(planned_trajectory_points, maneuver_plan[0]);
386 outgoing_mobility_request_pub_->publish(request);
387 if(!request_sent_){
388 request_sent_time_ = this->now();
389 request_sent_ = true;
390 }
391 carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
392 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::PLAN_SENT;
393 lc_status_msg.description = "Requested lane merge";
394 lanechange_status_pub_->publish(lc_status_msg);
395 }
396
397 //if ack mobility response, send lanechange response
398 if(!negotiate || is_lanechange_accepted_){
399 RCLCPP_DEBUG_STREAM(get_logger(), "negotiate:" << negotiate);
400 RCLCPP_DEBUG_STREAM(get_logger(), "is_lanechange_accepted:" << is_lanechange_accepted_);
401
402 RCLCPP_DEBUG_STREAM(get_logger(), "Adding to response");
403 add_trajectory_to_response(req,resp,planned_trajectory_points);
404
405 }
406 else{
407 if(!negotiate && !request_sent_){
408 request_sent_time_ = this->now();
409 request_sent_ = true;
410 }
411 rclcpp::Time planning_end_time = this->now();
412 rclcpp::Duration passed_time = planning_end_time - request_sent_time_;
413 if(passed_time.seconds() >= config_.lanechange_time_out){
414 carma_planning_msgs::msg::LaneChangeStatus lc_status_msg;
415 lc_status_msg.status = carma_planning_msgs::msg::LaneChangeStatus::TIMED_OUT;
416 lc_status_msg.description = "Request timed out for lane merge";
417 lanechange_status_pub_->publish(lc_status_msg);
418 request_sent_ = false; //Reset variable
419 }
420 }
421
422 // Set the planning plugin field name
423 for (auto& p : resp->trajectory_plan.trajectory_points) {
424 p.planner_plugin_name = get_plugin_name();
425 }
426
427 return;
428 }
429
430 void CooperativeLaneChangePlugin::add_trajectory_to_response(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
431 carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp,
432 const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& planned_trajectory_points)
433 {
434 carma_planning_msgs::msg::TrajectoryPlan trajectory_plan;
435 trajectory_plan.header.frame_id = "map";
436 trajectory_plan.header.stamp = this->now();
437 trajectory_plan.trajectory_id = boost::uuids::to_string(boost::uuids::random_generator()());
438
439 trajectory_plan.trajectory_points = planned_trajectory_points;
440 trajectory_plan.initial_longitudinal_velocity = std::max(req->vehicle_state.longitudinal_vel, config_.minimum_speed);
441 resp->trajectory_plan = trajectory_plan;
442
443 resp->related_maneuvers.push_back(req->maneuver_index_to_plan);
444
445 resp->maneuver_status.push_back(carma_planning_msgs::srv::PlanTrajectory::Response::MANEUVER_IN_PROGRESS);
446 }
447
448 carma_v2x_msgs::msg::MobilityRequest CooperativeLaneChangePlugin::create_mobility_request(std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& trajectory_plan, carma_planning_msgs::msg::Maneuver& maneuver)
449 {
450 carma_v2x_msgs::msg::MobilityRequest request_msg;
451 carma_v2x_msgs::msg::MobilityHeader header;
452 header.sender_id = config_.vehicle_id;
453 header.recipient_id = DEFAULT_STRING_;
454 header.sender_bsm_id = bsmIDtoString(bsm_core_);
455 header.plan_id = boost::uuids::to_string(boost::uuids::random_generator()());
456 clc_request_id_ = header.plan_id;
457 header.timestamp = rclcpp::Time(trajectory_plan.front().target_time).nanoseconds() * 1000000;
458 request_msg.m_header = header;
459
460 request_msg.strategy = "carma/cooperative-lane-change";
461 request_msg.plan_type.type = carma_v2x_msgs::msg::PlanType::CHANGE_LANE_LEFT;
462
463 //Urgency- Currently unassigned
464 int urgency;
466 urgency = 10;
467 }
469 urgency = 5;
470 }
471 else{
472 urgency = 1;
473 }
474 RCLCPP_DEBUG_STREAM(get_logger(), "Maneuver fraction completed:"<<maneuver_fraction_completed_);
475 request_msg.urgency = urgency;
476
477 //Strategy params
478 //Encode JSON with Boost Property Tree
479 using boost::property_tree::ptree;
480 ptree pt;
481 double end_speed_floor = std::floor(maneuver.lane_change_maneuver.end_speed);
482 int end_speed_fractional = (maneuver.lane_change_maneuver.end_speed - end_speed_floor) * 10;
483
484 RCLCPP_DEBUG_STREAM(get_logger(), "end_speed_floor: " << end_speed_floor);
485 RCLCPP_DEBUG_STREAM(get_logger(), "end_speed_fractional: " << end_speed_fractional);
486 RCLCPP_DEBUG_STREAM(get_logger(), "start_lanelet_id: " << maneuver.lane_change_maneuver.starting_lane_id);
487 RCLCPP_DEBUG_STREAM(get_logger(), "end_lanelet_id: " << maneuver.lane_change_maneuver.ending_lane_id);
488
489 pt.put("s",(int)end_speed_floor);
490 pt.put("f",end_speed_fractional);
491 pt.put("sl",maneuver.lane_change_maneuver.starting_lane_id);
492 pt.put("el", maneuver.lane_change_maneuver.ending_lane_id);
493
494 std::stringstream body_stream;
495 boost::property_tree::json_parser::write_json(body_stream,pt);
496 request_msg.strategy_params = body_stream.str();
497 RCLCPP_DEBUG_STREAM(get_logger(), "request_msg.strategy_params: " << request_msg.strategy_params);
498
499 //Trajectory
500 carma_v2x_msgs::msg::Trajectory trajectory;
501 if (map_projector_) {
502 trajectory = trajectory_plan_to_trajectory(trajectory_plan);
503 //Location
504 carma_planning_msgs::msg::TrajectoryPlanPoint temp_loc_to_convert;
505 temp_loc_to_convert.x = pose_msg_.pose.position.x;
506 temp_loc_to_convert.y = pose_msg_.pose.position.y;
507 carma_v2x_msgs::msg::LocationECEF location = trajectory_point_to_ecef(temp_loc_to_convert);
508
509 //Using trajectory first point time as location timestamp
510 location.timestamp = rclcpp::Time(trajectory_plan.front().target_time).nanoseconds() * 1000000;
511
512 request_msg.location = location;
513 }
514 else
515 {
516 RCLCPP_ERROR_STREAM(get_logger(), "Map projection not available to be used with request message");
517 }
518
519 request_msg.trajectory = trajectory;
520 request_msg.expiration = rclcpp::Time(trajectory_plan.back().target_time).seconds();
521 RCLCPP_DEBUG_STREAM(get_logger(), "request_msg.expiration: " << request_msg.expiration << " of which string size: " << std::to_string(request_msg.expiration).size());
522
523 return request_msg;
524 }
525
526 carma_v2x_msgs::msg::Trajectory CooperativeLaneChangePlugin::trajectory_plan_to_trajectory(const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& traj_points) const
527 {
528 carma_v2x_msgs::msg::Trajectory traj;
529 carma_v2x_msgs::msg::LocationECEF ecef_location = trajectory_point_to_ecef(traj_points[0]);
530
531 if (traj_points.size() < 2){
532 RCLCPP_WARN_STREAM(get_logger(), "Received Trajectory Plan is too small");
533 traj.offsets = {};
534 }
535 else{
536 carma_v2x_msgs::msg::LocationECEF prev_point = ecef_location;
537 for (size_t i = 1; i < traj_points.size(); i++){
538
539 carma_v2x_msgs::msg::LocationOffsetECEF offset;
540 carma_v2x_msgs::msg::LocationECEF new_point = trajectory_point_to_ecef(traj_points[i]); // m to cm to fit the msg standard
541 offset.offset_x = (int16_t)(new_point.ecef_x - prev_point.ecef_x);
542 offset.offset_y = (int16_t)(new_point.ecef_y - prev_point.ecef_y);
543 offset.offset_z = (int16_t)(new_point.ecef_z - prev_point.ecef_z);
544 prev_point = new_point;
545 traj.offsets.push_back(offset);
546 }
547 }
548
549 traj.location = ecef_location;
550
551 return traj;
552 }
553
554 carma_v2x_msgs::msg::LocationECEF CooperativeLaneChangePlugin::trajectory_point_to_ecef(const carma_planning_msgs::msg::TrajectoryPlanPoint& traj_point) const
555 {
556 if (!map_projector_) {
557 throw std::invalid_argument("No map projector available for ecef conversion");
558 }
559 carma_v2x_msgs::msg::LocationECEF location;
560
561 lanelet::BasicPoint3d ecef_point = map_projector_->projectECEF({traj_point.x, traj_point.y, 0.0}, 1);
562 location.ecef_x = ecef_point.x() * 100.0; // Convert cm to m
563 location.ecef_y = ecef_point.y() * 100.0;
564 location.ecef_z = ecef_point.z() * 100.0;
565
566 return location;
567 }
568
569 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> CooperativeLaneChangePlugin::plan_lanechange(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req)
570 {
571 lanelet::BasicPoint2d veh_pos(req->vehicle_state.x_pos_global, req->vehicle_state.y_pos_global);
572 double current_downtrack = wm_->routeTrackPos(veh_pos).downtrack;
573
574 // Only plan the trajectory for the requested LANE_CHANGE maneuver
575 std::vector<carma_planning_msgs::msg::Maneuver> maneuver_plan;
576 if(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan].type != carma_planning_msgs::msg::Maneuver::LANE_CHANGE) {
577 throw std::invalid_argument ("Cooperative Lane Change Plugin doesn't support this maneuver type");
578 }
579 maneuver_plan.push_back(req->maneuver_plan.maneuvers[req->maneuver_index_to_plan]);
580
581 if(current_downtrack >= maneuver_plan.front().lane_change_maneuver.end_dist){
582 request_sent_ = false;
583 }
584
587
589
596
597 RCLCPP_DEBUG_STREAM(get_logger(), "Current downtrack: " << current_downtrack);
598
599 std::string maneuver_id = maneuver_plan.front().lane_change_maneuver.parameters.maneuver_id;
600 double original_start_dist = current_downtrack; // Initialize so original_start_dist cannot be less than the current downtrack
601
602 if (original_lc_maneuver_values_.find(maneuver_id) != original_lc_maneuver_values_.end()) {
603 // Obtain the original start_dist associated with this lane change maneuver
604 original_start_dist = original_lc_maneuver_values_[maneuver_id].original_start_dist;
605 RCLCPP_DEBUG_STREAM(get_logger(), "Maneuver id " << maneuver_id << " original start_dist is " << original_start_dist);
606
607 // Set this maneuver's starting_lane_id to the original starting_lane_id associated with this lane change maneuver
608 maneuver_plan.front().lane_change_maneuver.starting_lane_id = original_lc_maneuver_values_[maneuver_id].original_starting_lane_id;
609 RCLCPP_DEBUG_STREAM(get_logger(), "Updated maneuver id " << maneuver_id << " starting_lane_id to its original value of " << original_lc_maneuver_values_[maneuver_id].original_starting_lane_id);
610
611 // If the vehicle has started this lane change, set the request's vehicle_state.longitudinal_vel to the velocity that the vehicle began this lane change at
612 if(original_lc_maneuver_values_[maneuver_id].has_started) {
613 req->vehicle_state.longitudinal_vel = original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms;
614 RCLCPP_DEBUG_STREAM(get_logger(), "Updating vehicle_state.longitudinal_vel to the initial lane change value of " << original_lc_maneuver_values_[maneuver_id].original_longitudinal_vel_ms);
615 }
616 }
617 else {
618 RCLCPP_WARN_STREAM(get_logger(), "No original values for lane change maneuver were found!");
619 }
620
621 double starting_downtrack = std::min(current_downtrack, original_start_dist);
622
623 auto points_and_target_speeds = basic_autonomy::waypoint_generation::create_geometry_profile(maneuver_plan, starting_downtrack, wm_, ending_state_before_buffer_, req->vehicle_state, wpg_general_config, wpg_detail_config);
624
625 // Calculate maneuver fraction completed (current_downtrack/(ending_downtrack-starting_downtrack)
626 auto maneuver_end_dist = maneuver_plan.back().lane_change_maneuver.end_dist;
627 auto maneuver_start_dist = maneuver_plan.front().lane_change_maneuver.start_dist;
628 maneuver_fraction_completed_ = (maneuver_start_dist - current_downtrack)/(maneuver_end_dist - maneuver_start_dist);
629
630 RCLCPP_DEBUG_STREAM(get_logger(), "Maneuvers to points size: " << points_and_target_speeds.size());
631 auto downsampled_points = carma_ros2_utils::containers::downsample_vector(points_and_target_speeds, config_.downsample_ratio);
632
633 std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> trajectory_points = basic_autonomy::waypoint_generation::compose_lanechange_trajectory_from_path(downsampled_points, req->vehicle_state, req->header.stamp,
634 wm_, ending_state_before_buffer_, wpg_detail_config);
635 RCLCPP_DEBUG_STREAM(get_logger(), "Compose Trajectory size: " << trajectory_points.size());
636 return trajectory_points;
637 }
638
639 void CooperativeLaneChangePlugin::georeference_cb(const std_msgs::msg::String::UniquePtr msg)
640 {
641 if (map_georeference_ != msg->data)
642 {
643 map_georeference_ = msg->data;
644 map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg->data.c_str()); // Build projector from proj string
645 }
646 }
647
648 std::string CooperativeLaneChangePlugin::bsmIDtoString(carma_v2x_msgs::msg::BSMCoreData bsm_core)
649 {
650 std::string res = "";
651 for (size_t i = 0; i < bsm_core.id.size(); i++){
652 res += std::to_string(bsm_core.id[i]);
653 }
654 return res;
655 }
656
658 return true;
659 }
660
662 return "v4.0"; // Version ID matches the value set in this package's package.xml
663 }
664
665} // cooperative_lanechange
666
667#include "rclcpp_components/register_node_macro.hpp"
668
669// Register the component with class_loader
670RCLCPP_COMPONENTS_REGISTER_NODE(cooperative_lanechange::CooperativeLaneChangePlugin)
std::string get_plugin_name() const
Return the name of this plugin.
virtual carma_wm::WorldModelConstPtr get_world_model() final
Method to return the default world model provided as a convience by this base class If this method or...
The class responsible for generating cooperative lanechange trajectories from received lane change ma...
carma_v2x_msgs::msg::LocationECEF trajectory_point_to_ecef(const carma_planning_msgs::msg::TrajectoryPlanPoint &traj_point) const
Converts Trajectory Point to ECEF frame using map projection.
carma_planning_msgs::msg::VehicleState ending_state_before_buffer_
bool get_availability() override
Get the availability status of this plugin based on the current operating environment....
carma_v2x_msgs::msg::MobilityRequest create_mobility_request(std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &trajectory_plan, carma_planning_msgs::msg::Maneuver &maneuver)
Creates a mobility request message from planned trajectory and requested maneuver info.
void bsm_cb(const carma_v2x_msgs::msg::BSM::UniquePtr msg)
Callback for the BSM subscriber, which will store the latest BSM Core Data broadcasted by the host ve...
carma_ros2_utils::SubPtr< geometry_msgs::msg::TwistStamped > twist_sub_
void add_trajectory_to_response(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp, const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &planned_trajectory_points)
Adds the generated trajectory plan to the service response.
std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > plan_lanechange(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req)
Creates a vector of Trajectory Points from maneuver information in trajectory request.
void mobilityresponse_cb(const carma_v2x_msgs::msg::MobilityResponse::UniquePtr msg)
Callback to subscribed mobility response topic.
std::unordered_map< std::string, LaneChangeManeuverOriginalValues > original_lc_maneuver_values_
double find_current_gap(long veh2_lanelet_id, double veh2_downtrack, carma_planning_msgs::msg::VehicleState &ego_state) const
Calculates distance between subject vehicle and vehicle 2.
void plan_trajectory_callback(std::shared_ptr< rmw_request_id_t >, carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req, carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override
Extending class provided callback which should return a planned trajectory based on the provided traj...
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::BSM > bsm_sub_
std::string bsmIDtoString(carma_v2x_msgs::msg::BSMCoreData bsm_core)
Method for extracting the BSM ID from a BSM Core Data object and converting it to a string.
CooperativeLaneChangePlugin(const rclcpp::NodeOptions &)
CooperativeLaneChangePlugin constructor.
void pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg)
Callback for the pose subscriber, which will store latest pose locally.
carma_ros2_utils::SubPtr< std_msgs::msg::String > georeference_sub_
std::string get_version_id() override
Returns the version id of this plugin.
void twist_cb(const geometry_msgs::msg::TwistStamped::UniquePtr msg)
Callback for the twist subscriber, which will store latest twist locally.
carma_ros2_utils::PubPtr< carma_planning_msgs::msg::LaneChangeStatus > lanechange_status_pub_
carma_ros2_utils::PubPtr< carma_v2x_msgs::msg::MobilityRequest > outgoing_mobility_request_pub_
void georeference_cb(const std_msgs::msg::String::UniquePtr msg)
Callback for map projection string to define lat/lon -> map conversion.
std::shared_ptr< lanelet::projection::LocalFrameProjector > map_projector_
rcl_interfaces::msg::SetParametersResult parameter_update_callback(const std::vector< rclcpp::Parameter > &parameters)
Callback for dynamic parameter updates.
carma_ros2_utils::CallbackReturn on_configure_plugin()
This method should be used to load parameters and will be called on the configure state transition.
carma_ros2_utils::SubPtr< geometry_msgs::msg::PoseStamped > pose_sub_
carma_ros2_utils::SubPtr< carma_v2x_msgs::msg::MobilityResponse > incoming_mobility_response_sub_
carma_v2x_msgs::msg::Trajectory trajectory_plan_to_trajectory(const std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > &traj_points) const
Converts Trajectory Plan to (Mobility) Trajectory.
GeneralTrajConfig compose_general_trajectory_config(const std::string &trajectory_type, int default_downsample_ratio, int turn_downsample_ratio)
std::vector< carma_planning_msgs::msg::TrajectoryPlanPoint > compose_lanechange_trajectory_from_path(const std::vector< PointSpeedPair > &points, const carma_planning_msgs::msg::VehicleState &state, const rclcpp::Time &state_time, const carma_wm::WorldModelConstPtr &wm, const carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const DetailedTrajConfig &detailed_config)
Method converts a list of lanelet centerline points and current vehicle state into a usable list of t...
DetailedTrajConfig compose_detailed_trajectory_config(double trajectory_time_length, double curve_resample_step_size, double minimum_speed, double max_accel, double lateral_accel_limit, int speed_moving_average_window_size, int curvature_moving_average_window_size, double back_distance, double buffer_ending_downtrack, std::string desired_controller_plugin="default")
std::vector< PointSpeedPair > create_geometry_profile(const std::vector< carma_planning_msgs::msg::Maneuver > &maneuvers, double max_starting_downtrack, const carma_wm::WorldModelConstPtr &wm, carma_planning_msgs::msg::VehicleState &ending_state_before_buffer, const carma_planning_msgs::msg::VehicleState &state, const GeneralTrajConfig &general_config, const DetailedTrajConfig &detailed_config)
Creates geometry profile to return a point speed pair struct for LANE FOLLOW and LANE CHANGE maneuver...
auto to_string(const UtmZone &zone) -> std::string
Definition: utm_zone.cpp:21
Stuct containing the algorithm configuration values for cooperative_lanechange.
Convenience struct for storing the original start_dist and starting_lane_id associated with a receive...