BB8 robot move in square - services quiz

Hello,

I’m working on this quiz since a while now, but still can’t properly have the desired behaviour.
Can you help me through ?

This is my current code :

#! /usr/bin/env python

import rospy
from my_custom_srv_msg_pkg.srv import BB8CustomServiceMessage, BB8CustomServiceMessageResponse
from geometry_msgs.msg import Twist
from math import pi

linear_speed  = 0.5        #m/s
angular_speed = pi/16      #0.39 rad/s

def stop(): 
    cmd = Twist()
    pub.publish(cmd)

def move_straight(): 
    global linear_speed
    cmd = Twist()
    cmd.linear.x = linear_speed
    pub.publish(cmd)

def turn_for_pi_2(): 
    global angular_speed
    cmd = Twist()
    cmd.angular.z = angular_speed
    pub.publish(cmd)

def callback(request): 

    global linear_speed

    linear_move_duration = request.side/linear_speed  
    rospy.loginfo("Linear move duration : " + str(linear_move_duration))  

    #repetitions time
    sec = 0
    for i in range(4 * request.repetitions):

        #move linearly for the side distance on X axis 
        while sec < linear_move_duration: 
            move_straight()
            sec += 1
            rate.sleep()
        sec = 0

        #stop/pause before turning
        stop()

        #turn clockwise
        #4seconds as (8sec * pi/16[rad/s]) should make the bot turn for pi/2 rad
        while sec < 8:          
            turn_for_pi_2()
            sec += 1
            rate.sleep()
        sec = 0

        #stop 
        stop()

    response = BB8CustomServiceMessageResponse()
    response.success = True
    return response
        

if __name__ =='__main__': 

    rospy.init_node('bb8_move_custom_srv_server')
    rate = rospy.Rate(1)                                                 
    pub = rospy.Publisher('/cmd_vel', Twist, queue_size = 1)
    my_service = rospy.Service('/move_bb8_in_square_custom', BB8CustomServiceMessage, callback)
    rospy.spin()

The robot performs the 4*repetitions as specified in the code, but does not move for the distance specified. The linear speed I choose is 0.5m/s to limit inertia effects. The time the bot should take to move for a given distance (side) is calculated in the code as the variable “linear_move_duration”.
However, when I call the service with for example the parameter side = 1, the bot doesn’t move for one tile but an half.
I suspect it may be an issue with my use of ropsy.Rate.sleep().
I specified a rate of 1Hz and used the sleep() method only inside the while loops. In the first one, I should have each iteration spending 1s to execute (from my understanding), and in the case of side = 1m, linear_move_duration == 2 sec with 0.5m/s. Thus, I should have 2 iterations for a total time of 2 secs and then a move on 1 meter (1 tile). And that’s not what’s happening.

I should have the same issue for the rotation movement, but it seems to run well even though the square is still not perfect :frowning:

How can I have a better square by the way? :confused:

Thanks for your help.

This topic was automatically closed 5 days after the last reply. New replies are no longer allowed.