这是我的测试函数：#include <memory>
#include <chrono>
#include <vector>
#include "rclcpp/rclcpp.hpp"
#include "kx_topic/msg/system_state.hpp"
#include "kx_topic/msg/planning_control.hpp"
#include "kx_topic/msg/pose.hpp"
#include "kx_topic/msg/localization_state.hpp"
#include "kx_topic/msg/way_points.hpp"
#include "kx_topic/msg/perception_state.hpp"
#include "kx_topic/msg/rigid_velocity.hpp"
#include "kx_topic/msg/driving_state.hpp"
#include "kx_topic/msg/planning_state.hpp"
#include <kx_common/transform/kxtransform.h>
#include <kx_common/system/topic_attr_define.h>
#include "geometry_msgs/msg/pose_array.hpp"
#include "visualization_msgs/msg/marker_array.hpp"
#include "tf2_ros/transform_broadcaster.h"
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
class TestPlanningNode : public rclcpp::Node {
public:
TestPlanningNode() : Node("test_planning_node") {
// 初始化变量
current_pose_.x = 0.0;
current_pose_.y = 0.0;
current_pose_.yaw = 0.0;
current_velocity_.vx = 0.5; // 初始速度0.5 m/s
// 创建发布器
system_state_pub_ = create_publisher<kx_topic::msg::SystemState>(
"system_state", 10);
planning_control_pub_ = create_publisher<kx_topic::msg::PlanningControl>(
"planning_control", 10);
pose_fusion_pub_ = create_publisher<kx_topic::msg::Pose>(
"pose_fusion", 10);
localization_state_pub_ = create_publisher<kx_topic::msg::LocalizationState>(
"localization_state", 10);
perception_state_pub_ = create_publisher<kx_topic::msg::PerceptionState>(
"perception_state", 10);
chassis_velocity_pub_ = create_publisher<kx_topic::msg::RigidVelocity>(
"chassis_velocity", 10);
driving_state_pub_ = create_publisher<kx_topic::msg::DrivingState>(
"driving_control", 10);
// 创建订阅器（用于可视化）
waypoints_sub_ = create_subscription<kx_topic::msg::WayPoints>(
"waypoints", 10,
std::bind(&TestPlanningNode::waypointsCallback, this, std::placeholders::_1));
planning_state_sub_ = create_subscription<kx_topic::msg::PlanningState>(
"planning_state", 10,
std::bind(&TestPlanningNode::planningStateCallback, this, std::placeholders::_1));
// RViz可视化发布器 - 确保使用正确的命名空间和ID
path_viz_pub_ = create_publisher<visualization_msgs::msg::Marker>(
"path_viz", 10);
waypoints_viz_pub_ = create_publisher<visualization_msgs::msg::Marker>(
"waypoints_viz", 10);
obstacle_viz_pub_ = create_publisher<visualization_msgs::msg::Marker>(
"obstacle_viz", 10);
car_viz_pub_ = create_publisher<visualization_msgs::msg::Marker>(
"car_viz", 10);
// 创建50米长的测试路径（直线）
createTestPath();
// 创建定时器发布测试数据
publish_timer_ = create_wall_timer(
std::chrono::milliseconds(100),
std::bind(&TestPlanningNode::publishTestData, this));
// 创建定时器更新车辆位置
update_timer_ = create_wall_timer(
std::chrono::milliseconds(200),
std::bind(&TestPlanningNode::updateVehiclePosition, this));
// 创建定时器用于可视化
viz_timer_ = create_wall_timer(
std::chrono::milliseconds(500),
std::bind(&TestPlanningNode::publishVisualization, this));
// 初始化可视化标记
initMarkers();
}
private:
void initMarkers() {
// 初始化路径标记
path_marker_.header.frame_id = "map";
path_marker_.ns = "test_path";
path_marker_.id = 1;
path_marker_.type = visualization_msgs::msg::Marker::LINE_STRIP;
path_marker_.action = visualization_msgs::msg::Marker::ADD;
path_marker_.scale.x = 0.1;
path_marker_.color.r = 0.0;
path_marker_.color.g = 1.0;
path_marker_.color.b = 0.0;
path_marker_.color.a = 1.0;
// 初始化路径点标记
waypoints_marker_.header.frame_id = "map";
waypoints_marker_.ns = "planning_waypoints";
waypoints_marker_.id = 2;
waypoints_marker_.type = visualization_msgs::msg::Marker::SPHERE_LIST;
waypoints_marker_.action = visualization_msgs::msg::Marker::ADD;
waypoints_marker_.scale.x = 0.3;
waypoints_marker_.scale.y = 0.3;
waypoints_marker_.scale.z = 0.3;
waypoints_marker_.color.r = 1.0;
waypoints_marker_.color.g = 0.0;
waypoints_marker_.color.b = 0.0;
waypoints_marker_.color.a = 1.0;
// 初始化障碍物标记
obstacle_marker_.header.frame_id = "map";
obstacle_marker_.ns = "obstacle";
obstacle_marker_.id = 3;
obstacle_marker_.type = visualization_msgs::msg::Marker::CUBE;
obstacle_marker_.action = visualization_msgs::msg::Marker::ADD;
obstacle_marker_.scale.x = 1.0;
obstacle_marker_.scale.y = 1.0;
obstacle_marker_.scale.z = 0.5;
obstacle_marker_.color.r = 1.0;
obstacle_marker_.color.g = 0.0;
obstacle_marker_.color.b = 0.0;
obstacle_marker_.color.a = 1.0;
// 初始化车辆标记
car_marker_.header.frame_id = "map";
car_marker_.ns = "vehicle";
car_marker_.id = 4;
car_marker_.type = visualization_msgs::msg::Marker::CUBE; // 使用立方体代替模型
car_marker_.action = visualization_msgs::msg::Marker::ADD;
car_marker_.scale.x = 2.0; // 车长
car_marker_.scale.y = 1.0; // 车宽
car_marker_.scale.z = 0.5; // 车高
car_marker_.color.r = 0.0;
car_marker_.color.g = 0.0;
car_marker_.color.b = 1.0;
car_marker_.color.a = 0.8;
}
void createTestPath() {
// 创建一条50米长的直线路径
kx_topic::msg::PlanningBezier3Route route;
// 起点
route.p0.x = 0.0;
route.p0.y = 0.0;
route.p1.x = 10.0;
route.p1.y = 0.0;
route.p2.x = 40.0;
route.p2.y = 0.0;
route.p3.x = 50.0;
route.p3.y = 0.0;
route.limit_speed = 1.0; // 限速1.0 m/s
route.tracking_precision = kx_topic::msg::PlanningBezier3Route::TRACKING_PRECISION_NORMAL;
route.stopping_precision = kx_topic::msg::PlanningBezier3Route::STOPPING_PRECISION_NORMAL;
route.avoid_left_enable = true;
route.avoid_right_enable = true;
// 添加到控制消息
planning_control_msg_.control_type = kx_topic::msg::PlanningControl::CONTROL_TYPE_TRACK_BEZIER3;
planning_control_msg_.track_bezier3_data.bezier3_route.push_back(route);
planning_control_msg_.track_bezier3_data.bezier3_route_current_index = 0;
// 设置障碍物在25米处
obstacle_position_.x = 25.0;
obstacle_position_.y = 0.0;
// 创建路径可视化点
for (double x = 0; x <= 50; x += 1.0) {
geometry_msgs::msg::Point p;
p.x = x;
p.y = 0.0;
p.z = 0.0;
path_marker_.points.push_back(p);
}
}
void publishTestData() {
// 发布系统状态
auto system_state_msg = kx_topic::msg::SystemState();
system_state_msg.state = 1; // AUTO状态
system_state_pub_->publish(system_state_msg);
// 发布规划控制
planning_control_pub_->publish(planning_control_msg_);
// 发布定位状态
auto localization_state_msg = kx_topic::msg::LocalizationState();
localization_state_msg.map_id = 1; // 有效地图ID
localization_state_pub_->publish(localization_state_msg);
// 发布感知状态（包含障碍物）
auto perception_state_msg = kx_topic::msg::PerceptionState();
// 添加障碍物
kx_topic::msg::Obstacle2d obstacle;
obstacle.center_x = obstacle_position_.x;
obstacle.center_y = obstacle_position_.y;
obstacle.center_z = 0.0;
obstacle.movable = false;
// 创建障碍物多边形（正方形）
geometry_msgs::msg::Point32 p;
p.x = obstacle_position_.x - 0.5;
p.y = obstacle_position_.y - 0.5;
obstacle.polygon.points.push_back(p);
p.x = obstacle_position_.x + 0.5;
p.y = obstacle_position_.y - 0.5;
obstacle.polygon.points.push_back(p);
p.x = obstacle_position_.x + 0.5;
p.y = obstacle_position_.y + 0.5;
obstacle.polygon.points.push_back(p);
p.x = obstacle_position_.x - 0.5;
p.y = obstacle_position_.y + 0.5;
obstacle.polygon.points.push_back(p);
perception_state_msg.obstacles.push_back(obstacle);
perception_state_pub_->publish(perception_state_msg);
// 发布底盘速度
chassis_velocity_pub_->publish(current_velocity_);
// 发布驾驶状态
auto driving_state_msg = kx_topic::msg::DrivingState();
driving_state_pub_->publish(driving_state_msg);
}
void updateVehiclePosition() {
// 根据速度更新车辆位置
double dt = 0.2; // 200ms
current_pose_.x += current_velocity_.vx * dt * cos(current_pose_.yaw);
current_pose_.y += current_velocity_.vx * dt * sin(current_pose_.yaw);
// 发布车辆位姿
auto pose_msg = kx_topic::msg::Pose();
pose_msg.x = current_pose_.x;
pose_msg.y = current_pose_.y;
pose_msg.z = 0.0;
pose_msg.roll = 0.0;
pose_msg.pitch = 0.0;
pose_msg.yaw = current_pose_.yaw;
pose_fusion_pub_->publish(pose_msg);
}
void waypointsCallback(const kx_topic::msg::WayPoints::SharedPtr msg) {
// 存储最新的路径点用于可视化
latest_waypoints_ = *msg;
}
void planningStateCallback(const kx_topic::msg::PlanningState::SharedPtr msg) {
// 打印规划状态信息
RCLCPP_INFO(get_logger(), "Planning State: control_type=%d, sub_state=%d",
msg->current_control_type, msg->track_bezier3_data.sub_state);
}
void publishVisualization() {
auto now = this->now();
// 更新并发布路径标记
path_marker_.header.stamp = now;
path_viz_pub_->publish(path_marker_);
// 更新并发布路径点标记
waypoints_marker_.header.stamp = now;
waypoints_marker_.points.clear();
if (latest_waypoints_.waypoints.size() > 0) {
for (const auto& wp : latest_waypoints_.waypoints) {
geometry_msgs::msg::Point p;
p.x = wp.x;
p.y = wp.y;
p.z = 0.0;
waypoints_marker_.points.push_back(p);
}
}
waypoints_viz_pub_->publish(waypoints_marker_);
// 更新并发布障碍物标记
obstacle_marker_.header.stamp = now;
obstacle_marker_.pose.position.x = obstacle_position_.x;
obstacle_marker_.pose.position.y = obstacle_position_.y;
obstacle_marker_.pose.position.z = 0.0;
obstacle_viz_pub_->publish(obstacle_marker_);
// 更新并发布车辆标记
car_marker_.header.stamp = now;
car_marker_.pose.position.x = current_pose_.x;
car_marker_.pose.position.y = current_pose_.y;
car_marker_.pose.position.z = 0.0;
// 设置车辆方向
tf2::Quaternion quat;
quat.setRPY(0, 0, current_pose_.yaw);
car_marker_.pose.orientation = tf2::toMsg(quat);
car_viz_pub_->publish(car_marker_);
}
// 变量声明
struct Pose {
double x;
double y;
double yaw;
};
Pose current_pose_;
kx_topic::msg::RigidVelocity current_velocity_;
kx_topic::msg::PlanningControl planning_control_msg_;
kx_topic::msg::WayPoints latest_waypoints_;
geometry_msgs::msg::Point obstacle_position_;
// 可视化标记
visualization_msgs::msg::Marker path_marker_;
visualization_msgs::msg::Marker waypoints_marker_;
visualization_msgs::msg::Marker obstacle_marker_;
visualization_msgs::msg::Marker car_marker_;
// 发布器
rclcpp::Publisher<kx_topic::msg::SystemState>::SharedPtr system_state_pub_;
rclcpp::Publisher<kx_topic::msg::PlanningControl>::SharedPtr planning_control_pub_;
rclcpp::Publisher<kx_topic::msg::Pose>::SharedPtr pose_fusion_pub_;
rclcpp::Publisher<kx_topic::msg::LocalizationState>::SharedPtr localization_state_pub_;
rclcpp::Publisher<kx_topic::msg::PerceptionState>::SharedPtr perception_state_pub_;
rclcpp::Publisher<kx_topic::msg::RigidVelocity>::SharedPtr chassis_velocity_pub_;
rclcpp::Publisher<kx_topic::msg::DrivingState>::SharedPtr driving_state_pub_;
// 订阅器
rclcpp::Subscription<kx_topic::msg::WayPoints>::SharedPtr waypoints_sub_;
rclcpp::Subscription<kx_topic::msg::PlanningState>::SharedPtr planning_state_sub_;
// 可视化发布器
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr path_viz_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr waypoints_viz_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr obstacle_viz_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr car_viz_pub_;
// 定时器
rclcpp::TimerBase::SharedPtr publish_timer_;
rclcpp::TimerBase::SharedPtr update_timer_;
rclcpp::TimerBase::SharedPtr viz_timer_;
};
int main(int argc, char** argv) {
rclcpp::init(argc, argv);
auto node = std::make_shared<TestPlanningNode>();
rclcpp::spin(node);
rclcpp::shutdown();
return 0;
}但是运行planning之后再运行这个测试节点会显示和planning终端里一样的内容即test_planning_node终端内容：[INFO] [1753150661.203834771] [kx_planning]: localization map_id=-1<0, now allow move
[INFO] [1753150662.223568424] [kx_planning]: running. real interval: 19.913 ms, calc: 0.008 ms, mode=0(idle)
[INFO] [1753150662.223612171] [kx_planning]: localization map_id=-1<0, now allow move
[INFO] [1753150663.244129907] [kx_planning]: running. real interval: 21.011 ms, calc: 0.008 ms, mode=0(idle)
等，planning终端里的内容：[INFO] [1753150639.991114081] [kx_planning]: running. real interval: 19.843 ms, calc: 0.022 ms, mode=0(idle)
[INFO] [1753150639.991145309] [kx_planning]: localization map_id=-1<0, now allow move
[INFO] [1753150641.011186408] [kx_planning]: running. real interval: 20.794 ms, calc: 0.009 ms, mode=0(idle)
[INFO] [1753150641.011215057] [kx_planning]: localization map_id=-1<0, now allow move
[INFO] [1753150642.011464393] [kx_planning]: running. real interval: 19.955 ms, calc: 0.018 ms, mode=0(idle)
[INFO] [1753150642.011516071] [kx_planning]: localization map_id=-1<0, now allow move
等。而且topic list内容如下/chassis_velocity
/driving_control
/error
/feedback_waypoints
/localization_state
/parameter_events
/perception_state
/planning_control
/planning_debug_all_points
/planning_debug_current_point
/planning_debug_current_pose
/planning_debug_response_car_limit_polygon
/planning_debug_response_car_polygon
/planning_debug_response_car_pose
/planning_debug_response_car_stop_polygon
/planning_debug_response_obstacle_point
/planning_state
/pose_fusion
/rosout
/system_state
/waypoints
没有我发布的path_viz等话题，请问怎么回事？
最新发布
本文详细介绍了ROS中常用的几何消息类型，包括点、多边形、位姿等，并解释了它们的应用场景，如点云处理、位姿估计等。
PoseArray 
扫一扫
1168
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
