From cc3f3fc8825d9585b1965956c72a51b77df215a8 Mon Sep 17 00:00:00 2001 From: SS47816 Date: Wed, 13 Mar 2024 12:22:15 +0800 Subject: [PATCH] feat: add check for nan --- .../me5413_world/path_publisher_node.hpp | 3 +- src/me5413_world/src/path_publisher_node.cpp | 40 ++++++++++++------- 2 files changed, 27 insertions(+), 16 deletions(-) diff --git a/src/me5413_world/include/me5413_world/path_publisher_node.hpp b/src/me5413_world/include/me5413_world/path_publisher_node.hpp index 244a836..28ab2d2 100644 --- a/src/me5413_world/include/me5413_world/path_publisher_node.hpp +++ b/src/me5413_world/include/me5413_world/path_publisher_node.hpp @@ -45,9 +45,10 @@ class PathPublisherNode private: void timerCallback(const ros::TimerEvent &); void robotOdomCallback(const nav_msgs::Odometry::ConstPtr &odom); - void publishGlobalPath(const double A, const double B, const double t_res); + void publishGlobalPath(); void publishLocalPath(const geometry_msgs::Pose &robot_pose, const size_t n_wp_prev, const size_t n_wp_post); + std::vector createGlobalPath(const double A, const double B, const double t_res); size_t closestWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const size_t id_start); size_t nextWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const size_t id_start); double getYawFromOrientation(const geometry_msgs::Quaternion &orientation); diff --git a/src/me5413_world/src/path_publisher_node.cpp b/src/me5413_world/src/path_publisher_node.cpp index c3b50e7..ee06901 100644 --- a/src/me5413_world/src/path_publisher_node.cpp +++ b/src/me5413_world/src/path_publisher_node.cpp @@ -29,21 +29,22 @@ PathPublisherNode::PathPublisherNode() : tf2_listener_(tf2_buffer_) this->global_path_msg_.header.frame_id = this->world_frame_; this->local_path_msg_.header.frame_id = this->world_frame_; + this->global_path_msg_.poses = createGlobalPath(10, 10, 0.01); this->abs_position_error_.data = 0.0; this->abs_heading_error_.data = 0.0; this->rms_position_error_.data = 0.0; this->rms_heading_error_.data = 0.0; - this->num_time_steps_ = 0; + this->num_time_steps_ = 1; this->sum_sqr_position_error_ = 0.0; this->sum_sqr_heading_error_ = 0.0; }; void PathPublisherNode::timerCallback(const ros::TimerEvent &) { - // Publish paths - publishGlobalPath(10, 10, 0.01); + // Create and Publish Paths + publishGlobalPath(); publishLocalPath(this->pose_world_robot_, 1, 9); // Calculate absolute errors (wrt to world frame) @@ -51,8 +52,7 @@ void PathPublisherNode::timerCallback(const ros::TimerEvent &) this->abs_position_error_.data = abs_errors.first; this->abs_heading_error_.data = abs_errors.second; - // TODO: calculate average errors - this->num_time_steps_++; + // Calculate average errors this->sum_sqr_position_error_ += std::pow(abs_errors.first, 2); this->sum_sqr_heading_error_ += std::pow(abs_errors.second, 2); this->rms_position_error_.data = std::sqrt(sum_sqr_position_error_/num_time_steps_); @@ -64,6 +64,9 @@ void PathPublisherNode::timerCallback(const ros::TimerEvent &) this->pub_rms_position_error_.publish(this->rms_position_error_); this->pub_rms_heading_error_.publish(this->rms_heading_error_); + // Count + this->num_time_steps_++; + return; }; @@ -88,7 +91,7 @@ void PathPublisherNode::robotOdomCallback(const nav_msgs::Odometry::ConstPtr &od return; }; -void PathPublisherNode::publishGlobalPath(const double A, const double B, const double t_res) +std::vector PathPublisherNode::createGlobalPath(const double A, const double B, const double t_res) { std::vector poses; const double t_increament = t_res * 2 * M_PI; @@ -116,11 +119,15 @@ void PathPublisherNode::publishGlobalPath(const double A, const double B, const } poses.back().pose.orientation = tf2::toMsg(q); + return poses; +}; + +void PathPublisherNode::publishGlobalPath() +{ // Update the message this->global_path_msg_.header.stamp = ros::Time::now(); - this->global_path_msg_.poses = poses; this->pub_global_path_.publish(this->global_path_msg_); -} +}; void PathPublisherNode::publishLocalPath(const geometry_msgs::Pose &robot_pose, const size_t n_wp_prev, const size_t n_wp_post) { @@ -136,7 +143,7 @@ void PathPublisherNode::publishLocalPath(const geometry_msgs::Pose &robot_pose, this->local_path_msg_.poses = std::vector(start, end); this->pub_local_path_.publish(this->local_path_msg_); this->pose_world_goal_ = this->local_path_msg_.poses[n_wp_prev].pose; -} +}; size_t PathPublisherNode::closestWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const size_t id_start = 0) { @@ -161,7 +168,7 @@ size_t PathPublisherNode::closestWaypoint(const geometry_msgs::Pose &robot_pose, } return id_closest; -} +}; size_t PathPublisherNode::nextWaypoint(const geometry_msgs::Pose &robot_pose, const nav_msgs::Path &path, const size_t id_start = 0) { @@ -179,7 +186,7 @@ size_t PathPublisherNode::nextWaypoint(const geometry_msgs::Pose &robot_pose, co } return id_closest; -} +}; double PathPublisherNode::getYawFromOrientation(const geometry_msgs::Quaternion &orientation) { @@ -190,7 +197,7 @@ double PathPublisherNode::getYawFromOrientation(const geometry_msgs::Quaternion m.getRPY(roll, pitch, yaw); return yaw; -} +}; tf2::Transform PathPublisherNode::convertPoseToTransform(const geometry_msgs::Pose &pose) { @@ -224,8 +231,11 @@ std::pair PathPublisherNode::calculatePoseError(const geometry_m const double heading_error = (yaw_robot - yaw_wp) / M_PI * 180.0; - return std::pair(position_error, heading_error); -} + return std::pair( + position_error, + isnan(heading_error)? 0.0 : heading_error + ); +}; } // namespace me5413_world @@ -235,4 +245,4 @@ int main(int argc, char **argv) me5413_world::PathPublisherNode path_publisher_node; ros::spin(); // spin the ros node. return 0; -} \ No newline at end of file +}; \ No newline at end of file