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，你应该发布具有该值以上的频率命令消息后，移动不同频率的尝试。

希望有帮助！