-
Notifications
You must be signed in to change notification settings - Fork 668
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(simple_planning_simulator): add delay model of velocity and stee…
…ring (#235) * add delay steer vel in psim Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * change wz to steer Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix param description Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * modify readme Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * modify cmake Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * ci: change file URL Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix: order to create callback (#220) Signed-off-by: tanaka3 <ttatcoder@outlook.jp> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * chore: remove unnecessary depends (#227) * ci: add check-build-depends.yaml Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * chore: simplify build_depends.repos Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * chore: remove exec_depend Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * chore: use register-autonomoustuff-repository Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * chore: add setup tasks to other workflows Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * ci: update .yamllint.yaml (#229) * ci: update .yamllint.yaml Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> * chore: fix for yamllint Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix: remove warning for compile error (#198) * fix: fix compile error of pointcloud preprocessor Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix: fix compiler warning for had map utils Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix: fix compiler warning for behavior velocity planner Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix: fix compiler warning for compare map segmentation Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix: fix compiler warning for occupancy grid map outlier filter Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix: fix compiler warning for detection by tracker Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> * fix: restore comment Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix: set control_mode false before autoware engage (#232) * fix: set control_mode false before autoware engage Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> * add input/engage remap in launch Signed-off-by: Takayuki Murooka <takayuki5168@gmail.com> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix: library path (#225) Signed-off-by: taikitanaka3 <taiki.tanaka@tier4.jp> Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix: interpolation (#791) (#218) Signed-off-by: tomoya.kimura <tomoya.kimura@tier4.jp> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * add missing function definition in .cpp Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * set input and state for DELAY_STEER_VEL model Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> * fix: fix typo Signed-off-by: kyoichi-sugahara <kyoichi.sugahara@tier4.jp> Co-authored-by: Kenji Miyake <kenji.miyake@tier4.jp> Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com> Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com> Co-authored-by: taikitanaka3 <taiki.tanaka@tier4.jp> Co-authored-by: Tomoya Kimura <tomoya.kimura@tier4.jp>
- Loading branch information
9 people
authored
Jan 8, 2022
1 parent
f4993f3
commit 5f09a5d
Showing
8 changed files
with
284 additions
and
11 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
150 changes: 150 additions & 0 deletions
150
...g_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,150 @@ | ||
| // Copyright 2021 The Autoware Foundation. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ | ||
| #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ | ||
|
|
||
| #include <deque> | ||
| #include <iostream> | ||
| #include <queue> | ||
|
|
||
| #include "eigen3/Eigen/Core" | ||
| #include "eigen3/Eigen/LU" | ||
|
|
||
| #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" | ||
| /** | ||
| * @class SimModelDelaySteerVel | ||
| * @brief calculate delay steering dynamics | ||
| */ | ||
| class SimModelDelaySteerVel : public SimModelInterface | ||
| { | ||
| public: | ||
| /** | ||
| * @brief constructor | ||
| * @param [in] vx_lim velocity limit [m/s] | ||
| * @param [in] steer_lim steering limit [rad] | ||
| * @param [in] vx_rate_lim acceleration limit [m/ss] | ||
| * @param [in] steer_rate_lim steering angular velocity limit [rad/ss] | ||
| * @param [in] wheelbase vehicle wheelbase length [m] | ||
| * @param [in] dt delta time information to set input buffer for delay | ||
| * @param [in] vx_delay time delay for velocity command [s] | ||
| * @param [in] vx_time_constant time constant for 1D model of velocity dynamics | ||
| * @param [in] steer_delay time delay for steering command [s] | ||
| * @param [in] steer_time_constant time constant for 1D model of steering dynamics | ||
| */ | ||
| SimModelDelaySteerVel( | ||
| float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, | ||
| float64_t wheelbase, float64_t dt, float64_t vx_delay, float64_t vx_time_constant, | ||
| float64_t steer_delay, float64_t steer_time_constant); | ||
|
|
||
| /** | ||
| * @brief destructor | ||
| */ | ||
| ~SimModelDelaySteerVel() = default; | ||
|
|
||
| private: | ||
| const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant | ||
|
|
||
| enum IDX | ||
| { | ||
| X = 0, | ||
| Y, | ||
| YAW, | ||
| VX, | ||
| STEER, | ||
| }; | ||
| enum IDX_U | ||
| { | ||
| VX_DES = 0, | ||
| STEER_DES, | ||
| }; | ||
|
|
||
| const float64_t vx_lim_; //!< @brief velocity limit | ||
| const float64_t vx_rate_lim_; //!< @brief acceleration limit | ||
| const float64_t steer_lim_; //!< @brief steering limit [rad] | ||
| const float64_t steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] | ||
| const float64_t wheelbase_; //!< @brief vehicle wheelbase length [m] | ||
| float64_t prev_vx_ = 0.0; | ||
| float64_t current_ax_ = 0.0; | ||
|
|
||
| std::deque<float64_t> vx_input_queue_; //!< @brief buffer for velocity command | ||
| std::deque<float64_t> steer_input_queue_; //!< @brief buffer for angular velocity command | ||
| const float64_t vx_delay_; //!< @brief time delay for velocity command [s] | ||
| const float64_t vx_time_constant_; | ||
| //!< @brief time constant for 1D model of velocity dynamics | ||
| const float64_t steer_delay_; //!< @brief time delay for angular-velocity command [s] | ||
| const float64_t | ||
| steer_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics | ||
|
|
||
| /** | ||
| * @brief set queue buffer for input command | ||
| * @param [in] dt delta time | ||
| */ | ||
| void initializeInputQueue(const float64_t & dt); | ||
|
|
||
| /** | ||
| * @brief get vehicle position x | ||
| */ | ||
| float64_t getX() override; | ||
|
|
||
| /** | ||
| * @brief get vehicle position y | ||
| */ | ||
| float64_t getY() override; | ||
|
|
||
| /** | ||
| * @brief get vehicle angle yaw | ||
| */ | ||
| float64_t getYaw() override; | ||
|
|
||
| /** | ||
| * @brief get vehicle longitudinal velocity | ||
| */ | ||
| float64_t getVx() override; | ||
|
|
||
| /** | ||
| * @brief get vehicle lateral velocity | ||
| */ | ||
| float64_t getVy() override; | ||
|
|
||
| /** | ||
| * @brief get vehicle longitudinal acceleration | ||
| */ | ||
| float64_t getAx() override; | ||
|
|
||
| /** | ||
| * @brief get vehicle angular-velocity wz | ||
| */ | ||
| float64_t getWz() override; | ||
|
|
||
| /** | ||
| * @brief get vehicle steering angle | ||
| */ | ||
| float64_t getSteer() override; | ||
|
|
||
| /** | ||
| * @brief update vehicle states | ||
| * @param [in] dt delta time [s] | ||
| */ | ||
| void update(const float64_t & dt) override; | ||
|
|
||
| /** | ||
| * @brief calculate derivative of states with delay steering model | ||
| * @param [in] state current model state | ||
| * @param [in] input input vector to model | ||
| */ | ||
| Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; | ||
| }; | ||
|
|
||
| #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_VEL_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
104 changes: 104 additions & 0 deletions
104
...nning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,104 @@ | ||
| // Copyright 2021 The Autoware Foundation. | ||
| // | ||
| // Licensed under the Apache License, Version 2.0 (the "License"); | ||
| // you may not use this file except in compliance with the License. | ||
| // You may obtain a copy of the License at | ||
| // | ||
| // http://www.apache.org/licenses/LICENSE-2.0 | ||
| // | ||
| // Unless required by applicable law or agreed to in writing, software | ||
| // distributed under the License is distributed on an "AS IS" BASIS, | ||
| // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. | ||
| // See the License for the specific language governing permissions and | ||
| // limitations under the License. | ||
|
|
||
| #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_vel.hpp" | ||
|
|
||
| #include <algorithm> | ||
|
|
||
| SimModelDelaySteerVel::SimModelDelaySteerVel( | ||
| float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, | ||
| float64_t wheelbase, float64_t dt, float64_t vx_delay, float64_t vx_time_constant, | ||
| float64_t steer_delay, float64_t steer_time_constant) | ||
| : SimModelInterface(5 /* dim x */, 2 /* dim u */), | ||
| MIN_TIME_CONSTANT(0.03), | ||
| vx_lim_(vx_lim), | ||
| vx_rate_lim_(vx_rate_lim), | ||
| steer_lim_(steer_lim), | ||
| steer_rate_lim_(steer_rate_lim), | ||
| wheelbase_(wheelbase), | ||
| vx_delay_(vx_delay), | ||
| vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), | ||
| steer_delay_(steer_delay), | ||
| steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) | ||
| { | ||
| initializeInputQueue(dt); | ||
| } | ||
|
|
||
| float64_t SimModelDelaySteerVel::getX() { return state_(IDX::X); } | ||
| float64_t SimModelDelaySteerVel::getY() { return state_(IDX::Y); } | ||
| float64_t SimModelDelaySteerVel::getYaw() { return state_(IDX::YAW); } | ||
| float64_t SimModelDelaySteerVel::getVx() { return state_(IDX::VX); } | ||
| float64_t SimModelDelaySteerVel::getVy() {return 0.0;} | ||
| float64_t SimModelDelaySteerVel::getAx() {return current_ax_;} | ||
| float64_t SimModelDelaySteerVel::getWz() | ||
| { | ||
| return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; | ||
| } | ||
| float64_t SimModelDelaySteerVel::getSteer() {return state_(IDX::STEER);} | ||
| void SimModelDelaySteerVel::update(const float64_t & dt) | ||
| { | ||
| Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); | ||
|
|
||
| vx_input_queue_.push_back(input_(IDX_U::VX_DES)); | ||
| delayed_input(IDX_U::VX_DES) = vx_input_queue_.front(); | ||
| vx_input_queue_.pop_front(); | ||
| steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); | ||
| delayed_input(IDX_U::STEER_DES) = steer_input_queue_.front(); | ||
| steer_input_queue_.pop_front(); | ||
| // do not use deadzone_delta_steer (Steer IF does not exist in this model) | ||
| updateRungeKutta(dt, delayed_input); | ||
| current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; | ||
| prev_vx_ = input_(IDX_U::VX_DES); | ||
| } | ||
|
|
||
| void SimModelDelaySteerVel::initializeInputQueue(const float64_t & dt) | ||
| { | ||
| size_t vx_input_queue_size = static_cast<size_t>(round(vx_delay_ / dt)); | ||
| for (size_t i = 0; i < vx_input_queue_size; i++) { | ||
| vx_input_queue_.push_back(0.0); | ||
| } | ||
| size_t steer_input_queue_size = static_cast<size_t>(round(steer_delay_ / dt)); | ||
| for (size_t i = 0; i < steer_input_queue_size; i++) { | ||
| steer_input_queue_.push_back(0.0); | ||
| } | ||
| } | ||
|
|
||
|
|
||
| Eigen::VectorXd SimModelDelaySteerVel::calcModel( | ||
| const Eigen::VectorXd & state, const Eigen::VectorXd & input) | ||
| { | ||
| auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);}; | ||
|
|
||
| const float64_t vx = sat(state(IDX::VX), vx_lim_, -vx_lim_); | ||
| const float64_t steer = sat(state(IDX::STEER), steer_lim_, -steer_lim_); | ||
| const float64_t yaw = state(IDX::YAW); | ||
| const float64_t delay_input_vx = input(IDX_U::VX_DES); | ||
| const float64_t delay_input_steer = input(IDX_U::STEER_DES); | ||
| const float64_t delay_vx_des = sat(delay_input_vx, vx_lim_, -vx_lim_); | ||
| const float64_t delay_steer_des = sat(delay_input_steer, steer_lim_, -steer_lim_); | ||
| float64_t vx_rate = -(vx - delay_vx_des) / vx_time_constant_; | ||
| float64_t steer_rate = -(steer - delay_steer_des) / steer_time_constant_; | ||
| vx_rate = sat(vx_rate, vx_rate_lim_, -vx_rate_lim_); | ||
| steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); | ||
|
|
||
| Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); | ||
| d_state(IDX::X) = vx * cos(yaw); | ||
| d_state(IDX::Y) = vx * sin(yaw); | ||
| d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; | ||
| d_state(IDX::VX) = vx_rate; | ||
| d_state(IDX::STEER) = steer_rate; | ||
|
|
||
| return d_state; | ||
| } | ||
|
|
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters