本文是想在第一次实现后重新规范一下代码，然后整体重新实现并记录一下过程，完整代码见github https://github.com/USE-jx/Robomaster-uav-competition。
先将任务简化为轨迹生成和轨迹跟踪，不考虑障碍环在一定范围内移动的问题，根据大致的圆环中心生成minimum jerk轨迹，轨迹跟踪采用Geometric controller。（写这篇时已经识别出圆心了，就差轨迹重规划一下了，但是还是想先把之前记录一下，修改一下代码整体结构）。
分析一波
整体分成轨迹规划和轨迹跟踪两个模块。
规划模块输入几个圆心waypoints，生成一条轨迹，发布轨迹上每一时刻的状态给控制模块作为期望状态，odom在发布状态，停止以及以后的重规划会用到，尤其时间戳很有用。所以规划模块订阅里程计消息，发布期望状态消息。
控制模块接收规划模块发来的期望状态和里程计的真实状态，发布控制器输出命令给飞控。所以控制模块订阅里程计消息和期望状态消息，发布控制命令消息。
综上，需要的模块之间通信的消息类型有里程计消息，期望状态消息和控制命令消息。
创建工作空间
第一步肯定是创建一个工作空间了，然后才是创建各种功能包。
mkdir -p uav_ws/src
cd uav_ws
catkin_make
cd src //在src中创建各种功能包
自定义消息
里程计消息直接用ros官方消息，所以自定义一个角速率推力消息和一个期望状态消息，期望状态根据控制器需要什么状态来定义，几何控制需要位置速度加速度和偏航,其他控制器需要什么再加上就好。
//创建功能包和依赖
catkin_create_pkg uav_msgs std_msgs geometry_msgs roscpp
cd uav_msgs/
mkdir msg
cd msg
touch AngleRateThrottle.msg
touch DesiredStates.msg
DesiredStates.msg中填入下面语句
std_msgs/Header header
geometry_msgs/Vector3 position
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 acceleration
float64 yaw
AngleRateThrottle.msg中填入下面语句
float64 rollRate
float64 pitchRate
float64 yawRate
float64 throttle
在package.xml中填入下面两行
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
在CmakeLists中做如下修改
//添加message_generation
find_package(catkin REQUIRED COMPONENTS
...
message_generation
)
//添加message_runtime
catkin_package(
...
CATKIN_DEPENDS message_runtime ...
...)
//添加新建的msg
add_message_files(
FILES
AngleRateThrottle.msg
DesiredStates.msg
)
//解注释生成消息函数
generate_messages(
DEPENDENCIES
geometry_msgs
std_msgs
)
接下来回到工作空间编译，会在devel文件夹中生成对应的头文件，source后即可查看消息结构
后面需要用到消息时引入头文件即可使用。
由于轨迹跟踪控制器需要轨迹才能调参，所以先完成规划模块，再完成控制模块。
本次整体框架采用库的思想，把轨迹生成和几何控制器写成一个库，然后轨迹发布节点和控制命令发布节点调用那两个库得到轨迹和控制命令。一个库中可以实现几种算法，如果需要验证多种规划和控制算法只需用节点调用库中的不同算法即可。参考fast_planner。
MinimumJerk轨迹生成
新建一个功能包trajectory_generator，依赖roscpp, rospy, nav_msgs, std_msgs,随便写几个依赖后便可以再加。
catkin_create_pkg trajectory_generator roscpp rospy std_msgs nav_msgs
在include和src文件夹中分别新建mini_jerk_traj.h和mini_jerk_traj.cpp,代码如下
#ifndef MINI_JERK_TRAJ_H
#define MINI_JERK_TRAJ_H
#include <iostream>
#include <Eigen/Eigen>
#include <vector>
#include <cmath>
using namespace std;
class TrajectoryGeneratorWaypoints
{
private:
public:
TrajectoryGeneratorWaypoints();
~TrajectoryGeneratorWaypoints();
Eigen::MatrixXd miniJerkTrajGenerate(
const Eigen::MatrixXd &waypoints, //3d waypoints coordinates
const Eigen::MatrixXd &vel, //boundary velocity
const Eigen::MatrixXd &acc, //boundary acceleration
const Eigen::VectorXd &time //time allocation in each segment
);
//get position,velocity,acceleration in kth segment at t
Eigen::Vector3d getPosition(Eigen::MatrixXd coeff_matrix, int k, double t);
Eigen::Vector3d getVelocity(Eigen::MatrixXd coeff_matrix, int k, double t);
Eigen::Vector3d getAcceleration(Eigen::MatrixXd coeff_matrix, int k, double t);
};
#endif
#include "trajectory_generator/mini_jerk_traj.h"
TrajectoryGeneratorWaypoints::TrajectoryGeneratorWaypoints() {}
TrajectoryGeneratorWaypoints::~TrajectoryGeneratorWaypoints() {}
Eigen::MatrixXd TrajectoryGeneratorWaypoints::miniJerkTrajGenerate(
const Eigen::MatrixXd &waypoints, //3d waypoints coordinates nx3
const Eigen::MatrixXd &vel, //boundary velocity 2x3
const Eigen::MatrixXd &acc, //boundary acceleration 2x3
const Eigen::VectorXd &time ) { //time allocation in each segment
//固定求jerk可以不写，用qp求不确定是jerk和snap时候可以写
//int p_order = 2 * d_order -1; //the order of polynomial (5)
//int p_num1d = p_order + 1; //the number of coeff in each segment (c0 ~ c5)
int segments = time.size(); //the number of segments
// M * coeffMatrix = b 就能解出系数矩阵
Eigen::MatrixXd coeffMatrix;
Eigen::MatrixXd M = Eigen::MatrixXd::Zero(6 * segments, 6 * segments);
Eigen::MatrixXd b = Eigen::MatrixXd::Zero(6 * segments, 3);
/**
* 构造M和b矩阵
* 1 起始pva 和 末尾pva约束
* 2 中间点p v a jerk snap 连续约束，即第一段T = 第二段0
* 这两个约束正好填满M矩阵
*/
//中间点连续需要用到T到T^5
Eigen::VectorXd T1 = time;
Eigen::VectorXd T2 = T1.cwiseProduct(T1);
Eigen::VectorXd T3 = T2.cwiseProduct(T1);
Eigen::VectorXd T4 = T3.cwiseProduct(T1);
Eigen::VectorXd T5 = T4.cwiseProduct(T1);
//initial pva
M(0, 0) = 1;
M(1, 1) = 1;
M(2, 2) = 2;
b.row(0) = waypoints.row(0);
b.row(1) = vel.row(0);
b.row(2) = acc.row(0);
//p v a jerk snap 连续
for (int i = 0; i < segments-1; ++i) {
//由前一段的时间得到下一点的开始位置
M(6 * i + 3, 6 * i) = 1;
M(6 * i + 3, 6 * i + 1) = T1(i);
M(6 * i + 3, 6 * i + 2) = T2(i);
M(6 * i + 3, 6 * i + 3) = T3(i);
M(6 * i + 3, 6 * i + 4) = T4(i);
M(6 * i + 3, 6 * i + 5) = T5(i);
b.row(6 * i + 3) = waypoints.row(i + 1);
//位置连续，前一段T时刻与后一段0时刻
M(6 * i + 4, 6 * i) = 1;
M(6 * i + 4, 6 * i + 1) = T1(i);
M(6 * i + 4, 6 * i + 2) = T2(i);
M(6 * i + 4, 6 * i + 3) = T3(i);
M(6 * i + 4, 6 * i + 4) = T4(i);
M(6 * i + 4, 6 * i + 5) = T5(i);
M(6 * i + 4, 6 * i + 6) = -1;
//速度连续
M(6 * i + 5, 6 * i + 1) = 1;
M(6 * i + 5, 6 * i + 2) = 2 * T1(i);
M(6 * i + 5, 6 * i + 3) = 3 * T2(i);
M(6 * i + 5, 6 * i + 4) = 4 * T3(i);
M(6 * i + 5, 6 * i + 5) = 5 * T4(i);
M(6 * i + 5, 6 * i + 7) = -1;
//加速度连续
M(6 * i + 6, 6 * i + 2) = 2;
M(6 * i + 6, 6 * i + 3) = 6 * T1(i);
M(6 * i + 6, 6 * i + 4) = 12 * T2(i);
M(6 * i + 6, 6 * i + 5) = 20 * T3(i);
M(6 * i + 6, 6 * i + 8) = -2;
//jerk连续
M(6 * i + 7, 6 * i + 3) = 6;
M(6 * i + 7, 6 * i + 4) = 24 * T1(i);
M(6 * i + 7, 6 * i + 5) = 60 * T2(i);
M(6 * i + 7, 6 * i + 9) = -6;
//snap连续
M(6 * i + 8, 6 * i + 4) = 24;
M(6 * i + 8, 6 * i + 5) = 120 * T1(i);
M(6 * i + 8, 6 * i + 10) = -24;
}
//末端pva
M(6 * segments - 3, 6 * segments - 6) = 1;
M(6 * segments - 3, 6 * segments - 5) = T1(segments-1);
M(6 * segments - 3, 6 * segments - 4) = T2(segments-1);
M(6 * segments - 3, 6 * segments - 3) = T3(segments-1);
M(6 * segments - 3, 6 * segments - 2) = T4(segments-1);
M(6 * segments - 3, 6 * segments - 1) = T5(segments-1);
M(6 * segments - 2, 6 * segments - 5) = 1;
M(6 * segments - 2, 6 * segments - 4) = 2 * T1(segments-1);
M(6 * segments - 2, 6 * segments - 3) = 3 * T2(segments-1);
M(6 * segments - 2, 6 * segments - 2) = 4 * T3(segments-1);
M(6 * segments - 2, 6 * segments - 1) = 5 * T4(segments-1);
M(6 * segments - 1, 6 * segments - 4) = 2 ;
M(6 * segments - 1, 6 * segments - 3) = 6 * T1(segments-1);
M(6 * segments - 1, 6 * segments - 2) = 12 * T2(segments-1);
M(6 * segments - 1, 6 * segments - 1) = 20 * T3(segments-1);
b.row(6 * segments - 3) = waypoints.row(segments);
b.row(6 * segments - 2) = vel.row(1);
b.row(6 * segments - 1) = acc.row(1);
coeffMatrix = M.fullPivLu().solve(b);
//transfrom to segments x (6*3) size 每行是一段x,y,z的系数
Eigen::MatrixXd coeffMatrix_t(segments, 6*3);
for (int i = 0; i < segments; ++i) {
coeffMatrix_t.block(i, 0, 1, 6) = coeffMatrix.col(0).segment(6*i, 6).transpose();
coeffMatrix_t.block(i, 6, 1, 6) = coeffMatrix.col(1).segment(6*i, 6).transpose();
coeffMatrix_t.block(i, 12, 1, 6) = coeffMatrix.col(2).segment(6*i, 6).transpose();
}
return coeffMatrix_t;
}
//get position,velocity,acceleration in kth segment at t
//p = c0 + c1*t + c2*t^2 + c3*t^3 + c4*t^4 + c5*t^5
Eigen::Vector3d TrajectoryGeneratorWaypoints::getPosition(Eigen::MatrixXd coeff_matrix, int k, double t) {
Eigen::Vector3d position(0.0, 0.0, 0.0);
for (int dim = 0; dim < 3; ++dim) {
double tn = 1.0;
for (int i = 0; i < 6; ++i) {
position(dim) += coeff_matrix.row(k).segment(dim*6,6)(i) * tn;
tn *= t;
}//这样写不太好，可能写成矩阵运算更快
}
return position;
}
//v = c1 + 2*c2*t + 3c3*t^2 + 4c4*t^3 + 5c5*t^4
Eigen::Vector3d TrajectoryGeneratorWaypoints::getVelocity(Eigen::MatrixXd coeff_matrix, int k, double t) {
Eigen::Vector3d velocity(0.0, 0.0, 0.0);
for (int dim = 0; dim < 3; ++dim) {
double tn = 1.0;
for (int i = 1; i < 6; ++i) {
velocity(dim) += coeff_matrix.row(k).segment(dim*6, 6)(i) * i * tn;
tn *= t;
}
}
return velocity;
}
//a = 2c2 + 6c3*t + 12c4*t^2 + 20c5*t^3
Eigen::Vector3d TrajectoryGeneratorWaypoints::getAcceleration(Eigen::MatrixXd coeff_matrix, int k, double t) {
Eigen::Vector3d acceleration(0.0, 0.0, 0.0);
for (int dim = 0; dim < 3; ++dim) {
double tn = 1.0;
for (int i = 2; i < 6; ++i) {
acceleration(dim) += coeff_matrix.row(k).segment(dim*6, 6)(i) * i * (i - 1) * tn;
tn *= t;
}
}
return acceleration;
}
CmakeLists.txt内容如下：
cmake_minimum_required(VERSION 3.0.2)
project(trajectory_generator)
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
nav_msgs
roscpp
rospy
std_msgs
)
catkin_package(
INCLUDE_DIRS include
#LIBRARIES trajectory_generator
CATKIN_DEPENDS nav_msgs roscpp rospy std_msgs
#DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_library(trajectory_generator
src/mini_jerk_traj.cpp
)
target_link_libraries(trajectory_generator
${catkin_LIBRARIES}
)
Geometric Controller
新建一个功能包tracking_controller，依赖roscpp, rospy, std_msgs,随便写几个依赖后便可以再加。
catkin_create_pkg tracking_controller roscpp rospy std_msgs
在include和src文件夹中分别新建geometric_controller.h和geometric_controller.cpp,代码如下:
#ifndef GEOMETRIC_CONTROLLER_H
#define GEOMETRIC_CONTROLLER_H
#include <Eigen/Eigen>
class GeometricController {
private:
Eigen::Vector3d odom_pos_, odom_vel_;
Eigen::Quaterniond odom_orient_;
Eigen::Vector3d desired_pos_, desired_vel_, desired_acc_;
double desired_yaw_;
const double gravity_ = 9.8;
public:
//parameters sever get params
Eigen::Vector3d position_gain_;
Eigen::Vector3d velocity_gain_;
Eigen::Vector3d attitude_gain_;
double vehicle_mass_;
public:
GeometricController();
~GeometricController();
//类内默认内联函数
void setOdometry(const Eigen::Vector3d &odom_pos,
const Eigen::Vector3d &odom_vel,
const Eigen::Quaterniond &odom_orient) {
odom_pos_ = odom_pos;
odom_vel_ = odom_vel;
odom_orient_ = odom_orient;
}
void setDesiredStates(const Eigen::Vector3d &desired_pos,
const Eigen::Vector3d &desired_vel,
const Eigen::Vector3d &desired_acc,
double desired_yaw ) {
desired_pos_ = desired_pos;
desired_vel_ = desired_vel;
desired_acc_ = desired_acc;
desired_yaw_ = desired_yaw;
}
void computeDesiredAcc(Eigen::Vector3d &new_acc,
Eigen::Vector3d odom_pos,
Eigen::Vector3d odom_vel,
Eigen::Quaterniond odom_orient,
Eigen::Vector3d desired_pos,
Eigen::Vector3d desired_vel,
Eigen::Vector3d desired_acc
);
void computeAttitudeError(const Eigen::Vector3d new_acc,
Eigen::Quaterniond odom_orient,
double yaw,
Eigen::Vector3d & attitudeError_v
);
void computeControlCmd(double &thrust, Eigen::Vector3d &angularVel);
};
#endif
#include "tracking_controller/geometric_controller.h"
#include <iostream>
GeometricController::GeometricController() {}
GeometricController::~GeometricController() {}
//angularRate and Throttle command
void GeometricController::computeControlCmd(double &thrust, Eigen::Vector3d &angularVel) {
Eigen::Vector3d new_acc;
computeDesiredAcc(new_acc, odom_pos_, odom_vel_, odom_orient_, desired_pos_, desired_vel_, desired_acc_);
Eigen::Vector3d attitudeError_v;
computeAttitudeError(new_acc, odom_orient_, desired_yaw_, attitudeError_v);
thrust = -vehicle_mass_ * new_acc.dot(odom_orient_.toRotationMatrix().col(2));
angularVel = -attitude_gain_.cwiseProduct(attitudeError_v);
}
void GeometricController::computeDesiredAcc(Eigen::Vector3d &new_acc,
Eigen::Vector3d odom_pos,
Eigen::Vector3d odom_vel,
Eigen::Quaterniond odom_orient,
Eigen::Vector3d desired_pos,
Eigen::Vector3d desired_vel,
Eigen::Vector3d desired_acc) {
//std::cout << "current position:" << odom_pos << std::endl;
Eigen::Vector3d position_error = odom_pos - desired_pos;
//std::cout << "position error:" << position_error.transpose() << std::endl;
Eigen::Vector3d velocity_error = odom_vel - desired_vel;
//std::cout << "velocity error:" << velocity_error.transpose() << std::endl;
Eigen::Vector3d e3(0, 0, 1);
Eigen::Vector3d e_3(Eigen::Vector3d::UnitZ());
//std::cout << "e_3:" << e_3.transpose() << std::endl;
new_acc = (-position_error.cwiseProduct(position_gain_) - velocity_error.cwiseProduct(velocity_gain_))
/ vehicle_mass_ - gravity_ * e3 + desired_acc;
//std::cout << "desirede acc:" << new_acc.transpose() << std::endl;
}
void GeometricController::computeAttitudeError(const Eigen::Vector3d new_acc,
Eigen::Quaterniond odom_orient,
double yaw,
Eigen::Vector3d & attitudeError_v) {
//get current attitude by matrix
Eigen::Matrix3d currentAttitude = odom_orient.toRotationMatrix();
//get desired attitude
Eigen::Vector3d desired_b3 = -new_acc / new_acc.norm();
Eigen::Vector3d desired_b1_p (cos(yaw), sin(yaw), 0);
Eigen::Vector3d desired_b2 = desired_b3.cross(desired_b1_p);
desired_b2.normalized();
Eigen::Vector3d desired_b1 = desired_b2.cross(desired_b3);
Eigen::Matrix3d attitude_matrix;
attitude_matrix.col(0) = desired_b1;
attitude_matrix.col(1) = desired_b2;
attitude_matrix.col(2) = desired_b3;
Eigen::Vector3d yaw_pitch_roll = attitude_matrix.eulerAngles(0,1,2);
//ned frame
Eigen::Vector3d desiredAttitude;
desiredAttitude(0) = yaw_pitch_roll(0);
desiredAttitude(1) = yaw_pitch_roll(1);
desiredAttitude(2) = yaw_pitch_roll(2);
std::cout << "desiredYaw:" << desiredAttitude(2) << std::endl;
//get attitude error and from matrix to vector
Eigen::Matrix3d attitudeError_m = 0.5 * (attitude_matrix.transpose() * currentAttitude - currentAttitude.transpose() * attitude_matrix);
attitudeError_v << attitudeError_m(2, 1), attitudeError_m(0, 2), attitudeError_m(1, 0);
std::cout << "attitudeError:" << attitudeError_v.transpose() <<std::endl;
}
CmakeLists.txt内容如下：
cmake_minimum_required(VERSION 3.0.2)
project(tracking_controller)
add_compile_options(-std=c++11)
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
std_msgs
)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES tracking_controller
CATKIN_DEPENDS roscpp rospy std_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
## Declare a C++ library
add_library(tracking_controller
src/geometric_controller.cpp
)
## Specify libraries to link a library or executable target against
target_link_libraries(tracking_controller
${catkin_LIBRARIES}
)
创建规划和控制节点
首先新建一个功能包plan_and_control，依赖roscpp rospy std_msgs geometry_msgs nav_msgs visualization_msgs uav_msgs，命令如下：
catkin_create_pkg plan_and_control roscpp rospy std_msgs geometry_msgs nav_msgs visualization_msgs uav_msgs
创建轨迹发布节点
轨迹发布节点输入几个waypoints，订阅里程计话题，发布期望状态消息。waypoints比较多，放在参数服务器里，在构造函数中获取，这样修改waypoints时也不用重新编译。
在功能包plan_and_control中的include和src中分别创建trajectory_publisher_node.h和trajectory_publisher_node.cpp,代码如下：
#ifndef TRAJECTORY_PUBLISHER_NODE_H
#define TRAJECTORY_PUBLISHER_NODE_H
//ros and related msg
#include <ros/ros.h>
#include <uav_msgs/DesiredStates.h>
#include <visualization_msgs/Marker.h>
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/PoseStamped.h>
//other utils
#include <vector>
#include <memory>
#include <Eigen/Eigen>
#include "trajectory_generator/mini_jerk_traj.h"
using namespace std;
class TrajectoryPublisherNode
{
private:
//ros related
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher desiredStates_pub_, traj_vis_pub_, cmd_vis_pub_;
ros::Publisher desiredPose_pub_, currentPose_pub_;
ros::Subscriber odom_sub_;
ros::ServiceClient takeoff_client_;
uav_msgs::DesiredStates cmd_;
nav_msgs::OdometryConstPtr odom_;
geometry_msgs::PoseStamped desiredPose_, currentPose_;
//parameters for planner
Eigen::Vector3d odom_pos_, odom_vel_;
Eigen::Quaterniond odom_orient_;
Eigen::Vector3d desiredPos_;
Eigen::Vector3d dir_;
Eigen::VectorXd times_;
Eigen::MatrixXd coeffMatrix_;
Eigen::MatrixXd waypoints_;
int waypoint_num_;
double max_vel_, max_acc_, max_jerk_;
ros::Time startTime_ ;
ros::Time finalTime_ ;
int segment_num_;
double trajDuration_;
double startYaw_, finalYaw_;
shared_ptr<TrajectoryGeneratorWaypoints> trajPlanWaypoints_ = make_shared<TrajectoryGeneratorWaypoints>();
public:
TrajectoryPublisherNode(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private);
~TrajectoryPublisherNode();
void odomCallback(const nav_msgs::OdometryConstPtr &msg);
Eigen::VectorXd timeAllocation(const Eigen::MatrixXd &waypoints);
void trajectoryGenerate(const Eigen::MatrixXd &waypoints);
void desiredStatesPub();
void displayTrajWithColor();
void drawCmd(const Eigen::Vector3d& pos, const Eigen::Vector3d& vec, const int& id,
const Eigen::Vector4d& color);
};
#endif
#include "plan_and_control/trajectory_publisher_node.h"
TrajectoryPublisherNode::TrajectoryPublisherNode(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
:nh_(nh), nh_private_(nh_private) {
odom_sub_ =
nh_.subscribe<nav_msgs::Odometry>("/airsim_node/drone_1/odom_local_ned",1, &TrajectoryPublisherNode::odomCallback,this);
desiredStates_pub_ =
nh_.advertise<uav_msgs::DesiredStates>("/reference/desiredStates", 50);
traj_vis_pub_ = nh_.advertise<visualization_msgs::Marker>("/trajectory_vis", 10);
cmd_vis_pub_ = nh_.advertise<visualization_msgs::Marker>("/cmd_vis",10);
desiredPose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/desiredPose", 10);
currentPose_pub_ = nh_.advertise<geometry_msgs::PoseStamped>("/currentPose", 10);
//cout << "waypoint init failed!" << endl;
//get params from sever
nh_private_.getParam("max_vel", max_vel_);
nh_private_.getParam("max_acc", max_acc_);
nh_private_.getParam("waypoint_num", waypoint_num_);
waypoints_.resize(waypoint_num_, 3);
cout << "waypoint number:" << waypoint_num_ << endl;
for (int i = 0; i < waypoint_num_; ++i) {
nh_private_.getParam("waypoint" + to_string(i) + "/x", waypoints_(i,0));
nh_private_.getParam("waypoint" + to_string(i) + "/y", waypoints_(i,1));
nh_private_.getParam("waypoint" + to_string(i) + "/z", waypoints_(i,2));
}
cout << "waypoints:" << waypoints_ << endl;
trajectoryGenerate(waypoints_);
}
TrajectoryPublisherNode::~TrajectoryPublisherNode() {}
void TrajectoryPublisherNode::trajectoryGenerate(const Eigen::MatrixXd &waypoints) {
//start final vel and acc
Eigen::MatrixXd vel = Eigen::MatrixXd::Zero(2, 3);
// vel(1,0) = 1.3;
// vel(1,1) = -4.67;
// vel(1,2) = 0;
// vel(1,0) = max_vel_ * cos(-75.0 / 180.0 * M_PI);
// vel(1,1) = max_vel_ * sin(-75.0 / 180.0 * M_PI);
// vel(1,2) = 0;
Eigen::MatrixXd acc = Eigen::MatrixXd::Zero(2, 3);
cout << "compute time !" << endl;
times_ = timeAllocation(waypoints);
cout << "time vector:" << times_.transpose() << endl;
coeffMatrix_ = trajPlanWaypoints_->miniJerkTrajGenerate(waypoints, vel, acc, times_);
cout << "trajectory generate finish!" << endl;
cout << "coeff Matrix:" << coeffMatrix_ << endl;
finalTime_ = startTime_ = ros::Time::now();
segment_num_ = times_.size();
for (int i = 0; i < segment_num_; ++i) {
finalTime_ += ros::Duration(times_(i));
}
}
/**
* compute current pos,vel,acc,yaw to controller
*/
void TrajectoryPublisherNode::desiredStatesPub() {
cmd_.header.frame_id = odom_->header.frame_id;
cmd_.header.stamp = odom_->header.stamp;
trajDuration_ = (finalTime_ - startTime_).toSec();
//ROS_WARN("timesum:%f:",trajDuration_);
//cout << "time sum:" << trajDuration_ << endl;
double t = max(0.0, (odom_->header.stamp - startTime_).toSec());
//cout <<odom_->header.stamp << startTime_ << endl;
//cout << "current:" << t << endl;
if (t > trajDuration_) {
cmd_.position.x = odom_->pose.pose.position.x;
cmd_.position.y = odom_->pose.pose.position.y;
cmd_.position.z = odom_->pose.pose.position.z;
cmd_.velocity.x = 0;
cmd_.velocity.y = 0;
cmd_.velocity.z = 0;
cmd_.acceleration.x = 0;
cmd_.acceleration.y = 0;
cmd_.acceleration.z = 0;
} else {
for (int i = 0; i < segment_num_; ++i) {
if (t > times_(i) ) {
t -= times_(i);
} else {
Eigen::Vector3d desiredPos = trajPlanWaypoints_->getPosition(coeffMatrix_, i, t);
//cout << "desiredPos:" << desiredPos.transpose() << endl;
//cout << "odom_Pos:" << odom_pos_.transpose() << endl;
Eigen::Vector3d desiredVel = trajPlanWaypoints_->getVelocity(coeffMatrix_, i, t);
//cout << "desiredVel:" << desiredVel.transpose() << endl;
//cout << "odom_vel:" << odom_vel_.transpose() << endl;
Eigen::Vector3d desiredAcc = trajPlanWaypoints_->getAcceleration(coeffMatrix_, i, t);
desiredPos_ = desiredPos;
cmd_.position.x = desiredPos.x();
cmd_.position.y = desiredPos.y();
cmd_.position.z = desiredPos.z();
cmd_.velocity.x = desiredVel.x();
cmd_.velocity.y = desiredVel.y();
cmd_.velocity.z = desiredVel.z();
cmd_.acceleration.x = desiredAcc.x();
cmd_.acceleration.y = desiredAcc.y();
cmd_.acceleration.z = desiredAcc.z();
cmd_.yaw = atan2(cmd_.velocity.y, cmd_.velocity.x);
//cmd_.yaw = 0.0;
//cout << "desiredyaw:" << cmd_.yaw << endl;
Eigen::Matrix3d currentAttitude = odom_orient_.toRotationMatrix();
Eigen::Vector3d RPY = currentAttitude.eulerAngles(0,1,2);
//cout << "currentyaw:" << RPY(0) << endl;
desiredPose_.header.frame_id = "map";
desiredPose_.header.stamp = odom_->header.stamp;
desiredPose_.pose.position.x = desiredPos.x();
desiredPose_.pose.position.y = desiredPos.y();
desiredPose_.pose.position.z = desiredPos.z();
break;
}
}
}
desiredStates_pub_.publish(cmd_);
desiredPose_pub_.publish(desiredPose_);
dir_ << cos(cmd_.yaw), sin(cmd_.yaw), 0;
drawCmd(desiredPos_, 2 * dir_, 1, Eigen::Vector4d(1, 1, 0, 0.7));
}
void TrajectoryPublisherNode::odomCallback(const nav_msgs::OdometryConstPtr &msg) {
odom_ = msg;
odom_pos_(0) = msg->pose.pose.position.x;
odom_pos_(1) = msg->pose.pose.position.y;
odom_pos_(2) = msg->pose.pose.position.z;
odom_vel_(0) = msg->twist.twist.linear.x;
odom_vel_(1) = msg->twist.twist.linear.y;
odom_vel_(2) = msg->twist.twist.linear.z;
odom_orient_.w() = msg->pose.pose.orientation.w;
odom_orient_.x() = msg->pose.pose.orientation.x;
odom_orient_.y() = msg->pose.pose.orientation.y;
odom_orient_.z() = msg->pose.pose.orientation.z;
currentPose_.header.frame_id = "map";
currentPose_.header.stamp = msg->header.stamp;
currentPose_.pose.position = msg->pose.pose.position;
currentPose_.pose.orientation = msg->pose.pose.orientation;
currentPose_pub_.publish(currentPose_);
desiredStatesPub();
displayTrajWithColor();
}
Eigen::VectorXd TrajectoryPublisherNode::timeAllocation(const Eigen::MatrixXd &waypoints) {
Eigen::VectorXd time(waypoints.rows() - 1);
for (int i = 0; i < waypoints.rows() - 1; ++i) {
double distance = (waypoints.row(i+1) - waypoints.row(i)).norm();
double t = max_vel_ / max_acc_;
double d = 0.5 * max_acc_ * t * t;
if (distance < 2 * d) {
time(i) = 2 * sqrt(distance / max_acc_);
} else {
time(i) = 2 * t + (distance - 2 * d) / max_vel_;
}
}
return time;
}
void TrajectoryPublisherNode::displayTrajWithColor() {
visualization_msgs::Marker points, line_strip;
points.header.frame_id = line_strip.header.frame_id = "map";
points.header.stamp = line_strip.header.stamp = ros::Time::now();
points.ns = line_strip.ns = "traj";
points.id = 0;
line_strip.id = 1;
points.action = line_strip.action = visualization_msgs::Marker::ADD;
points.pose.orientation.w =line_strip.pose.orientation.w = 1.0;
points.type = visualization_msgs::Marker::POINTS;
line_strip.type = visualization_msgs::Marker::LINE_STRIP;
points.scale.x = 0.5;
points.scale.y = 0.5;
line_strip.scale.x = 0.3;
points.color.a = 1.0;
points.color.g = 1.0;
line_strip.color.a = 1.0;
line_strip.color.b = 1.0;
line_strip.color.g = 1.0;
geometry_msgs::Point pt;
Eigen::Vector3d pos;
for (int i = 0; i < segment_num_; ++i) {
for (double t = 0; t < times_(i); t += 0.01) {
pos = trajPlanWaypoints_->getPosition(coeffMatrix_, i, t);
pt.x = pos(0);
pt.y = pos(1);
pt.z = pos(2);
line_strip.points.push_back(pt);
}
}
for (int i = 0; i < waypoint_num_; ++i) {
pt.x = waypoints_(i,0);
pt.y = waypoints_(i,1);
pt.z = waypoints_(i,2);
points.points.push_back(pt);
}
traj_vis_pub_.publish(points);
traj_vis_pub_.publish(line_strip);
ros::Duration(0.001).sleep();
}
void TrajectoryPublisherNode::drawCmd(const Eigen::Vector3d& pos, const Eigen::Vector3d& vec, const int& id,
const Eigen::Vector4d& color) {
visualization_msgs::Marker mk_state;
mk_state.header.frame_id = "map";
mk_state.header.stamp = ros::Time::now();
mk_state.id = id;
mk_state.type = visualization_msgs::Marker::ARROW;
mk_state.action = visualization_msgs::Marker::ADD;
mk_state.pose.orientation.w = 1.0;
mk_state.scale.x = 0.1;
mk_state.scale.y = 0.2;
mk_state.scale.z = 0.3;
geometry_msgs::Point pt;
pt.x = pos(0);
pt.y = pos(1);
pt.z = pos(2);
mk_state.points.push_back(pt);
pt.x = pos(0) + vec(0);
pt.y = pos(1) + vec(1);
pt.z = pos(2) + vec(2);
mk_state.points.push_back(pt);
mk_state.color.r = color(0);
mk_state.color.g = color(1);
mk_state.color.b = color(2);
mk_state.color.a = color(3);
cmd_vis_pub_.publish(mk_state);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "trajectory_publisher_node");
ros::NodeHandle nh;
ros::NodeHandle nh_p("~");
TrajectoryPublisherNode trajectoryPublisherNode(nh, nh_p);
ros::spin();
return 0;
}
创建轨迹跟踪节点
轨迹跟踪节点订阅轨迹发布节点以100Hz发布的期望状态，订阅里程计话题，发布三轴角速率和油门的控制命令。
在功能包plan_and_control中的include和src中分别创建trajectory_tracking_node.h和trajectory_tracking_node.cpp,代码如下：
#ifndef TRAJECTORY_TRACKING_NODE_H
#define TRAJECTORY_TRACKING_NODE_H
#include <ros/ros.h>
#include <uav_msgs/AngleRateThrottle.h>
#include <uav_msgs/DesiredStates.h>
#include <nav_msgs/Odometry.h>
#include <Eigen/Dense>
#include "tracking_controller/geometric_controller.h"
using namespace std;
class GeometricControllerNode {
private:
//ros related
ros::NodeHandle nh_;
ros::NodeHandle nh_private_;
ros::Publisher control_cmd_pub_;
ros::Subscriber odom_sub_, desired_states_sub_;
ros::Timer cmd_pub_timer_;
//parameters for controller
Eigen::Vector3d odom_pos_, odom_vel_;
Eigen::Quaterniond odom_orient_;
Eigen::Vector3d desired_pos_, desired_vel_, desired_acc_;
Eigen::Quaterniond desired_orient_;
double desired_yaw_;
GeometricController geometricController_;
bool states_cmd_updated_, states_cmd_init_;
public:
GeometricControllerNode(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private);
~GeometricControllerNode();
void odomCallback(const nav_msgs::OdometryConstPtr &msg);
void desiredStatesCallback(const uav_msgs::DesiredStatesConstPtr &msg);
void pubRrPrYrTCmd();
};
#endif
#include "plan_and_control/trajectory_tracking_node.h"
GeometricControllerNode::GeometricControllerNode(const ros::NodeHandle &nh, const ros::NodeHandle &nh_private)
: nh_(nh), nh_private_(nh_private), states_cmd_updated_(false),states_cmd_init_(false) {
control_cmd_pub_ =
nh_.advertise<uav_msgs::AngleRateThrottle>("/airsim_node/drone_1/angle_rate_throttle_frame",1);
odom_sub_ =
nh_.subscribe<nav_msgs::Odometry>("/airsim_node/drone_1/odom_local_ned",1, &GeometricControllerNode::odomCallback,this);
desired_states_sub_ =
nh_.subscribe<uav_msgs::DesiredStates>("/reference/desiredStates", 10, &GeometricControllerNode::desiredStatesCallback, this);
//get parameters from parameters sever
nh_private_.getParam("position_gain/x", geometricController_.position_gain_.x());
nh_private_.getParam("position_gain/y", geometricController_.position_gain_.y());
nh_private_.getParam("position_gain/z", geometricController_.position_gain_.z());
nh_private_.getParam("velocity_gain/x", geometricController_.velocity_gain_.x());
nh_private_.getParam("velocity_gain/y", geometricController_.velocity_gain_.y());
nh_private_.getParam("velocity_gain/z", geometricController_.velocity_gain_.z());
nh_private_.getParam("attitude_gain/x", geometricController_.attitude_gain_.x());
nh_private_.getParam("attitude_gain/y", geometricController_.attitude_gain_.y());
nh_private_.getParam("attitude_gain/z", geometricController_.attitude_gain_.z());
nh_private_.getParam("vehicle_mass", geometricController_.vehicle_mass_);
}
GeometricControllerNode::~GeometricControllerNode() { }
void GeometricControllerNode::odomCallback(const nav_msgs::OdometryConstPtr &msg) {
odom_pos_ = Eigen::Vector3d(msg->pose.pose.position.x,
msg->pose.pose.position.y,
msg->pose.pose.position.z);
odom_vel_ = Eigen::Vector3d(msg->twist.twist.linear.x,
msg->twist.twist.linear.y,
msg->twist.twist.linear.z);
odom_orient_.w() = msg->pose.pose.orientation.w;
odom_orient_.x() = msg->pose.pose.orientation.x;
odom_orient_.y() = msg->pose.pose.orientation.y;
odom_orient_.z() = msg->pose.pose.orientation.z;
// Eigen::Matrix3d odom_matrix (odom_orient_);
// Eigen::Vector3d currentYPR = odom_matrix.eulerAngles(2,1,0);
geometricController_.setOdometry(odom_pos_, odom_vel_, odom_orient_);
//当期望状态发送停止后也一直发送命令使飞机维持在最后的期望状态
if (states_cmd_init_) {
if (!states_cmd_init_) {
//pubRPYTCmd();
pubRrPrYrTCmd();
}
states_cmd_updated_ = false;
}
}
void GeometricControllerNode::desiredStatesCallback(const uav_msgs::DesiredStatesConstPtr &msg) {
desired_pos_ = Eigen::Vector3d(msg->position.x, msg->position.y, msg->position.z);
desired_vel_ = Eigen::Vector3d(msg->velocity.x, msg->velocity.y, msg->velocity.z);
desired_acc_ = Eigen::Vector3d(msg->acceleration.x, msg->acceleration.y, msg->acceleration.z);
desired_yaw_ = msg->yaw;
geometricController_.setDesiredStates(desired_pos_, desired_vel_,desired_acc_,desired_yaw_);
pubRrPrYrTCmd();
states_cmd_init_ = true;
states_cmd_updated_ = true;
}
void GeometricControllerNode::pubRrPrYrTCmd() {
uav_msgs::AngleRateThrottle cmd;
Eigen::Vector3d angular_vel;
double thrust;
geometricController_.computeControlCmd(thrust, angular_vel);
cmd.rollRate = angular_vel(0);
cmd.pitchRate = angular_vel(1);
cmd.yawRate = angular_vel(2);
cmd.throttle = thrust / 10;
control_cmd_pub_.publish(cmd);
}
int main(int argc, char **argv)
{
ros::init(argc, argv, "geometric_controller_node");
ros::NodeHandle nh;
ros::NodeHandle nh_p("~");
GeometricControllerNode geometricControllerNode(nh, nh_p);
ros::spin();
return 0;
}
新建rviz文件夹
在功能包中新建rviz文件夹用于保存rviz的设置，然后可以用launch启动，要不每次自己add，自己run rviz很费劲。
这个第一次需要自己运行，add完想可视化的消息后保存到rviz文件夹即可。rviz保存完是下面这样的，开始我还天真的以为是人自己写的呢，
Panels:
- Class: rviz/Displays
Help Height: 138
Name: Displays
Property Tree Widget:
Expanded:
- /Global Options1
- /Status1
- /Marker1
- /Marker2
- /Pose1
- /Pose2
Splitter Ratio: 0.5
Tree Height: 924
- Class: rviz/Selection
Name: Selection
- Class: rviz/Tool Properties
Expanded:
- /2D Pose Estimate1
- /2D Nav Goal1
- /Publish Point1
Name: Tool Properties
Splitter Ratio: 0.5886790156364441
- Class: rviz/Views
Expanded:
- /Current View1
Name: Views
Splitter Ratio: 0.5
- Class: rviz/Time
Name: Time
SyncMode: 0
SyncSource: ""
Preferences:
PromptSaveOnExit: true
Toolbars:
toolButtonStyle: 2
Visualization Manager:
Class: ""
Displays:
- Alpha: 0.5
Cell Size: 1
Class: rviz/Grid
Color: 160; 160; 164
Enabled: true
Line Style:
Line Width: 0.029999999329447746
Value: Lines
Name: Grid
Normal Cell Count: 0
Offset:
X: 0
Y: 0
Z: 0
Plane: XY
Plane Cell Count: 10
Reference Frame: <Fixed Frame>
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /trajectory_vis
Name: Marker
Namespaces:
traj: true
Queue Size: 100
Value: true
- Class: rviz/Marker
Enabled: true
Marker Topic: /cmd_vis
Name: Marker
Namespaces:
"": true
Queue Size: 100
Value: true
- Alpha: 1
Axes Length: 1
Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 255; 25; 0
Enabled: true
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: Pose
Queue Size: 10
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Axes
Topic: /desiredPose
Unreliable: false
Value: true
- Alpha: 1
Axes Length: 0.5
Axes Radius: 0.10000000149011612
Class: rviz/Pose
Color: 255; 25; 0
Enabled: true
Head Length: 0.30000001192092896
Head Radius: 0.10000000149011612
Name: Pose
Queue Size: 10
Shaft Length: 1
Shaft Radius: 0.05000000074505806
Shape: Axes
Topic: /currentPose
Unreliable: false
Value: true
Enabled: true
Global Options:
Background Color: 48; 48; 48
Default Light: true
Fixed Frame: map
Frame Rate: 30
Name: root
Tools:
- Class: rviz/Interact
Hide Inactive Objects: true
- Class: rviz/MoveCamera
- Class: rviz/Select
- Class: rviz/FocusCamera
- Class: rviz/Measure
- Class: rviz/SetInitialPose
Theta std deviation: 0.2617993950843811
Topic: /initialpose
X std deviation: 0.5
Y std deviation: 0.5
- Class: rviz/SetGoal
Topic: /move_base_simple/goal
- Class: rviz/PublishPoint
Single click: true
Topic: /clicked_point
Value: true
Views:
Current:
Class: rviz/Orbit
Distance: 35.1982536315918
Enable Stereo Rendering:
Stereo Eye Separation: 0.05999999865889549
Stereo Focal Distance: 1
Swap Stereo Eyes: false
Value: false
Field of View: 0.7853981852531433
Focal Point:
X: 0
Y: 0
Z: 0
Focal Shape Fixed Size: true
Focal Shape Size: 0.05000000074505806
Invert Z Axis: false
Name: Current View
Near Clip Distance: 0.009999999776482582
Pitch: -0.14960156381130219
Target Frame: <Fixed Frame>
Yaw: 1.3203967809677124
Saved: ~
Window Geometry:
Displays:
collapsed: false
Height: 1434
Hide Left Dock: false
Hide Right Dock: false
QMainWindow State: 000000ff00000000fd0000000400000000000001f700000494fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b000000b000fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000b0fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000006e000004940000018200fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000015f00000494fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000006e000004940000013200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000077e0000005efc0100000002fb0000000800540069006d006501000000000000077e000006dc00fffffffb0000000800540069006d00650100000000000004500000000000000000000004100000049400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
Selection:
collapsed: false
Time:
collapsed: false
Tool Properties:
collapsed: false
Views:
collapsed: false
Width: 1918
X: 60
Y: 30
新建config和launch文件夹
在功能包中新建launch文件夹用于保存launch文件，luanch文件一起启动两个节点和rviz。因为轨迹生成的waypoints和控制gain数量太大直接写在launch文件中不合适，可以用放在yaml文件中，在launch文件中find一下即可，这个yaml文件放在config文件夹中。两个yaml和一个launch内容如下：
waypoint_num: 8
waypoint0: {x: 0.0, y: 0.0, z: -0.02}
waypoint1: {x: 6.87, y: -0.79 , z: -2.74} #14.87
waypoint2: {x: 37.68, y: -12.27, z: -1.03}
waypoint3: {x: 67.52, y: -12.94, z: -0.3}
waypoint4: {x: 94.3, y: -8.1, z: 0.02}
#waypoint5: {x: 104.44, y: -15.05, z: -0.02}
waypoint5: {x: 113.95, y: -35.70, z: -0.34}
waypoint6: {x: 121.6, y: -67.73, z: -3.83}
waypoint7: {x: 121.80, y: -96.1, z: -7.39}
max_vel: 3
max_acc: 1
position_gain: {x: 6, y: 6, z: 6}
velocity_gain: {x: 3.8, y: 3.8, z: 3.8}
attitude_gain: {x: 4, y: 4, z: 1.2}
vehicle_mass: 0.606
<launch>
<node pkg="plan_and_control" type="trajectory_tracking_node" name="geometric_controller_node" output="screen">
<rosparam command="load" file="$(find plan_and_control)/config/controller_gains.yaml" />
</node>
<node pkg="plan_and_control" type="trajectory_publisher_node" name="trajectory_publisher_node" output="screen" >
<!--waypoints for trajectory generating-->
<rosparam command="load" file="$(find plan_and_control)/config/waypoints.yaml" />
</node>
<node pkg="rviz" type="rviz" name="rviz" output="screen"
args="-d $(find plan_and_control)/rviz/traj.rviz" >
</node>
</launch>
CmakeLists.txt
cmake_minimum_required(VERSION 3.0.2)
project(plan_and_control)
add_compile_options(-std=c++11)
find_package(Eigen3 REQUIRED)
find_package(catkin REQUIRED COMPONENTS
geometry_msgs
nav_msgs
roscpp
rospy
std_msgs
uav_msgs
visualization_msgs
tracking_controller
trajectory_generator
)
catkin_package(
INCLUDE_DIRS include
# LIBRARIES plan_and_control
CATKIN_DEPENDS geometry_msgs nav_msgs roscpp rospy std_msgs uav_msgs visualization_msgs
# DEPENDS system_lib
)
include_directories(
include
${catkin_INCLUDE_DIRS}
)
add_executable(trajectory_publisher_node src/trajectory_publisher_node.cpp)
target_link_libraries(trajectory_publisher_node
${catkin_LIBRARIES}
trajectory_generator
)
add_executable(trajectory_tracking_node src/trajectory_tracking_node.cpp)
target_link_libraries(trajectory_tracking_node
${catkin_LIBRARIES}
tracking_controller
)
运行
catkin_make
source devel/setup.bash
roslaunch plan_and_control trajectory_tracking.launch
结果如下
点击查看【bilibili】
2518
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
