【车路协同】车路坐标一致

由于路端设备是倾斜的，存在角度问题，需要变换

【路端局部坐标系】

1倾角计算

流程

1. 获得一帧点云
2. 剪裁点云，只剩下地面
3. RANSAC平面拟合
4. 计算平面与水平面夹角

1获得一帧点云，保存为pcd

#include <ros/ros.h>
#include <sensor_msgs/PointCloud2.h>
#include <pcl_conversions/pcl_conversions.h>
#include <pcl/point_cloud.h>
#include <pcl/point_types.h>
#include <pcl/io/ply_io.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/io/io.h>
#include <time.h>

 
int flag=1;
 
void cloudCallback(const sensor_msgs::PointCloud2 ros_cloud)
{
    //ROS_INFO("cloud is going");
    pcl::PointCloud<pcl::PointXYZ> pcl_cloud_temp;
    pcl::fromROSMsg(ros_cloud, pcl_cloud_temp);
    if(flag==1)
    {
        pcl::io::savePCDFileASCII ("./road_pcd.pcd", pcl_cloud_temp); //将点云保存到PCD文件中
        flag=0;
        std::cout<<"saved one"<<endl;
    }
}
int main(int argc, char **argv)
{ 
    ros::init(argc, argv, "pcd_get");
    ros::NodeHandle n;
    ROS_INFO("begin");
    ros::Subscriber cloud_sub = n.subscribe("/converted_velodyne_points", 50, cloudCallback);
    ros::spin();
    return 0;
}

2剪裁点云，提取地面

#include <iostream>
#include <vector>

#include <boost/shared_ptr.hpp>

#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/io/pcd_io.h>
#include <pcl/visualization/pcl_visualizer.h>

#include<pcl/filters/passthrough.h>  //直通滤波器头文件
#include<pcl/filters/voxel_grid.h>  //体素滤波器头文件
#include<pcl/filters/statistical_outlier_removal.h>   //统计滤波器头文件
#include <pcl/filters/conditional_removal.h>    //条件滤波器头文件
#include <pcl/filters/radius_outlier_removal.h>   //半径滤波器头文件

using pcl::visualization::PointCloudColorHandlerGenericField;
using pcl::visualization::PointCloudColorHandlerCustom;
using namespace std;
int main(int argc, char *argv[])
{
    std::cout<<"cnm"<<endl;
	//读取
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	if (pcl::io::loadPCDFile<pcl::PointXYZ>("/home/qm/my_projects/pcl_projects/pcd/road_pcd.pcd", *cloud) == -1)
	{
		std::cerr << "open failed!" << std::endl;
		return -1;
	}




	//
	//直通滤波器对点云 x方向进行处理。*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_x_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);//

	pcl::PassThrough<pcl::PointXYZ> passthrough_x;
	passthrough_x.setInputCloud(cloud);//输入点云

	passthrough_x.setFilterFieldName("x");//对z轴进行操作
	passthrough_x.setFilterLimits(-5.0, 5.0);//设置直通滤波器操作范围
	// passthrough.setFilterLimitsNegative(true);//true表示保留范围内，false表示保留范围外
	passthrough_x.filter(*cloud_x_PassThrough);//执行滤波，过滤结果保存在 cloud_after_PassThrough



	//直通滤波器对点云 y方向进行处理。*/
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_y_PassThrough(new pcl::PointCloud<pcl::PointXYZ>);//

	pcl::PassThrough<pcl::PointXYZ> passthrough_y;
	passthrough_y.setInputCloud(cloud_x_PassThrough);//输入点云

	passthrough_y.setFilterFieldName("y");//对z轴进行操作
	passthrough_y.setFilterLimits(-100, 0.0);//设置直通滤波器操作范围
	// passthrough.setFilterLimitsNegative(true);//true表示保留范围内，false表示保留范围外
	passthrough_y.filter(*cloud_y_PassThrough);//执行滤波，过滤结果保存在 cloud_after_PassThrough

	//点保存点云
	pcl::io::savePCDFileASCII("./road_ground.pcd", *cloud_y_PassThrough);//将点云保存到PCD文件中
	std::cerr << "Saved" << cloud_y_PassThrough->points.size() << "data points to test_pcd.pcd" << std::endl;








	//显示
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(cloud_y_PassThrough, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return 0;
}

3 RANSAC 点云平面拟合

注意看看提取的点云是不是真的一个平面
调整ransac.setDistanceThreshold(0.1);//设定阈值 默认0.3
越小越精细

#include <iostream>
#include <pcl/console/parse.h>
#include <pcl/io/pcd_io.h>
#include <pcl/point_types.h>
#include <pcl/point_cloud.h>
#include <pcl/sample_consensus/ransac.h>
#include <pcl/sample_consensus/sac_model_plane.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <boost/thread/thread.hpp>
#include <Eigen/Core>

using namespace std;

int main(int argc, char** argv)
{
	pcl::PointCloud<pcl::PointXYZ>::Ptr cloud(new pcl::PointCloud<pcl::PointXYZ>);
	pcl::PointCloud<pcl::PointXYZ>::Ptr final(new pcl::PointCloud<pcl::PointXYZ>);
	
	pcl::io::loadPCDFile<pcl::PointXYZ>("/home/qm/my_projects/pcl_projects/pcd/road_ground.pcd", *cloud);
	std::cout<<"origin points num:"<<cloud->points.size()<<endl;

	vector<int> inliers;	//用于存放合群点的vector

	pcl::SampleConsensusModelPlane<pcl::PointXYZ>::Ptr model_plane(new pcl::SampleConsensusModelPlane<pcl::PointXYZ>(cloud));
	pcl::RandomSampleConsensus<pcl::PointXYZ> ransac(model_plane);//定义RANSAC算法模型

	ransac.setDistanceThreshold(0.1);//设定阈值 默认0.3
	ransac.computeModel();//拟合平面


	ransac.getInliers(inliers);//获取合群点
	double a, b, c, d, A, B, C, D;//a,b,c为拟合平面的单位法向量，A,B,C为重定向后的法向量
	Eigen::VectorXf coeff;
	ransac.getModelCoefficients(coeff);//获取拟合平面参数，对于平面ax+by+cz+d=0，coeff分别按顺序保存a,b,c,d
	cout << "coeff " << coeff[0] << " \t" << coeff[1] << "\t " << coeff[2] << "\t " << coeff[3] << endl;
	a = coeff[0], b = coeff[1], c = coeff[2], d = coeff[3];
	//---------------平面法向量定向，与（1，1，1）同向，并输出平面与原点的距离D----------------------
	if (a + b + c > 0) {
		A = a;
		B = b;
		C = c;
		D = abs(d);
	}
	else {
		A = -a;
		B = -b;
		C = -c;
		D = abs(d);
	}
	cout<<" ABCD:"<<endl;
	cout << "" << A << ",\t" << "" << B << ",\t" << "" << C << ",\t" << "" << D << ",\t" << endl;
	pcl::copyPointCloud<pcl::PointXYZ>(*cloud, inliers, *final);


	std::cout<<"final points num:"<<final->points.size()<<endl;
	// pcl::io::savePCDFileASCII("1.11.pcd", *final);



	//同时显示
	// boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("cloud show"));
	// int v1 = 0;
	// int v2 = 1;
	// viewer->createViewPort(0, 0, 0.5, 1, v1);
	// viewer->createViewPort(0.5, 0, 1, 1, v2);
	// viewer->setBackgroundColor(0, 0, 0, v1);
	// viewer->setBackgroundColor(0.05, 0, 0, v2);
	// //----------------------------------原始点云绿色------------------------------
	// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> src_h(cloud, 0, 255, 0);
	// //--------------------------------平面拟合后的点云----------------------------
	// pcl::visualization::PointCloudColorHandlerCustom<pcl::PointXYZ> after_sac(final, 0, 0, 255);
	// viewer->setBackgroundColor(0, 0, 0);
	// viewer->addPointCloud(cloud, src_h, "source cloud", v1);
	// viewer->addPointCloud(final, after_sac, "target cloud1", v2);

	// while (!viewer->wasStopped())
	// {
	// 	viewer->spinOnce(100);
	// 	boost::this_thread::sleep(boost::posix_time::microseconds(10000));
	// }


	//显示单个
	boost::shared_ptr<pcl::visualization::PCLVisualizer> viewer(new pcl::visualization::PCLVisualizer("3D Viewer"));
	viewer->setBackgroundColor(0, 0, 0);
	viewer->addPointCloud<pcl::PointXYZ>(final, "sample cloud");
	viewer->setPointCloudRenderingProperties(pcl::visualization::PCL_VISUALIZER_POINT_SIZE, 1, "sample cloud");
	viewer->addCoordinateSystem(1.0);
	viewer->initCameraParameters();

	while (!viewer->wasStopped())
	{
		viewer->spinOnce(100);
		boost::this_thread::sleep(boost::posix_time::microseconds(100000));
	}

	return 0;
}

4 计算点云平面与地面夹角

Ax+By+Cz+D=0是一般方程
法向量就是（A,B,C）

#include<iostream>
#include <cmath>
using namespace std;
int main (int argc, char *argv[])
{
    // z=0
    double x1=0;
    double y1=0;
    double z1=1;


    // -0.0264245*x+0.382563*y+0.923552*z+3.34908=0 第一次
    //-0.0206086,	0.392848,	0.919372,	3.47223,	第二次
    double  x2=-0.0206086;
    double  y2=0.392848;
    double  z2=0.919372;

    // double  x2=1;
    // double  y2=0;
    // double  z2=0;

    double dz = z2 - z1;
    double dy = y2 - y1;
    double dx = x2 - x1;
    // cmath
    // arccos acos
    // 开根号 sqrt
    double t1=x1*x2+y1*y2+z1*z2;
    double l1=sqrt(x1*x1+y1*y1+z1*z1);
    double l2=sqrt(x2*x2+y2*y2+z2*z2);

    double cos_t=t1/(l1*l2);
    double angle_hd=acos(cos_t);
    double angle_jd=angle_hd * 180 / 3.1415926;
    // double angle = atan2( abs(dz), sqrt(dx * dx + dy * dy) );
    cout<<"angle"<<angle_jd<<endl;
    return 0;
}

5结果与准确性

量角器也试了，大概就是20°左右一点点。证明OK

2 路段感知

使用yoloact+激光雷达获得目标的

• 类别
• 激光雷达坐标系下的xy坐标，三维边界框

3 坐标变换

激光雷达坐标系下的xy坐标，转换到路端水平面的坐标

示意图

x1=x0
y1=y0cos(a)    //a为倾角

【路端全局坐标系】

车路协同感知系统坐标统一问题

1 五个坐标

①小蚂蚁局部坐标系v1
②小蚂蚁全局坐标v2
③路端激光雷达坐标系r1
④路端水平坐标系r2
⑤路端全局坐标系r3

其中：
v1为车端感知，（感知的坐标数据都是在该坐标系下的）
然后v1转v2（已有），获得路端全局坐标

r1为车端感知，（感知的坐标数据都是在该坐标系下的）
然后r1转r2（见下文）
然后r2转r3（见下文，代码与上面的v1转v2一模一样）

注：
①所有坐标均为xy，不要经纬度，不要z。
②全局坐标系原点均为GPS设备定义的xy原点

2 如何获得路端全局坐标？

小蚂蚁开到路端激光雷达下方即可，无需正下方，无需垂直。即可获得粗略的路端经纬度，进而得到全局xy坐标

3 问题

假设小蚂蚁刚刚好停在路测激光雷达正下方，刚刚好朝向和激光雷达重合，那么转换后同一个交通目标在路端与车端感知结果下应该是重合的，但是不可能刚刚好停到正下方，不可能刚刚好朝向准确，因此。需要计算误差（位置误差，角度误差）

默认小蚂蚁的感知结果无误，消除路端误差

计算偏移x，偏移y，偏转角a

计算代码

已知
1 路端感知到的目标坐标（2个）
x0=[75.0,82.0]
y0=[107.0,40.0]

2 小蚂蚁感知到的目标坐标（2个）
x1=[12.432667397366075,51.99484522385715]
y1=[188.46598839415688,133.94228634059948]

求 角度和位移

0 基础：坐标变换函数（位移和旋转）

# 顺时针为正，右边，上面为正
def zb_convert(x,y,jlx=20,jly=5,hd=math.radians(30)):
    hd=-hd
    #平移 jlx jly是要平移的量
    x0=x+jlx
    y0=y+jly
    #旋转 hd是角度
    x1=math.cos(hd)*x0-math.sin(hd)*y0
    y1=math.sin(hd)*x0+math.cos(hd)*y0
    return (x1,y1)

1 计算旋转角

v1 v2是向量
结果是角度

def angle(v1, v2):
    dx1 = v1[2] - v1[0]
    dy1 = v1[3] - v1[1]
    dx2 = v2[2] - v2[0]
    dy2 = v2[3] - v2[1]
    angle1 = math.atan2(dy1, dx1)
    angle1 = int(angle1 * 180/math.pi)
    # print(angle1)
    angle2 = math.atan2(dy2, dx2)
    angle2 = int(angle2 * 180/math.pi)
    # print(angle2)
    if angle1*angle2 >= 0:
        included_angle = abs(angle1-angle2)
    else:
        included_angle = abs(angle1) + abs(angle2)
        if included_angle > 180:
            included_angle = 360 - included_angle
    return included_angle

然后将x1,y1旋转到x11,y11

2 计算位移

lx=x0[0]-x11[0]
ly=y0[0]-y11[0]

然后将x11,y11旋转到x12,y12
经过检验 x12，y12与x0，y0重合

算法正确！

4 实战步骤

全部代码

调参吧，前面都是作废！！
road_data.txt

road_angle 20.917188  激光雷达倾斜角
road_delta_x 10  向左为正    路端偏移x
road_delta_y 10  向前为正   路端偏移y
road_delta_angle 90  左转为正  路端偏移角度
road_jd 88.88     路端经度
road_wd 77.77   路端纬度

#include <iostream>
#include<sstream>
#include <ros/ros.h>
#include<ros/time.h>
#include <ros/duration.h>
#include <cmath>

#include <visualization_msgs/Marker.h>
#include <visualization_msgs/MarkerArray.h>

#include <perception_msgs/ObstacleArray.h>
#include <perception_msgs/Obstacle.h>

#include"qm_txt/qm_txt.h"
#include"noval_convert_xy/qm_jwd_to_xy.h"


using namespace std;
# define PI 3.1415926

class tilt_to_horizon_to_global
{
private:
  std::string road_tilt_perception_topic_name;
  std::string road_horizon_perception_topic_name;
  std::string road_global_perception_topic_name;

  std::string road_data_path;

  double road_angle;

  double delta_x;
  double delta_y;
  double delta_angle;

  double road_jd;
  double road_wd;

  qm_jwd_to_xy my_jwd_to_xy;

  qm_txt my_txt;

  double GPS_x;
  double GPS_y;


private:
	//创建节点句柄
	ros::NodeHandle nh;
	//声明订阅器
	ros::Subscriber sub_road_lidar;
	//声明发布器
	ros::Publisher pub_road_horizon;
  ros::Publisher pub_road_global;

  ros::Publisher pub_marker_array_horizon;
  ros::Publisher pub_marker_array_global;
  //定时器
  // ros::Timer timer1;

//每一帧的变量
public:
  visualization_msgs::MarkerArray make_one_empty_markerarray();
  visualization_msgs::Marker make_one_empty_marker_car();
  visualization_msgs::Marker make_one_empty_marker_people();
  perception_msgs::ObstacleArray make_one_empty_ObstacleArray();
  perception_msgs::Obstacle make_one_empty_Obstacle();
  void pub_horizon_markers_from_obs(perception_msgs::ObstacleArray t);
  void pub_global_markers_from_obs(perception_msgs::ObstacleArray t);

public:
	//构造函数
	tilt_to_horizon_to_global()
	{
    //launch 参数赋值
    init_para();
		//订阅
		sub_road_lidar = nh.subscribe(road_tilt_perception_topic_name, 1, &tilt_to_horizon_to_global::callback_lidar, this);
		//发布
		pub_road_horizon = nh.advertise<perception_msgs::ObstacleArray>(road_horizon_perception_topic_name,1);
    pub_road_global = nh.advertise<perception_msgs::ObstacleArray>(road_global_perception_topic_name,1);

    pub_marker_array_horizon = nh.advertise<visualization_msgs::MarkerArray>("road_horizon_markers",1);
    pub_marker_array_global = nh.advertise<visualization_msgs::MarkerArray>("road_global_markers",1);

	};


  void init_para();

  //回调函数
  void callback_lidar(perception_msgs::ObstacleArray t);
  // void o_timer_callback(const ros::TimerEvent& event);
};

void tilt_to_horizon_to_global::init_para()
{

  ros::param::get("~road_tilt_perception_topic_name", road_tilt_perception_topic_name);
  ros::param::get("~road_horizon_perception_topic_name", road_horizon_perception_topic_name);
  ros::param::get("~road_global_perception_topic_name", road_global_perception_topic_name);

  ros::param::get("~road_data_path", road_data_path);


  my_txt.read_txt(road_data_path);

  std::cout<<"*********************"<<endl;
  // cout<<"my_txt.read_result[0][1]:"<<my_txt.read_result[0][1]<<endl;
  istringstream istr_road_angle(my_txt.read_result[0][1]);
  istr_road_angle >> road_angle;  //1234.56789
  cout<<"get road_angle: "<<road_angle<<endl;
  std::cout<<"*********************"<<endl;

  std::cout<<"*********************"<<endl;
  istringstream istr_delta_x(my_txt.read_result[1][1]);
  istr_delta_x >> delta_x;  //1234.56789
  cout<<"get delta_x: "<<delta_x<<endl;
  std::cout<<"*********************"<<endl;

  std::cout<<"*********************"<<endl;
  istringstream istr_delta_y(my_txt.read_result[2][1]);
  istr_delta_y >> delta_y;  //1234.56789
  cout<<"get delta_y: "<<delta_y<<endl;
  std::cout<<"*********************"<<endl;

  std::cout<<"*********************"<<endl;
  istringstream istr_delta_angle(my_txt.read_result[3][1]);
  istr_delta_angle >> delta_angle;  //1234.56789
  cout<<"get delta_angle: "<<delta_angle<<endl;
  std::cout<<"*********************"<<endl;

  std::cout<<"*********************"<<endl;
  istringstream istr_road_jd(my_txt.read_result[4][1]);
  istr_road_jd >> road_jd;  //1234.56789
  cout<<"get road_jd: "<<road_jd<<endl;
  std::cout<<"*********************"<<endl;

  std::cout<<"*********************"<<endl;
  istringstream istr_road_wd(my_txt.read_result[5][1]);
  istr_road_wd >> road_wd;  //1234.56789
  cout<<"get road_wd: "<<road_wd<<endl;
  std::cout<<"*********************"<<endl;

  my_jwd_to_xy.jwd_to_xy(road_jd,road_wd,GPS_x,GPS_y);
  std::cout<<"*********************"<<endl;
  std::cout<<"GPS x "<<GPS_x<<endl;
  std::cout<<"GPS y "<<GPS_y<<endl;
  std::cout<<"*********************"<<endl;

}
/空msg函数
visualization_msgs::MarkerArray tilt_to_horizon_to_global::make_one_empty_markerarray()
{
  visualization_msgs::MarkerArray t;
  return t;
}
visualization_msgs::Marker tilt_to_horizon_to_global::make_one_empty_marker_car()
{
  visualization_msgs::Marker t;
  t.header.frame_id = "laser_link";
  t.header.stamp = ros::Time::now();
  // Set the namespace and id for this marker.  This serves to create a unique ID
  // Any marker sent with the same namespace and id will overwrite the old one
  t.ns = "car";
  t.id = 0;
  // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
  t.type = visualization_msgs::Marker::CUBE;
  // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
  t.action = visualization_msgs::Marker::ADD;
  // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
  t.pose.position.x = 0;
  t.pose.position.y = 0;
  t.pose.position.z = 0;
  t.pose.orientation.x = 0.0;
  t.pose.orientation.y = 0.0;
  t.pose.orientation.z = 0.0;
  t.pose.orientation.w = 0.0;
  // Set the scale of the marker -- 1x1x1 here means 1m on a side
  t.scale.x = 0.0;
  t.scale.y = 0.0;
  t.scale.z = 0.0;
  // Set the color -- be sure to set alpha to something non-zero!
  t.color.r = 0.0f;
  t.color.g = 1.0f;
  t.color.b = 0.0f;
  t.color.a = 1.0;
  t.lifetime = ros::Duration(1);
  return t;
}
visualization_msgs::Marker tilt_to_horizon_to_global::make_one_empty_marker_people()
{
  visualization_msgs::Marker t;
  t.header.frame_id = "laser_link";
  t.header.stamp = ros::Time::now();
  // Set the namespace and id for this marker.  This serves to create a unique ID
  // Any marker sent with the same namespace and id will overwrite the old one
  t.ns = "people";
  t.id = 0;
  // Set the marker type.  Initially this is CUBE, and cycles between that and SPHERE, ARROW, and CYLINDER
  t.type = visualization_msgs::Marker::CYLINDER;
  // Set the marker action.  Options are ADD, DELETE, and new in ROS Indigo: 3 (DELETEALL)
  t.action = visualization_msgs::Marker::ADD;
  // Set the pose of the marker.  This is a full 6DOF pose relative to the frame/time specified in the header
  t.pose.position.x = 0;
  t.pose.position.y = 0;
  t.pose.position.z = 0;
  t.pose.orientation.x = 0.0;
  t.pose.orientation.y = 0.0;
  t.pose.orientation.z = 0.0;
  t.pose.orientation.w = 0.0;
  // Set the scale of the marker -- 1x1x1 here means 1m on a side
  t.scale.x = 0.0;
  t.scale.y = 0.0;
  t.scale.z = 0.0;
  // Set the color -- be sure to set alpha to something non-zero!
  t.color.r = 0.0f;
  t.color.g = 1.0f;
  t.color.b = 0.0f;
  t.color.a = 1.0;
  t.lifetime = ros::Duration(1);
  return t;
}
perception_msgs::ObstacleArray tilt_to_horizon_to_global::make_one_empty_ObstacleArray()
{
  perception_msgs::ObstacleArray t;
  return t;
}
perception_msgs::Obstacle tilt_to_horizon_to_global::make_one_empty_Obstacle()
{
  perception_msgs::Obstacle t;
  t.header.frame_id = "fake";
  t.header.stamp = ros::Time::now();
  t.ns="obstacle";
  t.id=0;

  t.label="obstacle";

  // 设置标记位姿
  t.pose.position.x = 0;
  t.pose.position.y = 0;
  t.pose.position.z = 0;
  t.pose.orientation.x = 0;
  t.pose.orientation.y = 0;
  t.pose.orientation.z = 0;
  t.pose.orientation.w = 0;

  // 设置标记尺寸
  t.scale.x = 0;
  t.scale.y = 0;
  t.scale.z = 0;

  t.v_validity=0;
  t.vx=0;
  t.vy=0;
  t.vz=0;

  t.a_validity=0;
  t.ax=0;
  t.ay=0;
  t.az=0;
  return t;
}
void tilt_to_horizon_to_global::pub_horizon_markers_from_obs(perception_msgs::ObstacleArray obs_array)
{
  visualization_msgs::MarkerArray tmp_marker_array;
  tmp_marker_array=make_one_empty_markerarray();
  for(int i=0;i<obs_array.obstacles.size();i++)
  {
    visualization_msgs::Marker tmp_marker;
    if(obs_array.obstacles[i].ns=="person")
    {
      tmp_marker=make_one_empty_marker_people();
      tmp_marker.color.r = 255/255;
      tmp_marker.color.g = 0/255;
      tmp_marker.color.b = 0/255;
    }
    else
    {
      tmp_marker=make_one_empty_marker_car();
      tmp_marker.color.r = 0/255;
      tmp_marker.color.g = 255/255;
      tmp_marker.color.b = 127/255;
    }
    //写入obs信息
    tmp_marker.id=i;
    tmp_marker.pose.position.x=obs_array.obstacles[i].pose.position.x;
    tmp_marker.pose.position.y=obs_array.obstacles[i].pose.position.y;
    tmp_marker.scale.x=obs_array.obstacles[i].scale.x;
    tmp_marker.scale.y=obs_array.obstacles[i].scale.y;
    tmp_marker.scale.z=obs_array.obstacles[i].scale.z;
    tmp_marker.pose.orientation.z = obs_array.obstacles[i].pose.orientation.z;
    tmp_marker.pose.orientation.w = obs_array.obstacles[i].pose.orientation.w;
    //
    tmp_marker_array.markers.push_back(tmp_marker);
  }
  pub_marker_array_horizon.publish(tmp_marker_array);
}
void tilt_to_horizon_to_global::pub_global_markers_from_obs(perception_msgs::ObstacleArray obs_array)
{
  visualization_msgs::MarkerArray tmp_marker_array;
  tmp_marker_array=make_one_empty_markerarray();
  //减掉gps_x，否则看不到
  for(int i=0;i<obs_array.obstacles.size();i++)
  {
    visualization_msgs::Marker tmp_marker;
    if(obs_array.obstacles[i].ns=="person")
    {
      tmp_marker=make_one_empty_marker_people();
      tmp_marker.color.r = 255/255;
      tmp_marker.color.g = 0/255;
      tmp_marker.color.b = 0/255;
    }
    else
    {
      tmp_marker=make_one_empty_marker_car();
      tmp_marker.color.r = 0/255;
      tmp_marker.color.g = 255/255;
      tmp_marker.color.b = 127/255;
    }
    //写入obs信息
    tmp_marker.id=i+500;
    tmp_marker.pose.position.x=obs_array.obstacles[i].pose.position.x-GPS_x;
    tmp_marker.pose.position.y=obs_array.obstacles[i].pose.position.y-GPS_y;
    tmp_marker.scale.x=obs_array.obstacles[i].scale.x;
    tmp_marker.scale.y=obs_array.obstacles[i].scale.y;
    tmp_marker.scale.z=obs_array.obstacles[i].scale.z;
    tmp_marker.pose.orientation.z = obs_array.obstacles[i].pose.orientation.z;
    tmp_marker.pose.orientation.w = obs_array.obstacles[i].pose.orientation.w;
    //
    tmp_marker_array.markers.push_back(tmp_marker);
  }
  pub_marker_array_global.publish(tmp_marker_array);
}
/空msg函数


/回调函数
void tilt_to_horizon_to_global::callback_lidar(perception_msgs::ObstacleArray obs_array_lidar)
{
  //##############################激光雷达——>水平
  perception_msgs::ObstacleArray obs_array_horizon;
  obs_array_horizon=obs_array_lidar;
  for(int i=0;i<obs_array_horizon.obstacles.size();i++)
  {
    //修改xyz参数
    double tmp_x=obs_array_horizon.obstacles[i].pose.position.x;
    double tmp_y=obs_array_horizon.obstacles[i].pose.position.y;
    double tmp_hd=PI/180*road_angle;
    double tmp_xx;
    double tmp_yy;
    //坐标变换
    tmp_xx=tmp_x;
    tmp_yy=tmp_y*acos(tmp_hd);
    // tmp_xx=acos(tmp_hd)*tmp_x-asin(tmp_hd)*tmp_y;
    // tmp_yy=asin(tmp_hd)*tmp_x+acos(tmp_hd)*tmp_y;

    //替换掉原有的xy
    obs_array_horizon.obstacles[i].pose.position.x = tmp_xx;
    obs_array_horizon.obstacles[i].pose.position.y = tmp_yy;
  }
  pub_road_horizon.publish(obs_array_horizon);
  pub_horizon_markers_from_obs(obs_array_horizon);
  //##############################激光雷达——>水平

  //##############################水平——>全局
  perception_msgs::ObstacleArray obs_array_global;
  obs_array_global=obs_array_horizon;
  for(int i=0;i<obs_array_global.obstacles.size();i++)
  {

    double tmp_x=obs_array_global.obstacles[i].pose.position.x;
    double tmp_y=obs_array_global.obstacles[i].pose.position.y;

    double tmp_delta_hd=PI/180*delta_angle;
    double tmp_xx;
    double tmp_yy;
    //坐标变换
    tmp_xx=cos(tmp_delta_hd)*tmp_x-sin(tmp_delta_hd)*tmp_y;
    tmp_yy=sin(tmp_delta_hd)*tmp_x+cos(tmp_delta_hd)*tmp_y;
    // cout<<"delta_angle: "<<delta_angle<<endl;
    // cout<<"tmp_delta_hd: "<<tmp_delta_hd<<endl;
    // cout<<"acos(tmp_delta_hd): "<<cos(tmp_delta_hd)<<endl;
    // cout<<"asin(tmp_delta_hd): "<<sin(tmp_delta_hd)<<endl;
    // tmp_xx=tmp_x;
    // tmp_yy=tmp_y;

    //替换掉原有的xy
    obs_array_global.obstacles[i].pose.position.x = tmp_xx+GPS_x+delta_x;
    obs_array_global.obstacles[i].pose.position.y = tmp_yy+GPS_y+delta_y;
  }
  pub_road_global.publish(obs_array_global);
  pub_global_markers_from_obs(obs_array_global);
  //##############################水平——>全局
}
int main(int argc, char **argv)
{
    ros::init(argc, argv, "tilt_to_horizon_to_global");
    ros::NodeHandle nh("~");

    tilt_to_horizon_to_global a;

    ros::spin();
    return 0;
}