Practice 4 - problems in follower

[geopimik]

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:

1. All this is wrong. You can't publish vehicle command from 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-L48
2. Need to check also self.path_linestring https://github.com/AnishShr/autoware_mini_practice/blob/8dd232a77319ab8eb8a20a69bd2aceb8b6246b32/practice_4/nodes/control/pure_pursuit_follower.py#L83
3. Can't return here - we need vehicle command at the end of the callback to be published all the time
   • https://github.com/AnishShr/autoware_mini_practice/blob/8dd232a77319ab8eb8a20a69bd2aceb8b6246b32/practice_4/nodes/control/pure_pursuit_follower.py#L84
   • One option instead of return is that you can set the steering and speed here so it would be published at the end of the code

[AnishShr]

1. Removed Lane message publishing from path_callback

2. added check for self.path_linestring https://github.com/AnishShr/autoware_mini_practice/blob/3aec090f4175fc6fe6067ff03cba0b64d799a368/practice_4/nodes/control/pure_pursuit_follower.py#L77

3. 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

pure_pursuit_controller latest commit

[geopimik]

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)   

[AnishShr]

Yes, I agree it seems more readable.

Modified the controller code making it more readable.

https://github.com/AnishShr/autoware_mini_practice/commit/f10b6f4f44b0e80e025b95bc4626a17923e438f5

[geopimik]

OK