Don’t Crash! Tailing Behaviour

Introduction

In this exercise, we managed to drive the duckiebot autonomously in a way that can follow a leader robot at a safe distance while it is in its sight, and also follow duckietown rules including stopping at intersections and changing LED lights when turning.

In summary,  for stopping at intersections, we used the apriltag detections as landmarks to indicate if the robot is at an intersection and should stop or not.

For maintaining a safe distance we used the duckiebot_distance_node, and for changing the LEDs, we used a service and changed the LED based on the duckiebot current state.

We will provide more details for each utility of tailing behavior in the next sections.

Strategy for Maintaining Distance and Avoiding Collisions

We define a subscriber to the “/front_center_tof_driver_node/range” topic. This topic has the value for the range in which an object is in front of our robot.

self.distance_sub = rospy.Subscriber("/" + self.veh + "/front_center_tof_driver_node/range", Range,

                                            self.distance_response,

                                            queue_size=1)

Based on this information, we can maintain a safe distance from the front duckiebot and perform tailing safely.

We define a callback function for the subscriber to the “/front_center_tof_driver_node/range” topic, and based on the distance, we change the self.hard_stop flag:

 def distance_response(self, msg):

       try:

           msg = msg.range

           if msg < 0.10:

               self.hard_stop = True

               self.time_since_hard_stop = rospy.get_time()

       except Exception as e:

           self.log("Distance calculation crashed " + str(e))

If the distance from the front duckiebot is more than 0.10, we set the hard_stop  to True, so that our robot stops following the front robot and waits until the distance is safe enough.

We later use this flag in the drive() method to send proper commands for the robot to stop. In the drive() function, we set v and omega for controlling the robot based on its state:

if self.current_state == "STOP" or self.current_state == "WAITING" or self.hard_stop:

           self.twist.v = 0

           self.twist.omega = 0

           self.last_error = 0

           self.last_time = rospy.get_time()

The way we chose 0.10 as the threshold to change the robot's state is by trial and error:

When we set the duckiebot’s v and omega to zero, it takes some time to reach zero velocity and for the robot to stop. We need to be cautious and set the safe distance(threshold) large enough so that the duckiebot has enough time to stop if the leader stops, and avoids collisions. On the other hand, we need to set the threshold small enough so that the duckiebot can still detect the front bot based on incoming camera images.

We also later check the hard_stop variable in the callback() method and if it has been too long since the robot is stopped, we change its value to false so that the robot can start moving again after a pause:

elif (self.hard_stop and rospy.get_time() - self.time_since_hard_stop > 0.2):

               self.hard_stop = False

Note: We also tried using the duckibot_distance_node provided for us. However, we realized it brings some unwanted delay for distance detection. Therefore, we switched to the “/front_center_tof_driver_node/range” topic.

Method for Following the Leader 

We control the robot by keeping track of the robot's state in self.current_state variable, and changing the control commands based on the corresponding state.

If a lead robot is detected, and the distance is more than the safe threshold, the state is set to “FOLLOWING” in the process_center() method:

def process_center(self, msg):

       if(msg.detection.data):

           self.last_tail_detection = rospy.get_time()

           self.log("Setting last x to " + str( msg.corners[3].x))

           self.last_x = msg.corners[3].x

           if (self.current_state != "FOLLOWING"):

               self.current_state = "FOLLOWING"

               self.change_color("FOLLOWING")

This means that our robot should follow the lead moving robot.

For implementing this part, we modify the PID error so that the modified error indicates the distance between the center of the detected back label of the robot and the center of the camera image. ( We aim to keep the leader in the center of our robot’s sight.)

The below code block in the callback method handles this part:

           # Search for lane in front

if(self.current_state == "FOLLOWING"):

               self.log("Gets here")

               if(self.last_x is not None):

                   self.proportional = (self.last_x - self.CENTER)*1

                   self.log("Sedding proportional to " + str(self.proportional))

                   self.pending_update = True

                   self.last_x = None

                   self.total_steps_to_take = 4

For updating the robot’s v and omega when in the following state, we have defined a self.pending_update variable which is set to True if a new leader robot is detected. We also define a self.total_steps_to_take which indicates how many updates we want to perform.

We check the state of the pending update in the callback() method, and if there is not a pending update and the robot is in “FOLLOWING”, we tell the robot to stop since it has lost sight of the leader. The robot will start with the “LANE” state after some seconds since there is a limit to how much the robot will stop.

On the other hand, if there is a pending update, we perform the update for the number of “self.total_steps_to_take” which is set to 4 in our case.

if not self.pending_update:

           if self.current_state == "FOLLOWING":

               self.log("Stopping because can't see the leader")

               self.twist.v = 0

               self.twist.omega = 0

               self.last_time = rospy.get_time()

       self.vel_pub.publish(self.twist)

       if self.pending_update:

           self.total_steps_to_take -= 1

           if self.total_steps_to_take == 0:

               self.pending_update = False

               # self.last_x = None

We also check in the callback() method if it has been too long from the robot’s last tail detection. 

In this case, If the state is “following”, we should set back the state to “LANE” which is the default lane following.

if(self.current_state == "FOLLOWING" and rospy.get_time() - self.last_tail_detection > 2):

               self.current_state = "LANE"

               self.change_color("LANE")

Strategy for Stopping at Intersections

The tag_ids for the apriltags at the intersections are:

intersection_tags = [153, 162, 58, 62, 133, 169]

Having this, and running the apriltag detection algorithm, we can set the current_state to STOP whenever the duckiebot detects an intersection apriltag and is closer than 0.2 to it. (The distance between the robot and the tag is the z value of the translation in the transform) We do this in the detect_april_tags function as follows:

for tag in tags:

               if int(tag.tag_id) in intersection_tags:

                   index_i += 1

                   # turn rotation matrix into quaternion

                   q = _matrix_to_quaternion(tag.pose_R)

                   p = tag.pose_t.T[0]

                   distance = p[2]

                   self.log("First position: p[0] val: " + str(p[0]))

                   self.log("Second position: p[1] val: " + str(p[1]))

                   if distance < 0.20 and rospy.get_time() - self.stop_at_time > 1:

                       self.log("Should stop here")

                       self.current_state = "STOP"

                       self.stop_at_time = rospy.get_time()

Method for changing the LED Lights

We define the service for changing the LED lights in the constructor of the lane follow node class:

rospy.wait_for_service("/" + self.veh + '/led_emitter_node/set_pattern')

self.log("Service detected")

self.led_service = rospy.ServiceProxy("/" + self.veh + '/led_emitter_node/set_pattern', ChangePattern)

We also define a function to call the service whenever needed:

def change_color(self, color_name):

       p = String()

       # self.log(str(p))

       try:

           p.data = color_name

           val = self.led_service(p)

       except Exception as e:

           self.log(str(e))

The following table shows the way we change the LED patterns and control commands based on the robot's state.

A summary of the possible states and corresponding actions

In our code, we control the duckiebot behavior with self.current_state variable which indicates how we want to control the robot and what actions we want to take based on its state.

The states include:

STATES = ["STOP", "FOLLOWING", "WAITING", "LANE"]

Here is a summary of the states and their corresponding commands and LED lights:

Results

Deliverable: Video

We recorded two videos. One shows the tailing behavior, and the other shows stopping at an intersection with an apriltag before turning right, and then tailing the leader for turning left. 

The tailing and lane following demo is available here whereas the right and left turn demo is available here

Questions

• How well did your implemented strategy work?

The implementation is not perfect but it works pretty well. It is successful in following the front robot at a safe distance and switching to lane following when it loses sight of the robot.

The LED pattern however gives a huge overload to the robot and does not perform well with the lane following and tailing behavior, so we just printed the change in the states that we intended to apply using the LED service we defined.

• Was it reliable?

In general, it is reliable, However, sometimes the apriltag detection has delays or mistakes in finding the apriltags which results in the robot not stopping at the intersections.

Also, when we first were using the duckiebot_distance_node, there was a delay for duckiebot detection which caused collision sometimes. We switched to”front_center_tof_driver_node/range” which made the detection faster and the number of collisions lessened.  This emphasizes the importance of real-time online detection in critical situations.

• In what situations did it perform poorly?

When the robot loses sight of the leader and changes its state from “following” to “lane”, in some cases the robot might be in a place on the duckietown where it is hard to get back on track on the road. 

Challenges and Takeaways

• Tuning the PID controller with the modified error to make the robot follow the leader took some time and effort.

• Another challenge was setting the velocity and safe threshold in a way that they match each other, and can avoid collisions.

Breakdown

We did the exercise with each other’s cooperation.

References

The initial template for duckiebot detection and duckiebot distance:

https://github.com/XZPshaw/CMPUT412503_exercise4

We used the lane following code shared on elcass