| 113 | } |
| 114 | |
| 115 | void resetStates(Eigen::Vector3d init_state) |
| 116 | { |
| 117 | auto node_shared = node_.lock(); |
| 118 | if (!node_shared) { |
| 119 | throw std::runtime_error{"Could not acquire node."}; |
| 120 | } |
| 121 | |
| 122 | prev_x_[0] = init_state; |
| 123 | prev_u_ = std::vector<Eigen::Vector2d>(T_ / dt_, Eigen::Vector2d::Ones()); |
| 124 | for (int t = 1; t < T_ / dt_; t++) { |
| 125 | prev_x_[t] = computeNextState(prev_x_[t - 1], prev_u_[t]); |
| 126 | } |
| 127 | |
| 128 | pubPath(); |
| 129 | } |
| 130 | |
| 131 | geometry_msgs::msg::TwistStamped computeVelocityCommands( |
| 132 | const geometry_msgs::msg::PoseStamped & pose, |
nothing calls this directly
no outgoing calls
no test coverage detected