Moveit телеоперация

Question

Я разработал мастер-устройство для управления роботом. Я хочу использовать moveit для планирования движения. Я хочу отправить три степеней свободы. данные о местоположении до конечного эффектора робота. Я написал узел говорящего, чтобы отправить данные, и мне нужен другой код слушателя, чтобы получить эти данные и отправить в moveit. Я прикрепил свой код здесь.

#!/usr/bin/env python
import sys
import moveit_commander
import moveit_msgs.msg
import geometry_msgs.msg
import rospy
from geometry_msgs.msg import Twist

vx = 0
vy = 0
vz = 0

def callback(data):
rospy.loginfo(data.linear)
global vx,vy,vz
vx = data.linear.x
vy = data.linear.y
vz = data.linear.z

def main():
moveit_commander.roscpp_initialize(sys.argv)
rospy.init_node('listener', anonymous=True)
rospy.Subscriber("fi", Twist, callback)
robot = moveit_commander.RobotCommander()
scene = moveit_commander.PlanningSceneInterface()
group = moveit_commander.MoveGroupCommander("manipulator")
display_trajectory_publisher = rospy.Publisher(
    '/move_group/display_planned_path',
    moveit_msgs.msg.DisplayTrajectory)
print robot.get_group_names()
print "============ Printing robot state"
print robot.get_current_state()
## Planning to a Pose goal:
print "============ Generating plan 1"
current_pose = group.get_current_pose().pose
rospy.loginfo('current_pose: {}'.format(current_pose))
pose_target = geometry_msgs.msg.Pose()
pose_target.orientation.w = current_pose.orientation.w
pose_target.orientation.x = current_pose.orientation.x
pose_target.orientation.y = current_pose.orientation.y
pose_target.orientation.z = current_pose.orientation.z
pose_target.position.x = current_pose.position.x + vx/10
pose_target.position.y = current_pose.position.y + vy/10
pose_target.position.z = current_pose.position.z + vz/10
group.set_pose_target(pose_target)
plan1 = group.plan()
group.go(wait=True)
moveit_commander.roscpp_shutdown()
print "stop"
rospy.spin()

if __name__ == '__main__':
print "Program start listerning..."
main()

Я надеюсь, что moveit сможет прочитать мои данные о времени чтения vx, vy и vz, а затем отправить их в текущую позицию. Тогда я мог добиться дистанционного управления с помощью моего устройства. Теперь результат работы:

 Program start listerning...
[ INFO] [1508329377.270614560]: Loading robot model 'abb_irb120_3_58'...
    [rospack] Error: package 'abb_irb120_support' not found
    [librospack]: error while executing command
    [ERROR] [1508329377.306347223]: Error retrieving file [package://abb_irb120_support/meshes/irb120_3_58/collision/base_link.stl]: Package [abb_irb120_support] does not exist
    [rospack] Error: package 'abb_irb120_support' not found
    [librospack]: error while executing command
    [ERROR] [1508329377.326927698]: Error retrieving file [package://abb_irb120_support/meshes/irb120_3_58/visual/base_link.stl]: Package [abb_irb120_support] does not exist
    [rospack] Error: package 'abb_irb120_support' not found
    [librospack]: error while executing command
    [ERROR] [1508329377.349068945]: Error retrieving file [package://abb_irb120_support/meshes/irb120_3_58/collision/link_1.stl]: Package [abb_irb120_support] does not exist
    [rospack] Error: package 'abb_irb120_support' not found
    [librospack]: error while executing command
...
      [ WARN] [1508329377.983231280]: No geometry is associated to any robot links
       [ WARN] [1508329377.983386064]: The root link base_link has an inertia specified in the URDF, but KDL does not support a root link with an inertia.  As a workaround, you can add an extra dummy link to your URDF.
        Traceback (most recent call last):
          File "moveit_listener3.py", line 58, in <module>
            main()
          File "moveit_listener3.py", line 27, in main
            group = moveit_commander.MoveGroupCommander("manipulator")
          File "/opt/ros/kinetic/lib/python2.7/dist-packages/moveit_commander/move_group.py", line 51, in __init__
            self._g = _moveit_move_group_interface.MoveGroupInterface(name, robot_description)
        RuntimeError: Unable to connect to move_group action server 'move_group' within allotted time (5s)
       [INFO] [1508329926.239926]: x: 0.0
        y: 0.0
        z: 0.0
        [INFO] [1508329926.242838]: x: 0.0
        y: 0.0
        z: 0.0
        [INFO] [1508329926.243337]: x: 0.0
        y: 0.0
        z: 0.0
    ...

Файл moveit_config был загружен онлайн, и файл запуска мог запустить и показать робота в Rviz. Я не знаю, почему это всегда показывает, что ошибки пакета не существует. Я новичок в Moveit и Python. Может ли кто-нибудь помочь мне проверить, что не так с кодом, это не тот результат, который я хочу. Я надеюсь, что потоковые данные x,y,z могут контролировать движение робота.

Заранее спасибо.