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
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:
self.husky_vel_publisher.publish(self.cmd)
self.rate.sleep()
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:
self.husky_vel_publisher.publish(self.cmd)
self.rate.sleep()
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:
self.husky_vel_publisher.publish(self.cmd)
self.rate.sleep()
i += 1
def move_square(self):
i = 0
while not self.ctrl_c and i < 4:
# Move Forwards
rospy.loginfo("######## Going Forwards...")
self.move_forward()
self.stop_husky()
# Turn
rospy.loginfo("######## Turning...")
self.turn()
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
return cov
if __name__ == '__main__':
rospy.init_node('move_husky_node', anonymous=True)
MoveHusky_object = MoveHusky()
cov = 1
while cov > 0.65:
MoveHusky_object.call_service()
MoveHusky_object.move_square()
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...")
else:
rospy.loginfo("######## Total Covariance is lower than 0.65. Robot correctly localized!")
rospy.loginfo("######## Exiting...")
My doubts are:
-
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.65or not, but that part is just printing some message. -
Till now I didn’t understand, crystal clear, why do we import Empty and EmptyRequest?
-
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 thisrospy.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 ? -
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 ?
-
I’m guessing that
call_service()function is publishing into the /global_localization service. But, what about thesub_callback()function ? -
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… -
Why are the few logics defined in the very end of the program in
if __name__ == '__main__'part. Logics like:
a. Why isMoveHusky()class called in theif __name__ == '__main__'?
b. I’m seeing this for the second time thatrospy.init_node()is called here in the end inif __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 fromif __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 thisif __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.