| 98 | void cleanup() override {} |
| 99 | |
| 100 | void setPlan(const nav_msgs::msg::Path & path) override |
| 101 | { |
| 102 | auto node_shared = node_.lock(); |
| 103 | if (!node_shared) { |
| 104 | throw std::runtime_error{"Could not acquire node."}; |
| 105 | } |
| 106 | |
| 107 | trajectory_.clear(); |
| 108 | std::transform( |
| 109 | path.poses.begin(), path.poses.end(), std::back_inserter( |
| 110 | trajectory_), StateFromMsg); |
| 111 | path_start_time_ = node_shared->now(); |
| 112 | |
| 113 | prev_error_ = Eigen::Vector3d::Zero(); |
| 114 | integral_error_ = Eigen::Vector3d::Zero(); |
| 115 | prev_time_ = 0; |
| 116 | } |
| 117 | |
| 118 | void resetStates(Eigen::Vector3d init_state) |
| 119 | { |
nothing calls this directly
no outgoing calls
no test coverage detected