Closed geopimik closed 5 months ago
Removed Lane message publishing from path_callback
added check for self.path_linestring https://github.com/AnishShr/autoware_mini_practice/blob/3aec090f4175fc6fe6067ff03cba0b64d799a368/practice_4/nodes/control/pure_pursuit_follower.py#L77
Set the steering angle and linear velocity to 0 when the path_linestring or distance_to_velocity_interpolator is None https://github.com/AnishShr/autoware_mini_practice/blob/3aec090f4175fc6fe6067ff03cba0b64d799a368/practice_4/nodes/control/pure_pursuit_follower.py#L77-L80
Now it is ok, but there is a lot of ways how to organize code and I won't say that the next suggestion is correct, but for me it would seem a bit more readable.
I would keep the message creation and publishing part together in one group at the end of callback and organize the code in the following manner. Note also that in first if I'm using local variables steering_angle
and velocity
.
def current_pose_callback(self, msg):
# Publish velcoities only when the initial pose is set and the distance to velocity interpolator is assigned a value
if self.path_linestring is None or self.distance_to_velocity_interpolator is None:
steering_angle = 0.0
velocity = 0.0
else:
current_pose = Point([msg.pose.position.x, msg.pose.position.y])
d_ego_from_path_start = self.path_linestring.project(current_pose)
# using euler_from_quaternion to get the heading angle
_, _, heading = euler_from_quaternion([msg.pose.orientation.x, msg.pose.orientation.y, msg.pose.orientation.z, msg.pose.orientation.w])
# dist from start of path to the lookahead distance
d_lookahead_point = d_ego_from_path_start + self.lookahead_distance
lookahead_point = self.path_linestring.interpolate(d_lookahead_point)
ld = distance(current_pose, lookahead_point)
# lookahead point heading calculation
lookahead_heading = np.arctan2(lookahead_point.y - current_pose.y, lookahead_point.x - current_pose.x)
# Compute the linear velocity and steering angle, and publish
velocity = self.distance_to_velocity_interpolator(d_ego_from_path_start)
alpha = lookahead_heading - heading
steering_angle = np.arctan2((2*self.wheel_base)*np.sin(alpha), ld)
vehicle_cmd = VehicleCmd()
vehicle_cmd.header.stamp = msg.header.stamp
vehicle_cmd.header.frame_id = "base_link"
vehicle_cmd.ctrl_cmd.steering_angle = steering_angle
vehicle_cmd.ctrl_cmd.linear_velocity = velocity
self.current_velocity_pub.publish(vehicle_cmd)
Yes, I agree it seems more readable.
Modified the controller code making it more readable.
https://github.com/AnishShr/autoware_mini_practice/commit/f10b6f4f44b0e80e025b95bc4626a17923e438f5
OK
There are some problems in the follower
pure_pursuit_follower.py
should constantly publish vehicle_cmd at 50Hz (frequency of current_pose). When the path ends (the global planner reaches the goal and clears it), it should still publish at 50Hz just the steering and speed have to be 0.Problems:
path_callback
, since it is only run when new path comes, but we need it whenever new pose update comes. And it duplicates message creation and the publishing code below. https://github.com/AnishShr/autoware_mini_practice/blob/8dd232a77319ab8eb8a20a69bd2aceb8b6246b32/practice_4/nodes/control/pure_pursuit_follower.py#L38-L48self.path_linestring
https://github.com/AnishShr/autoware_mini_practice/blob/8dd232a77319ab8eb8a20a69bd2aceb8b6246b32/practice_4/nodes/control/pure_pursuit_follower.py#L83