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之后开始移动，则应发布频率高于该值的命令消息。

希望有所帮助！

ROS robonaut2控制

1 个答案: