| 46 | } |
| 47 | |
| 48 | geometry_msgs::msg::PoseStamped TestPathGenerator::GetNextPoint() |
| 49 | { |
| 50 | /* Builds a Gerono lemniscate. |
| 51 | * Math taken from https://mathworld.wolfram.com/EightCurve.html |
| 52 | */ |
| 53 | |
| 54 | geometry_msgs::msg::PoseStamped pose; |
| 55 | pose.pose.position.x = scale_ * std::sin(t_); |
| 56 | pose.pose.position.y = scale_ * std::sin(t_) * std::cos(t_); |
| 57 | |
| 58 | const auto dx = scale_ * std::cos(t_); |
| 59 | const auto dy = (scale_ * std::cos(t_) * std::cos(t_)) - (scale_ * std::sin(t_) * std::sin(t_)); |
| 60 | |
| 61 | double yaw = std::atan2(dy, dx); |
| 62 | |
| 63 | if (t_delta_ < 0) { |
| 64 | yaw += M_PI; |
| 65 | pose.pose.position.y -= 0.1; |
| 66 | } |
| 67 | |
| 68 | pose.pose.orientation = tf2::toMsg( |
| 69 | Eigen::Quaterniond{Eigen::AngleAxisd{yaw, |
| 70 | Eigen::Vector3d::UnitZ()}}); |
| 71 | |
| 72 | t_ += t_delta_; |
| 73 | return pose; |
| 74 | } |
| 75 | |
| 76 | } // namespace controllers |
nothing calls this directly
no outgoing calls
no test coverage detected