zhujinzhao/auto_drive_misson_center_webots_simulation
1
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

transform path and optimize control algorithm

Browse Source
This commit is contained in:
liuhaoyan 2026-09-11 17:37:47 +08:00
parent 2dc7d7013d
commit fe65324a48
9 changed files with 110 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Readme.md
View File

@ -23,16 +23,16 @@ 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界面 左右两侧拉开留出视图空间。
`ros2 launch webots_simulation robot_launch.py` ，等待rviz2界面打开后，加载配置文件：` File->Open Config->` 选择 `SweetAiRviz.rviz` ，方便起见，打开后，File->Save Config 下次不用再重复加载。该 rviz界面 左右两侧拉开留出视图空间。(test:1701895544295370752.json)
测试版本中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内容填充到指令中：
另启一个终端，执行发送任务命令，这里的任务，必须和 `5` 中的匹配，将1701895544295370752.json内容填充到指令中：
```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: ''}"
ros2 service call /invoke/missionOn auto_drive_proj_msg/srv/ReqString "{request: '[{\"name\":\"value\",\"value\":\"https://test.sweetai.cn:4433/load/locationCmdPath/1701895544295370752.json\"}]', value: ''}"
```
> 观察任务轨迹以及rviz2右侧panel面板参数

BIN
rviz2.jpg
View File

Binary file not shown.
Side by Side Swipe Overlay

Before

Width:  |  Height:  |  Size: 1.3 MiB

After

Width:  |  Height:  |  Size: 2.1 MiB

2
src/auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp
View File

@ -85,7 +85,7 @@ private:
std::string& mission_data,
std::string& follow_data);
std::vector<std::vector<double>> interpolatePoints(const std::vector<std::vector<double>>& coordinates,
double max_distance = 0.5);
double max_distance);
// simulation only
std::vector<std::vector<double>> alignPathYaw( const std::vector<std::vector<double>>& coordinates,
double vehicle_initial_yaw);

4
src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp
View File

@ -185,7 +185,9 @@ protected:
// 保护成员变量，子类可以访问
double mission_speed_{0.0};
double turn_radius_{1.0};
double turn_radius_{0.3};
double max_allowed_speed_{1.0};
double wheel_dis_{0.95};
std::vector<PathPoint> path_points_;
int current_point_index_{0};
bool is_initialized_{false};

8
src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp
View File

@ -167,14 +167,16 @@ protected:
bool onInitialize() override {
float max_linear_vel = mission_speed_;
float max_angular_vel = mission_speed_ / turn_radius_;
float max_angular_vel_theory = max_allowed_speed_/wheel_dis_;
max_angular_vel = std::max(max_angular_vel, max_angular_vel_theory);
float kp = 1.76;
float ki = 0.0;
float kd = 0.8;
float look_ahead_dis = 0.4; // 前视距离，需要根据实际情况调整
float kv = 0.6; // 速度系数，需要根据实际情况调整
adapter_look_ahead_dis_ = look_ahead_dis;
float kv = 1.0; // 速度系数，需要根据实际情况调整
controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, look_ahead_dis, kv);
logger_->debug("PP para mission_speed_={}, turn_radius_={} frontdis={}", mission_speed_, turn_radius_,adapter_look_ahead_dis_ );
controller_->init(max_linear_vel, max_angular_vel, kp, ki, kd, adapter_look_ahead_dis_, kv);
return true;
}

1
src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp
View File

@ -33,6 +33,7 @@ public:
// std::cout<<"PurePursuit update!"<<std::endl;
logger_->trace("PurePursuit update!");
float w_val = (2 * fabs(linear_val) * sin(alpha) / (k_v * fabs(linear_val) + lookAheadDis));
//float w_val = (2 * fabs(linear_val) * sin(alpha) / (lookAheadDis));
w_val = fabs(w_val) > maxAngularVel ? (maxAngularVel * (w_val > 0.0 ? 1.0 : -1.0)) : w_val;
// std::cout << "linear_val: " << linear_val << ", w_val: " << w_val << std::endl;
logger_->trace("linear_val:{}, w_val:{}", linear_val, w_val);

2
src/auto_drive_misson_center/src/mission_center_node.cpp
View File

@ -849,7 +849,7 @@ bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data
interpolation_map_.clear();
std::vector<std::vector<double>> aligned_coords = alignPathYaw(coordinates, vehicle_initial_yaw_);
std::vector<std::vector<double>> interpolated_coords = interpolatePoints(aligned_coords, 0.5);
std::vector<std::vector<double>> interpolated_coords = interpolatePoints(aligned_coords, 0.2);
json processed_data = data;
json new_coordinates = json::array();

8
src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp
View File

@ -17,6 +17,7 @@
#include "rclcpp/rclcpp.hpp"
#include "nav_msgs/msg/odometry.hpp"
#include "visualization_msgs/msg/marker.hpp"
#include <visualization_msgs/msg/marker_array.hpp>
#include <nav_msgs/msg/path.hpp>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>
@ -64,8 +65,10 @@ private:
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 origin_path_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr history_pose_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_info_pub_;
rclcpp::Publisher<visualization_msgs::msg::MarkerArray>::SharedPtr path_text_pub_;
// topic sub
// rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr odom_sub_;
@ -85,6 +88,9 @@ private:
std::vector<double> history_x_;
std::vector<double> history_y_;
std::vector<double> history_yaw_;
// origin path info
std::vector<double> origin_x_;
std::vector<double> origin_y_;
int target_index_;
// localization info
@ -96,7 +102,7 @@ private:
double max_speed_ = 0.8;
double max_yaw_rate_ = 2.0;
double lookahead_distance_ =0.6;
double max_interval_distance_ = 0.5;
double max_interval_distance_ = 0.2;
double origin_lat_;
double origin_lon_;
double origin_yaw_;

139
src/webots_simulation/src/PathVisualizer.cpp
View File

@ -14,9 +14,11 @@ has_pose_(false)
local_pose_pub_ = this->create_publisher<geometry_msgs::msg::PoseWithCovarianceStamped>("/local_pose",10);
odom_path_pub_ = this->create_publisher<nav_msgs::msg::Path>("/odom_path",10);
path_pub_ =this->create_publisher<visualization_msgs::msg::Marker>("/rviz_planning_path",10);
origin_path_pub_ =this->create_publisher<visualization_msgs::msg::Marker>("/rviz_origin_path",10);
vehicle_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/rviz_vehicle_pose",10);
history_pose_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/rviz_history_traj",10);
vehicle_info_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/rviz_text_info",10);
path_text_pub_ = this->create_publisher<visualization_msgs::msg::MarkerArray>("/path_index_info", 10);
// sub
RCLCPP_INFO(this->get_logger(),"Start receving rtk pose data");
@ -97,29 +99,13 @@ void PathVisualizer::transformPathToVehicleStart(double vehicle_initial_yaw)
for (int i = 0; i < path_x_.size(); ++i)
{
// ------------------------------------------------
// 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 x = cos_offset * dx - sin_offset * dy;
const double y = sin_offset * dx + cos_offset * dy;
double yaw = path_yaw_[i] + yaw_offset;
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;
@ -128,6 +114,8 @@ void PathVisualizer::transformPathToVehicleStart(double vehicle_initial_yaw)
path_x_[i] = x;
path_y_[i] = y;
origin_x_.push_back(x);
origin_y_.push_back(y);
path_yaw_[i] = yaw;
RCLCPP_INFO(this->get_logger(),"Transformed path point %d: x = %.2f, y = %.2f, yaw = %.2f",
i, x, y, yaw);
@ -303,9 +291,9 @@ void PathVisualizer::publishPathRviz()
path.type=visualization_msgs::msg::Marker::SPHERE_LIST;
path.action=visualization_msgs::msg::Marker::ADD;
path.scale.x = 0.22;
path.scale.y = 0.22;
path.scale.z = 0.22;
path.scale.x = 0.17;
path.scale.y = 0.17;
path.scale.z = 0.17;
path.color.a=1.0;
path.color.r=1.0;
path.color.g=0.0;
@ -333,7 +321,7 @@ void PathVisualizer::publishPathRviz()
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.scale.x = 0.08;
path_line.color.a = 1.0;
path_line.color.r = 1.0;
@ -350,9 +338,49 @@ void PathVisualizer::publishPathRviz()
}
path_pub_->publish(path_line);
// 2. current tracking path point
if (path_track_index_ > 0 &&
path_track_index_ < static_cast<int>(path_x_.size()))
// 2. path index text
visualization_msgs::msg::MarkerArray text_array;
for (size_t i = 0; i < path_x_.size(); i++)
{
visualization_msgs::msg::Marker text;
text.header.frame_id = "odom";
text.header.stamp = this->now();
text.ns = "path_index";
text.id = static_cast<int>(i);
text.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING;
text.action = visualization_msgs::msg::Marker::ADD;
// 文字位置
text.pose.position.x = path_x_[i] + 0.1;
text.pose.position.y = path_y_[i];
text.pose.position.z = 0.30;
// TEXT_VIEW_FACING 不需要旋转
text.pose.orientation.w = 1.0;
// 字体大小
text.scale.z = 0.12;
// 文字颜色
text.color.a = 1.0;
text.color.r = 1.0;
text.color.g = 0.0;
text.color.b = 0.0;
// 显示 path index
text.text = std::to_string(i);
text_array.markers.push_back(text);
}
path_text_pub_->publish(text_array);
// 3. current tracking path point
if (path_track_index_ > 0)
{
visualization_msgs::msg::Marker track_point;
@ -379,6 +407,8 @@ void PathVisualizer::publishPathRviz()
track_point.color.g = 0.0;
track_point.color.b = 1.0;
if (path_track_index_ >= path_x_.size())
path_track_index_ = path_x_.size()-1;
track_point.pose.position.x = path_x_[path_track_index_];
track_point.pose.position.y = path_y_[path_track_index_];
@ -429,22 +459,10 @@ void PathVisualizer::publishPathRviz()
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;
@ -454,8 +472,35 @@ void PathVisualizer::publishPathRviz()
path_pub_->publish(track_arrow);
}
// 4. origin path position
visualization_msgs::msg::Marker origin_path;
origin_path.header.frame_id="odom";
origin_path.header.stamp=this->now();
origin_path.ns="origin_path";
origin_path.id=0;
origin_path.type=visualization_msgs::msg::Marker::SPHERE_LIST;
origin_path.action=visualization_msgs::msg::Marker::ADD;
origin_path.scale.x = 0.17;
origin_path.scale.y = 0.17;
origin_path.scale.z = 0.17;
origin_path.color.a=1.0;
origin_path.color.r=1.0;
origin_path.color.g=1.0;
origin_path.color.b=0.0;
for(size_t i=0;i<origin_x_.size();i++)
{
geometry_msgs::msg::Point p;
p.x=origin_x_[i];
p.y=origin_y_[i];
p.z=0.15;
origin_path.points.push_back(p);
}
origin_path_pub_->publish(origin_path);
}
void PathVisualizer::publishHistoryPathRviz(){
history_x_.push_back(x_);
@ -487,19 +532,14 @@ void PathVisualizer::publishHistoryPathRviz(){
}
history_pose_pub_->publish(history_pose);
}
void PathVisualizer::pathIndCallback(
const std_msgs::msg::Int32::SharedPtr msg)
{
path_track_index_ = msg->data;
void PathVisualizer::pathIndCallback(const std_msgs::msg::Int32::SharedPtr msg){
path_track_index_ = msg->data + 2;
RCLCPP_INFO(
this->get_logger(),
"Received auto path index: %d",
path_track_index_);
}
void PathVisualizer::localozation_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg)
{
void PathVisualizer::localozation_callback(const geometry_msgs::msg::PoseWithCovarianceStamped::SharedPtr msg){
const auto& pose = msg->pose.pose;
double lat = pose.position.x;
@ -553,8 +593,7 @@ void PathVisualizer::localozation_callback(const geometry_msgs::msg::PoseWithCov
}
}
void PathVisualizer::control()
{
void PathVisualizer::control(){
if(!has_pose_)
{
RCLCPP_WARN(
@ -927,7 +966,7 @@ void PathVisualizer::publishOdomPath(){
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
std::string file_name;
file_name = "1700080420079468544.json";
file_name = "1701895544295370752.json";
// std::cout << "请输入路径文件路径: ";
// std::getline(std::cin, file_name);