0

我编写了一个程序来控制 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()
4

1 回答 1

0

我怀疑它要么是发布频率，要么你只收到一个数据。

首先，尝试：

$ rostopic echo/mat_right (or /mat_left)

如果您正在接收消息并且机器人没有移动，请转到下一步（验证频率）。

事实上，一些机器人具有执行接收到的命令所需的一定频率。试着看看结果

$ rostopic hz /mat_right (or /mat_left)

并尝试使用不同的频率

$ rqt (message publisher)

如果它仅在 xHz 之后开始移动，则您应该以高于该值的频率发布命令消息。

希望有帮助！

于 2016-12-12T15:24:19.470 回答