Ex. 3.12. What is wrong in my code?

[Kane168]

The code looks suspended after global_localization execution. No move and print result is available somehow (square_move.py):

#! /usr/bin/env python

import rospy

from std_srvs.srv import Empty, EmptyResponse, EmptyRequest

from geometry_msgs.msg import PoseWithCovarianceStamped, Pose

from geometry_msgs.msg import Twist

robot_covariance = Pose()

def service_callback(request):

while (robot_covariance[0] or robot_covariance[7] or robot_covariance[35]) > 0.65:

    rospy.wait_for_service('/global_localization')

    GL_service = rospy.ServiceProxy('/global_localization', Empty)

    GL_object = EmptyRequest()

    result = GL_service(GL_object)

    pub = rospy.Publisher('/cmd_vel', Twist, queue_size=1)

    rate = rospy.Rate(1)

    move = Twist()

    move.linear.x = 1

    move.angular.z = 0

    move.linear.x = 0

    move.angular.z = 0.5

    move.linear.x = 1

    move.angular.z = 0

    move.linear.x = 0

    move.angular.z = 0.5

    move.linear.x = 1

    move.angular.z = 0

    move.linear.x = 0

    move.angular.z = 0.5

    move.linear.x = 1

    move.angular.z = 0

    move.linear.x = 0

    move.angular.z = 0.5

    print(xyz_covariance)


move.linear.x = 0

move.angular.z = 0

print(xyz_covariance)

while not rospy.is_shutdown():

    pub.publish(move)

    rate.sleep()

return EmptyResponse

def sub_callback(msg):

global robot_covariance

robot_covariance = msg.pose.covariance

rospy.init_node(‘square_amcl_server’)

rospy.Service(’/squaring_amcl_service’, Empty , service_callback)

rospy.Subscriber(’/amcl_pose’, PoseWithCovarianceStamped, sub_callback)

rospy.spin()

Launch the launch file:
(somehow I have to remove “<” to make the launch file content visible)

launch>

!-- Run the map server →
arg name=“map_file” default="$(find husky_navigation)/maps/my_map.yaml"/>
node name=“map_server” pkg=“map_server” type=“map_server” args="$(arg map_file)" />

!— Run AMCL →
include file="$(find husky_navigation)/launch/amcl.launch" />

!— Run Move Base →
include file="$(find husky_navigation)/launch/move_base.launch" />

!— Square Move →
node name=“square_amcl_server” pkg=“initialize_particles” type=“square_move.py” output=“screen” />

/launch>

No any error is reported. While using rosservice call /squaring_amcl_service (tab tab enter)
The global_localization can be executed but there is no following actions executed, like move the bot or print the covariance on the screen.

[albertoezquerro]

Hello @Kane168 ,

When calling your service, the following error comes up:

You need to solve this error (and other that might come after fixing this one) in order to make your code work. Have you done the ROS Basics in 5 Days course? If not, I’d suggest you complete this course before going for the navigation one.

Also, in case you find yourself really stuck, you can have a look at the solutions of the exercises, for reference, at the end of the Unit.

[Kane168]

Thanks Alberto. I am not quite sure the meaning of this error. Will check the solution for the reference.