To compute if a segment is backward or forward, only use the part of the segment inside the costmap.
assigned to @shernand
mentioned in commit 128010b2
merged