diff --git a/Readme.md b/Readme.md index 12adb87..16c2ea5 100644 --- a/Readme.md +++ b/Readme.md @@ -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面板参数 diff --git a/rviz2.jpg b/rviz2.jpg index 8d0374b..961eac3 100644 Binary files a/rviz2.jpg and b/rviz2.jpg differ diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp index 1bd1af1..c761e5e 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/mission_center_node.hpp @@ -85,7 +85,7 @@ private: std::string& mission_data, std::string& follow_data); std::vector> interpolatePoints(const std::vector>& coordinates, - double max_distance = 0.5); + double max_distance); // simulation only std::vector> alignPathYaw( const std::vector>& coordinates, double vehicle_initial_yaw); diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp index c1abbb9..94df307 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller.hpp @@ -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 path_points_; int current_point_index_{0}; bool is_initialized_{false}; diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp index 7b653a1..136feb9 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/path_tracking_controller_adapter.hpp @@ -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; } diff --git a/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp b/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp index bc61976..8ca8817 100644 --- a/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp +++ b/src/auto_drive_misson_center/include/auto_drive_misson_center/pure_pursuit_control.hpp @@ -33,6 +33,7 @@ public: // std::cout<<"PurePursuit update!"<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); diff --git a/src/auto_drive_misson_center/src/mission_center_node.cpp b/src/auto_drive_misson_center/src/mission_center_node.cpp index eda8f64..092866b 100644 --- a/src/auto_drive_misson_center/src/mission_center_node.cpp +++ b/src/auto_drive_misson_center/src/mission_center_node.cpp @@ -849,7 +849,7 @@ bool MissionCenterNode::processDownloadedData(const std::string& downloaded_data interpolation_map_.clear(); std::vector> aligned_coords = alignPathYaw(coordinates, vehicle_initial_yaw_); - std::vector> interpolated_coords = interpolatePoints(aligned_coords, 0.5); + std::vector> interpolated_coords = interpolatePoints(aligned_coords, 0.2); json processed_data = data; json new_coordinates = json::array(); diff --git a/src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp b/src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp index fbb3c09..d60a864 100644 --- a/src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp +++ b/src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp @@ -17,6 +17,7 @@ #include "rclcpp/rclcpp.hpp" #include "nav_msgs/msg/odometry.hpp" #include "visualization_msgs/msg/marker.hpp" +#include #include #include #include @@ -64,8 +65,10 @@ private: rclcpp::Publisher::SharedPtr odom_path_pub_; rclcpp::Publisher::SharedPtr vehicle_pub_; rclcpp::Publisher::SharedPtr path_pub_; + rclcpp::Publisher::SharedPtr origin_path_pub_; rclcpp::Publisher::SharedPtr history_pose_pub_; rclcpp::Publisher::SharedPtr vehicle_info_pub_; + rclcpp::Publisher::SharedPtr path_text_pub_; // topic sub // rclcpp::Subscription::SharedPtr odom_sub_; @@ -85,6 +88,9 @@ private: std::vector history_x_; std::vector history_y_; std::vector history_yaw_; + // origin path info + std::vector origin_x_; + std::vector 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_; diff --git a/src/webots_simulation/src/PathVisualizer.cpp b/src/webots_simulation/src/PathVisualizer.cpp index e303bab..c9e240b 100644 --- a/src/webots_simulation/src/PathVisualizer.cpp +++ b/src/webots_simulation/src/PathVisualizer.cpp @@ -14,9 +14,11 @@ has_pose_(false) local_pose_pub_ = this->create_publisher("/local_pose",10); odom_path_pub_ = this->create_publisher("/odom_path",10); path_pub_ =this->create_publisher("/rviz_planning_path",10); + origin_path_pub_ =this->create_publisher("/rviz_origin_path",10); vehicle_pub_ = this->create_publisher("/rviz_vehicle_pose",10); history_pose_pub_ = this->create_publisher("/rviz_history_traj",10); vehicle_info_pub_ = this->create_publisher("/rviz_text_info",10); + path_text_pub_ = this->create_publisher("/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(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(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;ipublish(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);