ROS robonaut2 управление

Question

Я делаю программу для управления robonau2 (R2).

Я получаю данные от внешнего издателя.

Я могу управлять R2 всего один раз.

'print (data.x)' - это код для проверки принимаемых данных.

Данные успешно принят, но R2 не двигается ... TT

Как я могу контролировать R2 несколько раз?

#!/usr/bin/env python 
    import sys, rospy, tf, moveit_commander, random 
    from geometry_msgs.msg import Pose, Point, Quaternion 
    from project_pkg.msg import arminfo 

    def left_callback(data): 
     orient = \ 
      Quaternion(*tf.transformations.quaternion_from_euler(data.phi, data.theta, data.psi)) # <1> 
     pose = Pose(Point(data.x, data.y, data.z), orient) 
     moveit_commander.MoveGroupCommander("left_arm").set_pose_target(pose) 
     moveit_commander.MoveGroupCommander("left_arm").go(True) 
     print(data.x) 

    def right_callback(data): 
     orient = \ 
      Quaternion(*tf.transformations.quaternion_from_euler(data.phi, data.theta, data.psi)) # <1> 
     pose = Pose(Point(data.x, data.y, data.z), orient) 
     moveit_commander.MoveGroupCommander("right_arm").set_pose_target(pose) 
     moveit_commander.MoveGroupCommander("right_arm").go(True) 
     print(data.x) 


    if __name__ == '__main__': 
     moveit_commander.roscpp_initialize(sys.argv) 
     rospy.init_node('r2_cli',anonymous=True) 
     argv = rospy.myargv(argv=sys.argv) # filter out any arguments used by ROS 
     rospy.Subscriber("mat_left",arminfo,left_callback) 
     rospy.Subscriber("mat_right",arminfo,right_callback) 
     rospy.spin() 
     moveit_commander.roscpp_shutdown() 

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

Чем отличается?

 #!/usr/bin/env python 
    import sys, rospy, tf, moveit_commander, random 
    from geometry_msgs.msg import Pose, Point, Quaternion 
    from math import pi 

orient = [Quaternion(*tf.transformations.quaternion_from_euler(pi, -pi/2, -pi/2)),Quaternion(*tf.transformations.quaternion_from_euler(pi, -pi/2, -pi/2))]  
pose = [Pose(Point(0.5,-0.5,1.3),orient[0]), Pose(Point(-0.5,-0.5,1.3),orient[1])] 
moveit_commander.roscpp_initialize(sys.argv)  
rospy.init_node('r2_wave_arm',anonymous=True) 
group = [moveit_commander.MoveGroupCommander("left_arm"), moveit_commander.MoveGroupCommander("right_arm")] 
    # now, wave arms around randomly 

while not rospy.is_shutdown():  
    pose[0].position.x = 0.5 + random.uniform(-0.1, 0.1)   
    pose[1].position.x = -0.5 + random.uniform(-0.1, 0.1)   
     for side in [0,1]:    
      pose[side].position.z = 1.5 + random.uniform(-0.1, 0.1)   
      group[side].set_pose_target(pose[side]) 
      group[side].go(True) 
moveit_commander.roscpp_shutdown() 

Answer 1

Я подозреваю, что это либо частота публикации, либо вы получаете только одну информацию.

Для первых, попробуйте:

$ rostopic echo/mat_right (or /mat_left) 

Если вы получаете сообщения и робот не двигается, чем перейти к следующему шагу (проверьте частоту).

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

$ rostopic hz /mat_right (or /mat_left) 

и попробовать с разными частотами, используя

$ rqt (message publisher) 

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

Надеюсь, что это поможет!