diff --git a/README.md b/README.md deleted file mode 100644 index 9261a52..0000000 --- a/README.md +++ /dev/null @@ -1,2 +0,0 @@ -auto drive misson center webots simulation: - diff --git a/Readme.md b/Readme.md new file mode 100644 index 0000000..12adb87 --- /dev/null +++ b/Readme.md @@ -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： + ![界面示意图](./rviz2.jpg) \ No newline at end of file diff --git a/rviz2.jpg b/rviz2.jpg new file mode 100644 index 0000000..8d0374b Binary files /dev/null and b/rviz2.jpg differ diff --git a/src/auto_drive_misson_center/CMakeLists.txt b/src/auto_drive_misson_center/CMakeLists.txt index e8e9356..e2ae54e 100644 --- a/src/auto_drive_misson_center/CMakeLists.txt +++ b/src/auto_drive_misson_center/CMakeLists.txt @@ -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 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 5328124..1bd1af1 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 @@ -55,7 +55,6 @@ private: rclcpp::Publisher::SharedPtr event_warn_pub_; rclcpp::Publisher::SharedPtr event_error_pub_; rclcpp::Publisher::SharedPtr properties_report_pub_; - std::vector> interpolation_map_; std::shared_future mission_future_; @@ -87,6 +86,10 @@ private: std::string& follow_data); std::vector> interpolatePoints(const std::vector>& coordinates, double max_distance = 0.5); + // simulation only + std::vector> alignPathYaw( const std::vector>& coordinates, + double vehicle_initial_yaw); + int updatePathIndices(const int original_ind, const std::vector>& 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 diff --git a/src/auto_drive_misson_center/package.xml b/src/auto_drive_misson_center/package.xml index d0e4348..2aad01e 100644 --- a/src/auto_drive_misson_center/package.xml +++ b/src/auto_drive_misson_center/package.xml @@ -12,6 +12,7 @@ rclcpp std_msgs std_srvs + visualization_msgs auto_drive_proj_utils auto_drive_proj_msg libcurl-dev 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 8dfbf17..eda8f64 100644 --- a/src/auto_drive_misson_center/src/mission_center_node.cpp +++ b/src/auto_drive_misson_center/src/mission_center_node.cpp @@ -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> MissionCenterNode::alignPathYaw( + const std::vector>& coordinates, + double vehicle_initial_yaw) +{ + std::vector> 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> interpolated_coords = interpolatePoints(coordinates, 0.5); + std::vector> aligned_coords = alignPathYaw(coordinates, vehicle_initial_yaw_); + std::vector> interpolated_coords = interpolatePoints(aligned_coords, 0.5); + json processed_data = data; json new_coordinates = json::array(); for (const auto& coord : interpolated_coords) { diff --git a/src/robots_rviz_panel/include/sweet_rviz_panel/sweet_rviz_panel.hpp b/src/robots_rviz_panel/include/sweet_rviz_panel/sweet_rviz_panel.hpp index 37fbd0d..9be46ab 100644 --- a/src/robots_rviz_panel/include/sweet_rviz_panel/sweet_rviz_panel.hpp +++ b/src/robots_rviz_panel/include/sweet_rviz_panel/sweet_rviz_panel.hpp @@ -15,6 +15,7 @@ #include #include "geometry_msgs/msg/twist.hpp" #include "geometry_msgs/msg/pose.hpp" +#include #include #include #include @@ -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::SharedPtr pose_sub_; + rclcpp::Subscription::SharedPtr pose_sub_; rclcpp::Subscription::SharedPtr twist_cmd_sub_; rclcpp::Subscription::SharedPtr odom_path_sub_; double current_x_ = 0.0; diff --git a/src/robots_rviz_panel/src/sweet_rviz_panel.cpp b/src/robots_rviz_panel/src/sweet_rviz_panel.cpp index 67b2a7f..aa6a72e 100644 --- a/src/robots_rviz_panel/src/sweet_rviz_panel.cpp +++ b/src/robots_rviz_panel/src/sweet_rviz_panel.cpp @@ -146,7 +146,7 @@ void SweetRvizPanel::onInitialize() } auto node = node_ptr_->get_raw_node(); - pose_sub_ = node->create_subscription( + pose_sub_ = node->create_subscription( "/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( diff --git a/src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp b/src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp index cd1e32e..fbb3c09 100644 --- a/src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp +++ b/src/webots_simulation/include/sweet_webots_robot/PathVisualizer.hpp @@ -5,6 +5,7 @@ #include //#include #include +#include #include #include #include @@ -19,6 +20,7 @@ #include #include #include +#include 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::SharedPtr odom_sub_; rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Subscription::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 geodetic_converter_; }; #endif diff --git a/src/webots_simulation/launch/robot_launch.py b/src/webots_simulation/launch/robot_launch.py index 5cf2da4..de538d8 100644 --- a/src/webots_simulation/launch/robot_launch.py +++ b/src/webots_simulation/launch/robot_launch.py @@ -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, diff --git a/src/webots_simulation/src/PathVisualizer.cpp b/src/webots_simulation/src/PathVisualizer.cpp index 4392d7a..e303bab 100644 --- a/src/webots_simulation/src/PathVisualizer.cpp +++ b/src/webots_simulation/src/PathVisualizer.cpp @@ -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( + "/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 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(); - const double lon = point[1].get(); - const double yaw =point[2].get(); - // 局部 坐标转换 - 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(point[0], point[1], 0.0); + data["properties"]["start_lat"] = point[0].get(); + data["properties"]["start_lon"] = point[1].get(); + data["properties"]["start_yaw"] = point.size() > 2 ? point[2].get() : 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() : 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;ipublish(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;ipublish(path_line); + + // 2. current tracking path point + if (path_track_index_ > 0 && + path_track_index_ < static_cast(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;ipublish(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( + 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 s(old_x.size(), 0.0); - - for (size_t i = 1; i < old_x.size(); ++i) + std::vector> 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 current = { + path_x_[i], + path_y_[i], + path_yaw_[i] + }; - s[i] = s[i - 1] + std::hypot(dx, dy); + // 下一个点 + const std::vector 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( + std::ceil(distance / max_interval_distance)); + + for (int j = 1; j < num_segments; ++j) + { + double ratio =static_cast(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; diff --git a/src/webots_simulation/worlds/sweet_robot.wbt b/src/webots_simulation/worlds/sweet_robot.wbt index 2e05ba7..ee84978 100644 --- a/src/webots_simulation/worlds/sweet_robot.wbt +++ b/src/webots_simulation/worlds/sweet_robot.wbt @@ -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 {