FeatProjLanelet2: invalid gid assigned duplicated id
Here is the code snippet:
for (auto& lane : waypoints_.lanes)
{
for (auto& wp : lane.waypoints)
{
if (wp.gid > waypoint_id)
{
waypoint_id = wp.gid;
}
}
}
for (auto& lane : waypoints_.lanes)
{
for (auto& wp : lane.waypoints)
{
// overwrite invalid gid
if (wp.gid == 0)
{
wp.gid = waypoint_id++;
}
}
}
The first invalid wp.gid
will be assigned to waypoint_id
which is a valid gid for another waypoint. It should be
wp.gid = ++waypoint_id;
Another thing we need to consider: why gid = 0 indicates an invalid waypoint. This is certainly not correct assumption. Here is the code where gid is set in decision_maker node:
std::pair<double, double> DecisionMakerNode::prepareActiveLaneArray()
{
int gid = 0;
for (auto& lane : current_status_.based_lane_array.lanes)
{
int lid = 0;
for (auto& wp : lane.waypoints)
{
wp.wpstate.aid = 0;
wp.wpstate.steering_state = autoware_msgs::WaypointState::NULLSTATE;
wp.wpstate.accel_state = autoware_msgs::WaypointState::NULLSTATE;
if (wp.wpstate.stop_state != autoware_msgs::WaypointState::TYPE_STOPLINE &&
wp.wpstate.stop_state != autoware_msgs::WaypointState::TYPE_STOP)
{
wp.wpstate.stop_state = autoware_msgs::WaypointState::NULLSTATE;
}
wp.wpstate.lanechange_state = autoware_msgs::WaypointState::NULLSTATE;
wp.wpstate.event_state = 0;
wp.gid = gid++;
wp.lid = lid++;
if (!isEventFlagTrue("received_back_state_waypoint") && wp.twist.twist.linear.x < 0.0)
{
setEventFlag("received_back_state_waypoint", true);
publishOperatorHelpMessage("Received back waypoint.");
}
}
}
// set waypoint state and insert interpolated waypoint for stopline if necessary
if (!ignore_map_)
{
if (use_lanelet_map_)
{
setWaypointStateUsingLanelet2Map(current_status_.based_lane_array);
}
else
{
setWaypointStateUsingVectorMap(current_status_.based_lane_array);
}
}
// re-index since setWaypointStateUsingLanelet2Map() or setWaypointStateUsingVectorMap()
// may change the size of waypoints.
gid = 0;
double min_dist = DBL_MAX;
double heading_diff = DBL_MAX;
geometry_msgs::Pose closest_wp_pose;
for (auto& lane : current_status_.based_lane_array.lanes)
{
int lid = 0;
for (auto& wp : lane.waypoints)
{
wp.gid = gid++;
wp.lid = lid++;
const double dist = amathutils::find_distance(current_status_.pose.position, wp.pose.pose.position);
if (min_dist > dist)
{
min_dist = dist;
closest_wp_pose = wp.pose.pose;
}
}
}
if (min_dist != DBL_MAX)
{
heading_diff = std::abs(amathutils::calcPosesAngleDiffDeg(current_status_.pose, closest_wp_pose));
}
return std::pair<double, double>(min_dist, heading_diff);
}
In this function, the gid
always starts from 0. Therefore we have two choices now:
- Either modify decision maker node so gid does not starts from 0. The autoware_msg definition does not specify 0 to be invalid for gid.
- Or remove this check in this node (preferred).
Edited by Jilin Zhou