搭建控制框架

• 说明：该框架融合了gazebo以及网上绝大多数的控制框架，作了一定的改进，结构清晰，欢迎大家的批评指正，有问题请私聊我，或者邮箱联系我2250017028@qq.com。

1.框架图

• 蓝色线表示正常的控制流程，红色线表示测试流程，即不考虑下位机的通信
  两者仅存其一。

2.节点作用分析(只给出主要代码)

trajectory_controller

• 定位：

1. 接收上层的控制指令，将其转换为目标点信息。
2. 基于各关节位置/末端位姿进行路径规划。
3. 发布指令，启动move_group客户端，准备控制机械臂。

参数定义

#include <ros/ros.h>
#include <sensor_msgs/JointState.h>
#include <robot_name_kinematics/joint_states.h>
#include <moveit/move_group_interface/move_group_interface.h> 
     
// 路径规划组
moveit::planning_interface::MoveGroupInterface _arm;

控制函数

/**
 * @brief 控制机械臂移动到目标点
 * @param target 目标点
 * @note 目标是各个关节的最终位置
 */
void MoveToTarget(std::vector<double>& target)
{
    // 保证关节点的数量符合要求
    assert(target.size() == JOINTS_NUM);

    // 设置路径规划参数
    _arm.setGoalJointTolerance(0.001);                  // 关节允许误差 
    _arm.setMaxAccelerationScalingFactor(0.2);          // 允许的最大加速度
    _arm.setMaxVelocityScalingFactor(0.2);              // 允许的最大速度

    // 机械臂执行
    ROS_INFO("Start Moving");
    _arm.setJointValueTarget(target);
    _arm.move();
    sleep(1);
    ROS_INFO("Moving Finish");
}

/**
 * @brief 控制机械臂移动到目标点
 * @param target 目标点
 * @note 目标是末端姿态
 */
void MoveToTarget(geometry_msgs::Pose& target)
{
    // 获取终端link的名称 
    std::string end_effector_link = _arm.getEndEffectorLink();

    // 设置目标位置所使用的参考坐标系 
    std::string reference_frame = "base_link";
    _arm.setPoseReferenceFrame(reference_frame);    

    // 当运动规划失败后，允许重新规划 
    _arm.allowReplanning(true);

    // 设置路径规划的参数  
    _arm.setGoalPositionTolerance(0.001); 
    _arm.setGoalOrientationTolerance(0.01);
    _arm.setMaxAccelerationScalingFactor(0.2);
    _arm.setMaxVelocityScalingFactor(0.2);

    // 设置机器臂当前的状态作为运动初始状态
    _arm.setStartStateToCurrentState();

    // 设置目标状态 
    _arm.setPoseTarget(target);

    // 计算机器人移动到目标的运动轨迹 
    moveit::planning_interface::MoveGroupInterface::Plan plan;
    moveit::planning_interface::MoveItErrorCode success = _arm.plan(plan);
    ROS_INFO("Plan (pose goal) %s",success?"":"FAILED");   

    // 控制机械臂按既定轨迹运动
    if(success) _arm.execute(plan);
    sleep(1);
}

测试代码

/**
 * @brief 主控制函数
 * @param ctrl_Hz 控制频率
 */
void Control(void)
{
	// 开启单线程 
	ros::AsyncSpinner spinner(1);
	spinner.start();       
	
	// 控制机械臂回到起点
	_arm.setNamedTarget("home");
	_arm.move();
	sleep(1);
	ROS_INFO("Successfully init joints Position ");
	
	// A点和B点具体的位置根据机器人的实际情况而定，可达即可。
	// 成功的标志是：仿真环境中，机器人由初始位姿移动到A点，再移动到B点
	// 测试：A点
	double targetPose[7] = {0.391410, -0.676384, -0.376217, 0.0, 1.052834, 0.454125, 0.10111};
	std::vector<double> joint_group_positions(7);
	joint_group_positions[0] = targetPose[0];
	joint_group_positions[1] = targetPose[1];
	joint_group_positions[2] = targetPose[2];
	joint_group_positions[3] = targetPose[3];
	joint_group_positions[4] = targetPose[4];
	joint_group_positions[5] = targetPose[5];   
	joint_group_positions[6] = targetPose[6];   
	MoveToTarget(joint_group_positions);
	
	// 测试：B点
	geometry_msgs::Pose target_pose;
	target_pose.orientation.x = 1;
	target_pose.orientation.y = 0;
	target_pose.orientation.z = 0;
	target_pose.orientation.w = 0;
	
	target_pose.position.x = 0.2593;
	target_pose.position.y = 0.0636;
	target_pose.position.z = 0.1787;
	MoveToTarget(target_pose);
	
	// 关闭线程 
	ros::shutdown(); 
}

joints_states_controller

• 定位：

1. 接收和存储move_group传来的轨迹点
2. 发送轨迹点给下位机(定频率发送/一次性发送)
3. 接收下位机的数据，并发布给move_group

参数定义

#include <ros/ros.h>
#include <actionlib/server/simple_action_server.h>
#include <control_msgs/FollowJointTrajectoryAction.h>
#include <sensor_msgs/JointState.h>
#include <robot_name_kinematics/joint_states.h>
#include <serial/serial.h>

// 动作服务端
Action_Server _as;

// 关节状态
sensor_msgs::JointState _js;

// 轨迹点存储变量
std::vector<trajectory_msgs::JointTrajectoryPoint> _points;

轨迹点接收函数

/**
 * @brief 接收轨迹点
 * @param goal move_group计算所得的轨迹点
 */
void Revtrajectory_CB(const control_msgs::FollowJointTrajectoryGoalConstPtr& goal)
{
    // 发布成功接收信息 
   ROS_INFO("Receive New Goal");

    // 读取轨迹点
    _points = goal->trajectory.points;

    // 按照一定频率发布
    uint16_t points_len = _points.size(); uint16_t index = 0;
    ros::Rate loop_rate(20);
    while(ros::ok() && index < points_len)
    {
        // 常规流程：发送数据给下位机
        /* CODE */

        // 测试：反馈关节信息给move_group
        for (int i = 0; i < JOINTS_NUM; i++)  _js.position[i] = _points.at(index).positions[i];
        JointsStatesPublish();

        // 查找下一个轨迹点
        index ++;

        // 查看中断
        ros::spinOnce();

        // 延时
        loop_rate.sleep();
    }

    // 告诉action客户端成功信息
    control_msgs::FollowJointTrajectoryResult result;
    result.error_code = 0;
    _as.setSucceeded(result);
}

关节状态更新函数

/**
 * @brief 关节状态更新函数
 */    
void JointsStatesUpdate(void)
{
    // 读取下位机的关节数据
    /* CODE */

    // 更新关节状态,即更新_js的位置信息
    Update_Joints_States(js);

    // 发布数据
   JointsStatesPublish();

   ROS_INFO("Successfully update and publishJoints States");
}

/**
 * @brief 关节信息发布函数
 */    
void JointsStatesPublish(void){_joints_states_puber.publish(_js);}

下一章将对通信协议(下位机通信，节点间通信)进行进一步的阐述。

有兴趣的同学记得关注哦！