moveit控制真实机械臂

一、实验环境

虚拟机：Ubuntu18
ros：melodic 1.14.12
moveit：moveit2

注意：控制器的配置文件好像用moveit2配置的功能包不需要更改，在网上看的资料都零零散散的，我也忘记我之前有没有改过了，抱歉。前面配置包是看古月居老师的教程的。

二、更改demo.launch

1.虚拟执行改成false

这一步操作告诉move_group我有真实的机械臂，不用虚拟仿真

2.更改joint_state_publisher节点订阅的话题

第一步告诉move_group不用仿真后，move_group就不会启动/move_group/fake_controller_joint_states这个话题，因此需要把joint_state_publisher仿真时订阅的/move_group/fake_controller_joint_states话题改成自己的话题（和joint_states有关）

3.编写自己的FollowJointTrajectoryAction

控制真实的机械臂需要一个控制器server接收move_group发送的轨迹数据，因此还需自己编写一个SimpleActionServer

总结：控制真实的机械臂就两个点：第一写一个和关节专题相关的话题，用于发布真实的关节信息告诉move_group当前机械臂的信息。第二点需要写一个控制器的server，用于接收move_group发送的机械臂路径规划信息，然后再把路径规划信息处理转发给机械臂执行，这样就完成了一个控制闭环，第一点完成反馈部分的内容，第二点完成执行命令的发送。

三、实操

看到这里可能很机友都迷迷糊糊的，下面进入实操环节。因为我没有真实的机械臂，所以我拿到move_group的路径规划信息后，延时0.1s后，把关节信息发送到joint_states话题上，这样模拟一个机械臂更方便展示控制真实机械臂的流程。

可以用rostopic info/echo/list 等命令查看topic的类型，然后模仿写一个自己的话题，joint_states就是用类似的方法写的。上代码：

#!/usr/bin/env python
# -*- coding: utf-8 -*-

########################################################################
####          Copyright 2020 GuYueHome (www.guyuehome.com).          ###
########################################################################

# 该例程将发布/person_info话题，自定义消息类型learning_topic::Person

import rospy
import roslib
import actionlib
import sensor_msgs.msg
import control_msgs.msg


class FollowJointTrajectoryActionServer:
    def __init__(self):
        self.FollowJointTrajectory  = control_msgs.msg.FollowJointTrajectoryAction()
        self.RecSucceeded = False
        self.server = actionlib.SimpleActionServer('/arm_controller/follow_joint_trajectory',control_msgs.msg.FollowJointTrajectoryAction,self.execute,False)
        self.server.start()
        print("FollowJointTrajectoryActionServer start succeeded\n")

    def execute(self,Trajectory ):
        # print(goal.trajectory.points)
        self.FollowJointTrajectory = Trajectory 
        self.RecSucceeded = True
        self.server.set_succeeded()


if __name__ == '__main__':
   
    try:
        # ROS节点初始化
        rospy.init_node('joint_states_publisher', anonymous=True)
        # 初始化FollowJointTrajectoryActionServer
        server = FollowJointTrajectoryActionServer()
        # 初始化joint_states话题
        joint_states_pub = rospy.Publisher('/sensor/joint_states_publisher', sensor_msgs.msg.JointState, queue_size=10)
        joint_states_msg=sensor_msgs.msg.JointState()
        joint_states_msg.header.frame_id="base_link"
        joint_states_msg.name.append('joint_1')
        joint_states_msg.name.append('joint_2')
        joint_states_msg.name.append('joint_3')
        joint_states_msg.name.append('joint_4')
        joint_states_msg.name.append('joint_5')
        joint_states_msg.name.append('joint_6')


        while True:
            if server.RecSucceeded:
                server.RecSucceeded = False
                for goal_joint in server.FollowJointTrajectory.trajectory.points :
                    t = rospy.get_rostime()
                    joint_states_msg.header.stamp.secs = t.secs
                    joint_states_msg.header.stamp.nsecs = t.nsecs
                    joint_states_msg.position = goal_joint.positions
                    joint_states_msg.velocity = goal_joint.velocities
                    joint_states_msg.effort = goal_joint.effort
                    # 发布消息
                    joint_states_pub.publish(joint_states_msg)
                    print(goal_joint)
                    rospy.sleep(0.1)

        # joint_states_publisher()
    except rospy.ROSInterruptException:
        pass

启动两个文件：
1、上面的代码作为一个节点
2、demo.launch
然后就可以开心的去查错误了，多少都会有错误的，加油！
我这里有个warn：
WARN] [1636683763.678032341]: Failed to receive current joint state
虽然不影响使用，但是还是想找到原因，有知道的求留言区告知。