Closed geopimik closed 4 months ago
Done in 42830d8
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. 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.
Improved the code in e916c6f
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:
We could combine to similarly behaving if's into one and make it a bit shorter
waypoint.pose.pose.position.x = x
waypoint.pose.pose.position.y = y
waypoint.pose.pose.position.z = z
stop
variable just to have previous group once is also a bit artificial.
if stop:
break
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
Made the suggested changes in 91cfd20
OK
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.
Added a break statement in dc273e1
OK
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
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
speed_ref
has a bigger value thanspeed_limit
?speed_limit
is also in km/hhttps://github.com/Karljohan99/autoware_mini_practice/blob/e44f89f3182ba5c0396eff1d5fb47b4cfaa99efe/practice_4/nodes/planning/global/lanelet2_global_planner.py#L110-L113
RuntimeWarning: invalid value encountered in line_locate_point return lib.line_locate_point(line, other)
https://github.com/Karljohan99/autoware_mini_practice/blob/e44f89f3182ba5c0396eff1d5fb47b4cfaa99efe/practice_4/nodes/planning/global/lanelet2_global_planner.py#L154-L162