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
How can I have a better square by the way?
Thanks for your help.