liuhaoyan
/
webots_simulation
1
0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

publish local pose topic

This commit is contained in:
liuhaoyan 2026-08-24 18:06:35 +08:00
parent c5f2a11066
commit 855cf99c28
2 changed files with 116 additions and 62 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
include/sweet_webots_robot/PathVisualizer.hpp
View File

@ -23,7 +23,7 @@ class PathVisualizer : public rclcpp::Node
public:
PathVisualizer(const std::string& file_name);
private:
void odom_callback(const geometry_msgs::msg::Pose::SharedPtr msg);
void localozation_callback(const geometry_msgs::msg::Pose::SharedPtr msg);
void control();
double quaternion_to_yaw(double x,
double y,
@ -39,24 +39,28 @@ private:
int search_nearest_point();
int findLookAheadPoint();
int findClosestPoint(int forward_index_limit);
void publishPath();
void publishVehiclePose();
void publishVehicleTF();
void publishHistoryPath();
void publishVehicleInfo();
// pub rviz info
void publishPathRviz();
void publishVehiclePoseRviz();
void publishVehicleTFRviz();
void publishHistoryPathRviz();
void publishVehicleInfoRviz();
void transformPathToVehicleStart();
void densifyPath(double interval);
void publishLocalPose();
void publishOdomPath();
private:
// 发布cmd_vel
// topic pub
rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_pub_;
rclcpp::Publisher<geometry_msgs::msg::Pose>::SharedPtr local_pose_pub_;
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 history_pose_pub_;
rclcpp::Publisher<visualization_msgs::msg::Marker>::SharedPtr vehicle_info_pub_;
// 定位订阅
// topic sub
rclcpp::Subscription<geometry_msgs::msg::Pose>::SharedPtr odom_sub_;
rclcpp::TimerBase::SharedPtr timer_;

156
src/PathVisualizer.cpp
View File

@ -10,15 +10,18 @@ has_pose_(false)
RCLCPP_INFO(this->get_logger(),"PathVisualizer started");
// pub
cmd_pub_ = this->create_publisher<geometry_msgs::msg::Twist>("/twist_cmd_vel",10);
path_pub_ =this->create_publisher<visualization_msgs::msg::Marker>("/planned_path",10);
vehicle_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/vehicle_pose",10);
history_pose_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/history_pose",10);
vehicle_info_pub_ = this->create_publisher<visualization_msgs::msg::Marker>("/vehicle_text_info",10);
local_pose_pub_ = this->create_publisher<geometry_msgs::msg::Pose>("/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);
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);
// sub
RCLCPP_INFO(this->get_logger(),"Start receving rtk pose data");
odom_sub_ = this->create_subscription<geometry_msgs::msg::Pose>("/rtk_pose",10,std::bind(
&PathVisualizer::odom_callback,this,std::placeholders::_1));
&PathVisualizer::localozation_callback,this,std::placeholders::_1));
// timer
timer_ =this->create_wall_timer(std::chrono::milliseconds(50),std::bind(
&PathVisualizer::control,this));
// urdf pose
@ -26,6 +29,8 @@ has_pose_(false)
// path get
downloadPathJson(path_url_,json_data_);
loadPathFromJson(json_data_);
}
using json = nlohmann::json;
static size_t curlWriteCallback(
@ -242,7 +247,7 @@ bool PathVisualizer::loadPathFromJson(
}
}
void PathVisualizer::publishVehiclePose()
void PathVisualizer::publishVehiclePoseRviz()
{
visualization_msgs::msg::Marker marker;
marker.header.frame_id="odom";
@ -263,12 +268,12 @@ void PathVisualizer::publishVehiclePose()
marker.color.a=1.0;
marker.color.r=1.0;
marker.color.g=1.0;
marker.color.b=0.0;
marker.color.b=1.0;
// 位置
marker.pose.position.x=x_;
marker.pose.position.y=y_;
marker.pose.position.z=0.1;
marker.pose.position.z=0.4;
// yaw转四元数
double cy = cos(yaw_/2.0);
@ -280,39 +285,39 @@ void PathVisualizer::publishVehiclePose()
marker.pose.orientation.w=cy;
vehicle_pub_->publish(marker);
// 2. 车辆矩形框
visualization_msgs::msg::Marker body;
// 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.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.5; // 车长
body.scale.y = 0.2; // 车宽
body.scale.z = 0.3; // 高度
// // 车辆尺寸
// 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;
// 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);
// vehicle_pub_->publish(body);
}
void PathVisualizer::publishVehicleInfo(){
void PathVisualizer::publishVehicleInfoRviz(){
visualization_msgs::msg::Marker text;
text.header.stamp = this->get_clock()->now();
@ -333,10 +338,8 @@ void PathVisualizer::publishVehicleInfo(){
text.color.a = 0.8;
text.color.r = 1.0;
text.color.g = 1.0;
text.color.b = 0.0;
text.scale.z = 0.3;
text.color.a = 1.0;
text.color.b = 1.0;
text.scale.z = 0.2;
text.text =
"X: " + std::to_string(x_) + "\n" +
@ -344,7 +347,7 @@ void PathVisualizer::publishVehicleInfo(){
"Yaw: " + std::to_string(yaw_);
vehicle_info_pub_->publish(text);
}
void PathVisualizer::publishPath()
void PathVisualizer::publishPathRviz()
{
visualization_msgs::msg::Marker path;
path.header.frame_id="odom";
@ -375,7 +378,7 @@ void PathVisualizer::publishPath()
path_pub_->publish(path);
}
void PathVisualizer::publishHistoryPath(){
void PathVisualizer::publishHistoryPathRviz(){
history_x_.push_back(x_);
history_y_.push_back(y_);
history_yaw_.push_back(yaw_);
@ -407,8 +410,7 @@ void PathVisualizer::publishHistoryPath(){
}
history_pose_pub_->publish(history_pose);
}
void PathVisualizer::odom_callback(
const geometry_msgs::msg::Pose::SharedPtr msg)
void PathVisualizer::localozation_callback(const geometry_msgs::msg::Pose::SharedPtr msg)
{
double lat =msg->position.x;
double lon =msg->position.y;
@ -435,9 +437,9 @@ const geometry_msgs::msg::Pose::SharedPtr msg)
msg->orientation.w
);
has_pose_=true;
// 发布 TF
publishVehicleTF();
publishHistoryPath();
// 发布 TF 与 path 可视化
publishVehicleTFRviz();
publishHistoryPathRviz();
if(origin_set_ && !path_transed){
transformPathToVehicleStart();
path_transed=true;
@ -445,7 +447,8 @@ const geometry_msgs::msg::Pose::SharedPtr msg)
this->get_logger(),
"Path transformation completed.");
densifyPath(0.2);
publishPath();
publishPathRviz();
publishOdomPath();
}
}
@ -454,8 +457,9 @@ void PathVisualizer::control()
{
if(!has_pose_)
return;
publishVehiclePose();
publishVehicleInfo();
publishVehiclePoseRviz();
publishVehicleInfoRviz();
publishLocalPose();
target_index_= findClosestPoint(forward_index_limit_);
int target =findLookAheadPoint();
@ -541,8 +545,7 @@ void PathVisualizer::control()
);
}
double PathVisualizer::normalize_angle(
double angle)
double PathVisualizer::normalize_angle(double angle)
{
while(angle>M_PI)
angle-=2*M_PI;
@ -583,7 +586,6 @@ int PathVisualizer::findLookAheadPoint()
return i;
}
}
return path_x_.size()-1;
}
@ -607,7 +609,22 @@ int PathVisualizer::findClosestPoint(int forward_index_limit)
}
return index;
}
void PathVisualizer::publishVehicleTF()
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::publishVehicleTFRviz()
{
geometry_msgs::msg::TransformStamped transform;
@ -744,6 +761,39 @@ void PathVisualizer::densifyPath(double interval)
path_y_.push_back(old_y.back());
path_yaw_.push_back(old_yaw.back());
}
void PathVisualizer::publishOdomPath(){
nav_msgs::msg::Path path_msg;
path_msg.header.stamp = this->now();
path_msg.header.frame_id = "odom";
path_msg.poses.reserve(path_x_.size());
for (size_t i = 0; i <path_x_.size(); ++i) {
geometry_msgs::msg::PoseStamped pose;
pose.header.stamp = path_msg.header.stamp;
pose.header.frame_id = "odom";
pose.pose.position.x = path_x_[i];
pose.pose.position.y = path_y_[i];
pose.pose.position.z = 0.0;
tf2::Quaternion q;
q.setRPY(0.0, 0.0, path_yaw_[i]);
pose.pose.orientation.x = q.x();
pose.pose.orientation.y = q.y();
pose.pose.orientation.z = q.z();
pose.pose.orientation.w = q.w();
path_msg.poses.push_back(pose);
}
odom_path_pub_->publish(path_msg);
}
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
std::string file_name;