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，你應該發佈具有該值以上的頻率命令消息後，移動不同頻率的嘗試。

希望有幫助！