carla shenlan project cross

#include "carla_shenlan_lattice_planner/lattice_lqr_lateral_longitudinal.h" #include <chrono> #include <cstdio> #include <functional> #include <memory> #include <string> #include "rclcpp/rclcpp.hpp" #include "std_msgs/msg/string.hpp" //// //// #include <algorithm> #include <iomanip> #include <iostream> #include <iterator> #include <map> #include <queue> #include <utility> #include "time.h" #include <ctime> #include "dnn/hb_dnn.h" #include "gflags/gflags.h" #include "glog/logging.h" #include "opencv2/core/mat.hpp" #include "opencv2/imgcodecs.hpp" #include "opencv2/imgproc.hpp" #define EMPTY "" DEFINE_string(model_file, EMPTY, "model file path"); DEFINE_string(image_file, EMPTY, "Test image path"); DEFINE_int32(top_k, 5, "Top k classes, 5 by default"); enum VLOG_LEVEL { EXAMPLE_SYSTEM = 0, EXAMPLE_REPORT = 1, EXAMPLE_DETAIL = 2, EXAMPLE_DEBUG = 3 }; /* #define HB_CHECK_SUCCESS(value, errmsg) \ do { \ //value can be call of function \ auto ret_code = value; \ if (ret_code != 0) { \ VLOG(EXAMPLE_SYSTEM) << errmsg << ", error code:" << ret_code; \ return ret_code; \ } \ } while (0); */ #define HB_CHECK_SUCCESS(value, errmsg) \ do { \ auto ret_code = value; \ if (ret_code != 0) { \ VLOG(EXAMPLE_SYSTEM) << errmsg << ", error code:" << ret_code; \ } \ } while (0) typedef struct Classification { int id; float score; const char *class_name; Classification() : class_name(0), id(0), score(0.0) {} Classification(int id, float score, const char *class_name) : id(id), score(score), class_name(class_name) {} friend bool operator>(const Classification &lhs, const Classification &rhs) { return (lhs.score > rhs.score); } ~Classification() {} } Classification; std::map<int32_t, int32_t> element_size{{HB_DNN_IMG_TYPE_Y, 1}, {HB_DNN_IMG_TYPE_NV12, 1}, {HB_DNN_IMG_TYPE_NV12_SEPARATE, 1}, {HB_DNN_IMG_TYPE_YUV444, 1}, {HB_DNN_IMG_TYPE_RGB, 1}, {HB_DNN_IMG_TYPE_BGR, 1}, {HB_DNN_TENSOR_TYPE_S8, 1}, {HB_DNN_TENSOR_TYPE_U8, 1}, {HB_DNN_TENSOR_TYPE_F16, 2}, {HB_DNN_TENSOR_TYPE_S16, 2}, {HB_DNN_TENSOR_TYPE_U16, 2}, {HB_DNN_TENSOR_TYPE_F32, 4}, {HB_DNN_TENSOR_TYPE_S32, 4}, {HB_DNN_TENSOR_TYPE_U32, 4}, {HB_DNN_TENSOR_TYPE_F64, 8}, {HB_DNN_TENSOR_TYPE_S64, 8}, {HB_DNN_TENSOR_TYPE_U64, 8}}; int prepare_tensor(hbDNNTensor *input_tensor, hbDNNTensor *output_tensor, hbDNNHandle_t dnn_handle); int32_t read_image_2_tensor_as_nv12(std::string &image_file, hbDNNTensor *input_tensor); void get_topk_result(hbDNNTensor *tensor, std::vector<Classification> &top_k_cls, int top_k); // using namespace std; using std::placeholders::_1; static const rclcpp::Logger LOGGER = rclcpp::get_logger("lattice_lqr_lateral_longitudinal"); LatticePlannerNode::LatticePlannerNode() : Node("lattice_lqr_lateral_longitudinal") /*'''************************************************************************************** - FunctionName: None - Function : None - Inputs : None - Outputs : None - Comments : None **************************************************************************************'''*/ { this->declare_parameter<std::string>("roadmap_path", roadmap_path); // 读取路网文件名 this->declare_parameter<double>("target_speed", target_speed); // 读取目标速度 this->declare_parameter<double>("goal_tolerance", goalTolerance_); // 读取目标速度 this->declare_parameter<double>("speed_P", speed_P); // 读取参数 this->declare_parameter<double>("speed_I", speed_I); this->declare_parameter<double>("speed_D", speed_D); this->get_parameter<std::string>("roadmap_path", roadmap_path); // 读取路网文件名 this->get_parameter<double>("target_speed", target_speed); // 读取目标速度 this->get_parameter<double>("goal_tolerance", goalTolerance_); // 读取目标速度 this->get_parameter<double>("speed_P", speed_P); // 读取PID参数 this->get_parameter<double>("speed_I", speed_I); this->get_parameter<double>("speed_D", speed_D); float c_speed_, c_d_, c_d_d_, c_d_dd_, s0_; this->declare_parameter<float>("c_speed", c_speed_); //读取Frenet规划器，初始目标速度 this->declare_parameter<float>("c_d", c_d_); //读取Frenet规划器，初始横向偏差 this->declare_parameter<float>("c_d_d", c_d_d_); //读取Frenet规划器，初始横向速度偏差 this->declare_parameter<float>("c_d_dd", c_d_dd_); //读取Frenet规划器，初始横向加速度偏差 this->declare_parameter<float>("s0", s0_); //读取Frenet规划器，初始纵向距离 this->get_parameter<float>("c_speed", c_speed_); //读取Frenet规划器，初始目标速度 this->get_parameter<float>("c_d", c_d_); //读取Frenet规划器，初始横向偏差 this->get_parameter<float>("c_d_d", c_d_d_); //读取Frenet规划器，初始横向速度偏差 this->get_parameter<float>("c_d_dd", c_d_dd_); //读取Frenet规划器，初始横向加速度偏差 this->get_parameter<float>("s0", s0_); //加载路网文件 // std::cout << "roadmap_path: " << roadmap_path << " " << target_speed << std::endl; //loadRoadmap(roadmap_path); //GetWayPoints(); //在参考路径的基础上进行采点 // 构建相对平滑的Frenet曲线坐标系，一个中间暂时方案 //csp_obj_ = new Spline2D(wx_, wy_); // 构造全局路径变量 //GenerateGlobalPath(); // Update Obstacle 添加虚拟障碍物 //UpdateStaticObstacle(); //pid_controller_longitudinal = std::make_unique<shenlan::control::PIDController>(speed_P, speed_I, speed_D); //lqr_controller_lateral = std::make_unique<shenlan::control::LqrController>(); //lqr_controller_lateral->LoadControlConf(); //lqr_controller_lateral->Init(); /* localization_data_subscriber = this->create_subscription<nav_msgs::msg::Odometry>("/carla/ego_vehicle/odometry", 10, std::bind(&LatticePlannerNode::OdomCallback, this, _1)); lacalization_data_imu_subscriber = this->create_subscription<sensor_msgs::msg::Imu>("/carla/ego_vehicle/imu", 10, std::bind(&LatticePlannerNode::IMUCallback, this, _1)); */ vehicle_control_publisher = this->create_publisher<carla_msgs::msg::CarlaEgoVehicleControl>("/carla/ego_vehicle/vehicle_control_cmd", 10); control_cmd.header.stamp = this->now(); control_cmd.gear = 1; control_cmd.man