Exercise 3.12.........Navigation

Respected Professor,
I’m sorry if my doubts are too much, but I’ve to understand these codes because I have my project final review on 9th and I’m the one who has to manage the code for Navigation with my friend.
So I, being in hurry, straight away jumped to the solution of exercise 3.12 when I couldn’t do it.
#!/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
        self.disperse_particles_service = rospy.ServiceProxy('/global_localization', Empty)
        self.srv_request = EmptyRequest()
        # Other stuff
        self.ctrl_c = False
        self.rate = rospy.Rate(10)
    def shutdownhook(self):
        # works better than the rospy.is_shut_down()
        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:
            i += 1

    def move_forward(self, linear_speed=0.5, angular_speed=0.0):
        self.cmd.linear.x = linear_speed
        self.cmd.angular.z = angular_speed
        i = 0
        while i < 50:
            i += 1
    def turn(self, linear_speed=0.0, angular_speed=0.8):
        self.cmd.linear.x = linear_speed
        self.cmd.angular.z = angular_speed
        i = 0
        while i < 25:
            i += 1
    def move_square(self):
        i = 0
        while not self.ctrl_c and i < 4:
            # Move Forwards
            rospy.loginfo("######## Going Forwards...")
            # Turn
            rospy.loginfo("######## Turning...")
            i += 1
        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
        return cov
if __name__ == '__main__':
    rospy.init_node('move_husky_node', anonymous=True)
    MoveHusky_object = MoveHusky()
    cov = 1
    while cov > 0.65:
        cov = MoveHusky_object.calculate_covariance()
        rospy.loginfo("######## Total Covariance: " + str(cov))
        if cov > 0.65:
            rospy.loginfo("######## Total Covariance is greater than 0.65. Repeating the process...")
            rospy.loginfo("######## Total Covariance is lower than 0.65. Robot correctly localized!")
            rospy.loginfo("######## Exiting...")

My doubts are:

  1. Where is the main logic where it checks if the covariance is less than 0.65 and then stopping the program? I can see the logic in the last part for checking for greater than value, but nothing seems to be there for less than 0.65 to stop the program. In the very end they are checking for condition if cov > 0.65 or not, but that part is just printing some message.

  2. Till now I didn’t understand, crystal clear, why do we import Empty and EmptyRequest?

  3. In def move_forward() function, how long is it going to move linearly before taking turn? This is how I thought it works: There is this rospy.Rate(10), and the loop goes upto 50. So in each loop it stays for 10 seconds and 10 * 50 = 500, hence it moves 500 sec forward before it takes a turn. But, that doesn’t make sense since that much big square cannot fit into map. Even after running the code, I realised my calculation is wrong. I wonder why what I just calculated is not what is actually happening ?

  4. What is covariance and why is the code based on the logic that the covariance should be below 0.65 ? Also, What are the covariance values other than 1st(x), 8th(y) and last(z) shown in the command line ?

  5. I’m guessing that call_service() function is publishing into the /global_localization service. But, what about the sub_callback() function ?

  6. What is the following line doing?
    cov_x = self.sub_msg.pose.covariance[0]
    Would you please break down it’s pieces? like sub_msg, then pose then covariance

  7. Why are the few logics defined in the very end of the program in if __name__ == '__main__' part. Logics like:
    a. Why is MoveHusky() class called in the if __name__ == '__main__' ?
    b. I’m seeing this for the second time that rospy.init_node() is called here in the end in if __name__ == '__main__' , why ?
    c. Similarly calculating covariance is also called here and not anywhere else.
    d. So are there specific parts of the program that has to be called from if __name__ == '__main__'. Is there some specific structure to what has to be called from here. If yes, would you please tell me that. If no, then what is going on here?
    e. What happens if we just skip this if __name__ == '__main__' part in our normal codes.

I really appreciate the time of all my respected professors. Once again, I cordially thank you, in advance, for making everything clear to me.


Well lets go step by step:

  1. The covariance is only used to check that the robot is correctly localized. The movement is made in open loop , without any sensor readings.

  2. We import EMptyRequetsbecause we use it in the initializer for the service request message called self.srv_request.

  3. The rate is 10hz, that means that rate.sleep() will wait only 0.1 seconds. If we do it 50 times, it will only perform the move action 50*0.1=5 seconds.

  4. The Covariance here related to how well its localized the robot using mapping. Because its based in probabilities, we want that there is not a lot of deviation between what the robot reads and what it thinks its position is.

  5. Sub_callback its only storing the amcl pose in the python class variable self.sub_msg. So that when someone need to get that, it will be updated.

  6. That line is getting the first element of the covariance array that you can find in the amcl pose message that we are storing all the time through the sub_callback method.

  7. If name == “main” is just to tell python that if we execute this script then it executed the code inside this if. This is used because python scripts can be used as executables for programs and also be imported to get the classes defined inside. Have a look at the python free course :. The MoveHusky is called to initialize the class. The init_node to start ros systems ( without it rospy.loginfo wouldn’t work or subscribers ). The other questions I think they are getting answered with what I just explained in the other answers.

Hope it helped and I highly recommend you to do the Python Basics course and the ROSBasics in 5 days based on your doubt and questions, I think they would help you a lot .

1 Like

Sure Professor Miguel @duckfrost. Even Professor Alberto told me the same thing. Even though I have done both of those courses, I think my basics are still weak and I need to make them strong enough.
Thank you Professor for recommending it to me and for all your answers. Can’t thank you more.

in the course it says this:

The only values that you need to pay attention to is the first one (which is the covariance in x), the 8th one (which is the covariance in y), and the last one (which is the covariance in z).

But instead of the last one shouldn’t be the 15th element?

Let me clear it up a bit more:

The covariance matrix provided in the PoseWithCovarianceStamped message from ROS’s amcl_pose topic is a 6x6 matrix. This matrix represents the uncertainty in the robot’s pose, which is represented as [x, y, z, roll, pitch, yaw].

The matrix is flattened into an array of 36 elements. The primary diagonal of the matrix represents the variance of each variable:

  • self.sub_msg.pose.covariance[0] represents the variance in the x position (cov_x).
  • self.sub_msg.pose.covariance[7] represents the variance in the y position (cov_y).
  • self.sub_msg.pose.covariance[35] represents the variance in the yaw orientation (cov_z).

Note that in 2D robot localization with ROS’s AMCL, the z position, roll, and pitch are typically ignored since the robot is considered to be operating on a plane. That’s why the only elements you are concerned with are x, y, and yaw.

The off-diagonal elements of the covariance matrix represent the covariances between pairs of variables. If two variables increase or decrease together, then their covariance is positive. If one variable tends to increase when the other decreases, their covariance is negative. If the covariance is zero, it suggests that the two variables do not change together.

For instance:

  • Element [1] would be the covariance between x and y.
  • Element [6] would be the covariance between y and x, which should be the same as the covariance between x and y.
  • Element [30] would represent the covariance between yaw and x.