Closed geopimik closed 5 months ago
1 & 2. Removed all unused class variables, and set them to local variables
Created a function to create the list of waypoints https://github.com/AnishShr/autoware_mini_practice/blob/3aec090f4175fc6fe6067ff03cba0b64d799a368/practice_4/nodes/planning/global/lanelet2_global_planner.py#L54-L81
Used min replacing the if block https://github.com/AnishShr/autoware_mini_practice/blob/3aec090f4175fc6fe6067ff03cba0b64d799a368/practice_4/nodes/planning/global/lanelet2_global_planner.py#L63-L64
Computed speed in a more concise manner https://github.com/AnishShr/autoware_mini_practice/blob/3aec090f4175fc6fe6067ff03cba0b64d799a368/practice_4/nodes/planning/global/lanelet2_global_planner.py#L66
self.goal_point
and self.goal_pos
. I would have kept one of them. You went for creating self.waypoints
as a global variable. I think it makes things a bit less transparent and more complicated for you.To me it seems that you have made the node a bit more complicated than it was before. The previous solution was almost working and needed some tiny adjustments.
In current solution you loop over all of the waypoints twice, here:
and then again here:
current_pose_callback
- it will do it with 50Hz!My suggestion:
goal_callback
lanelet_sequence_to_waypoints
and handle everything you are trying to achieve with the second loop (projecting goal point to path is one of the valid solutions) also inside that function.Lane
message and will publish the list of waypoints in that messagecurrent_pose_callback
checks if goal is reached and once it is, it will call the same function with empty list to publish empty Lane
message.The lanelet_sqeuqnce_to_waypoints function now returns the list of waypoints where waypoints projected distance in the path is less than or equal to the goal point in path. However, I could not think of anything simpler than using another loop. Is there a simpler method? https://github.com/AnishShr/autoware_mini_practice/blob/e0f954de60f512726e37c8a4a1c7d7d7ce474d30/practice_4/nodes/planning/global/lanelet2_global_planner.py#L63-L103
publishing Lane msg from current_pose_callback is removed. Once the goal is reached the same function (create_and_publish_lane_msg) publishes empty waypoints once. https://github.com/AnishShr/autoware_mini_practice/blob/e0f954de60f512726e37c8a4a1c7d7d7ce474d30/practice_4/nodes/planning/global/lanelet2_global_planner.py#L169-L172
Latest commit: https://github.com/AnishShr/autoware_mini_practice/commit/e0f954de60f512726e37c8a4a1c7d7d7ce474d30
Here you should have the warning, because it is a problem when do to have the current_location
https://github.com/AnishShr/autoware_mini_practice/blob/5d312201cd897519df89f0435e257897b6112d0f/practice_4/nodes/planning/global/lanelet2_global_planner.py#L117-L118
I would keep only one of them as global variables https://github.com/AnishShr/autoware_mini_practice/blob/5d312201cd897519df89f0435e257897b6112d0f/practice_4/nodes/planning/global/lanelet2_global_planner.py#L44-L45
goal_pos
, and it would be more logical then that in current_pose_callback
you check if the goal_pos
is not none and in the goal_callback
you check current_pose
waypoints
should be global variable, it more like a result from goal_callback
I don't like also taking last wp as goal point on path. I would just reset its value when the path is created and you have synced the goal point with path ending. https://github.com/AnishShr/autoware_mini_practice/blob/5d312201cd897519df89f0435e257897b6112d0f/practice_4/nodes/planning/global/lanelet2_global_planner.py#L162
You should add z coordinate also for the goal point (last wp on the path). All the other waypoints have the z coordinate. https://github.com/AnishShr/autoware_mini_practice/blob/5d312201cd897519df89f0435e257897b6112d0f/practice_4/nodes/planning/global/lanelet2_global_planner.py#L96-L99
Added rospy warning log in console https://github.com/AnishShr/autoware_mini_practice/blob/f9132c4de756dcbc10dca970f90e2089eaa3f208/practice_4/nodes/planning/global/lanelet2_global_planner.py#L120-L122
Removed self.waypoints
as class variables. In the current_pose_callback
, it checks and runs according to the self.goal_pos
variable
https://github.com/AnishShr/autoware_mini_practice/blob/f9132c4de756dcbc10dca970f90e2089eaa3f208/practice_4/nodes/planning/global/lanelet2_global_planner.py#L42-L44
https://github.com/AnishShr/autoware_mini_practice/blob/f9132c4de756dcbc10dca970f90e2089eaa3f208/practice_4/nodes/planning/global/lanelet2_global_planner.py#L158-L160
In the lanelet_sequence_to_waypoints
function, the self.goal_pos
variable is updated in the end with the coordinates of the goal point in path.
https://github.com/AnishShr/autoware_mini_practice/blob/f9132c4de756dcbc10dca970f90e2089eaa3f208/practice_4/nodes/planning/global/lanelet2_global_planner.py#L101-L102
Added z coordinate of the goal point as well in the last waypoint https://github.com/AnishShr/autoware_mini_practice/blob/f9132c4de756dcbc10dca970f90e2089eaa3f208/practice_4/nodes/planning/global/lanelet2_global_planner.py#L95-L99
It would also be nice to have a node name at the beginning of the info, log, warning and error messages. Similarly as you can see here. https://github.com/AnishShr/autoware_mini_practice/blob/a750cd623ce0c1a6fcfd52bd598459fb2946784b/practice_4/nodes/planning/global/lanelet2_global_planner.py#L124
OK
and 4.
lanelet_sequence_to_waypoints
and update its x and y coordinates, but you keep the z? Why not update also z? goal_pos
to eventually find goal_point_in_path
and then use the global variable self.goal_pos
to set the z value. The global variable might have already changed because there was a new goal point entered.I would not create that variable
goal_point_in_path.x
and goal_point_in_path.y
instead of goal_pos_in_path
and using indeciesCurrently, if speed_ref
would not be present, it would skip that if
and you have an error that there is no variable speed
defined
https://github.com/AnishShr/autoware_mini_practice/blob/a750cd623ce0c1a6fcfd52bd598459fb2946784b/practice_4/nodes/planning/global/lanelet2_global_planner.py#L63-L66
3 and 4. Modified the code to update the z-coordinate for goal pose taking the z-coordinate of the closest waypoint. https://github.com/AnishShr/autoware_mini_practice/blob/88aa679257b27f8de6cb0d80a6a00370dbe4eabf/practice_4/nodes/planning/global/lanelet2_global_planner.py#L97-L112
self.speed_limit
. If there is speed_ref
, it will replace the speed with minimum from the two speeds.
https://github.com/AnishShr/autoware_mini_practice/blob/88aa679257b27f8de6cb0d80a6a00370dbe4eabf/practice_4/nodes/planning/global/lanelet2_global_planner.py#L66-L70Latest commit: https://github.com/AnishShr/autoware_mini_practice/commit/78ac6f1205ea00c723de6c0b2709274b425b866b
OK
Please fix the following:
You are not using this anywhere: https://github.com/AnishShr/autoware_mini_practice/blob/8dd232a77319ab8eb8a20a69bd2aceb8b6246b32/practice_4/nodes/planning/global/lanelet2_global_planner.py#L45
This does not need to be the global variable, used only in one callback. And you could also do without it, since you have
self.goal.pos
, but it's on you to decide https://github.com/AnishShr/autoware_mini_practice/blob/8dd232a77319ab8eb8a20a69bd2aceb8b6246b32/practice_4/nodes/planning/global/lanelet2_global_planner.py#L41As stated in Task 4 the point 5, there should be a separate function for publishing waypoints:
publish_waypoints(waypoints)
Lane
message creationLane
message should be created only once inside that functioncurrent_pose_callback
, you can also call that function with just an empty listYou could make this code more compact by using the
min
operator. Then you wouldn't needif
andelse
here: https://github.com/AnishShr/autoware_mini_practice/blob/8dd232a77319ab8eb8a20a69bd2aceb8b6246b32/practice_4/nodes/planning/global/lanelet2_global_planner.py#L65-L70Correct way to do it, but a bit clumsy by having these multiple numbers. Could do also
/ 3.6
https://github.com/AnishShr/autoware_mini_practice/blob/8dd232a77319ab8eb8a20a69bd2aceb8b6246b32/practice_4/nodes/planning/global/lanelet2_global_planner.py#L73