fbpx

Movimento com ROS

Movimentando o robô virtual turtlesim

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.

3 de abril de 2021

Sobre nós

A Linux Force Brasil é uma empresa que ama a arte de ensinar. Nossa missão é criar talentos para a área de tecnologia e atender com excelência nossos clientes.

CNPJ: 13.299.207/0001-50
SAC:         0800 721 7901

sac@linuxforce.com.br

Comercial  Comercial = 11 3796 5900

Suporte:    11 3796-5900
contato@linuxforce.com.br

Últimos Tweets

Erro ao receber tweets

Administrador Online

Não há usuários online neste momento
Copyright © Linux Force Security  - Desde 2011.