ROS Navigation, Automated localisation, getting the right answer only on every second try

Hi, in exercise 3.12, it’s asked to automate the process of localization using the global_localization service. I am able to get the correct answer most of the time using the below code, but only the second time I run the program. The program below is a modification of the solution provided. I made the addition to check for covariance value every time the robot is moving and not just at the end of the square movement.

Error is as follows:

  1. First time I run the program, everything works great
  2. Second time I run the program, it gives the output of the previous programs as the value of covariance.
  3. The third time I run the program, it gives the correct output and gives the current covariance value
  4. The fourth time I run the program, I get the covariance value achieved in the previous program and so on.

This way I am only able to get the correct value every second time, can i know where i went wrong? i reason i call covariance every time the robot is moving is to get a more accurate result on the correct robot position compared to the position it would have been after completing the entire square movement.

Looking forward to your views on the topic

#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist, PoseWithCovarianceStamped
from std_srvs.srv import Empty, EmptyRequest
import time
import math

class MoveHusky():
    
    def __init__(self):
        
        # Init Publisher
        self.husky_vel_publisher = rospy.Publisher('/cmd_vel', Twist, queue_size=1)
        self.cmd = Twist()
        # Init Subscriber
        self.amcl_pose_sub = rospy.Subscriber('/amcl_pose', PoseWithCovarianceStamped, self.sub_callback)

        self.sub_msg = PoseWithCovarianceStamped()
        # Initialize Service Client
        rospy.wait_for_service('/global_localization')
        self.disperse_particles_service = rospy.ServiceProxy('/global_localization', Empty)
        self.srv_request = EmptyRequest()
        # Other stuff
        self.ctrl_c = False
        rospy.on_shutdown(self.shutdownhook)
        self.rate = rospy.Rate(10)
        
    def shutdownhook(self):
        
        # works better than the rospy.is_shut_down()
        self.stop_husky()
        self.ctrl_c = True

    def stop_husky(self):
        
        # rospy.loginfo("Shutdown time! Stop the robot")
        self.cmd.linear.x = 0.0
        self.cmd.angular.z = 0.0
        i = 0
        
        while (i < 20 and (self.calculate_covariance()>0.65)):
            self.husky_vel_publisher.publish(self.cmd)
            self.rate.sleep()
            i += 1

    def forward(self):
        print("Forward---")
        self.cmd.linear.x=1
        self.cmd.angular.z=0
        i=0
        while (i<6 and (self.calculate_covariance()>0.65)):
            self.husky_vel_publisher.publish(self.cmd)
            rospy.sleep(0.5)
            i=i+1
        # forward()
    def turn_right(self):
        print("Turning Right-----")
        self.cmd.linear.x=0
        self.cmd.angular.z=-1
        i=0
        while(i<7 and (self.calculate_covariance()>0.65)):
            self.husky_vel_publisher.publish(self.cmd)
            rospy.sleep(0.5)
            i=i+1


    
    def move_square(self):
        
        i = 0
        
        while not self.ctrl_c and i < 4:
            # Move Forwards
            rospy.loginfo("######## Going Forwards...")
            self.forward()
            self.stop_husky()
            # Turn
            rospy.loginfo("######## Turning...")
            self.turn_right()
            self.stop_husky()
            i += 1
            
        self.stop_husky()
        rospy.loginfo("######## Finished Moving in a Square")
        
    def call_service(self):
        
        rospy.loginfo("######## Calling Service...")
        result = self.disperse_particles_service(self.srv_request)
        
    def sub_callback(self, msg):
        
        self.sub_msg = msg

    def calculate_covariance(self):
        
        # rospy.loginfo("######## Calculating Covariance...")
        cov_x = self.sub_msg.pose.covariance[0]
        cov_y = self.sub_msg.pose.covariance[7]
        cov_z = self.sub_msg.pose.covariance[35]
        rospy.loginfo("## Cov X: " + str(cov_x) + " ## Cov Y: " + str(cov_y) + " ## Cov Z: " + str(cov_z))
        cov = (cov_x+cov_y+cov_z)/3
        print("cov=",cov)
        
        return cov
        
            
if __name__ == '__main__':
    rospy.init_node('move_husky_node', anonymous=True)
    
    cov = 1
    MoveHusky_object = MoveHusky()
    
    
    # cov = MoveHusky_object.calculate_covariance()

    while cov > 0.65:
        MoveHusky_object.call_service()
        MoveHusky_object.move_square()
        
        rospy.loginfo("######## Total Covariance: " + str(MoveHusky_object.calculate_covariance()))
        if MoveHusky_object.calculate_covariance() > 0.65:
            rospy.loginfo("######## Total Covariance is greater than 0.65. Repeating the process...")
        else:
            rospy.loginfo("######## Total Covariance is lower than 0.65. Robot correctly localized!")
            rospy.loginfo("######## Exiting...")
            break

Have you checked that when you finish the program everything is terminated? Maybe you have some old node running still ?
What does this command show? Any process related to your program even when you terminate it?

ps faux

Upon opening the course without any program on any of the 4 shells, the ps faux ouput is as below:

USER       PID %CPU %MEM    VSZ   RSS TTY      STAT START   TIME COMMAND
user         1  0.0  0.0   7180  6336 ?        Ss   10:34   0:00 /bin/bash -c sudo ln -sfv /home/s
user        19  0.0  0.0   3980  1580 ?        S    10:34   0:00 /bin/bash -c sudo ln -sfv /home/s
user        22  0.0  0.0   3980  3028 ?        S    10:34   0:00  \_ /bin/bash /usr/bin/link_model
user      3505  0.0  0.0   2512   588 ?        S    10:56   0:00      \_ sleep 5
user       424  0.2  0.2 1143980 45760 ?       Sl   10:34   0:03 node /usr/local/share/shell/demo/
user       567  0.0  0.0   6032  5408 pts/0    Ss+  10:34   0:00  \_ bash
user       574  0.0  0.0   6032  5356 pts/1    Ss   10:34   0:00  \_ bash
user      3506  0.0  0.0   5904  2872 pts/1    R+   10:56   0:00  |   \_ ps faux
user       581  0.0  0.0   6032  5420 pts/2    Ss+  10:34   0:00  \_ bash
user       584  0.0  0.0   6032  5396 pts/3    Ss+  10:34   0:00  \_ bash

Upon starting the launch file (amcl node and map), rviz and finally the program to move in square, the program exits within seconds stating that covariance condition is achieved.
the ps faux is as below:(First attempt)

USER       PID %CPU %MEM    VSZ   RSS TTY      STAT START   TIME COMMAND
user         1  0.0  0.0   7180  6336 ?        Ss   10:34   0:00 /bin/bash -c sudo ln -sfv /home/simulations /home/robox; sudo rm -f /op
user        19  0.0  0.0   3980  1580 ?        S    10:34   0:00 /bin/bash -c sudo ln -sfv /home/simulations /home/robox; sudo rm -f /op
user        22  0.0  0.0   3980  3028 ?        S    10:34   0:00  \_ /bin/bash /usr/bin/link_models.sh
user      3732  0.0  0.0   2512   588 ?        S    10:58   0:00      \_ sleep 5
user       424  0.2  0.2 1143980 45548 ?       Sl   10:34   0:03 node /usr/local/share/shell/demo/app.js
user       567  0.0  0.0   6032  5408 pts/0    Ss   10:34   0:00  \_ bash
user      3563  2.1  0.1 255788 30388 pts/0    Sl+  10:57   0:00  |   \_ /usr/bin/python3 /opt/ros/noetic/bin/roslaunch my_amcl_launcher
user      3579  4.4  0.1 372156 25252 ?        Ssl  10:57   0:01  |       \_ /opt/ros/noetic/lib/map_server/map_server /home/user/catkin
user      3580  8.1  0.2 533744 40180 ?        Ssl  10:57   0:03  |       \_ /opt/ros/noetic/lib/amcl/amcl scan:=scan __name:=amcl __log
user       574  0.0  0.0   6032  5356 pts/1    Ss   10:34   0:00  \_ bash
user      3733  0.0  0.0   5904  2920 pts/1    R+   10:58   0:00  |   \_ ps faux
user       581  0.0  0.0   6032  5420 pts/2    Ss   10:34   0:00  \_ bash
user      3591 91.0  1.3 4008176 214968 pts/2  Rl+  10:57   0:35  |   \_ rviz
user       584  0.0  0.0   6032  5396 pts/3    Ss+  10:34   0:00  \_ bash
user      3601  0.0  0.0   7040  2232 pts/2    S+   10:57   0:00 dbus-launch --autolaunch adfd0ac7f4b141e2a4d8d4728b1cd147 --binary-synt
user      3602  0.0  0.0   7116  2380 ?        Ss   10:57   0:00 /usr/bin/dbus-daemon --syslog-only --fork --print-pid 5 --print-address

Upon starting the move in square program again, the program works as its supposed to, with ps faux result as below: (Second attempt)

USER       PID %CPU %MEM    VSZ   RSS TTY      STAT START   TIME COMMAND
user         1  0.0  0.0   7180  6336 ?        Ss   10:34   0:00 /bin/bash -c sudo ln -sfv /home/simulations /home/robox; sudo rm -f /op
user        19  0.0  0.0   3980  1580 ?        S    10:34   0:00 /bin/bash -c sudo ln -sfv /home/simulations /home/robox; sudo rm -f /op
user        22  0.0  0.0   3980  3028 ?        S    10:34   0:00  \_ /bin/bash /usr/bin/link_models.sh
user      4670  0.0  0.0   2512   520 ?        S    11:07   0:00      \_ sleep 5
user       424  0.4  0.3 1175164 56204 ?       Sl   10:34   0:08 node /usr/local/share/shell/demo/app.js
user       567  0.0  0.0   6032  5408 pts/0    Ss   10:34   0:00  \_ bash
user      3563  0.3  0.1 255788 30408 pts/0    Sl+  10:57   0:02  |   \_ /usr/bin/python3 /opt/ros/noetic/bin/roslaunch my_amcl_launcher
user      3579  4.1  0.1 372156 25252 ?        Ssl  10:57   0:25  |       \_ /opt/ros/noetic/lib/map_server/map_server /home/user/catkin
user      3580  7.8  0.2 533124 40436 ?        Ssl  10:57   0:49  |       \_ /opt/ros/noetic/lib/amcl/amcl scan:=scan __name:=amcl __log
user       574  0.0  0.0   6032  5356 pts/1    Ss   10:34   0:00  \_ bash
user      4671  0.0  0.0   5904  2816 pts/1    R+   11:07   0:00  |   \_ ps faux
user       581  0.0  0.0   6032  5420 pts/2    Ss   10:34   0:00  \_ bash
user      3591 91.8  1.4 4009056 226776 pts/2  Rl+  10:57   9:26  |   \_ rviz
user       584  0.0  0.0   6032  5396 pts/3    Ss+  10:34   0:00  \_ bash
user      3601  0.0  0.0   7040  2232 pts/2    S+   10:57   0:00 dbus-launch --autolaunch adfd0ac7f4b141e2a4d8d4728b1cd147 --binary-synt
user      3602  0.0  0.0   7116  2380 ?        Ss   10:57   0:00 /usr/bin/dbus-daemon --syslog-only --fork --print-pid 5 --print-address

Which is exactly as before. I could not find any program that was left running. Incase i missed something, do let me know. My knowledge in ps faux is limited to the content taught on the Linux course provided.

Let me know if you need any more info.