Neste post usaremos a ferramenta turtlesim para aplicar comandos de movimentos. Essa ferramenta permite compreender através de simulação em uma tartaruga virtual que equivale a um robô. Faremos isso através de tópicos e mensagens (vistos anteriormente).
Após iniciar o turtlesim através de
$ rosrun turtlesim turtlesim_node
é possível ver os tópicos e mensagens disponíveis pro desenvolvimento da aplicação usando
$ rostopic list
nota-se que há 3 novos topics disponíveis e /turtle1/cmd_vel é o responsável pelo movimento do robô.
Para obter informações sobre esse tópico faça
$ rostopic info /turtle1/cmd_vel
Nota-se que o tipo de mensagem usado nesse tópico é geometry_msgs/Twist e será usado como import na aplicação.
Para obter os parâmetros da mensagem faça
$ rosmsg show geometry_msgs/Twist
e perceba que a mensagem é composta por componentes x, y e z linear e angular. Os movimentos dessa aplicação serão apenas planos, nesse caso, o componente z linear será 0(zero), assim como os componentes x e y angular.
O tópico pose é o responsável por informações sobre a localização atual do robô no plano e também será usado.
Da mesma forma é possível obter os parâmetros da sua mensagem que referem-se ao posicionamento x e y no plano e também a orientação theta. As velocidades angular e linear serão as componentes linear.x e angular.z da mensagem Twist.
O primeiro passo da aplicação aqui desenvolvida é fazer um movimento em linha reta. No git clonado o código encontra-se com nome de turtlesim_cleaner.py e é desenvolvido em python.
Utilizando a ferramenta turtlesim, vamos assumir que o movimento ocorre de um ponto inicial (x0,y0) até um ponto final (x,y)
########################################################
#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.msg import Pose
import math
import time
from std_srvs.srv import Empty
def move(velocity_publisher, speed, distance, is_forward):
#declare a Twist message to send velocity commands
velocity_message = Twist()
global x, y
x0 = x
y0 = y
if (is_forward):
velocity_message.linear.x =abs(speed)
else:
velocity_message.linear.x =-abs(speed)
distance_moved = 0.0
loop_rate = rospy.Rate(10) # we publish the velocity at 10 Hz (10 times a second)
while True :
rospy.loginfo(“Turtlesim moves forwards”)
velocity_publisher.publish(velocity_message)
loop_rate.sleep()
t1 = rospy.Time.now().to_sec()
#rospy.Duration(1.0)
distance_moved = abs(0.5 * math.sqrt(((x-x0) ** 2) + ((y-y0) ** 2)))
print distance_moved
if not (distance_moved<distance):
rospy.loginfo(“reached”)
break
#finally, stop the robot when the distance is moved
velocity_message.linear.x =0
velocity_publisher.publish(velocity_message)
def poseCallback(pose_message):
global x
global y, yaw
x= pose_message.x
y= pose_message.y
yaw = pose_message.theta
if __name__ == ‘__main__’:
try:
rospy.init_node(‘turtlesim_motion_pose’, anonymous=True)
#declare velocity publisher
cmd_vel_topic=’/turtle1/cmd_vel’
velocity_publisher = rospy.Publisher(cmd_vel_topic, Twist, queue_size=10)
position_topic = “/turtle1/pose”
pose_subscriber = rospy.Subscriber(position_topic, Pose, poseCallback)
time.sleep(2)
move(velocity_publisher, 1.0, 4.0, True)
except rospy.ROSInterruptException:
rospy.loginfo(“node terminated.”)
##############################################################################
A função move() tem como parâmetros velocity_publisher que será usada para publicar cada nova mensagem twist, a velocidade, a distância do movimento e a direção.
O laço if(is_forward) define o sentido do movimento no eixo horizontal, se negativo o movimento é realizado no sentido contrário.
velocity_message = Twist() define a variável com todos os parâmetros da mensagem.
No loop while True a todo momento é publicado a velocity_message que são as coordenadas x e y para o movimento, distance_moved é o quanto o robô se moveu e quando este valor ultrapassa o valor distance dado como parâmetro o movimento é interrompido. Em seguida a posição 0 é passada para que o robô pare.
A função poseCallback será usada como subscriber sendo chamada a cada nova mensagem recebida armazenando nas variáveis o estado atual do robô.
Nas linhas finais está a chamada da função move com os parâmetros desejados, nesse caso o robô se moverá com velocidade 1 m/s por uma distância de 4 metros no sentido positivo.
Com a aplicação turtlesim aberta execute em outro terminal o comando do seu código, nesse caso
$ rosrun ros_essentials_cpp turtlesim_cleaner.py
Será possível observar na janela do turtlesim o movimento.
Vamos agora definir a função de rotação da tartaruga, essa função está contida no mesmo código anterior basta acrescentá-la nos locais adequados.
#######################################################################
def rotate (velocity_pusblisher, angular_speed_degree, relative_angle_degree, clockwise):
velocity_message = Twist()
angular_speed=math.radians(abs(angular_speed_degree))
if (clockwise):
velocity_message.angular.z =-abs(angular_speed)
else:
velocity_message.angular.z =abs(angular_speed)
loop_rate = rospy.Rate(10) # we publish the velocity at 10 Hz (10 times a second)
t0 = rospy.Time.now().to_sec()
while True :
rospy.loginfo(“Turtlesim rotates”)
velocity_publisher.publish(velocity_message)
t1 = rospy.Time.now().to_sec()
current_angle_degree = (t1-t0)*angular_speed_degree
loop_rate.sleep()
if (current_angle_degree>relative_angle_degree):
rospy.loginfo(“reached”)
break
#finally, stop the robot when the distance is moved
velocity_message.angular.z =0
velocity_publisher.publish(velocity_message)
##########################################################################
Do mesmo modo que a função move(), a função rotate() recebe parâmetros de velocidade, posição e sentido.
Os ângulos são recebidos em graus então usamos uma conversão para radianos e o salvamos numa variável para melhor manipulação.
Aqui o ângulo de rotação é calculado a partir do tempo, diferente de move() que usava a distância absoluta.
Por fim a função rotate() pode ser aplicada no final do código juntamente com a função move, basta adicionar por exemplo a linha rotate(velocity_publisher, 30, 90, True) após move, com isso após realizar o movimento linear definido ele irá girar no sentido horário a 30 graus/segundo durante 90 graus.
Você pode agora adicionar quantos comandos move() e rotate() forem necessários para o movimento desejado, existem outras funções de movimentos tal como go_to_goal() e spiralClean() contidas no código original que podem ser estudadas mas aqui não serão abordadas.
