Practice 4 - global_planner code

[geopimik]

1. I would call these global variables and put them below actual parameters. I think real parameters should be read in first they might matter how the node internally behaves and which global variables are created etc...

https://github.com/Karljohan99/autoware_mini_practice/blob/e44f89f3182ba5c0396eff1d5fb47b4cfaa99efe/practice_4/nodes/planning/global/lanelet2_global_planner.py#L22-L24

2. The first thing in goal_point_callback should also check if the current pose is available. Without it, you can't plan a path. And if it is unavailable, there should be a warning message, because it is expected to be available.

https://github.com/Karljohan99/autoware_mini_practice/blob/e44f89f3182ba5c0396eff1d5fb47b4cfaa99efe/practice_4/nodes/planning/global/lanelet2_global_planner.py#L58-L60

3. Couple of problems here:
   • what if speed_ref has a bigger value than speed_limit?
   • speed_limit is also in km/h

https://github.com/Karljohan99/autoware_mini_practice/blob/e44f89f3182ba5c0396eff1d5fb47b4cfaa99efe/practice_4/nodes/planning/global/lanelet2_global_planner.py#L110-L113

4. When running your code, I get this error message in the console:
   • RuntimeWarning: invalid value encountered in line_locate_point return lib.line_locate_point(line, other)
   • The problem lies in the code below. You do not remove overlapping points (the start and end of the lanelet will have the same point added to waypoints)

https://github.com/Karljohan99/autoware_mini_practice/blob/e44f89f3182ba5c0396eff1d5fb47b4cfaa99efe/practice_4/nodes/planning/global/lanelet2_global_planner.py#L154-L162

5. Just very tiny remark: it would be nice if empty lines don't have tabs. Sometimes you have them and sometimes you don't.

[Karljohan99]

Done in 42830d8

[geopimik]

1. Ok
2. Ok
3. Ok - can be done like this, but a bit clumsy. Here You could also use the min operator. For example:

           speed = float(self.speed_limit)
           if 'speed_ref' in lanelet.attributes:
               speed = min(speed, float(lanelet.attributes['speed_ref']))
           speed = speed / 3.6

4. New comment
5. Ok

[geopimik]

4. It does the correct thing, but it is very long and clumsy.

And this group is there 3 times !! With some reorganization of your code, you could bring it down to 1 time and it would look a lot cleaner.

      waypoint.pose.pose.position.x = point.x
      waypoint.pose.pose.position.y = point.y
      waypoint.pose.pose.position.z = point.z
      waypoint.twist.twist.linear.x = speed
      waypoints.append(waypoint)

Could you make an effort and try to make it a bit more compact.

https://github.com/Karljohan99/autoware_mini_practice/blob/97fe8bdf67522ec700c30b187dffd733ebe82ae2/practice_4/nodes/planning/global/lanelet2_global_planner.py#L111-L169

[Karljohan99]

Improved the code in e916c6f

[geopimik]

I was too optimistic. My mistake!

With these if's and the way you add the final point, it is quite a hassle to make it more compact! In general current solution works well! and gives the best result in the sense of how the goal point and path end are synced. Well done.

Still noticed one issue:

• This group should be brought level up (out of the waypoint level loop), because currently they are run with every point in the last lanelet up to a point where the break happens https://github.com/Karljohan99/autoware_mini_practice/blob/32ac8b4f6c64309b810fde34d169687b6a34e413/practice_5/nodes/planning/global/lanelet2_global_planner.py#L127-L131

Experimental

• We could combine to similarly behaving if's into one and make it a bit shorter

  • but the condition itself goes very long and hard to interpret, so not sure if I would add like this.
  • the new variables of x, y and z are now a bit artificial, just to have this group once. Again have to admit that I might have been wrong here and it is not worth it.

                  waypoint.pose.pose.position.x = x
                  waypoint.pose.pose.position.y = y
                  waypoint.pose.pose.position.z = z

  • and additional stop variable just to have previous group once is also a bit artificial.

    if stop:
    break

• So the best I could do currently with this is:

    def lanelet_sequence_to_waypoints(self, lanelet_path):
        waypoints = []
        is_last_lanelet = False

        for i, lanelet in enumerate(lanelet_path):
            if i == len(lanelet_path) - 1:
                is_last_lanelet = True
                last_lanelet_centerline = LineString([[point.x, point.y] for point in lanelet.centerline])
                proj_dist = last_lanelet_centerline.project(Point(self.goal_point.x, self.goal_point.y))
                last_waypoint = last_lanelet_centerline.interpolate(proj_dist)

            speed = float(self.speed_limit)
            if 'speed_ref' in lanelet.attributes:
                speed = min(speed, float(lanelet.attributes['speed_ref']))
            speed = speed / 3.6

            for j, point in enumerate(lanelet.centerline):
                waypoint = Waypoint()
                waypoint.twist.twist.linear.x = speed

                if not is_last_lanelet or is_last_lanelet and (j == 0 or LineString(last_lanelet_centerline.coords[:j+1]).length < proj_dist):
                    if j >= len(lanelet.centerline) - 1:
                        continue

                    waypoint.pose.pose.position.x = point.x
                    waypoint.pose.pose.position.y = point.y
                    waypoint.pose.pose.position.z = point.z

                else:
                    first_trimmed_point = Point(point.x, point.y)
                    previous_point = Point(lanelet.centerline[j-1].x, lanelet.centerline[j-1].y)

                    old_dist = previous_point.distance(first_trimmed_point)
                    new_dist = previous_point.distance(last_waypoint)

                    waypoint.pose.pose.position.x = last_waypoint.x
                    waypoint.pose.pose.position.y = last_waypoint.y

                    # calculate z-coordinate for the goal point assuming linear change in elevation
                    waypoint.pose.pose.position.z = lanelet.centerline[j-1].z + (lanelet.centerline[j-1].z-point.z)*new_dist/old_dist

                waypoints.append(waypoint)

        return waypoints

[Karljohan99]

Made the suggested changes in 91cfd20

[geopimik]

OK

[geopimik]

Found a small issue. Sometimes when I enter the destination, I have this message in the console:

/home/edgar/.local/lib/python3.8/site-packages/shapely/linear.py:88: RuntimeWarning: invalid value encountered in line_locate_point
  return lib.line_locate_point(line, other)

It hints that the path has duplicate points.

The problem is here: https://github.com/Karljohan99/autoware_mini_practice/blob/a09abb4acfa7dc931d7fc454a4906f6bcbae5884/practice_4/nodes/planning/global/lanelet2_global_planner.py#L138-L154

You are looping over lanelet.centerline points and if you are in last lanelet and passing the proj_dist (see previous if) you end up here and add the last waypoint. But the lanelet.centerline might have additional points so the for loop continues and all these points are added using last_waypoint, so you end up having multiple points on top of each other at the path end.

[Karljohan99]

Added a break statement in dc273e1

[geopimik]

OK