Skip to content

Commit

Permalink
Fix all instances of georeference (#2404)
Browse files Browse the repository at this point in the history 
* Add guard for georeference

* Fix all instances of georeference

* Re-trigger

* PR comment

* fix build error

* Update motion_computation_worker.cpp

* Update cooperative_lanechange_node.cpp

* Update port_drayage_worker.cpp

* fix motion_comp cpplint error less than 100
  • Loading branch information
@MishkaMN
MishkaMN committed Jun 17, 2024
1 parent 9913963 commit d0aac33
Show file tree
Hide file tree
Showing 20 changed files with 849 additions and 797 deletions.
7 changes: 5 additions & 2 deletions carma_cooperative_perception/src/external_object_list_to_sdsm_component.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -139,8 +139,11 @@ auto ExternalObjectListToSdsmNode::publish_as_sdsm(const external_objects_msg_ty

auto ExternalObjectListToSdsmNode::update_georeference(const georeference_msg_type & msg) -> void
{
map_georeference_ = msg.data;
map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(msg.data.c_str());
if (map_georeference_ != msg.data)
{
map_georeference_ = msg.data;
map_projector_ = std::make_shared<lanelet::projection::LocalFrameProjector>(map_georeference_.c_str());
}
}

auto ExternalObjectListToSdsmNode::update_current_pose(const pose_msg_type & msg) -> void
Expand Down
73 changes: 40 additions & 33 deletions carma_wm/src/SignalizedIntersectionManager.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -55,8 +55,8 @@ namespace carma_wm
return is_same;
}

void SignalizedIntersectionManager::convertLaneToLaneletId(std::unordered_map<uint8_t, lanelet::Id>& entry, std::unordered_map<uint8_t, lanelet::Id>& exit, const carma_v2x_msgs::msg::IntersectionGeometry& intersection,
const std::shared_ptr<lanelet::LaneletMap>& map, std::shared_ptr<const lanelet::routing::RoutingGraph> current_routing_graph)
void SignalizedIntersectionManager::convertLaneToLaneletId(std::unordered_map<uint8_t, lanelet::Id>& entry, std::unordered_map<uint8_t, lanelet::Id>& exit, const carma_v2x_msgs::msg::IntersectionGeometry& intersection,
const std::shared_ptr<lanelet::LaneletMap>& map, std::shared_ptr<const lanelet::routing::RoutingGraph> current_routing_graph)
{
std::unordered_map<uint8_t, std::unordered_set<uint16_t>> signal_group_to_exit_lanes;
std::unordered_map<uint8_t, std::unordered_set<uint16_t>> signal_group_to_entry_lanes;
Expand All @@ -76,7 +76,7 @@ namespace carma_wm
auto ref_node = local_projector.forward(gps_point);

RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Reference node in map frame x: " << ref_node.x() << ", y: " << ref_node.y());


for (auto lane : intersection.lane_list)
{
Expand All @@ -99,7 +99,7 @@ namespace carma_wm
}

RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Processing Lane id: " << (int)lane.lane_id);

size_t min_number_of_points = 2; // two points minimum are required

size_t size_of_available_points = lane.node_list.nodes.node_set_xy.size();
Expand All @@ -121,15 +121,15 @@ namespace carma_wm
node_list.push_back(curr_node);
}

RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Lane directions: " << (int)lane.lane_attributes.directional_use.lane_direction);
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Lane directions: " << (int)lane.lane_attributes.directional_use.lane_direction);

if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS)
{
// flip direction if ingress to pick up correct lanelets
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Reversed the node list!");
std::reverse(node_list.begin(), node_list.end());
}

for (auto node : node_list)
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), node.x() << ", " << node.y());
Expand All @@ -139,40 +139,40 @@ namespace carma_wm
for (auto connection : lane.connect_to_list)
{
signal_group_to_exit_lanes[connection.signal_group].emplace(connection.connecting_lane.lane);

if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::INGRESS)
signal_group_to_entry_lanes[connection.signal_group].emplace(lane.lane_id);
}

// query corresponding lanelet lane from local map
auto affected_llts = carma_wm::query::getAffectedLaneletOrAreas(node_list, map, current_routing_graph, max_lane_width_);

if (affected_llts.empty())
{
// https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
// https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
// Open issue TODO on how this error is handled
RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Given offset points are not inside the map for lane: " << (int)lane.lane_id);
continue;
}

lanelet::Id corresponding_lanelet_id;
if (lane.lane_attributes.directional_use.lane_direction == LANE_DIRECTION::EGRESS)
{
corresponding_lanelet_id = affected_llts.front().id();
{
corresponding_lanelet_id = affected_llts.front().id();
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Default corresponding_lanelet_id: " << corresponding_lanelet_id <<", in EGRESS");

}
else //ingress
{
corresponding_lanelet_id = affected_llts.back().id();
{
corresponding_lanelet_id = affected_llts.back().id();
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Default corresponding_lanelet_id: " << corresponding_lanelet_id <<", in INGRESS");
}
}

for (auto llt : affected_llts) // filter out intersection lanelets
{
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Checking if we can get entry/exit from lanelet " << llt.id());
//TODO direction of affected_llts may play role, but it should be good
if (llt.lanelet().get().hasAttribute("turn_direction") &&
if (llt.lanelet().get().hasAttribute("turn_direction") &&
(llt.lanelet().get().attribute("turn_direction").value().compare("left") == 0 ||
llt.lanelet().get().attribute("turn_direction").value().compare("right") == 0))
{
Expand All @@ -188,7 +188,7 @@ namespace carma_wm
connecting_llts = current_routing_graph->previous(llt.lanelet().get());
}

if (!connecting_llts.empty())
if (!connecting_llts.empty())
{
corresponding_lanelet_id = connecting_llts[0].id();
}
Expand All @@ -214,7 +214,7 @@ namespace carma_wm
RCLCPP_DEBUG_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Detected EGRESS, " << (int)lane.lane_id);
exit[lane.lane_id] = corresponding_lanelet_id;
}
// ignoring types that are neither ingress nor egress
// ignoring types that are neither ingress nor egress
}

// convert and save exit lane ids into lanelet ids with their corresponding signal group ids
Expand All @@ -229,7 +229,7 @@ namespace carma_wm
}
else
{
// https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
// https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
// Open issue TODO on how this error is handled
RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Unable to convert exit lane Id: " + std::to_string((int)exit_lane) + ", to lanelet id using the given MAP.msg!");
}
Expand All @@ -248,7 +248,7 @@ namespace carma_wm
}
else
{
// https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
// https://github.com/usdot-fhwa-stol/carma-platform/issues/1593
// Open issue TODO on how this error is handled
RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Unable to convert entry lane Id: " + std::to_string((int)entry_lane) + ", to lanelet id using the given MAP.msg!");
}
Expand Down Expand Up @@ -328,12 +328,12 @@ namespace carma_wm

lanelet::LineString3d stop_line(lanelet::utils::getId(), points);
stop_lines.push_back(stop_line);
}
}

lanelet::Id traffic_light_id = lanelet::utils::getId();
std::shared_ptr<lanelet::CarmaTrafficSignal> traffic_light(new lanelet::CarmaTrafficSignal(lanelet::CarmaTrafficSignal::buildData(traffic_light_id, stop_lines, entry_lanelets, exit_lanelets)));
signal_group_to_traffic_light_id_[signal_group_id] = traffic_light_id;

for (auto llt : exit_lanelets)
{
signal_group_to_exit_lanelet_ids_[signal_group_id].insert(llt.id());
Expand All @@ -345,10 +345,16 @@ namespace carma_wm
return traffic_light;
}

void SignalizedIntersectionManager::createIntersectionFromMapMsg(std::vector<lanelet::SignalizedIntersectionPtr>& sig_intersections, std::vector<lanelet::CarmaTrafficSignalPtr>& traffic_signals, const carma_v2x_msgs::msg::MapData& map_msg,
void SignalizedIntersectionManager::createIntersectionFromMapMsg(std::vector<lanelet::SignalizedIntersectionPtr>& sig_intersections, std::vector<lanelet::CarmaTrafficSignalPtr>& traffic_signals, const carma_v2x_msgs::msg::MapData& map_msg,
const std::shared_ptr<lanelet::LaneletMap>& map, std::shared_ptr<const lanelet::routing::RoutingGraph> routing_graph)
{

{

if (target_frame_ == "")
{
RCLCPP_WARN_STREAM(rclcpp::get_logger("carma_wm::SignalizedIntersectionManager"), "Georeference is not loaded yet. Returning without processing this MAP msg.");
return;
}

for (auto const& intersection : map_msg.intersections)
{
std::unordered_map<uint8_t, lanelet::Id> entry;
Expand All @@ -361,11 +367,11 @@ namespace carma_wm

for (auto iter = entry.begin(); iter != entry.end(); iter++)
{
entry_llts.push_back(map->laneletLayer.get(iter->second));
entry_llts.push_back(map->laneletLayer.get(iter->second));
}
for (auto iter = exit.begin(); iter != exit.end(); iter++)
{
exit_llts.push_back(map->laneletLayer.get(iter->second));
exit_llts.push_back(map->laneletLayer.get(iter->second));
}

lanelet::Id intersection_id = matchSignalizedIntersection(entry_llts, exit_llts);
Expand All @@ -376,14 +382,15 @@ namespace carma_wm
intersection_id = lanelet::utils::getId();

std::vector<lanelet::Lanelet> interior_llts = identifyInteriorLanelets(entry_llts, map);

std::shared_ptr<lanelet::SignalizedIntersection> sig_inter(new lanelet::SignalizedIntersection
(lanelet::SignalizedIntersection::buildData(intersection_id, entry_llts, exit_llts, interior_llts)));
intersection_id_to_regem_id_[intersection.id.id] = intersection_id;
sig_intersections.push_back(sig_inter);
}
}


// create signal group for each from the message
// check if it already exists
for (auto sig_grp_pair : signal_group_to_exit_lanelet_ids_)
Expand All @@ -395,7 +402,7 @@ namespace carma_wm
{
continue;
}

std::vector<lanelet::Lanelet> exit_lanelets;
for (auto iter = sig_grp_pair.second.begin(); iter != sig_grp_pair.second.end(); iter++)
{
Expand All @@ -414,7 +421,7 @@ namespace carma_wm
lanelet::Lanelets SignalizedIntersectionManager::identifyInteriorLanelets(const lanelet::Lanelets& entry_llts, const std::shared_ptr<lanelet::LaneletMap>& map)
{
lanelet::BasicLineString2d polygon_corners;

if (entry_llts.size() < 2) //at least two lanes (1 ingress and 1 egress) needed to form intersection
{
return {};
Expand All @@ -428,15 +435,15 @@ namespace carma_wm
lanelet::BasicPolygon2d polygon(polygon_corners);
auto interior_llt_pairs = lanelet::geometry::findWithin2d(map->laneletLayer, polygon);
lanelet::Lanelets interior_llts;

for (auto pair : interior_llt_pairs)
{
if (std::find(entry_llts.begin(),entry_llts.end(), pair.second) == entry_llts.end())
interior_llts.push_back(pair.second);
}
return interior_llts;
}

SignalizedIntersectionManager& SignalizedIntersectionManager::operator=(SignalizedIntersectionManager other)
{
this->signal_group_to_entry_lanelet_ids_ = other.signal_group_to_entry_lanelet_ids_;
Expand Down
39 changes: 20 additions & 19 deletions cooperative_lanechange/include/cooperative_lanechange/cooperative_lanechange_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -48,7 +48,7 @@ namespace cooperative_lanechange
using PointSpeedPair = basic_autonomy::waypoint_generation::PointSpeedPair;

/**
* \brief Convenience struct for storing the original start_dist and starting_lane_id associated
* \brief Convenience struct for storing the original start_dist and starting_lane_id associated
* with a received lane change maneuver.
*/
struct LaneChangeManeuverOriginalValues
Expand Down Expand Up @@ -86,11 +86,12 @@ namespace cooperative_lanechange
carma_wm::WorldModelConstPtr wm_;

// Map projection string, which defines the lat/lon -> map conversion
std::string map_georeference_{""};
std::shared_ptr<lanelet::projection::LocalFrameProjector> map_projector_;

// Trajectory frequency
double traj_freq_ = 10;

// Default recipient ID to be used for cooperative lane change Mobility Requests
std::string DEFAULT_STRING_= "";

Expand Down Expand Up @@ -120,7 +121,7 @@ namespace cooperative_lanechange
* \param msg Latest pose message
*/
void pose_cb(const geometry_msgs::msg::PoseStamped::UniquePtr msg);

/**
* \brief Callback for the twist subscriber, which will store latest twist locally
* \param msg Latest twist message
Expand Down Expand Up @@ -154,34 +155,34 @@ namespace cooperative_lanechange
carma_planning_msgs::msg::VehicleState ending_state_before_buffer_;

/**
* \brief CooperativeLaneChangePlugin constructor
* \brief CooperativeLaneChangePlugin constructor
*/
explicit CooperativeLaneChangePlugin(const rclcpp::NodeOptions &);

/**
* \brief Callback for dynamic parameter updates
*/
rcl_interfaces::msg::SetParametersResult
rcl_interfaces::msg::SetParametersResult
parameter_update_callback(const std::vector<rclcpp::Parameter> &parameters);

/**
* \brief Creates a vector of Trajectory Points from maneuver information in trajectory request
*
*
* \param req The service request
*
*
* \return vector of unobstructed lane change trajectory points
*/
*/
std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint> plan_lanechange(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req);

/**
* \brief Calculates distance between subject vehicle and vehicle 2
*
*
* \param veh2_lanelet_id Current lanelet id of vehicle 2
* \param veh2_downtrack Downtrack of vehicle 2 in its current lanelet
* \param ego_state vehicle state of the ego vehicle
*
*
* \return the distance between subject vehicle and vehicle 2
*/
*/
double find_current_gap(long veh2_lanelet_id, double veh2_downtrack, carma_planning_msgs::msg::VehicleState& ego_state) const;

/**
Expand All @@ -201,7 +202,7 @@ namespace cooperative_lanechange
* \brief Converts Trajectory Plan to (Mobility) Trajectory
* \param traj_points vector of Trajectory Plan points to be converted to Trajectory type message
* \return The Trajectory type message in world frame
*/
*/
carma_v2x_msgs::msg::Trajectory trajectory_plan_to_trajectory(const std::vector<carma_planning_msgs::msg::TrajectoryPlanPoint>& traj_points) const;

/**
Expand All @@ -218,31 +219,31 @@ namespace cooperative_lanechange
* \param resp The plan trajectory service response
* \param planned_trajectory_points The generated trajectory plan points, which are added to the service response
*/
void add_trajectory_to_response(carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp,
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);

/**
* \brief Callback for map projection string to define lat/lon -> map conversion
* \brief msg The proj string defining the projection.
*/
*/
void georeference_cb(const std_msgs::msg::String::UniquePtr msg);

////
// Overrides
////
void plan_trajectory_callback(
std::shared_ptr<rmw_request_id_t>,
carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
std::shared_ptr<rmw_request_id_t>,
carma_planning_msgs::srv::PlanTrajectory::Request::SharedPtr req,
carma_planning_msgs::srv::PlanTrajectory::Response::SharedPtr resp) override;

bool get_availability() override;

std::string get_version_id() override;

/**
* \brief This method should be used to load parameters and will be called on the configure state transition.
*/
*/
carma_ros2_utils::CallbackReturn on_configure_plugin();

// Unit Tests
Expand Down
Loading

0 comments on commit d0aac33

Please sign in to comment.