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

156
src/PathVisualizer.cpp
View File

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