104 lines
3.1 KiB
C++
104 lines
3.1 KiB
C++
#ifndef PATH_VISUALIZER_HPP_
|
|
#define PATH_VISUALIZER_HPP_
|
|
|
|
#include <memory>
|
|
#include <curl/curl.h>
|
|
#include <nlohmann/json.hpp>
|
|
#include <cmath>
|
|
#include <fstream>
|
|
#include <iostream>
|
|
#include <string>
|
|
#include <vector>
|
|
#include <algorithm>
|
|
#include "geometry_msgs/msg/twist.hpp"
|
|
#include "rclcpp/rclcpp.hpp"
|
|
#include "nav_msgs/msg/odometry.hpp"
|
|
#include "visualization_msgs/msg/marker.hpp"
|
|
#include <nav_msgs/msg/path.hpp>
|
|
#include <tf2_ros/transform_broadcaster.h>
|
|
#include <tf2/LinearMath/Quaternion.h>
|
|
|
|
class PathVisualizer : public rclcpp::Node
|
|
{
|
|
public:
|
|
PathVisualizer(const std::string& file_name);
|
|
private:
|
|
void localozation_callback(const geometry_msgs::msg::Pose::SharedPtr msg);
|
|
void control();
|
|
double quaternion_to_yaw(double x,
|
|
double y,
|
|
double z,
|
|
double w);
|
|
double normalize_angle(double angle);
|
|
bool downloadPathJson(const std::string &url,std::string &json_data);
|
|
bool loadPathFromJson(const std::string &json_data);
|
|
|
|
double gps_to_x(double lat,double lon);
|
|
double gps_to_y(double lat,double lon);
|
|
|
|
int search_nearest_point();
|
|
int findLookAheadPoint();
|
|
int findClosestPoint(int forward_index_limit);
|
|
// pub rviz info
|
|
void publishPathRviz();
|
|
void publishVehiclePoseRviz();
|
|
void publishVehicleTFRviz();
|
|
void publishHistoryPathRviz();
|
|
void publishVehicleInfoRviz();
|
|
void transformPathToVehicleStart();
|
|
void densifyPath(double interval);
|
|
void publishLocalPose();
|
|
void publishOdomPath();
|
|
|
|
private:
|
|
// topic pub
|
|
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
|
|
rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr local_pose_pub_;
|
|
rclcpp::Publisher<nav_msgs::msg::Path>::SharedPtr odom_path_pub_;
|
|
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_pub_;
|
|
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr path_pub_;
|
|
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr history_pose_pub_;
|
|
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_info_pub_;
|
|
|
|
// topic sub
|
|
rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr odom_sub_;
|
|
rclcpp::TimerBase::SharedPtr timer_;
|
|
|
|
// 坐标转换broadcaster
|
|
std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_; // odom -> baselink
|
|
|
|
private:
|
|
// odom path info
|
|
std::vector<double> path_x_;
|
|
std::vector<double> path_y_;
|
|
std::vector<double> path_yaw_;
|
|
// odom history pose
|
|
std::vector<double> history_x_;
|
|
std::vector<double> history_y_;
|
|
std::vector<double> history_yaw_;
|
|
|
|
int target_index_;
|
|
// localization info
|
|
double x_;
|
|
double y_;
|
|
double yaw_;
|
|
bool has_pose_;
|
|
// contorl paramss
|
|
double max_speed_ = 0.8;
|
|
double max_yaw_rate_ = 2.0;
|
|
double lookahead_distance_ =0.6;
|
|
double origin_lat_;
|
|
double origin_lon_;
|
|
double origin_yaw_;
|
|
int forward_index_limit_=100;
|
|
|
|
|
|
bool origin_set_= false;
|
|
bool path_transed = false;
|
|
std::string file_name_;
|
|
std::string path_url_ ;
|
|
std::string json_data_;
|
|
|
|
};
|
|
|
|
#endif |