1、机械臂控制

官网：http://docs.ros.org/en/melodic/api/moveit_tutorials/html/index.html

单目MoveIt：~/software/transbot_library/src/transbot_config_camera

Astra_MoveIt：~/software/transbot_library/src/transbot_config_astra

机械臂控制功能包：~/software/transbot_library/src/transbot_description

玩此功能前需将大程序和所有打开的功能关闭，MoveIT建议在虚拟机运行。（电脑须有独立显卡！！！）

1.1、启动

本节课主要是将真机与MoveIT仿真实现同步操作。以树莓派、单目相机配置为例，Astra配置类似。建议将虚拟机设置为主机，MoveIT启动会比较慢，在虚拟机里启动后，再将树莓派设置为从机。

虚拟机端（主机）

roslaunch transbot_config_camera demo.launch  # 单目相机  
roslaunch transbot_config_astra demo.launch   # astra

树莓派端（从机）

xxxxxxxxxx
rosrun transbot_description 03_machine_move.py 

MoveIt仿真环境启动比较慢，耐心等待，观察终端，出现如下图所示错误，解决方法如图所示，如果终端报错且运动规划库未加载，点击左下角【Reset】，重新加载即可。起初是在加载的过程中，不要着急点击【Reset】，如果还没加载完就点击，系统就会重新加载，这样子就启动不了啦。

如下图所示，终端出现【Replanning：yes】，且Planning Library下方出现绿色【OMPL】,则启动成功。

示例图片

待启动成功后，如上图所示，滑动【joint_state_publisher_gui】的滑动条，机械臂和云台舵机会做出相应的动作，操作过程中，不可滑动太快，以免出现事故。

【camera_joint1】：云台舵机左右旋转，左正右负。

【camera_joint2】：云台舵机上下旋转，上正下负。

【arm_joint1】：靠近车身的关节，数据越小越往下。

【arm_joint2】：机械臂第二关节，数据越小越往下。

【Jaws_joint】：夹爪，数据越小越张开。

【Randomize】：随机产生动作，在没有安全防护措施下，不可点击，很危险！！！

【Center】：一键归中，刚打开时，默认处于归中位置，此时点击，看不出什么反应。

1.2、源码解析

demo.launch

xxxxxxxxxx
  <arg name="use_gui" default="true" />
  <arg name="use_rviz" default="true" />
  <!-- If needed, broadcast static tf for robot root -->
  <!-- We do not have a robot connected, so publish fake joint states -->
  <node name="joint_state_publisher" pkg="joint_state_publisher" type="joint_state_publisher" unless="$(arg use_gui)">
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
  </node>
  <node name="joint_state_publisher" pkg="joint_state_publisher_gui" type="joint_state_publisher_gui" if="$(arg use_gui)">
    <rosparam param="source_list">[move_group/fake_controller_joint_states]</rosparam>
  </node>
  <!-- Given the published joint states, publish tf for the robot links -->
  <node name="robot_state_publisher" pkg="robot_state_publisher" type="robot_state_publisher" respawn="true" output="screen" />
... ...
  <!-- Run Rviz and load the default config to see the state of the move_group node -->
  <include file="$(find transbot_config_camera)/launch/moveit_rviz.launch" if="$(arg use_rviz)">
    <arg name="rviz_config" value="$(find transbot_config_camera)/rviz/transbot_config_camera.rviz"/>
    <arg name="debug" value="$(arg debug)"/>
  </include>

启动时，GUI控制器和rviz的设置。

py文件：03_machine_move.py

订阅MoveIt发布出来的【/joint_states】话题，获取MoveIt中机器人的当前位置。

xxxxxxxxxx
subscriber = rospy.Subscriber("/joint_states", JointState, JointTopic)

回调函数【JointTopic】

xxxxxxxxxx
def JointTopic(msg):
    # 如果不是该话题的数据直接返回
    if not isinstance(msg, JointState): return
    if len(msg.position) == 10:
        joints1 = (msg.position[0] * RA2DE) + 90
        joints2 = -(msg.position[1] * RA2DE) + 90
        joints3 = (msg.position[2] * RA2DE) + 225
        joints4 = (msg.position[3] * RA2DE) + 45
        joints5 = (msg.position[4] * RA2DE) * 5 / 3 + 180
        bot.set_pwm_servo(1, int(joints1))
        if int(joints2) > 140: joints2 = 140
        bot.set_pwm_servo(2, int(joints2))
        bot.set_uart_servo_angle_array(joints3, joints4, joints5)
    elif len(msg.position) == 9:
        joints1 = (msg.position1 * RA2DE) + 90
        joints2 = (msg.position2 * RA2DE) + 225
        joints3 = (msg.position3 * RA2DE) + 45
        joints4 = (msg.position4 * RA2DE) * 5 / 3 + 180
        bot.set_pwm_servo(1, int(joints1))
        bot.set_uart_servo_angle_array(joints2, joints3, joints4)

以单目相机为例，打印【msg.name】、【msg.position】消息如下：

xxxxxxxxxx
msg.name:  ['camera_joint1', 'camera_joint2', 'arm_joint1', 'arm_joint2', 'Jaws_joint', 
             'right_joint2', 'right_joint3', 'left_joint1', 'left_joint2', 'left_joint3']
msg.position:  (0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0)

【msg.name】的内容与【joint_state_publisher_gui】相对应，['camera_joint1', 'camera_joint2', 'arm_joint1', 'arm_joint2', 'Jaws_joint']是我们所需要关注的。