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:
- First time I run the program, everything works great
- Second time I run the program, it gives the output of the previous programs as the value of covariance.
- The third time I run the program, it gives the correct output and gives the current covariance value
- 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