diff --git a/include/sweet_webots_robot/PathVisualizer.hpp b/include/sweet_webots_robot/PathVisualizer.hpp index 3c3021c..4f54add 100644 --- a/include/sweet_webots_robot/PathVisualizer.hpp +++ b/include/sweet_webots_robot/PathVisualizer.hpp @@ -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::SharedPtr cmd_pub_; + rclcpp::Publisher::SharedPtr local_pose_pub_; + rclcpp::Publisher::SharedPtr odom_path_pub_; rclcpp::Publisher::SharedPtr vehicle_pub_; rclcpp::Publisher::SharedPtr path_pub_; rclcpp::Publisher::SharedPtr history_pose_pub_; rclcpp::Publisher::SharedPtr vehicle_info_pub_; - // 定位订阅 + // topic sub rclcpp::Subscription::SharedPtr odom_sub_; rclcpp::TimerBase::SharedPtr timer_; diff --git a/src/PathVisualizer.cpp b/src/PathVisualizer.cpp index 9ad2f5f..c0886c3 100644 --- a/src/PathVisualizer.cpp +++ b/src/PathVisualizer.cpp @@ -10,15 +10,18 @@ has_pose_(false) RCLCPP_INFO(this->get_logger(),"PathVisualizer started"); // pub cmd_pub_ = this->create_publisher("/twist_cmd_vel",10); - path_pub_ =this->create_publisher("/planned_path",10); - vehicle_pub_ = this->create_publisher("/vehicle_pose",10); - history_pose_pub_ = this->create_publisher("/history_pose",10); - vehicle_info_pub_ = this->create_publisher("/vehicle_text_info",10); + 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); + 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); // sub RCLCPP_INFO(this->get_logger(),"Start receving rtk pose data"); odom_sub_ = this->create_subscription("/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 publish(path_msg); + +} int main(int argc, char *argv[]) { rclcpp::init(argc, argv); std::string file_name;