Test branch:
This commit is contained in:
liuhaoyan
parent
d63c1435b6
commit
2dc7d7013d
40
Readme.md
Normal file
40
Readme.md
Normal file
@ -0,0 +1,40 @@
|
||||
# auto drive misson center webots simulation:
|
||||
|
||||
## 1, `ubuntu20.04`, `ubuntu22.04`
|
||||
## 2, 安装 `webots`
|
||||
`https://github.com/cyberbotics/webots/tags`， 版本： `ubuntu20.04： R2023a， ubuntu22.04：R2025a`
|
||||
## 3, 创建ros2 工作空间
|
||||
如 `~/sweet_ws `, 编译 `sweet_webots_robot, robots_rviz_panel, webots_simulation, auto_drive_proj_utils,auto_drive_proj_msg, auto_drive_misson_center`
|
||||
```shell
|
||||
cd ~/sweet_ws
|
||||
colcon build --packages-select sweet_webots_robot --cmake-clean-cache
|
||||
source install/setup.bash
|
||||
colcon build --packages-select robots_rviz_panel --cmake-clean-cache
|
||||
source install/setup.bash
|
||||
colcon build --packages-select webots_simulation --cmake-clean-cache
|
||||
source install/setup.bash
|
||||
colcon build --packages-select auto_drive_proj_utils --cmake-clean-cache
|
||||
source install/setup.bash
|
||||
colcon build --packages-select auto_drive_proj_msg --cmake-clean-cache
|
||||
source install/setup.bash
|
||||
colcon build --packages-select auto_drive_misson_center --cmake-clean-cache
|
||||
source install/setup.bash
|
||||
```
|
||||
## 4, 运行 webots:
|
||||
终端执行命令： `webots`
|
||||
## 5, 运行 webots 与 rviz可视化界面
|
||||
`ros2 launch webots_simulation robot_launch.py` ，等待rviz2界面打开后，加载配置文件：` File->Open Config->` 选择 `SweetAiRviz.rviz` ，方便起见，打开后，File->Save Config 下次不用再重复加载。该 rviz界面 左右两侧拉开留出视图空间。
|
||||
测试版本中path_visualizer pkg目前与launch一起启动，其路径代码可在src/webots_simulation/src/PathVisualizer.cpp 中修改file_name实现
|
||||
## 6, 模拟 `/start_pose` 服务
|
||||
另启一个终端，模拟 `/start_pose` 服务，常开，执行脚本(注意路径)：`python3 /home/ada/sweet_ws/install/auto_drive_misson_center/share/auto_drive_misson_center/scripts/mock_start_pose_and_publish_rtk.py `
|
||||
## 7, 运行 mission_center 任务节点
|
||||
运行 `ros2 launch auto_drive_misson_center mission_center_launch.py` 启动 任务节点。
|
||||
## 8, 请求 /invoke/missionOn 服务
|
||||
另启一个终端，执行发送任务命令，这里的任务，必须和 `6` 中的匹配，将11699983696942006272.json1内容填充到指令中：
|
||||
```shell
|
||||
ros2 service call /invoke/missionOn auto_drive_proj_msg/srv/ReqString "{request: '[{\"name\":\"value\",\"value\":\"https://test.sweetai.cn:4433/load/locationCmdPath/1699983696942006272.json\"}]', value: ''}"
|
||||
```
|
||||
> 观察任务轨迹以及rviz2右侧panel面板参数
|
||||
|
||||
rviz2 panel：
|
||||
|
||||
@ -14,6 +14,7 @@ find_package(auto_drive_proj_utils REQUIRED)
|
||||
find_package(auto_drive_proj_msg REQUIRED)
|
||||
find_package(Eigen3 REQUIRED)
|
||||
find_package(fmt REQUIRED)
|
||||
find_package(visualization_msgs REQUIRED)
|
||||
|
||||
include_directories(
|
||||
include
|
||||
@ -66,6 +67,7 @@ ament_target_dependencies(mission_center_node
|
||||
builtin_interfaces
|
||||
auto_drive_proj_utils
|
||||
auto_drive_proj_msg
|
||||
visualization_msgs
|
||||
)
|
||||
|
||||
ament_target_dependencies(auto_mission_handler_node
|
||||
|
||||
@ -55,7 +55,6 @@ private:
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_warn_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr event_error_pub_;
|
||||
rclcpp::Publisher<std_msgs::msg::String>::SharedPtr properties_report_pub_;
|
||||
|
||||
std::vector<std::pair<int, int>> interpolation_map_;
|
||||
|
||||
std::shared_future<typename auto_drive_proj_msg::srv::ReqString::Response::SharedPtr> mission_future_;
|
||||
@ -87,6 +86,10 @@ private:
|
||||
std::string& follow_data);
|
||||
std::vector<std::vector<double>> interpolatePoints(const std::vector<std::vector<double>>& coordinates,
|
||||
double max_distance = 0.5);
|
||||
// simulation only
|
||||
std::vector<std::vector<double>> alignPathYaw( const std::vector<std::vector<double>>& coordinates,
|
||||
double vehicle_initial_yaw);
|
||||
|
||||
int updatePathIndices(const int original_ind, const std::vector<std::pair<int, int>>& interpolation_map);
|
||||
|
||||
void handleAsyncResponses();
|
||||
@ -145,6 +148,9 @@ private:
|
||||
int logger_max_size_;
|
||||
int logger_max_files_;
|
||||
int logger_set_level_;
|
||||
|
||||
//
|
||||
double vehicle_initial_yaw_ = 0.0;
|
||||
};
|
||||
|
||||
} // namespace auto_drive_misson_center
|
||||
|
||||
@ -12,6 +12,7 @@
|
||||
<depend>rclcpp</depend>
|
||||
<depend>std_msgs</depend>
|
||||
<depend>std_srvs</depend>
|
||||
<depend>visualization_msgs</depend>
|
||||
<depend>auto_drive_proj_utils</depend>
|
||||
<depend>auto_drive_proj_msg</depend>
|
||||
<depend>libcurl-dev</depend>
|
||||
|
||||
@ -349,7 +349,6 @@ void MissionCenterNode::missionOnCallback(
|
||||
const auto_drive_proj_msg::srv::ReqString::Request::SharedPtr request,
|
||||
auto_drive_proj_msg::srv::ReqString::Response::SharedPtr response)
|
||||
{
|
||||
LOG_INFO("========================== /invoke/missionOn request=");
|
||||
try {
|
||||
LOG_INFO("Received /invoke/missionOn request: {}", request->request);
|
||||
|
||||
@ -745,6 +744,67 @@ std::string MissionCenterNode::downloadFile(const std::string& url)
|
||||
return readBuffer;
|
||||
}
|
||||
|
||||
std::vector<std::vector<double>> MissionCenterNode::alignPathYaw(
|
||||
const std::vector<std::vector<double>>& coordinates,
|
||||
double vehicle_initial_yaw)
|
||||
{
|
||||
std::vector<std::vector<double>> aligned_coordinates;
|
||||
|
||||
if (coordinates.empty())
|
||||
return aligned_coordinates;
|
||||
|
||||
// Path 起点
|
||||
const double origin_x = coordinates.front()[0];
|
||||
const double origin_y = coordinates.front()[1];
|
||||
|
||||
// Path 原始起始航向
|
||||
const double path_initial_yaw = coordinates.front()[2];
|
||||
|
||||
// 将 Path 初始航向旋转到车辆初始航向
|
||||
const double yaw_offset =
|
||||
vehicle_initial_yaw - path_initial_yaw;
|
||||
|
||||
const double cos_offset = std::cos(yaw_offset);
|
||||
const double sin_offset = std::sin(yaw_offset);
|
||||
|
||||
aligned_coordinates.reserve(coordinates.size());
|
||||
|
||||
for (const auto& coord : coordinates)
|
||||
{
|
||||
if (coord.size() < 3)
|
||||
continue;
|
||||
|
||||
const double dx = coord[0] - origin_x;
|
||||
const double dy = coord[1] - origin_y;
|
||||
|
||||
const double x =
|
||||
cos_offset * dx - sin_offset * dy;
|
||||
|
||||
const double y =
|
||||
sin_offset * dx + cos_offset * dy;
|
||||
|
||||
double yaw = coord[2] + yaw_offset;
|
||||
|
||||
while (yaw > M_PI)
|
||||
yaw -= 2.0 * M_PI;
|
||||
|
||||
while (yaw < -M_PI)
|
||||
yaw += 2.0 * M_PI;
|
||||
|
||||
aligned_coordinates.push_back({
|
||||
x,
|
||||
y,
|
||||
yaw
|
||||
});
|
||||
LOG_INFO(
|
||||
"Path new: x={} y={} yaw={} deg",
|
||||
x,
|
||||
y,
|
||||
yaw * 180.0 / M_PI);
|
||||
}
|
||||
|
||||
return aligned_coordinates;
|
||||
}
|
||||
bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data,
|
||||
std::string& mission_data, std::string& follow_data)
|
||||
{
|
||||
@ -787,8 +847,10 @@ bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data
|
||||
}
|
||||
|
||||
interpolation_map_.clear();
|
||||
std::vector<std::vector<double>> interpolated_coords = interpolatePoints(coordinates, 0.5);
|
||||
|
||||
std::vector<std::vector<double>> aligned_coords = alignPathYaw(coordinates, vehicle_initial_yaw_);
|
||||
std::vector<std::vector<double>> interpolated_coords = interpolatePoints(aligned_coords, 0.5);
|
||||
|
||||
json processed_data = data;
|
||||
json new_coordinates = json::array();
|
||||
for (const auto& coord : interpolated_coords) {
|
||||
|
||||
@ -15,6 +15,7 @@
|
||||
#include <rclcpp/rclcpp.hpp>
|
||||
#include "geometry_msgs/msg/twist.hpp"
|
||||
#include "geometry_msgs/msg/pose.hpp"
|
||||
#include <geometry_msgs/msg/pose_with_covariance_stamped.hpp>
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <std_msgs/msg/float64.hpp>
|
||||
#include <rviz_common/display_context.hpp>
|
||||
@ -40,7 +41,7 @@ private Q_SLOTS:
|
||||
void onStop();
|
||||
|
||||
private:
|
||||
void poseCallback(const geometry_msgs::msg::Pose::SharedPtr msg);
|
||||
void poseCallback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
|
||||
void velCmdCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
|
||||
void pathCallback(const nav_msgs::msg::Path::SharedPtr msg);
|
||||
void calculateData();
|
||||
@ -63,7 +64,7 @@ private:
|
||||
rclcpp::Node::SharedPtr node_;
|
||||
std::shared_ptr<
|
||||
rviz_common::ros_integration::RosNodeAbstractionIface> node_ptr_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr pose_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr pose_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr twist_cmd_sub_;
|
||||
rclcpp::Subscription<nav_msgs::msg::Path>::SharedPtr odom_path_sub_;
|
||||
double current_x_ = 0.0;
|
||||
|
||||
@ -146,7 +146,7 @@ void SweetRvizPanel::onInitialize()
|
||||
}
|
||||
auto node = node_ptr_->get_raw_node();
|
||||
|
||||
pose_sub_ = node->create_subscription<geometry_msgs::msg::Pose>(
|
||||
pose_sub_ = node->create_subscription<geometry_msgs::msg::PoseWithCovarianceStamped>(
|
||||
"/local_pose",10,
|
||||
std::bind(&SweetRvizPanel::poseCallback,this,std::placeholders::_1));
|
||||
|
||||
@ -163,38 +163,42 @@ void SweetRvizPanel::onInitialize()
|
||||
}
|
||||
|
||||
void SweetRvizPanel::poseCallback(
|
||||
const geometry_msgs::msg::Pose::SharedPtr msg)
|
||||
const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
|
||||
{
|
||||
if (!running_) {
|
||||
return;
|
||||
}
|
||||
const auto& pose = msg->pose.pose;
|
||||
|
||||
const double x = msg->position.x;
|
||||
const double y = msg->position.y;
|
||||
const auto &q = msg->orientation;
|
||||
double x = pose.position.x;
|
||||
double y = pose.position.y;
|
||||
|
||||
tf2::Quaternion quaternion(
|
||||
q.x,
|
||||
q.y,
|
||||
q.z,
|
||||
q.w);
|
||||
tf2::Quaternion quaternion(
|
||||
pose.orientation.x,
|
||||
pose.orientation.y,
|
||||
pose.orientation.z,
|
||||
pose.orientation.w);
|
||||
|
||||
double roll;
|
||||
double pitch;
|
||||
double yaw;
|
||||
double roll;
|
||||
double pitch;
|
||||
double yaw;
|
||||
|
||||
tf2::Matrix3x3(quaternion).getRPY(
|
||||
roll,
|
||||
pitch,
|
||||
yaw);
|
||||
pose_received_ = true;
|
||||
current_x_ = x;
|
||||
current_y_ = y;
|
||||
current_yaw_ = yaw;
|
||||
x_label_->setText(QString("X: %1 m").arg(x, 0, 'f', 3));
|
||||
y_label_->setText(QString("Y: %1 m").arg(y, 0, 'f', 3));
|
||||
yaw_label_->setText(QString("Yaw: %1 rad").arg(yaw, 0, 'f', 3));
|
||||
|
||||
tf2::Matrix3x3(quaternion).getRPY(
|
||||
roll,
|
||||
pitch,
|
||||
yaw);
|
||||
|
||||
pose_received_ = true;
|
||||
|
||||
current_x_ = x;
|
||||
current_y_ = y;
|
||||
current_yaw_ = yaw;
|
||||
|
||||
x_label_->setText(
|
||||
QString("X: %1 m").arg(x, 0, 'f', 3));
|
||||
|
||||
y_label_->setText(
|
||||
QString("Y: %1 m").arg(y, 0, 'f', 3));
|
||||
|
||||
yaw_label_->setText(
|
||||
QString("Yaw: %1 rad").arg(yaw, 0, 'f', 3));
|
||||
}
|
||||
|
||||
void SweetRvizPanel::velCmdCallback(
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
#include <curl/curl.h>
|
||||
//#include <nlohmann/json.hpp>
|
||||
#include <auto_drive_proj_utils/Json/json.hpp>
|
||||
#include <auto_drive_proj_utils/GeodeticConverter/GeodeticConverter.hpp>
|
||||
#include <cmath>
|
||||
#include <fstream>
|
||||
#include <iostream>
|
||||
@ -19,6 +20,7 @@
|
||||
#include <nav_msgs/msg/path.hpp>
|
||||
#include <tf2_ros/transform_broadcaster.h>
|
||||
#include <tf2/LinearMath/Quaternion.h>
|
||||
#include <std_msgs/msg/int32.hpp>
|
||||
|
||||
class PathVisualizer : public rclcpp::Node
|
||||
{
|
||||
@ -27,6 +29,7 @@ public:
|
||||
private:
|
||||
// void localozation_callback(const geometry_msgs::msg::Pose::SharedPtr msg);
|
||||
void localozation_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg);
|
||||
void pathIndCallback(const std_msgs::msg::Int32::SharedPtr msg);
|
||||
void control();
|
||||
double quaternion_to_yaw(double x,
|
||||
double y,
|
||||
@ -44,12 +47,12 @@ private:
|
||||
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 transformPathToVehicleStart(double vehicle_initial_yaw);
|
||||
//void densifyPath(double interval);
|
||||
void interpolatePoints(double max_interval_distance);
|
||||
void publishLocalPose();
|
||||
void publishOdomPath();
|
||||
|
||||
@ -67,6 +70,7 @@ private:
|
||||
// topic sub
|
||||
// rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr odom_sub_;
|
||||
rclcpp::Subscription<geometry_msgs::msg::PoseWithCovarianceStamped>::SharedPtr odom_sub_;
|
||||
rclcpp::Subscription<std_msgs::msg::Int32>::SharedPtr path_track_ind_sub_;
|
||||
rclcpp::TimerBase::SharedPtr timer_;
|
||||
|
||||
// 坐标转换broadcaster
|
||||
@ -92,10 +96,13 @@ private:
|
||||
double max_speed_ = 0.8;
|
||||
double max_yaw_rate_ = 2.0;
|
||||
double lookahead_distance_ =0.6;
|
||||
double max_interval_distance_ = 0.5;
|
||||
double origin_lat_;
|
||||
double origin_lon_;
|
||||
double origin_yaw_;
|
||||
double vehicle_initial_yaw_ = 0.0;
|
||||
int forward_index_limit_=100;
|
||||
int path_track_index_ = -1;
|
||||
|
||||
|
||||
bool origin_set_= false;
|
||||
@ -103,7 +110,7 @@ private:
|
||||
std::string file_name_;
|
||||
std::string path_url_ ;
|
||||
std::string json_data_;
|
||||
|
||||
std::unique_ptr<GeodeticConverter> geodetic_converter_;
|
||||
};
|
||||
|
||||
#endif
|
||||
|
||||
@ -69,7 +69,7 @@ def generate_launch_description():
|
||||
my_robot_driver,
|
||||
robot_state_publisher,
|
||||
rviz_node,
|
||||
#path_visualizer,
|
||||
path_visualizer,
|
||||
launch.actions.RegisterEventHandler(
|
||||
event_handler=launch.event_handlers.OnProcessExit(
|
||||
target_action=webots,
|
||||
|
||||
@ -26,6 +26,11 @@ has_pose_(false)
|
||||
"/rtk_pose",
|
||||
10,
|
||||
std::bind(&PathVisualizer::localozation_callback, this, std::placeholders::_1));
|
||||
path_track_ind_sub_ = this->create_subscription<std_msgs::msg::Int32>(
|
||||
"/auto_path_ind",
|
||||
10,
|
||||
std::bind(
|
||||
&PathVisualizer::pathIndCallback,this,std::placeholders::_1));
|
||||
// timer
|
||||
timer_ =this->create_wall_timer(std::chrono::milliseconds(50),std::bind(
|
||||
&PathVisualizer::control,this));
|
||||
@ -55,78 +60,79 @@ static size_t curlWriteCallback(
|
||||
|
||||
return total_size;
|
||||
}
|
||||
void PathVisualizer::transformPathToVehicleStart()
|
||||
|
||||
void PathVisualizer::transformPathToVehicleStart(double vehicle_initial_yaw)
|
||||
{
|
||||
if (path_x_.empty())
|
||||
if (path_x_.empty() || path_y_.empty() || path_yaw_.empty())
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"Path is empty, cannot transform");
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(),
|
||||
"Path is empty, cannot transform");
|
||||
return;
|
||||
}
|
||||
|
||||
if (!origin_set_)
|
||||
{
|
||||
RCLCPP_WARN(this->get_logger(),"Vehicle initial pose has not been set");
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(),
|
||||
"Vehicle initial pose has not been set");
|
||||
return;
|
||||
}
|
||||
const double tx = gps_to_x(origin_lat_,origin_lon_);
|
||||
const double ty = gps_to_y(origin_lat_,origin_lon_);
|
||||
const double theta =origin_yaw_;
|
||||
|
||||
const double path_origin_x = path_x_[0];
|
||||
const double path_origin_y = path_y_[0];
|
||||
const double yaw_offset = theta - path_yaw_[0];
|
||||
double normalized_yaw_offset = yaw_offset;
|
||||
const double cos_theta =std::cos(yaw_offset);
|
||||
const double sin_theta =std::sin(yaw_offset);
|
||||
// Path 起点
|
||||
const double origin_x = path_x_.front();
|
||||
const double origin_y = path_y_.front();
|
||||
|
||||
while (normalized_yaw_offset > M_PI)
|
||||
// Path 原始起始航向
|
||||
const double path_initial_yaw = path_yaw_.front();
|
||||
|
||||
// 将 Path 初始航向旋转到车辆初始航向
|
||||
const double yaw_offset =
|
||||
vehicle_initial_yaw - path_initial_yaw;
|
||||
|
||||
const double cos_offset = std::cos(yaw_offset);
|
||||
const double sin_offset = std::sin(yaw_offset);
|
||||
|
||||
|
||||
for (int i = 0; i < path_x_.size(); ++i)
|
||||
{
|
||||
normalized_yaw_offset -= 2.0 * M_PI;
|
||||
}
|
||||
while (normalized_yaw_offset < -M_PI)
|
||||
{
|
||||
normalized_yaw_offset += 2.0 * M_PI;
|
||||
|
||||
|
||||
// ------------------------------------------------
|
||||
// 1. 以 Path 起点为原点
|
||||
// ------------------------------------------------
|
||||
const double dx = path_x_[i] - origin_x;
|
||||
const double dy = path_y_[i] - origin_y;
|
||||
|
||||
// ------------------------------------------------
|
||||
// 2. XY 绕 Path 起点旋转
|
||||
// ------------------------------------------------
|
||||
const double x =
|
||||
cos_offset * dx - sin_offset * dy;
|
||||
|
||||
const double y =
|
||||
sin_offset * dx + cos_offset * dy;
|
||||
|
||||
// ------------------------------------------------
|
||||
// 3. 航向角同步旋转
|
||||
// ------------------------------------------------
|
||||
double yaw = path_yaw_[i] + yaw_offset;
|
||||
// ------------------------------------------------
|
||||
// 4. yaw 归一化到 [-PI, PI]
|
||||
// ------------------------------------------------
|
||||
while (yaw > M_PI)
|
||||
yaw -= 2.0 * M_PI;
|
||||
|
||||
while (yaw < -M_PI)
|
||||
yaw += 2.0 * M_PI;
|
||||
|
||||
path_x_[i] = x;
|
||||
path_y_[i] = y;
|
||||
path_yaw_[i] = yaw;
|
||||
RCLCPP_INFO(this->get_logger(),"Transformed path point %d: x = %.2f, y = %.2f, yaw = %.2f",
|
||||
i, x, y, yaw);
|
||||
}
|
||||
|
||||
for (size_t i = 0;i < path_x_.size();++i)
|
||||
{
|
||||
path_x_[i] -= path_origin_x;
|
||||
path_y_[i] -= path_origin_y;
|
||||
const double x = path_x_[i];
|
||||
const double y = path_y_[i];
|
||||
// 旋转 + 平移
|
||||
path_x_[i] = cos_theta * x - sin_theta * y + tx;
|
||||
path_y_[i] = sin_theta * x + cos_theta * y + ty;
|
||||
path_yaw_[i] += normalized_yaw_offset;
|
||||
|
||||
// 归一化到 [-pi, pi]
|
||||
while (path_yaw_[i] > M_PI)
|
||||
{
|
||||
path_yaw_[i] -= 2.0 * M_PI;
|
||||
}
|
||||
|
||||
while (path_yaw_[i] < -M_PI)
|
||||
{
|
||||
path_yaw_[i] += 2.0 * M_PI;
|
||||
}
|
||||
}
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Path transformed to vehicle initial pose");
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Vehicle initial: x=%.3f y=%.3f yaw=%.3f",
|
||||
tx,
|
||||
ty,
|
||||
theta);
|
||||
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"New Path[0]: x=%.3f y=%.3f yaw=%.3f",
|
||||
path_x_[0],
|
||||
path_y_[0],
|
||||
path_yaw_[0]);
|
||||
}
|
||||
bool PathVisualizer::downloadPathJson(const std::string &url,
|
||||
std::string &json_data)
|
||||
@ -202,6 +208,9 @@ bool PathVisualizer::loadPathFromJson(
|
||||
path_y_.clear();
|
||||
path_yaw_.clear();
|
||||
|
||||
std::unique_ptr<GeodeticConverter> geodetic_converter;
|
||||
bool geo_init_flag = false;
|
||||
|
||||
// 经纬度 → 局部 XY
|
||||
for (const auto &point : coordinates)
|
||||
{
|
||||
@ -210,18 +219,19 @@ bool PathVisualizer::loadPathFromJson(
|
||||
RCLCPP_WARN(this->get_logger(),"Invalid path point, skip");
|
||||
continue;
|
||||
}
|
||||
|
||||
// 原始数据
|
||||
const double lat = point[0].get<double>();
|
||||
const double lon = point[1].get<double>();
|
||||
const double yaw =point[2].get<double>();
|
||||
// 局部 坐标转换
|
||||
const double x = gps_to_x(lat,lon);
|
||||
const double y = gps_to_y(lat,lon);
|
||||
double east, north, up;
|
||||
if (!geo_init_flag) {
|
||||
geodetic_converter = std::make_unique<GeodeticConverter>(point[0], point[1], 0.0);
|
||||
data["properties"]["start_lat"] = point[0].get<double>();
|
||||
data["properties"]["start_lon"] = point[1].get<double>();
|
||||
data["properties"]["start_yaw"] = point.size() > 2 ? point[2].get<double>() : 0.0;
|
||||
geo_init_flag = true;
|
||||
}
|
||||
geodetic_converter->geodetic2Enu(point[0], point[1], 0.0, &east, &north, &up);
|
||||
// 保存
|
||||
path_x_.push_back(x);
|
||||
path_y_.push_back(y);
|
||||
path_yaw_.push_back(yaw);
|
||||
path_x_.push_back(east);
|
||||
path_y_.push_back(north);
|
||||
path_yaw_.push_back(point.size() > 2 ? point[2].get<double>() : 0.0);
|
||||
}
|
||||
// 检查
|
||||
if (path_x_.empty())
|
||||
@ -252,76 +262,6 @@ bool PathVisualizer::loadPathFromJson(
|
||||
}
|
||||
}
|
||||
|
||||
void PathVisualizer::publishVehiclePoseRviz()
|
||||
{
|
||||
visualization_msgs::msg::Marker marker;
|
||||
marker.header.frame_id="odom";
|
||||
marker.header.stamp=this->now();
|
||||
marker.ns="vehicle";
|
||||
marker.id=0;
|
||||
|
||||
// 箭头
|
||||
marker.type= visualization_msgs::msg::Marker::ARROW;
|
||||
marker.action= visualization_msgs::msg::Marker::ADD;
|
||||
|
||||
// 箭头大小
|
||||
marker.scale.x=2.5; // 长度
|
||||
marker.scale.y=0.10; // 宽度
|
||||
marker.scale.z=0.15;
|
||||
|
||||
// 颜色
|
||||
marker.color.a=1.0;
|
||||
marker.color.r=1.0;
|
||||
marker.color.g=1.0;
|
||||
marker.color.b=1.0;
|
||||
|
||||
// 位置
|
||||
marker.pose.position.x=x_;
|
||||
marker.pose.position.y=y_;
|
||||
marker.pose.position.z=0.4;
|
||||
|
||||
// yaw转四元数
|
||||
double cy = cos(yaw_/2.0);
|
||||
|
||||
double sy = sin(yaw_/2.0);
|
||||
marker.pose.orientation.x=0;
|
||||
marker.pose.orientation.y=0;
|
||||
marker.pose.orientation.z=sy;
|
||||
marker.pose.orientation.w=cy;
|
||||
vehicle_pub_->publish(marker);
|
||||
|
||||
// 2. vehicle cube
|
||||
// visualization_msgs::msg::Marker body;
|
||||
|
||||
// body.header.frame_id = "odom";
|
||||
// body.header.stamp = this->now();
|
||||
// body.ns = "vehicle";
|
||||
// body.id = 1;
|
||||
// body.type = visualization_msgs::msg::Marker::CUBE;
|
||||
// body.action = visualization_msgs::msg::Marker::ADD;
|
||||
|
||||
// // 车辆尺寸
|
||||
// body.scale.x = 0.1; // 车长
|
||||
// body.scale.y = 0.1; // 车宽
|
||||
// body.scale.z = 0.3; // 高度
|
||||
|
||||
// body.color.a = 0.8;
|
||||
// body.color.r = 0.0;
|
||||
// body.color.g = 0.0;
|
||||
// body.color.b = 1.0;
|
||||
// // 位置
|
||||
// body.pose.position.x = x_;
|
||||
// body.pose.position.y = y_;
|
||||
// body.pose.position.z = 0.1;
|
||||
// // 姿态
|
||||
// body.pose.orientation.x = 0;
|
||||
// body.pose.orientation.y = 0;
|
||||
// body.pose.orientation.z = sy;
|
||||
// body.pose.orientation.w = cy;
|
||||
|
||||
// vehicle_pub_->publish(body);
|
||||
|
||||
}
|
||||
void PathVisualizer::publishVehicleInfoRviz(){
|
||||
visualization_msgs::msg::Marker text;
|
||||
|
||||
@ -341,9 +281,9 @@ void PathVisualizer::publishVehicleInfoRviz(){
|
||||
text.pose.position.y = 0.0;
|
||||
text.pose.position.z = 1.5;
|
||||
text.color.a = 0.8;
|
||||
text.color.r = 1.0;
|
||||
text.color.g = 1.0;
|
||||
text.color.b = 1.0;
|
||||
text.color.r = 0.0;
|
||||
text.color.g = 0.0;
|
||||
text.color.b = 0.0;
|
||||
text.scale.z = 0.2;
|
||||
|
||||
text.text =
|
||||
@ -354,6 +294,7 @@ void PathVisualizer::publishVehicleInfoRviz(){
|
||||
}
|
||||
void PathVisualizer::publishPathRviz()
|
||||
{
|
||||
// 1. completed path
|
||||
visualization_msgs::msg::Marker path;
|
||||
path.header.frame_id="odom";
|
||||
path.header.stamp=this->now();
|
||||
@ -362,15 +303,14 @@ void PathVisualizer::publishPathRviz()
|
||||
path.type=visualization_msgs::msg::Marker::SPHERE_LIST;
|
||||
path.action=visualization_msgs::msg::Marker::ADD;
|
||||
|
||||
path.scale.x = 0.15;
|
||||
path.scale.y = 0.15;
|
||||
path.scale.z = 0.15;
|
||||
path.scale.x = 0.22;
|
||||
path.scale.y = 0.22;
|
||||
path.scale.z = 0.22;
|
||||
path.color.a=1.0;
|
||||
path.color.r=1.0;
|
||||
path.color.g=0.0;
|
||||
path.color.b=0.0;
|
||||
double cos_yaw = std::cos(yaw_);
|
||||
double sin_yaw = std::sin(yaw_);
|
||||
|
||||
for(size_t i=0;i<path_x_.size();i++)
|
||||
{
|
||||
geometry_msgs::msg::Point p;
|
||||
@ -382,6 +322,140 @@ void PathVisualizer::publishPathRviz()
|
||||
}
|
||||
path_pub_->publish(path);
|
||||
|
||||
visualization_msgs::msg::Marker path_line;
|
||||
|
||||
path_line.header.frame_id = "odom";
|
||||
path_line.header.stamp = this->now();
|
||||
|
||||
path_line.ns = "path_line";
|
||||
path_line.id = 1;
|
||||
|
||||
path_line.type = visualization_msgs::msg::Marker::LINE_STRIP;
|
||||
path_line.action = visualization_msgs::msg::Marker::ADD;
|
||||
|
||||
path_line.scale.x = 0.12;
|
||||
|
||||
path_line.color.a = 1.0;
|
||||
path_line.color.r = 1.0;
|
||||
path_line.color.g = 0.0;
|
||||
path_line.color.b = 1.0;
|
||||
|
||||
for(size_t i=0;i<path_x_.size();i++)
|
||||
{
|
||||
geometry_msgs::msg::Point p;
|
||||
p.x=path_x_[i];
|
||||
p.y=path_y_[i];
|
||||
p.z=0.05;
|
||||
path_line.points.push_back(p);
|
||||
}
|
||||
path_pub_->publish(path_line);
|
||||
|
||||
// 2. current tracking path point
|
||||
if (path_track_index_ > 0 &&
|
||||
path_track_index_ < static_cast<int>(path_x_.size()))
|
||||
{
|
||||
visualization_msgs::msg::Marker track_point;
|
||||
|
||||
track_point.header.frame_id = "odom";
|
||||
track_point.header.stamp = this->now();
|
||||
|
||||
track_point.ns = "path_track";
|
||||
track_point.id = 0;
|
||||
|
||||
track_point.type =
|
||||
visualization_msgs::msg::Marker::SPHERE;
|
||||
|
||||
track_point.action =
|
||||
visualization_msgs::msg::Marker::ADD;
|
||||
|
||||
// 比普通 Path 点大
|
||||
track_point.scale.x = 0.3;
|
||||
track_point.scale.y = 0.35;
|
||||
track_point.scale.z = 0.35;
|
||||
|
||||
// 黄色
|
||||
track_point.color.a = 1.0;
|
||||
track_point.color.r = 0.0;
|
||||
track_point.color.g = 0.0;
|
||||
track_point.color.b = 1.0;
|
||||
|
||||
track_point.pose.position.x = path_x_[path_track_index_];
|
||||
|
||||
track_point.pose.position.y = path_y_[path_track_index_];
|
||||
|
||||
track_point.pose.position.z = 0.15;
|
||||
|
||||
track_point.pose.orientation.w = 1.0;
|
||||
|
||||
path_pub_->publish(track_point);
|
||||
|
||||
visualization_msgs::msg::Marker track_arrow;
|
||||
|
||||
track_arrow.header.frame_id = "odom";
|
||||
track_arrow.header.stamp = this->now();
|
||||
|
||||
track_arrow.ns = "path_track_arrow";
|
||||
track_arrow.id = 0;
|
||||
|
||||
track_arrow.type =
|
||||
visualization_msgs::msg::Marker::ARROW;
|
||||
|
||||
track_arrow.action =
|
||||
visualization_msgs::msg::Marker::ADD;
|
||||
|
||||
// 箭头位置
|
||||
const double track_x =
|
||||
path_x_[path_track_index_];
|
||||
|
||||
const double track_y =
|
||||
path_y_[path_track_index_];
|
||||
|
||||
// ------------------------------------------------------------
|
||||
// 箭头起点：tracking point 上方
|
||||
// 箭头终点：tracking point
|
||||
// ------------------------------------------------------------
|
||||
|
||||
geometry_msgs::msg::Point start;
|
||||
geometry_msgs::msg::Point end;
|
||||
|
||||
start.x = track_x;
|
||||
start.y = track_y;
|
||||
start.z = 3;
|
||||
|
||||
end.x = track_x;
|
||||
end.y = track_y;
|
||||
end.z = 0.2;
|
||||
|
||||
track_arrow.points.push_back(start);
|
||||
track_arrow.points.push_back(end);
|
||||
|
||||
// ------------------------------------------------------------
|
||||
// 箭头尺寸
|
||||
//
|
||||
// scale.x = 箭杆直径
|
||||
// scale.y = 箭头宽度
|
||||
// scale.z = 箭头长度
|
||||
// ------------------------------------------------------------
|
||||
|
||||
track_arrow.scale.x = 0.04;
|
||||
track_arrow.scale.y = 0.15;
|
||||
track_arrow.scale.z = 0.3;
|
||||
|
||||
// ------------------------------------------------------------
|
||||
// 颜色
|
||||
// ------------------------------------------------------------
|
||||
|
||||
track_arrow.color.a = 1.0;
|
||||
track_arrow.color.r = 1.0;
|
||||
track_arrow.color.g = 1.0;
|
||||
track_arrow.color.b = 0.0;
|
||||
|
||||
track_arrow.pose.orientation.w = 1.0;
|
||||
|
||||
path_pub_->publish(track_arrow);
|
||||
}
|
||||
|
||||
|
||||
}
|
||||
void PathVisualizer::publishHistoryPathRviz(){
|
||||
history_x_.push_back(x_);
|
||||
@ -402,8 +476,6 @@ void PathVisualizer::publishHistoryPathRviz(){
|
||||
history_pose.color.r=0.0;
|
||||
history_pose.color.g=0.0;
|
||||
history_pose.color.b=1.0;
|
||||
double cos_yaw = std::cos(yaw_);
|
||||
double sin_yaw = std::sin(yaw_);
|
||||
for(size_t i=0;i<history_x_.size();i++)
|
||||
{
|
||||
geometry_msgs::msg::Point p;
|
||||
@ -415,62 +487,44 @@ void PathVisualizer::publishHistoryPathRviz(){
|
||||
}
|
||||
history_pose_pub_->publish(history_pose);
|
||||
}
|
||||
// void PathVisualizer::localozation_callback(const geometry_msgs::msg::Pose::SharedPtr msg)
|
||||
// {
|
||||
// double lat =msg->position.x;
|
||||
// double lon =msg->position.y;
|
||||
|
||||
// if(!origin_set_)
|
||||
// {
|
||||
// origin_lat_=lat;
|
||||
// origin_lon_=lon;
|
||||
// origin_yaw_ = quaternion_to_yaw(msg->orientation.x,
|
||||
// msg->orientation.y,
|
||||
// msg->orientation.z,
|
||||
// msg->orientation.w);
|
||||
// origin_set_=true;
|
||||
// RCLCPP_INFO(this->get_logger(),"GPS origin set lat: %f lon: %f",
|
||||
// origin_lat_,origin_lon_);
|
||||
// }
|
||||
// // 转换局部坐标
|
||||
// x_=gps_to_x(lat,lon);
|
||||
// y_=gps_to_y(lat,lon);
|
||||
|
||||
// yaw_=quaternion_to_yaw(msg->orientation.x,
|
||||
// msg->orientation.y,
|
||||
// msg->orientation.z,
|
||||
// msg->orientation.w
|
||||
// );
|
||||
// has_pose_=true;
|
||||
// // 发布 TF 与 path 可视化
|
||||
// publishVehicleTFRviz();
|
||||
// publishHistoryPathRviz();
|
||||
// if(origin_set_ && !path_transed){
|
||||
// transformPathToVehicleStart();
|
||||
// path_transed=true;
|
||||
// RCLCPP_INFO(
|
||||
// this->get_logger(),
|
||||
// "Path transformation completed.");
|
||||
// densifyPath(0.2);
|
||||
// publishPathRviz();
|
||||
// publishOdomPath();
|
||||
// }
|
||||
|
||||
// }
|
||||
void PathVisualizer::pathIndCallback(
|
||||
const std_msgs::msg::Int32::SharedPtr msg)
|
||||
{
|
||||
path_track_index_ = msg->data;
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Received auto path index: %d",
|
||||
path_track_index_);
|
||||
}
|
||||
void PathVisualizer::localozation_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
|
||||
{
|
||||
const auto& pose = msg->pose.pose;
|
||||
|
||||
double lat = pose.position.x;
|
||||
double lon = pose.position.y;
|
||||
if(!origin_set_)
|
||||
if (std::isnan(pose.position.x) ||std::isnan(pose.position.y))
|
||||
{
|
||||
return;
|
||||
}
|
||||
if(!origin_set_)
|
||||
{
|
||||
geodetic_converter_ =
|
||||
std::make_unique<GeodeticConverter>(
|
||||
lat,
|
||||
lon,
|
||||
0.0);
|
||||
origin_lat_=lat;
|
||||
origin_lon_=lon;
|
||||
origin_yaw_ = quaternion_to_yaw(pose.orientation.x,
|
||||
pose.orientation.y,
|
||||
pose.orientation.z,
|
||||
pose.orientation.w);
|
||||
if (std::abs(origin_yaw_) < 1e-4)
|
||||
{
|
||||
origin_yaw_ = 0.0;
|
||||
}
|
||||
origin_set_=true;
|
||||
RCLCPP_INFO(this->get_logger(),"GPS origin set lat: %f lon: %f",
|
||||
origin_lat_,origin_lon_);
|
||||
@ -487,13 +541,13 @@ void PathVisualizer::localozation_callback(const geometry_msgs::msg::PoseWithCov
|
||||
// 发布 TF 与 path 可视化
|
||||
publishVehicleTFRviz();
|
||||
publishHistoryPathRviz();
|
||||
if(origin_set_ && !path_transed){
|
||||
transformPathToVehicleStart();
|
||||
path_transed=true;
|
||||
RCLCPP_INFO(
|
||||
this->get_logger(),
|
||||
"Path transformation completed.");
|
||||
densifyPath(0.2);
|
||||
if(origin_set_){
|
||||
if(!path_transed){
|
||||
transformPathToVehicleStart(vehicle_initial_yaw_);
|
||||
interpolatePoints(max_interval_distance_);
|
||||
path_transed=true;
|
||||
RCLCPP_INFO(this->get_logger(),"Path transformation completed");
|
||||
}
|
||||
publishPathRviz();
|
||||
publishOdomPath();
|
||||
}
|
||||
@ -509,7 +563,6 @@ void PathVisualizer::control()
|
||||
return;
|
||||
}
|
||||
|
||||
publishVehiclePoseRviz();
|
||||
publishVehicleInfoRviz();
|
||||
publishLocalPose();
|
||||
target_index_= findClosestPoint(forward_index_limit_);
|
||||
@ -547,54 +600,54 @@ void PathVisualizer::control()
|
||||
|
||||
//cmd_pub_->publish(cmd);
|
||||
|
||||
RCLCPP_INFO_THROTTLE(
|
||||
this->get_logger(),
|
||||
*this->get_clock(),
|
||||
1000,
|
||||
"\n"
|
||||
"========== Pure Pursuit ==========\n"
|
||||
"Robot:\n"
|
||||
" x=%.3f y=%.3f yaw=%.3f\n"
|
||||
// RCLCPP_INFO_THROTTLE(
|
||||
// this->get_logger(),
|
||||
// *this->get_clock(),
|
||||
// 1000,
|
||||
// "\n"
|
||||
// "========== Pure Pursuit ==========\n"
|
||||
// "Robot:\n"
|
||||
// " x=%.3f y=%.3f yaw=%.3f\n"
|
||||
|
||||
"Target:\n"
|
||||
" index=%d\n"
|
||||
" x=%.3f y=%.3f\n,yaw=%.3f\n"
|
||||
// "Target:\n"
|
||||
// " index=%d\n"
|
||||
// " x=%.3f y=%.3f\n,yaw=%.3f\n"
|
||||
|
||||
"World error:\n"
|
||||
" dx=%.3f dy=%.3f\n"
|
||||
// "World error:\n"
|
||||
// " dx=%.3f dy=%.3f\n"
|
||||
|
||||
"Vehicle frame:\n"
|
||||
" local_x=%.3f local_y=%.3f\n"
|
||||
// "Vehicle frame:\n"
|
||||
// " local_x=%.3f local_y=%.3f\n"
|
||||
|
||||
"PP:\n"
|
||||
" LookAhead=%.3f\n"
|
||||
" curvature=%.5f\n"
|
||||
// "PP:\n"
|
||||
// " LookAhead=%.3f\n"
|
||||
// " curvature=%.5f\n"
|
||||
|
||||
"Command:\n"
|
||||
" v=%.3f w=%.3f",
|
||||
// "Command:\n"
|
||||
// " v=%.3f w=%.3f",
|
||||
|
||||
x_,
|
||||
y_,
|
||||
yaw_,
|
||||
// x_,
|
||||
// y_,
|
||||
// yaw_,
|
||||
|
||||
target,
|
||||
// target,
|
||||
|
||||
target_x,
|
||||
target_y,
|
||||
target_yaw,
|
||||
// target_x,
|
||||
// target_y,
|
||||
// target_yaw,
|
||||
|
||||
dx,
|
||||
dy,
|
||||
// dx,
|
||||
// dy,
|
||||
|
||||
local_x,
|
||||
local_y,
|
||||
// local_x,
|
||||
// local_y,
|
||||
|
||||
Ld,
|
||||
curvature,
|
||||
// Ld,
|
||||
// curvature,
|
||||
|
||||
cmd.linear.x,
|
||||
cmd.angular.z
|
||||
);
|
||||
// cmd.linear.x,
|
||||
// cmd.angular.z
|
||||
// );
|
||||
}
|
||||
|
||||
double PathVisualizer::normalize_angle(double angle)
|
||||
@ -615,16 +668,53 @@ double PathVisualizer::quaternion_to_yaw(double x,double y,double z,double w)
|
||||
|
||||
double PathVisualizer::gps_to_x(double lat,double lon)
|
||||
{
|
||||
double R=6378137.0;
|
||||
double dlon =(lon-origin_lon_)*M_PI/180.0;
|
||||
double lat_rad =origin_lat_*M_PI/180.0;
|
||||
return R*cos(lat_rad)*dlon;
|
||||
if (!geodetic_converter_)
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(),
|
||||
"GeodeticConverter is not initialized");
|
||||
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
double east = 0.0;
|
||||
double north = 0.0;
|
||||
double up = 0.0;
|
||||
|
||||
geodetic_converter_->geodetic2Enu(
|
||||
lat,
|
||||
lon,
|
||||
0.0,
|
||||
&east,
|
||||
&north,
|
||||
&up);
|
||||
|
||||
return east;
|
||||
}
|
||||
double PathVisualizer::gps_to_y(double lat,double lon)
|
||||
{
|
||||
double R=6378137.0;
|
||||
double dlat =(lat-origin_lat_)*M_PI/180.0;
|
||||
return R*dlat;
|
||||
if (!geodetic_converter_)
|
||||
{
|
||||
RCLCPP_WARN(
|
||||
this->get_logger(),
|
||||
"GeodeticConverter is not initialized");
|
||||
|
||||
return 0.0;
|
||||
}
|
||||
|
||||
double east = 0.0;
|
||||
double north = 0.0;
|
||||
double up = 0.0;
|
||||
|
||||
geodetic_converter_->geodetic2Enu(
|
||||
lat,
|
||||
lon,
|
||||
0.0,
|
||||
&east,
|
||||
&north,
|
||||
&up);
|
||||
|
||||
return north;
|
||||
}
|
||||
int PathVisualizer::findLookAheadPoint()
|
||||
{
|
||||
@ -661,21 +751,7 @@ int PathVisualizer::findClosestPoint(int forward_index_limit)
|
||||
}
|
||||
return index;
|
||||
}
|
||||
// void PathVisualizer::publishLocalPose()
|
||||
// {
|
||||
// geometry_msgs::msg::Pose local_pose;
|
||||
|
||||
// local_pose.position.x = x_;
|
||||
// local_pose.position.y = y_;
|
||||
|
||||
// tf2::Quaternion orientation;
|
||||
// orientation.setRPY(0, 0, yaw_);
|
||||
// local_pose.orientation.x = orientation.x();
|
||||
// local_pose.orientation.y = orientation.y();
|
||||
// local_pose.orientation.z = orientation.z();
|
||||
// local_pose.orientation.w = orientation.w();
|
||||
// local_pose_pub_->publish(local_pose);
|
||||
// }
|
||||
void PathVisualizer::publishLocalPose()
|
||||
{
|
||||
geometry_msgs::msg::PoseWithCovarianceStamped local_pose_msg;
|
||||
@ -732,111 +808,89 @@ void PathVisualizer::publishVehicleTFRviz()
|
||||
|
||||
tf_broadcaster_->sendTransform(transform);
|
||||
}
|
||||
|
||||
void PathVisualizer::densifyPath(double interval)
|
||||
void PathVisualizer::interpolatePoints(double max_interval_distance)
|
||||
{
|
||||
if (path_x_.size() < 2 ||
|
||||
path_x_.size() != path_y_.size() ||
|
||||
path_x_.size() != path_yaw_.size() ||
|
||||
interval <= 0.0)
|
||||
max_interval_distance <= 0.0)
|
||||
{
|
||||
return;
|
||||
}
|
||||
|
||||
// 保存原始路径
|
||||
const auto old_x = path_x_;
|
||||
const auto old_y = path_y_;
|
||||
const auto old_yaw = path_yaw_;
|
||||
|
||||
// 计算累计弧长
|
||||
std::vector<double> s(old_x.size(), 0.0);
|
||||
|
||||
for (size_t i = 1; i < old_x.size(); ++i)
|
||||
std::vector<std::vector<double>> result;
|
||||
// 保留第一个点
|
||||
result.push_back({path_x_[0],path_y_[0],path_yaw_[0]});
|
||||
for (size_t i = 0; i < path_x_.size() - 1; ++i)
|
||||
{
|
||||
double dx = old_x[i] - old_x[i - 1];
|
||||
double dy = old_y[i] - old_y[i - 1];
|
||||
// 当前点
|
||||
const std::vector<double> current = {
|
||||
path_x_[i],
|
||||
path_y_[i],
|
||||
path_yaw_[i]
|
||||
};
|
||||
|
||||
s[i] = s[i - 1] + std::hypot(dx, dy);
|
||||
// 下一个点
|
||||
const std::vector<double> next = {
|
||||
path_x_[i + 1],
|
||||
path_y_[i + 1],
|
||||
path_yaw_[i + 1]
|
||||
};
|
||||
double dx = next[0] - current[0];
|
||||
double dy = next[1] - current[1];
|
||||
double distance = std::sqrt(dx * dx + dy * dy);
|
||||
|
||||
if (distance > max_interval_distance)
|
||||
{
|
||||
int num_segments =
|
||||
static_cast<int>(
|
||||
std::ceil(distance / max_interval_distance));
|
||||
|
||||
for (int j = 1; j < num_segments; ++j)
|
||||
{
|
||||
double ratio =static_cast<double>(j) / num_segments;
|
||||
double x =current[0] + dx * ratio;
|
||||
double y =current[1] + dy * ratio;
|
||||
// yaw 插值
|
||||
double yaw1 = current[2];
|
||||
double yaw2 = next[2];
|
||||
double dyaw = yaw2 - yaw1;
|
||||
|
||||
// 处理 ±PI 跳变
|
||||
while (dyaw > M_PI)
|
||||
dyaw -= 2.0 * M_PI;
|
||||
|
||||
while (dyaw < -M_PI)
|
||||
dyaw += 2.0 * M_PI;
|
||||
|
||||
double yaw =
|
||||
yaw1 + dyaw * ratio;
|
||||
|
||||
// 归一化到 [-PI, PI]
|
||||
while (yaw > M_PI)
|
||||
yaw -= 2.0 * M_PI;
|
||||
|
||||
while (yaw < -M_PI)
|
||||
yaw += 2.0 * M_PI;
|
||||
result.push_back({x,y,yaw
|
||||
});
|
||||
}
|
||||
}
|
||||
// 保留原始的下一个点
|
||||
result.push_back(next);
|
||||
}
|
||||
|
||||
const double total_length = s.back();
|
||||
|
||||
if (total_length < 1e-6)
|
||||
return;
|
||||
|
||||
// 清空原路径，重新生成
|
||||
// 用插值后的结果替换原来的 Path
|
||||
path_x_.clear();
|
||||
path_y_.clear();
|
||||
path_yaw_.clear();
|
||||
|
||||
size_t segment = 0;
|
||||
|
||||
for (double target_s = 0.0;
|
||||
target_s < total_length;
|
||||
target_s += interval)
|
||||
for (const auto& point : result)
|
||||
{
|
||||
// 找到 target_s 所在的原始路径段
|
||||
while (segment + 1 < s.size() &&
|
||||
s[segment + 1] < target_s)
|
||||
{
|
||||
++segment;
|
||||
}
|
||||
|
||||
if (segment + 1 >= s.size())
|
||||
break;
|
||||
|
||||
double segment_length = s[segment + 1] - s[segment];
|
||||
|
||||
double ratio = 0.0;
|
||||
|
||||
if (segment_length > 1e-6)
|
||||
{
|
||||
ratio = (target_s - s[segment]) /
|
||||
segment_length;
|
||||
}
|
||||
|
||||
// x 插值
|
||||
double x = old_x[segment] +
|
||||
ratio * (old_x[segment + 1] -
|
||||
old_x[segment]);
|
||||
|
||||
// y 插值
|
||||
double y = old_y[segment] +
|
||||
ratio * (old_y[segment + 1] -
|
||||
old_y[segment]);
|
||||
|
||||
// yaw 插值
|
||||
double yaw1 = old_yaw[segment];
|
||||
double yaw2 = old_yaw[segment + 1];
|
||||
|
||||
double dyaw = yaw2 - yaw1;
|
||||
|
||||
// 处理 yaw 的 ±PI 跳变
|
||||
while (dyaw > M_PI)
|
||||
dyaw -= 2.0 * M_PI;
|
||||
|
||||
while (dyaw < -M_PI)
|
||||
dyaw += 2.0 * M_PI;
|
||||
|
||||
double yaw = yaw1 + ratio * dyaw;
|
||||
|
||||
// 归一化到 [-PI, PI]
|
||||
while (yaw > M_PI)
|
||||
yaw -= 2.0 * M_PI;
|
||||
|
||||
while (yaw < -M_PI)
|
||||
yaw += 2.0 * M_PI;
|
||||
|
||||
path_x_.push_back(x);
|
||||
path_y_.push_back(y);
|
||||
path_yaw_.push_back(yaw);
|
||||
path_x_.push_back(point[0]);
|
||||
path_y_.push_back(point[1]);
|
||||
path_yaw_.push_back(point[2]);
|
||||
}
|
||||
|
||||
// 确保最后一个原始点保留
|
||||
path_x_.push_back(old_x.back());
|
||||
path_y_.push_back(old_y.back());
|
||||
path_yaw_.push_back(old_yaw.back());
|
||||
}
|
||||
|
||||
void PathVisualizer::publishOdomPath(){
|
||||
nav_msgs::msg::Path path_msg;
|
||||
|
||||
|
||||
@ -43,8 +43,8 @@ UnevenTerrain {
|
||||
DirectionalLight {
|
||||
}
|
||||
DEF TRACKED_ROBOT Robot {
|
||||
translation 0.5632120679223307 2.4350350664219973 0.3818860621187393
|
||||
rotation 0.05410137548637035 -0.029849168918586494 0.9980892085807523 0.457
|
||||
translation 0.0 0.0 0.3818860621187393
|
||||
rotation 0.05410137548637035 -0.029849168918586494 0.9980892085807523 0.0
|
||||
|
||||
children [
|
||||
Group {
|
||||
|
||||
Loading…
Reference in New Issue
Block a user