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

Продолжаю двигаться к тому, чтобы управлять тестовым роботом с помощью PS3 контроллера. То, как можно вручную отправлять команды роботу на установку в новую позицию можно посмотреть в предыдущем посте. 

Следующий шаг - скрипт, который будет посылать сообщения в топик /joint_states, задающие углы поворота звеньев - сделал его на python.

Однократно задавать позицию роботу как-то неинтересно, поэтому сделал перебор углов для звеньев 2, 3 в диапазоне +- 1,5 рад. При этом звено 1 постоянно вращается в одном направлении. 

В итоге получился такой скрипт- PS3_test.py. В дальнейшем буду его модернизировать для работы с джойстиком.

#!/usr/bin/env python

import rospy

from sensor_msgs.msg import JointState

from std_msgs.msg import Header

def talker():

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

rospy.init_node('PS3_test')

rate = rospy.Rate(20) #10 Hz

msg_str = JointState()

msg_str.header = Header()

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

msg_str.velocity = []

msg_str.effort = []

counter_1 = 0.0

counter_2 = 0.0

dir_2 = True

counter_3 = 0.0

dir_3 = False

step = 0.01

while not rospy.is_shutdown():

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

msg_str.position = [counter_1, counter_2, counter_3]

counter_1 += step

if counter_2 >= 1.5:

dir_2 = False

elif counter_2 <= -1.5:

dir_2 = True

if dir_2:

counter_2 += step

else:

counter_2 -= step

if counter_3 >= 1.5:

dir_3 = False

elif counter_3 <= -1.5:

dir_3 = True

if dir_3:

counter_3 += step

else:

counter_3 -= step

pub.publish(msg_str)

rate.sleep()

if __name__ == '__main__':

try:

talker()

except rospy.ROSInterruptException:

pass

Теперь нужно включить его в пакет urdf_test и запустить:

1. файл скрипта PS3_test.py помещаем в папку ~/ws_moveit/src/urdf_test/python

2. назначаем ему права на исполнение:

# chmod +x PS3_test.py

3. в файл CMakeLists.txt пакета добавляем:

catkin_install_python(PROGRAMS python/PS3_test.py
  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) 

4. в рабочем пространсвте ~/ws_moveit запускаем catkin_make:

# catkin_make

5. для запуска робота со скриптом лучше создать отдельный launch файл - ps3_test.launch, который будет находится в папке 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="robot_state_publisher" pkg="robot_state_publisher" type="state_publisher" />

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

</launch>

В отличие от display.launch вместо узла joint_state_publisher теперь скрипт PS3_test. 

Для запуска отображения робота в таком случае используется команда: 

# roslaunch urdf_test ps3_test.launch

После запуска робот должен начать двигаться:

 

 

На видео, почему-то, движение не очень ровное, но в действительности все должно быть плавно.

Граф узлов в этом случае выглядит следующим образом:

Остался последний шаг: преобразовать данные с джойстика в координаты звеньев.Об этом будет последняя, третья часть.

Let's go design!