ROS - управление роботом с помощью контроллера PS3 ч.3

Последняя часть, в которой, наконец, запустим управление импортированным роботом в RViz с помощью PS3 контроллера. Подготовительные шаги для этого можно посмотреть в предыдущих частях: подключение PS3 контроллера, ч.1, ч.2.

 

Пару слов о специфике работы с роботом через RViz и топик /joint_states - координаты звеньев пересчитываются по каждому новому сообщению. Это означает, что если робот должен оставаться на месте, то необходимо постоянно отправлять в топик его текущие координаты. А для того, чтобы он двигался - координаты в сообщении должны, соответственно, изменяться.

При этом, сообщения от контроллера в топике /joy обновляются, только если происходит какое-либо действие: нажимается / отпускается кнопка, изменяется положение осей. Если управлять роботом с помощью осей, то это приводит к тому, что смещение оси в крайнее положение (координата 1 или -1) не приводит к реакции робота: он двигается только пока ось идет до своего края, когда она его достигает, сообщения в топике /joy перестают приходить.

Чтобы это исправить необходимо сделать узел, постоянно передающий текущие координаты от контроллера: если они обновляются, то он должен их просто прокидывать, если они не обновляются - то повторять последние. Для этого потребуется новый вид сообщений, в которым эти координаты будут передаваться. 

Для данного примера я использую пакет urdf_test. Файл joy_data.msg с описанием сообщения будет находится в папке urdf_test/msg:

Header header
float32 j1
float32 j2
float32 j3

В нем передаются только три координаты осей, которые будут использоваться для управления поворотом звеньев.

Как обеспечить передачу сообщений описывать не буду - в ROS есть подробное руководство, а в конце поста будет ссылка на исходный код пакета urdf_test.

Если все сделано правильно, то команда

$ rosmsg show joy_data

должна выдать информацию о сообщении:

[urdf_test/joy_data]:
std_msgs/Header header
  uint32 seq
  time stamp
  string frame_id
float32 j1
float32 j2
float32 j3

Следующий шаг - узел, принимающий данные от контроллера и отправляющий сообщения joy_data - Joy_talker.py, он будет находится в папке urdf_test/python:

#!/usr/bin/env python

import rospy

from std_msgs.msg import Header

from sensor_msgs.msg import Joy

from urdf_test.msg import joy_data

joint_1 = 0.

joint_2 = 0.

joint_3 = 0.

# called when joy message is received

def joy_callback(data):

global joint_1

joint_1 = data.axes[3]

global joint_2

joint_2 = data.axes[1]

global joint_3

joint_3 = data.axes[4]

def talker():

# publishing to topic "joy_axes_coords"

pub = rospy.Publisher('joy_axes_coords',joy_data, queue_size=10)

rospy.init_node('Joy_talker')

rate = rospy.Rate(10) #10 Hz

msg_str = joy_data()

msg_str.header = Header()

while not rospy.is_shutdown():

msg_str.header.stamp = rospy.Time.now()

msg_str.j1 = joint_1

msg_str.j2 = joint_2

msg_str.j3 = joint_3

pub.publish(msg_str)

rate.sleep()

# subscribe to joystick messages on topic "joy"

rospy.Subscriber("joy", Joy, joy_callback, queue_size=1)

if __name__ == '__main__':

try:

talker()

except rospy.ROSInterruptException:

pass 

Узел принимает в цикле сообщения от контроллера в топике /joy для осей 1, 3, 4 и через глобальные переменные joint_1 ... 3 передает их в топик /joy_axes_coords сообщениями joy_data.

Чтобы узел заработал, его необходимо прописать в файле CMakeLists.txt:

catkin_install_python(PROGRAMS python/PS3_test.py python/Joy_talker.py

DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}

)

и выполнить команду 

$ catkin_make

в рабочем пространстве, в котором находится пакет.

Теперь необходимо перенастроить узел PS3_test.py, чтобы он принимал сообщения joy_data:

#!/usr/bin/env python

# script recieves data from /joy topic

import rospy

from sensor_msgs.msg import JointState

from std_msgs.msg import Header

from urdf_test.msg import joy_data

joint_1 = 0.

joint_2 = 0.

joint_3 = 0.

# called when joy message is received

def joy_callback(msg):

divider = 0.001

global joint_1

joint_1 += divider * msg.j1

global joint_2

joint_2 += divider * msg.j2

global joint_3

joint_3 += divider * msg.j3

def talker():

# publishing to topic "joint_states" to control robot joints

pub = rospy.Publisher('joint_states', JointState, queue_size=10)

rospy.init_node('PS3_test')

rate = rospy.Rate(10) #10 Hz

msg_str = JointState()

msg_str.header = Header()

msg_str.name = ['joint_1', 'joint_2', 'joint_3']

msg_str.velocity = []

msg_str.effort = []

while not rospy.is_shutdown():

msg_str.header.stamp = rospy.Time.now()

msg_str.position = [joint_1, joint_2, joint_3]

pub.publish(msg_str)

rate.sleep()

# subscribe to joystick messages on topic "joy"

rospy.Subscriber('joy_axes_coords', joy_data, joy_callback, queue_size=1)

if __name__ == '__main__':

try:

talker()

except rospy.ROSInterruptException:

pass

 Последнее, что остается сделать, внести изменения в файл запуска ps3_test.launch:

<launch>

<arg name="model" default="$(find urdf_test)/urdf/robot_test.urdf"/>

<arg name="gui" default="true" />

<arg name="rvizconfig" default="$(find urdf_test)/rviz/urdf.rviz" />

<param name="robot_description" command="cat $(find urdf_test)/urdf/robot_test.urdf" />

<param name="use_gui" value="$(arg gui)"/>

<node name="PS3_test" pkg="urdf_test" type="PS3_test.py" />

<node name="Joy_talker" pkg="urdf_test" type="Joy_talker.py" />

<node name="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

<node name="rviz" pkg="rviz" type="rviz" args="-d $(arg rvizconfig)" required="true" />

<node name="joy" pkg="joy" type="joy_node"/>

</launch>

 Теперь можно его запускать и наслаждаться результатом:

$  roslaunch urdf_test ps3_test.launch

Rqt_graph показывает цепочку узлов от команд контроллера /joy до Rviz:

 

Полный пакет urdf_test можно скачать с моего GitHub.

Let`s go design!