人工势力场

引用他人代码：

#include "ros/ros.h"
#include <stdio.h>
#include <iostream>
#include <geometry_msgs/Twist.h> //速度
#include <sensor_msgs/LaserScan.h>
#include <math.h>//数学库,,,,因为用了cos sin
#include <algorithm>  //雷达
#include <geometry_msgs/PoseWithCovarianceStamped.h>  //amcl
#include <Eigen/Dense>

using namespace std;
using namespace Eigen;

#define PI 3.1415
#define INF 100000000
#define k 6 //yinlicahngshu
#define c 12//斥力常数
#define p 1   //阈值


//定义全局变量
double q0;
double q1;
double q2;
double q3;//q0,1,2,3为四元数
double alpha;//偏航角
double x;
double y;
double z;
double a;//
double b;//目标点的全局坐标
double aaa;//目标点转换到小车坐标系下
double bbb;//目标点转换到小车坐标系下
double attractive;//引力
double xattractive;//x轴上的引力
double yattractive;//y轴上的引力
double conpulsive;//斥力
double xconpulsive;
double yconpulsive;//
double attractangle;//引力角,,,用来控制小车转弯
double conangle;//斥力角
double xsum;
double ysum;
double angle;//合力的方向角
double newspeed;
double newturnrate;
double Fsum;
double max_speed=0.1;
double max_turning_speed_rad = 0.3;//角速度上限

geometry_msgs::Twist cmd_vel;//订阅一个对象   定义一个消息
ros::Publisher cmd_vel_pub;//  发布速度

//定义回调函数
void callback(const geometry_msgs::PoseWithCovarianceStamped::ConstPtr &msg)
{
   
	x = msg->pose.pose.position.x;
	y = msg->pose.pose.position.y;  
	z = msg->pose.pose.position.z; 
	q0=msg->pose.pose.orientation.x;
	q1=msg->pose.pose.orientation.y;
	q2=msg->pose.pose.orientation.z;
	q3=msg->pose.pose.orientation.w;
}

//定义取值的上限和下限  
double limit(double n, double min, double max)
{
   
	if(n < min)
		return min;
	else if(n > max)