From 8e97427a905b3283b61dbd8b0fc0dd6ba1522092 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Fri, 18 Sep 2020 15:10:43 +0900 Subject: [PATCH 01/52] release v0.4.0 --- .../simple_planning_simulator/CMakeLists.txt | 76 +++ ...qt_multiplot_simple_planning_simulator.xml | 317 ++++++++++++ .../config/simulator_model.yaml | 29 ++ .../simple_planning_simulator_core.hpp | 254 ++++++++++ .../sim_model_constant_acceleration.hpp | 116 +++++ .../vehicle_model/sim_model_ideal.hpp | 257 ++++++++++ .../vehicle_model/sim_model_interface.hpp | 138 +++++ .../vehicle_model/sim_model_time_delay.hpp | 351 +++++++++++++ .../launch/simple_planning_simulator.launch | 36 ++ ...ple_planning_simulator_dummy_pacmod.launch | 82 +++ .../simple_planning_simulator/package.xml | 23 + .../scripts/README_fitParamDelayInputModel.md | 28 + .../scripts/fitParamDelayInputModel.py | 139 +++++ .../src/simple_planning_simulator_core.cpp | 478 ++++++++++++++++++ .../src/simple_planning_simulator_node.cpp | 25 + .../sim_model_constant_acceleration.cpp | 65 +++ .../src/vehicle_model/sim_model_ideal.cpp | 107 ++++ .../src/vehicle_model/sim_model_interface.cpp | 41 ++ .../vehicle_model/sim_model_time_delay.cpp | 305 +++++++++++ 19 files changed, 2867 insertions(+) create mode 100644 simulator/simple_planning_simulator/CMakeLists.txt create mode 100644 simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml create mode 100644 simulator/simple_planning_simulator/config/simulator_model.yaml create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp create mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator.launch create mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch create mode 100644 simulator/simple_planning_simulator/package.xml create mode 100644 simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md create mode 100644 simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt new file mode 100644 index 0000000000000..22c75381ca470 --- /dev/null +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -0,0 +1,76 @@ +cmake_minimum_required(VERSION 3.0.2) +project(simple_planning_simulator) + +add_compile_options(-std=c++14) + +find_package(catkin REQUIRED COMPONENTS + roscpp + std_msgs + tf2 + tf2_ros + geometry_msgs + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs + rostest + rosunit +) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS + roscpp + std_msgs + tf2 + tf2_ros + geometry_msgs + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs +) + +########### +## Build ## +########### + +SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}") + +include_directories( + include + ${catkin_INCLUDE_DIRS} +) + +set(simple_planning_simulator_SRC + src/simple_planning_simulator_core.cpp + src/vehicle_model/sim_model_interface.cpp + src/vehicle_model/sim_model_ideal.cpp + src/vehicle_model/sim_model_constant_acceleration.cpp + src/vehicle_model/sim_model_time_delay.cpp +) +add_executable(simple_planning_simulator src/simple_planning_simulator_node.cpp ${simple_planning_simulator_SRC}) +target_link_libraries(simple_planning_simulator ${catkin_LIBRARIES}) +add_dependencies(simple_planning_simulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) + +## Install executables and/or libraries +install(TARGETS simple_planning_simulator + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} +) + +## Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) + +install( + DIRECTORY + config + launch + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} +) + +catkin_install_python(PROGRAMS scripts/fitParamDelayInputModel.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) + +if (CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) +endif () diff --git a/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml b/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml new file mode 100644 index 0000000000000..d75f6f4383a44 --- /dev/null +++ b/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml @@ -0,0 +1,317 @@ + + + + #242424 + #9e9e9e + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /sim_velocity + geometry_msgs/TwistStamped + + + twist/linear/x + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /sim_velocity + geometry_msgs/TwistStamped + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + sim + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_cmd + autoware_vehicle_msgs/VehicleCommandStamped + + + ctrl_cmd/linear_velocity + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_cmd + autoware_vehicle_msgs/VehicleCommandStamped + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + ctrl_cmd + + + + true + + 30 + ctrl_cmd : vx + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /sim_vehicle_status + autoware_msgs/VehicleStatus + + + angle + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /sim_vehicle_status + autoware_msgs/VehicleStatus + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + sim + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_cmd + autoware_vehicle_msgs/VehicleCommandStamped + + + ctrl_cmd/steering_angle + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_cmd + autoware_vehicle_msgs/VehicleCommandStamped + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + steer_cmd + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_status + autoware_msgs/VehicleStatus + + + angle + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_status + autoware_msgs/VehicleStatus + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + actual + + + + true + + 30 + steer + + + + false +
+ diff --git a/simulator/simple_planning_simulator/config/simulator_model.yaml b/simulator/simple_planning_simulator/config/simulator_model.yaml new file mode 100644 index 0000000000000..1b91975334996 --- /dev/null +++ b/simulator/simple_planning_simulator/config/simulator_model.yaml @@ -0,0 +1,29 @@ +acc_time_constant: 0.1 +acc_time_delay: 0.1 +accel_rate: 7.0 +add_measurement_noise: true +angvel_lim: 3.0 +angvel_noise_stddev: 0.0 +angvel_rate: 1.0 +angvel_time_constant: 0.5 +angvel_time_delay: 0.2 +initial_engage_state: true +pos_noise_stddev: 0.01 +rpy_noise_stddev: 0.0001 +sim_steering_gear_ratio: 15.0 +steer_lim: 1.0 +steer_noise_stddev: 0.0001 +steer_rate_lim: 5.0 +steer_time_constant: 0.27 +steer_time_delay: 0.24 +tread_length: 1.0 + +# Option for vehicle_model_type: +# - IDEAL_STEER : reads velocity command. The steering and velocity changes exactly the same as commanded. +# - DELAY_STEER : reads velocity command. The steering and velocity changes with delay model. +# - DELAY_STEER_ACC : reads acceleration command. The steering and acceleration changes with delay model. +vehicle_model_type: DELAY_STEER_ACC +vel_lim: 50.0 +vel_noise_stddev: 0.0 +vel_time_constant: 0.61 +vel_time_delay: 0.25 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp new file mode 100644 index 0000000000000..6bec5522fadbb --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -0,0 +1,254 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file simple_planning_simulator_core.hpp + * @brief vehicle dynamics simulation for autoware + * @author Takamasa Horibe + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_CORE_H_ +#define SIMPLE_PLANNING_SIMULATOR_CORE_H_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" + +class Simulator +{ +public: + /** + * @brief constructor + */ + Simulator(); + + /** + * @brief default destructor + */ + // ~Simulator() = default; + +private: + /* ros system */ + ros::NodeHandle nh_; //!< @brief ros node handle + ros::NodeHandle pnh_; //!< @brief private ros node handle + ros::Publisher pub_pose_; //!< @brief topic ros publisher for current pose + ros::Publisher pub_twist_; //!< @brief topic ros publisher for current twist + ros::Publisher pub_steer_; + ros::Publisher pub_velocity_; + ros::Publisher pub_turn_signal_; + ros::Publisher pub_shift_; + ros::Publisher pub_control_mode_; + + ros::Subscriber sub_vehicle_cmd_; //!< @brief topic subscriber for vehicle_cmd + ros::Subscriber sub_turn_signal_cmd_; //!< @brief topic subscriber for turn_signal_cmd + ros::Subscriber sub_trajectory_; //!< @brief topic subscriber for trajectory used for z ppsition + ros::Subscriber sub_initialpose_; //!< @brief topic subscriber for initialpose topic + ros::Subscriber sub_initialtwist_; //!< @brief topic subscriber for initialtwist topic + ros::Subscriber sub_engage_; //!< @brief topic subscriber for engage topic + ros::Timer timer_simulation_; //!< @brief timer for simulation + + /* tf */ + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; //!< @brief tf listener + tf2_ros::TransformBroadcaster tf_broadcaster_; //!< @brief tf broadcaster + + /* received & published topics */ + geometry_msgs::PoseStampedConstPtr initial_pose_ptr_; //!< @brief initial vehicle pose + geometry_msgs::PoseWithCovarianceStampedConstPtr + initial_pose_with_cov_ptr_; //!< @brief initial vehicle pose with cov + geometry_msgs::TwistStampedConstPtr initial_twist_ptr_; //!< @brief initial vehicle velocity + geometry_msgs::Pose current_pose_; //!< @brief current vehicle position and angle + geometry_msgs::Twist current_twist_; //!< @brief current vehicle velocity + autoware_vehicle_msgs::VehicleCommandConstPtr + current_vehicle_cmd_ptr_; //!< @brief latest received vehicle_cmd + autoware_planning_msgs::TrajectoryConstPtr + current_trajectory_ptr_; //!< @brief latest received trajectory + double closest_pos_z_; //!< @brief z position on closest trajectory + autoware_vehicle_msgs::TurnSignalConstPtr current_turn_signal_cmd_ptr_; + autoware_vehicle_msgs::ControlMode control_mode_; + + /* frame_id */ + std::string + simulation_frame_id_; //!< @brief vehicle frame id simulated by simple_planning_simulator + std::string map_frame_id_; //!< @brief map frame_id + + /* simple_planning_simulator parameters */ + double loop_rate_; //!< @brief frequency to calculate vehicle model & pubish result + double wheelbase_; //!< @brief wheelbase length to convert angular-velocity & steering + double sim_steering_gear_ratio_; //!< @brief for steering wheel angle calcultion + + /* flags */ + bool is_initialized_; //!< @brief flag to check the initial position is set + bool add_measurement_noise_; //!< @brief flag to add measurement noise + bool simulator_engage_; //!< @brief flag to engage simulator + + /* saved values */ + std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time + + /* vehicle model */ + enum class VehicleModelType { + IDEAL_TWIST = 0, + IDEAL_STEER = 1, + DELAY_TWIST = 2, + DELAY_STEER = 3, + CONST_ACCEL_TWIST = 4, + IDEAL_FORKLIFT_RLS = 5, + DELAY_FORKLIFT_RLS = 6, + IDEAL_ACCEL = 7, + DELAY_STEER_ACC = 8, + } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics + std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer + + /* to generate measurement noise */ + std::shared_ptr rand_engine_ptr_; //!< @brief random engine for measurement noise + std::shared_ptr> + pos_norm_dist_ptr_; //!< @brief Gaussian noise for position + std::shared_ptr> + vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity + std::shared_ptr> + rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw + std::shared_ptr> + angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity + std::shared_ptr> + steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle + + /** + * @brief set current_vehicle_cmd_ptr_ with received message + */ + void callbackVehicleCmd(const autoware_vehicle_msgs::VehicleCommandConstPtr & msg); + + /** + * @brief set current_turn_signal_cmd_ptr with received message + */ + void callbackTurnSignalCmd(const autoware_vehicle_msgs::TurnSignalConstPtr & msg); + + /** + * @brief set current_trajectory_ptr_ with received message + */ + void callbackTrajectory(const autoware_planning_msgs::TrajectoryConstPtr & msg); + + /** + * @brief set initial pose for simulation with received message + */ + void callbackInitialPoseWithCov(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg); + + /** + * @brief set initial pose with received message + */ + void callbackInitialPoseStamped(const geometry_msgs::PoseStampedConstPtr & msg); + + /** + * @brief set initial twist with received message + */ + void callbackInitialTwistStamped(const geometry_msgs::TwistStampedConstPtr & msg); + + /** + * @brief set simulator engage with received message + */ + void callbackEngage(const std_msgs::BoolConstPtr & msg); + + /** + * @brief get transform from two frame_ids + * @param [in] parent_frame parent frame id + * @param [in] child frame id + * @param [out] transform transform from parent frame to child frame + */ + void getTransformFromTF( + const std::string parent_frame, const std::string child_frame, + geometry_msgs::TransformStamped & transform); + + /** + * @brief timer callback for simulation with loop_rate + */ + void timerCallbackSimulation(const ros::TimerEvent & e); + + /** + * @brief set initial state of simulated vehicle + * @param [in] pose initial position and orientation + * @param [in] twist initial velocity and angular velocity + */ + void setInitialState(const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist); + + /** + * @brief set initial state of simulated vehicle with pose transformation based on frame_id + * @param [in] pose initial position and orientation with header + * @param [in] twist initial velocity and angular velocity + */ + void setInitialStateWithPoseTransform( + const geometry_msgs::PoseStamped & pose, const geometry_msgs::Twist & twist); + + /** + * @brief set initial state of simulated vehicle with pose transformation based on frame_id + * @param [in] pose initial position and orientation with header + * @param [in] twist initial velocity and angular velocity + */ + void setInitialStateWithPoseTransform( + const geometry_msgs::PoseWithCovarianceStamped & pose, const geometry_msgs::Twist & twist); + + /** + * @brief publish pose and twist + * @param [in] pose pose to be published + * @param [in] twist twist to be published + */ + void publishPoseTwist(const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist); + + /** + * @brief publish tf + * @param [in] pose pose used for tf + */ + void publishTF(const geometry_msgs::Pose & pose); + + /** + * @brief update closest pose to calculate pos_z + */ + double getPosZFromTrajectory(const double x, const double y); + + /** + * @brief convert roll-pitch-yaw Eular angle to ros Quaternion + * @param [in] roll roll angle [rad] + * @param [in] pitch pitch angle [rad] + * @param [in] yaw yaw angle [rad] + */ + geometry_msgs::Quaternion getQuaternionFromRPY( + const double & roll, const double & pitch, const double & yaw); +}; + +#endif diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp new file mode 100644 index 0000000000000..26bfb4437a29d --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp @@ -0,0 +1,116 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file sim_model_constant_acceleration.hpp + * @brief simple planning simulator model with constant acceleration for velocity & steeiring + * @author Takamasa Horibe + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_CONSTANT_ACCELERATION_H_ +#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_CONSTANT_ACCELERATION_H_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include +#include + +/** + * @class simple_planning_simulator constant acceleration twist model + * @brief calculate velocity & angular-velocity with constant acceleration + */ +class SimModelConstantAccelTwist : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] wz_lim angular velocity limit [m/s] + * @param [in] vx_rate acceleration for velocity [m/ss] + * @param [in] wz_rate acceleration for angular velocity [rad/ss] + */ + SimModelConstantAccelTwist(double vx_lim, double wz_lim, double vx_rate, double wz_rate); + + /** + * @brief default destructor + */ + ~SimModelConstantAccelTwist() = default; + +private: + enum IDX { + X = 0, + Y, + YAW, + VX, + WZ, + }; + enum IDX_U { + VX_DES = 0, + WZ_DES, + }; + + const double vx_lim_; //!< @brief velocity limit + const double wz_lim_; //!< @brief angular velocity limit + const double vx_rate_; //!< @brief velocity rate + const double wz_rate_; //!< @brief angular velocity rate + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with constant acceleration + * @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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp new file mode 100644 index 0000000000000..25fead30d403d --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp @@ -0,0 +1,257 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file sim_model_ideal.hpp + * @brief simple planning simulator ideal velocity model (no dynamics for desired velocity & anguler-velocity or + * steering) + * @author Takamasa Horibe + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_IDEAL_H_ +#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_IDEAL_H_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include +#include + +/** + * @class simple_planning_simulator ideal twist model + * @brief calculate ideal twist dynamics + */ +class SimModelIdealTwist : public SimModelInterface +{ +public: + /** + * @brief constructor + */ + SimModelIdealTwist(); + + /** + * @brief destructor + */ + ~SimModelIdealTwist() = default; + +private: + enum IDX { + X = 0, + Y, + YAW, + }; + enum IDX_U { + VX_DES = 0, + WZ_DES, + }; + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with ideal twist 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; +}; + +/** + * @class simple_planning_simulator ideal steering model + * @brief calculate ideal steering dynamics + */ +class SimModelIdealSteer : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + SimModelIdealSteer(double wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealSteer() = default; + +private: + enum IDX { + X = 0, + Y, + YAW, + }; + enum IDX_U { + VX_DES = 0, + STEER_DES, + }; + + const double wheelbase_; //!< @brief vehicle wheelbase length + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with ideal 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; +}; + +/** + * @class wf_simulator ideal acceleration and steering model + * @brief calculate ideal steering dynamics + */ +class SimModelIdealAccel : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + SimModelIdealAccel(double wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealAccel() = default; + +private: + enum IDX { + X = 0, + Y, + YAW, + VX, + }; + enum IDX_U { + AX_DES = 0, + STEER_DES, + }; + + const double wheelbase_; //!< @brief vehicle wheelbase length + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with ideal 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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp new file mode 100644 index 0000000000000..a1862275f05c5 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -0,0 +1,138 @@ +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file sim_model_interface.hpp + * @brief simple planning simulator model interface class + * @author Takamasa Horibe + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ +#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ + +#include +#include + +/** + * @class simple_planning_simulator vehicle model class + * @brief calculate vehicle dynamics + */ +class SimModelInterface +{ +protected: + const int dim_x_; //!< @brief dimension of state x + const int dim_u_; //!< @brief dimension of input u + Eigen::VectorXd state_; //!< @brief vehicle state vector + Eigen::VectorXd input_; //!< @brief vehicle input vector + +public: + /** + * @brief constructor + * @param [in] dim_x dimension of state x + * @param [in] dim_u dimension of input u + */ + SimModelInterface(int dim_x, int dim_u); + + /** + * @brief destructor + */ + ~SimModelInterface() = default; + + /** + * @brief get state vector of model + * @param [out] state state vector + */ + void getState(Eigen::VectorXd & state); + + /** + * @brief get input vector of model + * @param [out] input input vector + */ + void getInput(Eigen::VectorXd & input); + + /** + * @brief set state vector of model + * @param [in] state state vector + */ + void setState(const Eigen::VectorXd & state); + + /** + * @brief set input vector of model + * @param [in] input input vector + */ + void setInput(const Eigen::VectorXd & input); + + /** + * @brief update vehicle states with Runge-Kutta methods + * @param [in] dt delta time [s] + * @param [in] input vehicle input + */ + void updateRungeKutta(const double & dt, const Eigen::VectorXd & input); + + /** + * @brief update vehicle states with Euler methods + * @param [in] dt delta time [s] + * @param [in] input vehicle input + */ + void updateEuler(const double & dt, const Eigen::VectorXd & input); + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + virtual void update(const double & dt) = 0; + + /** + * @brief get vehicle position x + */ + virtual double getX() = 0; + + /** + * @brief get vehicle position y + */ + virtual double getY() = 0; + + /** + * @brief get vehicle angle yaw + */ + virtual double getYaw() = 0; + + /** + * @brief get vehicle velocity vx + */ + virtual double getVx() = 0; + + /** + * @brief get vehicle angular-velocity wz + */ + virtual double getWz() = 0; + + /** + * @brief get vehicle steering angle + */ + virtual double getSteer() = 0; + + /** + * @brief calculate derivative of states with vehicle model + * @param [in] state current model state + * @param [in] input input vector to model + */ + virtual Eigen::VectorXd calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) = 0; +}; + +#endif \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp new file mode 100644 index 0000000000000..868c10a2a9334 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp @@ -0,0 +1,351 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file sim_model_time_delay.hpp + * @brief simple planning simulator model with time delay and 1-dimensional dynamics for velocity & steeiring + * @author Takamasa Horibe, Kim-Ngoc-Khanh Nguyen + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_TIME_DELAY_H_ +#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_TIME_DELAY_H_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include +#include +#include +#include + +/** + * @class simple_planning_simulator time delay twist model + * @brief calculate time delay twist dynamics + */ +class SimModelTimeDelayTwist : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] angvel_lim angular velocity limit [m/s] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] wz_rate_lim angular acceleration llimit [rad/ss] + * @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] wx_delay time delay for angular-velocity command [s] + * @param [in] wz_time_constant time constant for 1D model of angular-velocity dynamics + */ + SimModelTimeDelayTwist( + double vx_lim, double angvel_lim, double vx_rate_lim, double wz_rate_lim, double dt, + double vx_delay, double vx_time_constant, double wz_delay, double wz_time_constant); + + /** + * @brief default destructor + */ + ~SimModelTimeDelayTwist() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + WZ, + }; + enum IDX_U { + VX_DES = 0, + WZ_DES, + }; + + const double vx_lim_; //!< @brief velocity limit + const double vx_rate_lim_; //!< @brief acceleration limit + const double wz_lim_; //!< @brief angular velocity limit + const double wz_rate_lim_; //!< @brief angular acceleration limit + + std::deque vx_input_queue_; //!< @brief buffer for velocity command + std::deque wz_input_queue_; //!< @brief buffer for angular velocity command + const double vx_delay_; //!< @brief time delay for velocity command [s] + const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics + const double wz_delay_; //!< @brief time delay for angular-velocity command [s] + const double + wz_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 double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time delay twist 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; +}; + +class SimModelTimeDelaySteer : 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 + */ + SimModelTimeDelaySteer( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant); + + /** + * @brief default destructor + */ + ~SimModelTimeDelaySteer() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + }; + enum IDX_U { + VX_DES = 0, + STEER_DES, + }; + + const double vx_lim_; //!< @brief velocity limit [m/s] + const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const double steer_lim_; //!< @brief steering limit [rad] + const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_; //!< @brief vehicle wheelbase length [m] + + std::deque vx_input_queue_; //!< @brief buffer for velocity command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double vx_delay_; //!< @brief time delay for velocity command [s] + const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics + const double steer_delay_; //!< @brief time delay for steering command [s] + const double steer_time_constant_; //!< @brief time constant for 1D model of steering dynamics + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time 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; +}; + +class SimModelTimeDelaySteerAccel : 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] acc_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + */ + SimModelTimeDelaySteerAccel( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double acc_delay, double acc_time_constant, double steer_delay, + double steer_time_constant); + + /** + * @brief default destructor + */ + ~SimModelTimeDelaySteerAccel() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U { + ACCX_DES = 0, + STEER_DES, + DRIVE_SHIFT, + }; + + const double vx_lim_; //!< @brief velocity limit [m/s] + const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const double steer_lim_; //!< @brief steering limit [rad] + const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_; //!< @brief vehicle wheelbase length [m] + + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + const double acc_time_constant_; //!< @brief time constant for 1D model of accel dynamics + const double steer_delay_; //!< @brief time delay for steering command [s] + const double steer_time_constant_; //!< @brief time constant for 1D model of steering dynamics + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time 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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch new file mode 100644 index 0000000000000..2bae8b588fb7b --- /dev/null +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch b/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch new file mode 100644 index 0000000000000..2300de112a8db --- /dev/null +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml new file mode 100644 index 0000000000000..6f8ee2e2fd150 --- /dev/null +++ b/simulator/simple_planning_simulator/package.xml @@ -0,0 +1,23 @@ + + + simple_planning_simulator + 0.1.0 + The simple_planning_simulator package + Takamasa HORIBE + Takamasa HORIBE + + Apache 2 + + catkin + + geometry_msgs + roscpp + rostest + rosunit + std_msgs + tf2 + tf2_ros + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs + diff --git a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md new file mode 100644 index 0000000000000..511852ede903f --- /dev/null +++ b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md @@ -0,0 +1,28 @@ +# fitParamDelayInputModel.py scripts +## How to use +``` +python fitParamDelayInputModel.py --bag_file [bagfile] (--cutoff_time [cutoff_time] --cutoff_freq [cutoff_freq] --min_delay [min_delay] --max_delay [max_delay] --delay_incr [delay_incr]) +# in round brakets is optional arguments +python fitParamDelayInputModel.py --help # for more information +``` + +## Requirements python packages: +* numpy +* pandas + +## Required topics +* autoware_msgs::VehicleCmd /vehicle_cmd: assuming + * vehicle_cmd/ctrl_cmd/steering_angle is the steering angle input [rad] + * vehicle_cmd/ctrl_cmd/linear_velocity is the vehicle velocity input [m/s] +* autoware_msgs::VehicleStatus /vehicle_status : assuming + * vehicle_status/angle is the measured steering angle [rad] + * vehicle_status/speed is the measured vehicle speed [km/h] + +## Description +* Paramter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input +* Arguments explaining: + * CUTOFF_TIME: Cutoff time[sec]. Rosbag file often was start recording before autoware was run. Time before autoware was run should be cut off. This script will only consider data from t=cutoff_time to the end of the bag file (default is 1.0) + * CUTOFF_FREQ: Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1) + * MIN_DELAY: Min value for searching delay loop (default is 0.1) + * MAX_DELAY: Max value for searching delay loop (default is 1.0) + * DELAY_INCR: Step value for searching delay loop (default is 0.01) diff --git a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py new file mode 100644 index 0000000000000..b40899e929222 --- /dev/null +++ b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import numpy as np +import argparse +import subprocess +import sys +from os import getcwd +from os.path import dirname, basename, splitext, join, exists +try: + import pandas as pd +except ImportError: + print ('Please install pandas. See http://pandas.pydata.org/pandas-docs/stable/') + sys.exit(1) + +FREQ_SAMPLE = 0.001 + +# low pass filter +def lowpass_filter(data, cutoff_freq=2, order=1, dt=FREQ_SAMPLE): + tau = 1.0 / (2 * np.pi * cutoff_freq) + for _ in range(order): + for i in range(1,len(data)): + data[i] = (tau / (tau + dt) * data[i-1] + dt / (tau + dt) * data[i]) + return data + +def rel2abs(path): + ''' + Return absolute path from relative path input + ''' + return join(getcwd(), path) + +def rosbag_to_csv(path, topic_name): + name = splitext(basename(path))[0] + suffix = topic_name.replace('/', '-') + output_path = join(dirname(path), name + '_' + suffix + '.csv') + if exists(output_path): + return output_path + else: + command = "rostopic echo -b {0} -p /{1} | sed -e 's/,/ /g' > {2}".format(path, topic_name, output_path) + print (command) + subprocess.check_call(command, shell=True) + return output_path + +def getActValue(df, speed_type): + tm = np.array(list(df['%time'])) * 1e-9 + # Unit Conversion + if speed_type: + val = np.array(list(df['field'])) / 3.6 + else: + val = np.array(list(df['field'])) + # Calc differential + dval = (val[2:] - val[:-2]) / (tm[2:] - tm[:-2]) + return tm[1:-1], val[1:-1], dval + +def getCmdValueWithDelay(df, delay): + tm = np.array(list(df['%time'])) * 1e-9 + val = np.array(list(df['field'])) + return tm + delay, val + +def getLinearInterpolate(_tm, _val, _index, ti): + tmp_t = _tm[_index] + tmp_nextt = _tm[_index + 1] + tmp_val = _val[_index] + tmp_nextval = _val[_index + 1] + val_i = tmp_val + (tmp_nextval - tmp_val) / (tmp_nextt - tmp_t) * (ti - tmp_t) + return val_i + +def getFittingTimeConstantParam(cmd_data, act_data, \ + delay, args, speed_type = False): + tm_cmd, cmd_delay = getCmdValueWithDelay(cmd_data, delay) + tm_act, act, dact = getActValue(act_data, speed_type) + _t_min = max(tm_cmd[0], tm_act[0]) + _t_max = min(tm_cmd[-1], tm_act[-1]) + tm_cmd = tm_cmd - _t_min + tm_act = tm_act - _t_min + MAX_CNT = int((_t_max - _t_min - args.cutoff_time) / FREQ_SAMPLE) + dact_samp = [None] * MAX_CNT + diff_actcmd_samp = [None] * MAX_CNT + ind_cmd = 0 + ind_act = 0 + for ind in range(MAX_CNT): + ti = ind * FREQ_SAMPLE + args.cutoff_time + while (tm_cmd[ind_cmd + 1] < ti): + ind_cmd += 1 + cmd_delay_i = getLinearInterpolate(tm_cmd, cmd_delay, ind_cmd, ti) + while (tm_act[ind_act + 1] < ti): + ind_act += 1 + act_i = getLinearInterpolate(tm_act, act, ind_act, ti) + dact_i = getLinearInterpolate(tm_act, dact, ind_act, ti) + dact_samp[ind] = dact_i + diff_actcmd_samp[ind] = act_i - cmd_delay_i + dact_samp = np.array(dact_samp) + diff_actcmd_samp = np.array(diff_actcmd_samp) + if args.cutoff_freq > 0: + dact_samp = lowpass_filter(dact_samp, cutoff_freq=args.cutoff_freq) + diff_actcmd_samp = lowpass_filter(diff_actcmd_samp, cutoff_freq=args.cutoff_freq) + dact_samp = dact_samp.reshape(1,-1) + diff_actcmd_samp = diff_actcmd_samp.reshape(1,-1) + tau = -np.dot(diff_actcmd_samp, np.linalg.pinv(dact_samp))[0,0] + error = np.linalg.norm(diff_actcmd_samp + tau * dact_samp) / dact_samp.shape[1] + return tau, error + +def getFittingParam(cmd_data, act_data, args, speed_type = False): + delay_range = int((args.max_delay - args.min_delay) / args.delay_incr) + delays = [args.min_delay + i * args.delay_incr for i in range(delay_range + 1)] + error_min = 1.0e10 + delay_opt = -1 + tau_opt = -1 + for delay in delays: + tau, error = getFittingTimeConstantParam(cmd_data, act_data, delay, args, speed_type=speed_type) + if tau > 0: + if error < error_min: + error_min = error + delay_opt = delay + tau_opt = tau + else: + break + return tau_opt, delay_opt, error_min + +if __name__ == '__main__': + topics = [ 'vehicle_cmd/ctrl_cmd/steering_angle', 'vehicle_status/angle', \ + 'vehicle_cmd/ctrl_cmd/linear_velocity', 'vehicle_status/speed'] + pd_data = [None] * len(topics) + parser = argparse.ArgumentParser(description='Paramter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input') + parser.add_argument('--bag_file', '-b', required=True, type=str, help='rosbag file', metavar='file') + parser.add_argument('--cutoff_time', default=1.0, type=float, help='Cutoff time[sec], Parameter fitting will only consider data from t= cutoff_time to the end of the bag file (default is 1.0)') + parser.add_argument('--cutoff_freq', default=0.1, type=float, help='Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1)') + parser.add_argument('--min_delay', default=0.1, type=float, help='Min value for searching delay loop (default is 0.1)') + parser.add_argument('--max_delay', default=1.0, type=float, help='Max value for searching delay loop (default is 1.0)') + parser.add_argument('--delay_incr', default=0.01, type=float, help='Step value for searching delay loop (default is 0.01)') + args = parser.parse_args() + + for i, topic in enumerate(topics): + csv_log = rosbag_to_csv(rel2abs(args.bag_file), topic) + pd_data[i] = pd.read_csv(csv_log, sep=' ') + tau_opt, delay_opt, error = getFittingParam(pd_data[0], pd_data[1], args, speed_type=False) + print ('Steer angle: tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' %(tau_opt, delay_opt, error)) + tau_opt, delay_opt, error = getFittingParam(pd_data[2], pd_data[3], args, speed_type=True) + print ('Velocity : tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' %(tau_opt, delay_opt, error)) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp new file mode 100644 index 0000000000000..fdb53fef5d99c --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -0,0 +1,478 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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/simple_planning_simulator_core.hpp" + +// clang-format off +#define DEBUG_INFO(...) { ROS_INFO(__VA_ARGS__); } + +// clang-format on +namespace +{ +template +T waitForParam(const ros::NodeHandle & nh, const std::string & key) +{ + T value; + ros::Rate rate(1.0); + + while (ros::ok()) { + const auto result = nh.getParam(key, value); + if (result) { + return value; + } + + ROS_WARN("waiting for parameter `%s` ...", key.c_str()); + rate.sleep(); + } + + return {}; +} +} // namespace + +Simulator::Simulator() : nh_(""), pnh_("~"), tf_listener_(tf_buffer_), is_initialized_(false) +{ + /* simple_planning_simulator parameters */ + pnh_.param("loop_rate", loop_rate_, double(50.0)); + wheelbase_ = waitForParam(pnh_, "/vehicle_info/wheel_base"); + pnh_.param("sim_steering_gear_ratio", sim_steering_gear_ratio_, double(15.0)); + pnh_.param("simulation_frame_id", simulation_frame_id_, std::string("base_link")); + pnh_.param("map_frame_id", map_frame_id_, std::string("map")); + pnh_.param("add_measurement_noise", add_measurement_noise_, bool(true)); + + /* set pub sub topic name */ + pub_pose_ = pnh_.advertise("output/current_pose", 1); + pub_twist_ = pnh_.advertise("output/current_twist", 1); + pub_control_mode_ = pnh_.advertise("output/control_mode", 1); + pub_steer_ = nh_.advertise("/vehicle/status/steering", 1); + pub_velocity_ = nh_.advertise("/vehicle/status/velocity", 1); + pub_turn_signal_ = + nh_.advertise("/vehicle/status/turn_signal", 1); + pub_shift_ = nh_.advertise("/vehicle/status/shift", 1); + sub_vehicle_cmd_ = pnh_.subscribe("input/vehicle_cmd", 1, &Simulator::callbackVehicleCmd, this); + sub_turn_signal_cmd_ = + pnh_.subscribe("input/turn_signal_cmd", 1, &Simulator::callbackTurnSignalCmd, this); + sub_initialtwist_ = + pnh_.subscribe("input/initial_twist", 1, &Simulator::callbackInitialTwistStamped, this); + sub_engage_ = pnh_.subscribe("input/engage", 1, &Simulator::callbackEngage, this); + timer_simulation_ = + nh_.createTimer(ros::Duration(1.0 / loop_rate_), &Simulator::timerCallbackSimulation, this); + + bool use_trajectory_for_z_position_source; + pnh_.param( + "use_trajectory_for_z_position_source", use_trajectory_for_z_position_source, bool(true)); + if (use_trajectory_for_z_position_source) { + sub_trajectory_ = nh_.subscribe("base_trajectory", 1, &Simulator::callbackTrajectory, this); + } + + /* set vehicle model parameters */ + double tread_length, angvel_lim, vel_lim, steer_lim, accel_rate, angvel_rate, steer_rate_lim, + vel_time_delay, acc_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, + angvel_time_delay, angvel_time_constant, acc_time_constant; + pnh_.param("tread_length", tread_length, double(1.0)); + pnh_.param("angvel_lim", angvel_lim, double(3.0)); + pnh_.param("vel_lim", vel_lim, double(10.0)); + pnh_.param("steer_lim", steer_lim, double(3.14 / 3.0)); + pnh_.param("accel_rate", accel_rate, double(1.0)); + pnh_.param("angvel_rate", angvel_rate, double(1.0)); + pnh_.param("steer_rate_lim", steer_rate_lim, double(0.3)); + pnh_.param("vel_time_delay", vel_time_delay, double(0.25)); + pnh_.param("vel_time_constant", vel_time_constant, double(0.6197)); + pnh_.param("steer_time_delay", steer_time_delay, double(0.24)); + pnh_.param("steer_time_constant", steer_time_constant, double(0.27)); + pnh_.param("angvel_time_delay", angvel_time_delay, double(0.2)); + pnh_.param("angvel_time_constant", angvel_time_constant, double(0.5)); + pnh_.param("acc_time_delay", acc_time_delay, double(0.3)); + pnh_.param("acc_time_constant", acc_time_constant, double(0.3)); + pnh_.param("initial_engage_state", simulator_engage_, bool(true)); + const double dt = 1.0 / loop_rate_; + + /* set vehicle model type */ + std::string vehicle_model_type_str; + pnh_.param("vehicle_model_type", vehicle_model_type_str, std::string("IDEAL_STEER")); + ROS_INFO("vehicle_model_type = %s", vehicle_model_type_str.c_str()); + if (vehicle_model_type_str == "IDEAL_STEER") { + vehicle_model_type_ = VehicleModelType::IDEAL_STEER; + vehicle_model_ptr_ = std::make_shared(wheelbase_); + } else if (vehicle_model_type_str == "DELAY_STEER") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, accel_rate, steer_rate_lim, wheelbase_, dt, vel_time_delay, + vel_time_constant, steer_time_delay, steer_time_constant); + } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, accel_rate, steer_rate_lim, wheelbase_, dt, acc_time_delay, + acc_time_constant, steer_time_delay, steer_time_constant); + } else { + ROS_ERROR("Invalid vehicle_model_type. Initialization failed."); + } + + /* set normal distribution noises */ + int random_seed; + pnh_.param("random_seed", random_seed, -1); + if (random_seed >= 0) { + rand_engine_ptr_ = std::make_shared(random_seed); + } else { + std::random_device seed; + rand_engine_ptr_ = std::make_shared(seed()); + } + double pos_noise_stddev, vel_noise_stddev, rpy_noise_stddev, angvel_noise_stddev, + steer_noise_stddev; + pnh_.param("pos_noise_stddev", pos_noise_stddev, 1e-2); + pnh_.param("vel_noise_stddev", vel_noise_stddev, 1e-2); + pnh_.param("rpy_noise_stddev", rpy_noise_stddev, 1e-4); + pnh_.param("angvel_noise_stddev", angvel_noise_stddev, 1e-3); + pnh_.param("steer_noise_stddev", steer_noise_stddev, 1e-4); + pos_norm_dist_ptr_ = std::make_shared>(0.0, pos_noise_stddev); + vel_norm_dist_ptr_ = std::make_shared>(0.0, vel_noise_stddev); + rpy_norm_dist_ptr_ = std::make_shared>(0.0, rpy_noise_stddev); + angvel_norm_dist_ptr_ = std::make_shared>(0.0, angvel_noise_stddev); + steer_norm_dist_ptr_ = std::make_shared>(0.0, steer_noise_stddev); + + /* set initialize source */ + std::string initialize_source; + pnh_.param("initialize_source", initialize_source, std::string("ORIGIN")); + ROS_INFO_STREAM("initialize_source : " << initialize_source); + if (initialize_source == "RVIZ") { + sub_initialpose_ = + nh_.subscribe("initialpose", 1, &Simulator::callbackInitialPoseWithCov, this); + } else if (initialize_source == "NDT") { + sub_initialpose_ = nh_.subscribe("ndt_pose", 1, &Simulator::callbackInitialPoseStamped, this); + } else if (initialize_source == "GNSS") { + sub_initialpose_ = nh_.subscribe("gnss_pose", 1, &Simulator::callbackInitialPoseStamped, this); + } else if (initialize_source == "ORIGIN") { + geometry_msgs::Pose p; + p.orientation.w = 1.0; // yaw = 0 + geometry_msgs::Twist t; + setInitialState(p, t); // initialize with 0 for all variables + } else { + ROS_WARN("initialize_source is undesired, setting error!!"); + } + current_pose_.orientation.w = 1.0; + + closest_pos_z_ = 0.0; +} + +void Simulator::callbackTrajectory(const autoware_planning_msgs::TrajectoryConstPtr & msg) +{ + current_trajectory_ptr_ = msg; +} +void Simulator::callbackInitialPoseWithCov( + const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) +{ + geometry_msgs::Twist initial_twist; // initialized with zero for all components + if (initial_twist_ptr_) { + initial_twist = initial_twist_ptr_->twist; + } + //save initial pose + initial_pose_with_cov_ptr_ = msg; + setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist); +} + +void Simulator::callbackInitialPoseStamped(const geometry_msgs::PoseStampedConstPtr & msg) +{ + geometry_msgs::Twist initial_twist; // initialized with zero for all components + if (initial_twist_ptr_) { + initial_twist = initial_twist_ptr_->twist; + } + //save initial pose + initial_pose_ptr_ = msg; + setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist); +} + +void Simulator::callbackInitialTwistStamped(const geometry_msgs::TwistStampedConstPtr & msg) +{ + //save initial pose + initial_twist_ptr_ = msg; + if (initial_pose_ptr_) { + setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist_ptr_->twist); + //input twist to simulator's internal parameter + current_pose_ = initial_pose_ptr_->pose; + current_twist_ = initial_twist_ptr_->twist; + } else if (initial_pose_with_cov_ptr_) { + setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist_ptr_->twist); + } +} + +void Simulator::callbackEngage(const std_msgs::BoolConstPtr & msg) +{ + simulator_engage_ = msg->data; +} + +void Simulator::timerCallbackSimulation(const ros::TimerEvent & e) +{ + if (!is_initialized_) { + ROS_INFO_DELAYED_THROTTLE(3.0, "[simple_planning_simulator] waiting initial position..."); + return; + } + + if (prev_update_time_ptr_ == nullptr) { + prev_update_time_ptr_ = std::make_shared(ros::Time::now()); + } + + /* calculate delta time */ + const double dt = (ros::Time::now() - *prev_update_time_ptr_).toSec(); + *prev_update_time_ptr_ = ros::Time::now(); + + if (simulator_engage_) { + /* update vehicle dynamics when simulator_engage_ is true */ + vehicle_model_ptr_->update(dt); + /* set control mode */ + control_mode_.data = autoware_vehicle_msgs::ControlMode::AUTO; + } else { + /* set control mode */ + control_mode_.data = autoware_vehicle_msgs::ControlMode::MANUAL; + } + + /* save current vehicle pose & twist */ + current_pose_.position.x = vehicle_model_ptr_->getX(); + current_pose_.position.y = vehicle_model_ptr_->getY(); + closest_pos_z_ = getPosZFromTrajectory( + current_pose_.position.x, + current_pose_.position.y); // update vehicle z position from trajectory + current_pose_.position.z = closest_pos_z_; + double roll = 0.0; + double pitch = 0.0; + double yaw = vehicle_model_ptr_->getYaw(); + current_twist_.linear.x = vehicle_model_ptr_->getVx(); + current_twist_.angular.z = vehicle_model_ptr_->getWz(); + + if (simulator_engage_ && add_measurement_noise_) { + current_pose_.position.x += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); + current_pose_.position.y += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); + current_pose_.position.z += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); + roll += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); + pitch += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); + yaw += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); + if (current_twist_.linear.x >= 0.0) { + current_twist_.linear.x += (*vel_norm_dist_ptr_)(*rand_engine_ptr_); + } else { + current_twist_.linear.x -= (*vel_norm_dist_ptr_)(*rand_engine_ptr_); + } + current_twist_.angular.z += (*angvel_norm_dist_ptr_)(*rand_engine_ptr_); + } + + current_pose_.orientation = getQuaternionFromRPY(roll, pitch, yaw); + + /* publish pose & twist */ + publishPoseTwist(current_pose_, current_twist_); + publishTF(current_pose_); + + /* publish steering */ + autoware_vehicle_msgs::Steering steer_msg; + steer_msg.header.frame_id = simulation_frame_id_; + steer_msg.header.stamp = ros::Time::now(); + steer_msg.data = vehicle_model_ptr_->getSteer(); + if (add_measurement_noise_) { + steer_msg.data += (*steer_norm_dist_ptr_)(*rand_engine_ptr_); + } + pub_steer_.publish(steer_msg); + + /* float info publishers */ + std_msgs::Float32 velocity_msg; + velocity_msg.data = current_twist_.linear.x; + pub_velocity_.publish(velocity_msg); + + autoware_vehicle_msgs::TurnSignal turn_signal_msg; + turn_signal_msg.header.frame_id = simulation_frame_id_; + turn_signal_msg.header.stamp = ros::Time::now(); + turn_signal_msg.data = autoware_vehicle_msgs::TurnSignal::NONE; + if (current_turn_signal_cmd_ptr_) { + const auto cmd = current_turn_signal_cmd_ptr_->data; + // ignore invalid data such as cmd=999 + if ( + cmd == autoware_vehicle_msgs::TurnSignal::LEFT || + cmd == autoware_vehicle_msgs::TurnSignal::RIGHT || + cmd == autoware_vehicle_msgs::TurnSignal::HAZARD) { + turn_signal_msg.data = cmd; + } + } + pub_turn_signal_.publish(turn_signal_msg); + + autoware_vehicle_msgs::ShiftStamped shift_msg; + shift_msg.header.frame_id = simulation_frame_id_; + shift_msg.header.stamp = ros::Time::now(); + shift_msg.shift.data = current_twist_.linear.x >= 0.0 ? autoware_vehicle_msgs::Shift::DRIVE + : autoware_vehicle_msgs::Shift::REVERSE; + pub_shift_.publish(shift_msg); + + /* publish control mode */ + pub_control_mode_.publish(control_mode_); +} + +void Simulator::callbackVehicleCmd(const autoware_vehicle_msgs::VehicleCommandConstPtr & msg) +{ + current_vehicle_cmd_ptr_ = msg; + + if ( + vehicle_model_type_ == VehicleModelType::IDEAL_STEER || + vehicle_model_type_ == VehicleModelType::DELAY_STEER) { + Eigen::VectorXd input(2); + input << msg->control.velocity, msg->control.steering_angle; + vehicle_model_ptr_->setInput(input); + } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + Eigen::VectorXd input(3); + double drive_shift = (msg->shift.data == autoware_vehicle_msgs::Shift::REVERSE) ? -1.0 : 1.0; + input << msg->control.acceleration, msg->control.steering_angle, drive_shift; + vehicle_model_ptr_->setInput(input); + } else { + ROS_WARN("[%s] : invalid vehicle_model_type_ error.", __func__); + } +} +void Simulator::callbackTurnSignalCmd(const autoware_vehicle_msgs::TurnSignalConstPtr & msg) +{ + current_turn_signal_cmd_ptr_ = msg; +} + +void Simulator::setInitialStateWithPoseTransform( + const geometry_msgs::PoseStamped & pose_stamped, const geometry_msgs::Twist & twist) +{ + geometry_msgs::TransformStamped transform; + getTransformFromTF(map_frame_id_, pose_stamped.header.frame_id, transform); + geometry_msgs::Pose pose; + pose.position.x = pose_stamped.pose.position.x + transform.transform.translation.x; + pose.position.y = pose_stamped.pose.position.y + transform.transform.translation.y; + pose.position.z = pose_stamped.pose.position.z + transform.transform.translation.z; + pose.orientation = pose_stamped.pose.orientation; + setInitialState(pose, twist); +} + +void Simulator::setInitialStateWithPoseTransform( + const geometry_msgs::PoseWithCovarianceStamped & pose, const geometry_msgs::Twist & twist) +{ + geometry_msgs::PoseStamped ps; + ps.header = pose.header; + ps.pose = pose.pose.pose; + setInitialStateWithPoseTransform(ps, twist); +} + +void Simulator::setInitialState( + const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist) +{ + const double x = pose.position.x; + const double y = pose.position.y; + const double yaw = tf2::getYaw(pose.orientation); + const double vx = twist.linear.x; + const double steer = 0.0; + const double acc = 0.0; + + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER) { + Eigen::VectorXd state(3); + state << x, y, yaw; + vehicle_model_ptr_->setState(state); + } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER) { + Eigen::VectorXd state(5); + state << x, y, yaw, vx, steer; + vehicle_model_ptr_->setState(state); + } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + Eigen::VectorXd state(6); + state << x, y, yaw, vx, steer, acc; + vehicle_model_ptr_->setState(state); + } else { + ROS_WARN("undesired vehicle model type! Initialization failed."); + return; + } + + is_initialized_ = true; +} + +void Simulator::getTransformFromTF( + const std::string parent_frame, const std::string child_frame, + geometry_msgs::TransformStamped & transform) +{ + while (1) { + try { + transform = tf_buffer_.lookupTransform(parent_frame, child_frame, ros::Time(0)); + break; + } catch (tf2::TransformException & ex) { + ROS_ERROR("%s", ex.what()); + ros::Duration(1.0).sleep(); + } + } +} + +void Simulator::publishPoseTwist( + const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist) +{ + ros::Time current_time = ros::Time::now(); + + // simulatied pose + geometry_msgs::PoseStamped ps; + ps.header.frame_id = map_frame_id_; + ps.header.stamp = current_time; + ps.pose = pose; + pub_pose_.publish(ps); + + geometry_msgs::TwistStamped ts; + ts.header.frame_id = simulation_frame_id_; + ts.header.stamp = current_time; + ts.twist = twist; + pub_twist_.publish(ts); +} + +void Simulator::publishTF(const geometry_msgs::Pose & pose) +{ + ros::Time current_time = ros::Time::now(); + + // send odom transform + geometry_msgs::TransformStamped odom_trans; + odom_trans.header.stamp = current_time; + odom_trans.header.frame_id = map_frame_id_; + odom_trans.child_frame_id = simulation_frame_id_; + odom_trans.transform.translation.x = pose.position.x; + odom_trans.transform.translation.y = pose.position.y; + odom_trans.transform.translation.z = pose.position.z; + odom_trans.transform.rotation = pose.orientation; + tf_broadcaster_.sendTransform(odom_trans); +} + +double Simulator::getPosZFromTrajectory(const double x, const double y) +{ + // calculae cloest point on trajectory + /* + write me... + */ + if (current_trajectory_ptr_ != nullptr) { + const double max_sqrt_dist = 100.0 * 100.0; + double min_sqrt_dist = max_sqrt_dist; + int index; + bool found = false; + for (size_t i = 0; i < current_trajectory_ptr_->points.size(); ++i) { + const double dist_x = (current_trajectory_ptr_->points.at(i).pose.position.x - x); + const double dist_y = (current_trajectory_ptr_->points.at(i).pose.position.y - y); + double sqrt_dist = dist_x * dist_x + dist_y * dist_y; + if (sqrt_dist < min_sqrt_dist) { + min_sqrt_dist = sqrt_dist; + index = i; + found = true; + } + } + if (found) + return current_trajectory_ptr_->points.at(index).pose.position.z; + else + return 0; + } else { + return 0.0; + } +} + +geometry_msgs::Quaternion Simulator::getQuaternionFromRPY( + const double & roll, const double & pitch, const double & yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp new file mode 100644 index 0000000000000..8a7acafa9ecc8 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp @@ -0,0 +1,25 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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/simple_planning_simulator_core.hpp" + +int main(int argc, char ** argv) +{ + ros::init(argc, argv, "simple_planning_simulator"); + Simulator obj; + ros::spin(); + return 0; +}; \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp new file mode 100644 index 0000000000000..0b04d92c0b97e --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp @@ -0,0 +1,65 @@ + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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_constant_acceleration.hpp" + +SimModelConstantAccelTwist::SimModelConstantAccelTwist( + double vx_lim, double wz_lim, double vx_rate, double wz_rate) +: SimModelInterface(5 /* dim x */, 2 /* dim u */), + vx_lim_(vx_lim), + wz_lim_(wz_lim), + vx_rate_(vx_rate), + wz_rate_(wz_rate){}; + +double SimModelConstantAccelTwist::getX() { return state_(IDX::X); }; +double SimModelConstantAccelTwist::getY() { return state_(IDX::Y); }; +double SimModelConstantAccelTwist::getYaw() { return state_(IDX::YAW); }; +double SimModelConstantAccelTwist::getVx() { return state_(IDX::VX); }; +double SimModelConstantAccelTwist::getWz() { return state_(IDX::WZ); }; +double SimModelConstantAccelTwist::getSteer() { return 0.0; }; +void SimModelConstantAccelTwist::update(const double & dt) { updateRungeKutta(dt, input_); } +Eigen::VectorXd SimModelConstantAccelTwist::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vel = state(IDX::VX); + const double angvel = state(IDX::WZ); + const double yaw = state(IDX::YAW); + const double vx_des = std::max(std::min(input(IDX_U::VX_DES), vx_lim_), -vx_lim_); + const double wz_des = std::max(std::min(input(IDX_U::WZ_DES), wz_lim_), -wz_lim_); + double vx_rate = 0.0; + double wz_rate = 0.0; + if (vx_des > vel) { + vx_rate = vx_rate_; + } else if (vx_des < vel) { + vx_rate = -vx_rate_; + } + + if (wz_des > angvel) { + wz_rate = wz_rate_; + } else if (wz_des < angvel) { + wz_rate = -wz_rate_; + } + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = angvel; + d_state(IDX::VX) = vx_rate; + d_state(IDX::WZ) = wz_rate; + + return d_state; +}; \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp new file mode 100644 index 0000000000000..a955846118520 --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp @@ -0,0 +1,107 @@ + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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_ideal.hpp" + +SimModelIdealTwist::SimModelIdealTwist() : SimModelInterface(3 /* dim x */, 2 /* dim u */){}; + +double SimModelIdealTwist::getX() { return state_(IDX::X); }; +double SimModelIdealTwist::getY() { return state_(IDX::Y); }; +double SimModelIdealTwist::getYaw() { return state_(IDX::YAW); }; +double SimModelIdealTwist::getVx() { return input_(IDX_U::VX_DES); }; +double SimModelIdealTwist::getWz() { return input_(IDX_U::WZ_DES); }; +double SimModelIdealTwist::getSteer() { return 0.0; }; +void SimModelIdealTwist::update(const double & dt) { updateRungeKutta(dt, input_); } +Eigen::VectorXd SimModelIdealTwist::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double yaw = state(IDX::YAW); + const double vx = input(IDX_U::VX_DES); + const double wz = input(IDX_U::WZ_DES); + + 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) = wz; + + return d_state; +}; + +SimModelIdealSteer::SimModelIdealSteer(double wheelbase) +: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase){}; + +double SimModelIdealSteer::getX() { return state_(IDX::X); }; +double SimModelIdealSteer::getY() { return state_(IDX::Y); }; +double SimModelIdealSteer::getYaw() { return state_(IDX::YAW); }; +double SimModelIdealSteer::getVx() { return input_(IDX_U::VX_DES); }; +double SimModelIdealSteer::getWz() +{ + return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +}; +double SimModelIdealSteer::getSteer() { return input_(IDX_U::STEER_DES); }; +void SimModelIdealSteer::update(const double & dt) { updateRungeKutta(dt, input_); } +Eigen::VectorXd SimModelIdealSteer::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double yaw = state(IDX::YAW); + const double vx = input(IDX_U::VX_DES); + const double steer = input(IDX_U::STEER_DES); + + 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_; + + return d_state; +}; + +SimModelIdealAccel::SimModelIdealAccel(double wheelbase) +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase){}; + +double SimModelIdealAccel::getX() { return state_(IDX::X); }; +double SimModelIdealAccel::getY() { return state_(IDX::Y); }; +double SimModelIdealAccel::getYaw() { return state_(IDX::YAW); }; +double SimModelIdealAccel::getVx() { return state_(IDX::VX); }; +double SimModelIdealAccel::getWz() +{ + return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +}; +double SimModelIdealAccel::getSteer() { return input_(IDX_U::STEER_DES); }; +void SimModelIdealAccel::update(const double & dt) +{ + updateRungeKutta(dt, input_); + if (state_(IDX::VX) < 0) { + state_(IDX::VX) = 0; + } +} + +Eigen::VectorXd SimModelIdealAccel::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vx = state(IDX::VX); + const double yaw = state(IDX::YAW); + const double ax = input(IDX_U::AX_DES); + const double steer = input(IDX_U::STEER_DES); + + 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::VX) = ax; + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + + return d_state; +}; diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp new file mode 100644 index 0000000000000..5a1533bfbda29 --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp @@ -0,0 +1,41 @@ +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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_interface.hpp" + +SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) +{ + state_ = Eigen::VectorXd::Zero(dim_x_); + input_ = Eigen::VectorXd::Zero(dim_u_); +}; + +void SimModelInterface::updateRungeKutta(const double & dt, const Eigen::VectorXd & input) +{ + Eigen::VectorXd k1 = calcModel(state_, input); + Eigen::VectorXd k2 = calcModel(state_ + k1 * 0.5 * dt, input); + Eigen::VectorXd k3 = calcModel(state_ + k2 * 0.5 * dt, input); + Eigen::VectorXd k4 = calcModel(state_ + k3 * dt, input); + + state_ += 1.0 / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4) * dt; +} +void SimModelInterface::updateEuler(const double & dt, const Eigen::VectorXd & input) +{ + state_ += calcModel(state_, input) * dt; +} +void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; }; +void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; }; +void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; }; +void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; }; diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp new file mode 100644 index 0000000000000..d0208ca839785 --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp @@ -0,0 +1,305 @@ + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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_time_delay.hpp" + +/* + * + * SimModelTimeDelayTwist + * + */ + +SimModelTimeDelayTwist::SimModelTimeDelayTwist( + double vx_lim, double wz_lim, double vx_rate_lim, double wz_rate_lim, double dt, double vx_delay, + double vx_time_constant, double wz_delay, double wz_time_constant) +: SimModelInterface(5 /* dim x */, 2 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + wz_lim_(wz_lim), + wz_rate_lim_(wz_rate_lim), + vx_delay_(vx_delay), + vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), + wz_delay_(wz_delay), + wz_time_constant_(std::max(wz_time_constant, MIN_TIME_CONSTANT)) +{ + if (vx_time_constant < MIN_TIME_CONSTANT) { + ROS_WARN("Settings vx_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); + } + if (wz_time_constant < MIN_TIME_CONSTANT) { + ROS_WARN("Settings wz_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); + } + initializeInputQueue(dt); +}; + +double SimModelTimeDelayTwist::getX() { return state_(IDX::X); }; +double SimModelTimeDelayTwist::getY() { return state_(IDX::Y); }; +double SimModelTimeDelayTwist::getYaw() { return state_(IDX::YAW); }; +double SimModelTimeDelayTwist::getVx() { return state_(IDX::VX); }; +double SimModelTimeDelayTwist::getWz() { return state_(IDX::WZ); }; +double SimModelTimeDelayTwist::getSteer() { return 0.0; }; +void SimModelTimeDelayTwist::update(const double & 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(); + wz_input_queue_.push_back(input_(IDX_U::WZ_DES)); + delayed_input(IDX_U::WZ_DES) = wz_input_queue_.front(); + wz_input_queue_.pop_front(); + + updateRungeKutta(dt, delayed_input); +}; +void SimModelTimeDelayTwist::initializeInputQueue(const double & dt) +{ + size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); + for (size_t i = 0; i < vx_input_queue_size; i++) { + vx_input_queue_.push_back(0.0); + } + size_t wz_input_queue_size = static_cast(round(wz_delay_ / dt)); + for (size_t i = 0; i < wz_input_queue_size; i++) { + wz_input_queue_.push_back(0.0); + } +} + +Eigen::VectorXd SimModelTimeDelayTwist::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vx = state(IDX::VX); + const double wz = state(IDX::WZ); + const double yaw = state(IDX::YAW); + const double delay_input_vx = input(IDX_U::VX_DES); + const double delay_input_wz = input(IDX_U::WZ_DES); + const double delay_vx_des = std::max(std::min(delay_input_vx, vx_lim_), -vx_lim_); + const double delay_wz_des = std::max(std::min(delay_input_wz, wz_lim_), -wz_lim_); + double vx_rate = -(vx - delay_vx_des) / vx_time_constant_; + double wz_rate = -(wz - delay_wz_des) / wz_time_constant_; + vx_rate = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, vx_rate)); + wz_rate = std::min(wz_rate_lim_, std::max(-wz_rate_lim_, wz_rate)); + + 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) = wz; + d_state(IDX::VX) = vx_rate; + d_state(IDX::WZ) = wz_rate; + + return d_state; +}; + +/* + * + * SimModelTimeDelaySteer + * + */ +SimModelTimeDelaySteer::SimModelTimeDelaySteer( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double 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)) +{ + if (vx_time_constant < MIN_TIME_CONSTANT) { + ROS_WARN("Settings vx_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); + } + if (steer_time_constant < MIN_TIME_CONSTANT) { + ROS_WARN("Settings steer_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); + } + + initializeInputQueue(dt); +}; + +double SimModelTimeDelaySteer::getX() { return state_(IDX::X); }; +double SimModelTimeDelaySteer::getY() { return state_(IDX::Y); }; +double SimModelTimeDelaySteer::getYaw() { return state_(IDX::YAW); }; +double SimModelTimeDelaySteer::getVx() { return state_(IDX::VX); }; +double SimModelTimeDelaySteer::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +}; +double SimModelTimeDelaySteer::getSteer() { return state_(IDX::STEER); }; +void SimModelTimeDelaySteer::update(const double & 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(); + + updateRungeKutta(dt, delayed_input); +}; +void SimModelTimeDelaySteer::initializeInputQueue(const double & dt) +{ + size_t vx_input_queue_size = static_cast(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(round(steer_delay_ / dt)); + for (size_t i = 0; i < steer_input_queue_size; i++) { + steer_input_queue_.push_back(0.0); + } +} + +Eigen::VectorXd SimModelTimeDelaySteer::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vel = state(IDX::VX); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + const double delay_input_vel = input(IDX_U::VX_DES); + const double delay_input_steer = input(IDX_U::STEER_DES); + const double delay_vx_des = std::max(std::min(delay_input_vel, vx_lim_), -vx_lim_); + const double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); + double vx_rate = -(vel - delay_vx_des) / vx_time_constant_; + double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; + vx_rate = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, vx_rate)); + steer_rate = std::min(steer_rate_lim_, std::max(-steer_rate_lim_, steer_rate)); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = vx_rate; + d_state(IDX::STEER) = steer_rate; + + return d_state; +}; + +SimModelTimeDelaySteerAccel::SimModelTimeDelaySteerAccel( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double acc_delay, double acc_time_constant, double steer_delay, + double steer_time_constant) +: SimModelInterface(6 /* dim x */, 3 /* 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), + acc_delay_(acc_delay), + acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) +{ + if (acc_time_constant < MIN_TIME_CONSTANT) { + ROS_WARN("Settings acc_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); + } + if (steer_time_constant < MIN_TIME_CONSTANT) { + ROS_WARN("Settings steer_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); + } + + initializeInputQueue(dt); +}; + +double SimModelTimeDelaySteerAccel::getX() { return state_(IDX::X); }; +double SimModelTimeDelaySteerAccel::getY() { return state_(IDX::Y); }; +double SimModelTimeDelaySteerAccel::getYaw() { return state_(IDX::YAW); }; +double SimModelTimeDelaySteerAccel::getVx() { return state_(IDX::VX); }; +double SimModelTimeDelaySteerAccel::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +}; +double SimModelTimeDelaySteerAccel::getSteer() { return state_(IDX::STEER); }; +void SimModelTimeDelaySteerAccel::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); + delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); + acc_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(); + delayed_input(IDX_U::DRIVE_SHIFT) = input_(IDX_U::DRIVE_SHIFT); + + updateRungeKutta(dt, delayed_input); + // clip velocity and accel + if (delayed_input(IDX_U::DRIVE_SHIFT) >= 0.0) { + state_(IDX::VX) = std::max(0.0, std::min(state_(IDX::VX), vx_lim_)); + if ( + std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || std::abs((state_(IDX::VX) - vx_lim_)) < 10e-9) { + state_(IDX::ACCX) = 0.0; + } + } else { + state_(IDX::VX) = std::min(0.0, std::max(state_(IDX::VX), -vx_lim_)); + if ( + std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || + std::abs((state_(IDX::VX) - (-vx_lim_))) < 10e-9) { + state_(IDX::ACCX) = 0.0; + } + } +} + +void SimModelTimeDelaySteerAccel::initializeInputQueue(const double & dt) +{ + size_t vx_input_queue_size = static_cast(round(acc_delay_ / dt)); + for (size_t i = 0; i < vx_input_queue_size; i++) { + acc_input_queue_.push_back(0.0); + } + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + for (size_t i = 0; i < steer_input_queue_size; i++) { + steer_input_queue_.push_back(0.0); + } +} + +Eigen::VectorXd SimModelTimeDelaySteerAccel::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + double vel = state(IDX::VX); + double acc = state(IDX::ACCX); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + const double delay_input_acc = input(IDX_U::ACCX_DES); + const double delay_input_steer = input(IDX_U::STEER_DES); + const double drive_shift = input(IDX_U::DRIVE_SHIFT); + double delay_acc_des = std::max(std::min(delay_input_acc, vx_rate_lim_), -vx_rate_lim_); + if (!(drive_shift >= 0.0)) delay_acc_des *= -1; // reverse front-back + double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); + double accx_rate = -(acc - delay_acc_des) / acc_time_constant_; + double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; + acc = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, acc)); + steer_rate = std::min(steer_rate_lim_, std::max(-steer_rate_lim_, steer_rate)); + + if (drive_shift >= 0.0) { + vel = std::max(0.0, std::min(vel, vx_lim_)); + } else { + vel = std::min(0.0, std::max(vel, -vx_lim_)); + } + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = acc; + d_state(IDX::STEER) = steer_rate; + d_state(IDX::ACCX) = accx_rate; + + return d_state; +}; From bbcddb4fd2bde04bd47046bf678a2693a0e07d83 Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Tue, 29 Sep 2020 19:22:18 +0900 Subject: [PATCH 02/52] remove ROS1 packages temporarily Signed-off-by: mitsudome-r --- .../simple_planning_simulator/CMakeLists.txt | 76 --- ...qt_multiplot_simple_planning_simulator.xml | 317 ------------ .../config/simulator_model.yaml | 29 -- .../simple_planning_simulator_core.hpp | 254 ---------- .../sim_model_constant_acceleration.hpp | 116 ----- .../vehicle_model/sim_model_ideal.hpp | 257 ---------- .../vehicle_model/sim_model_interface.hpp | 138 ----- .../vehicle_model/sim_model_time_delay.hpp | 351 ------------- .../launch/simple_planning_simulator.launch | 36 -- ...ple_planning_simulator_dummy_pacmod.launch | 82 --- .../simple_planning_simulator/package.xml | 23 - .../scripts/README_fitParamDelayInputModel.md | 28 - .../scripts/fitParamDelayInputModel.py | 139 ----- .../src/simple_planning_simulator_core.cpp | 478 ------------------ .../src/simple_planning_simulator_node.cpp | 25 - .../sim_model_constant_acceleration.cpp | 65 --- .../src/vehicle_model/sim_model_ideal.cpp | 107 ---- .../src/vehicle_model/sim_model_interface.cpp | 41 -- .../vehicle_model/sim_model_time_delay.cpp | 305 ----------- 19 files changed, 2867 deletions(-) delete mode 100644 simulator/simple_planning_simulator/CMakeLists.txt delete mode 100644 simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml delete mode 100644 simulator/simple_planning_simulator/config/simulator_model.yaml delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp delete mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator.launch delete mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch delete mode 100644 simulator/simple_planning_simulator/package.xml delete mode 100644 simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md delete mode 100644 simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py delete mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp delete mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp delete mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp delete mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp delete mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp delete mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt deleted file mode 100644 index 22c75381ca470..0000000000000 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ /dev/null @@ -1,76 +0,0 @@ -cmake_minimum_required(VERSION 3.0.2) -project(simple_planning_simulator) - -add_compile_options(-std=c++14) - -find_package(catkin REQUIRED COMPONENTS - roscpp - std_msgs - tf2 - tf2_ros - geometry_msgs - autoware_planning_msgs - autoware_control_msgs - autoware_vehicle_msgs - rostest - rosunit -) - -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - roscpp - std_msgs - tf2 - tf2_ros - geometry_msgs - autoware_planning_msgs - autoware_control_msgs - autoware_vehicle_msgs -) - -########### -## Build ## -########### - -SET(CMAKE_CXX_FLAGS "-O2 -g -Wall ${CMAKE_CXX_FLAGS}") - -include_directories( - include - ${catkin_INCLUDE_DIRS} -) - -set(simple_planning_simulator_SRC - src/simple_planning_simulator_core.cpp - src/vehicle_model/sim_model_interface.cpp - src/vehicle_model/sim_model_ideal.cpp - src/vehicle_model/sim_model_constant_acceleration.cpp - src/vehicle_model/sim_model_time_delay.cpp -) -add_executable(simple_planning_simulator src/simple_planning_simulator_node.cpp ${simple_planning_simulator_SRC}) -target_link_libraries(simple_planning_simulator ${catkin_LIBRARIES}) -add_dependencies(simple_planning_simulator ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Install executables and/or libraries -install(TARGETS simple_planning_simulator - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) - -## Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) - -install( - DIRECTORY - config - launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) - -catkin_install_python(PROGRAMS scripts/fitParamDelayInputModel.py DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) - -if (CATKIN_ENABLE_TESTING) - find_package(rostest REQUIRED) -endif () diff --git a/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml b/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml deleted file mode 100644 index d75f6f4383a44..0000000000000 --- a/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml +++ /dev/null @@ -1,317 +0,0 @@ - - - - #242424 - #9e9e9e - false - false - - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /sim_velocity - geometry_msgs/TwistStamped - - - twist/linear/x - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /sim_velocity - geometry_msgs/TwistStamped - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - sim - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_cmd - autoware_vehicle_msgs/VehicleCommandStamped - - - ctrl_cmd/linear_velocity - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_cmd - autoware_vehicle_msgs/VehicleCommandStamped - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - ctrl_cmd - - - - true - - 30 - ctrl_cmd : vx - - - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /sim_vehicle_status - autoware_msgs/VehicleStatus - - - angle - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /sim_vehicle_status - autoware_msgs/VehicleStatus - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - sim - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_cmd - autoware_vehicle_msgs/VehicleCommandStamped - - - ctrl_cmd/steering_angle - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_cmd - autoware_vehicle_msgs/VehicleCommandStamped - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - steer_cmd - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_status - autoware_msgs/VehicleStatus - - - angle - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_status - autoware_msgs/VehicleStatus - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - actual - - - - true - - 30 - steer - - - - false -
- diff --git a/simulator/simple_planning_simulator/config/simulator_model.yaml b/simulator/simple_planning_simulator/config/simulator_model.yaml deleted file mode 100644 index 1b91975334996..0000000000000 --- a/simulator/simple_planning_simulator/config/simulator_model.yaml +++ /dev/null @@ -1,29 +0,0 @@ -acc_time_constant: 0.1 -acc_time_delay: 0.1 -accel_rate: 7.0 -add_measurement_noise: true -angvel_lim: 3.0 -angvel_noise_stddev: 0.0 -angvel_rate: 1.0 -angvel_time_constant: 0.5 -angvel_time_delay: 0.2 -initial_engage_state: true -pos_noise_stddev: 0.01 -rpy_noise_stddev: 0.0001 -sim_steering_gear_ratio: 15.0 -steer_lim: 1.0 -steer_noise_stddev: 0.0001 -steer_rate_lim: 5.0 -steer_time_constant: 0.27 -steer_time_delay: 0.24 -tread_length: 1.0 - -# Option for vehicle_model_type: -# - IDEAL_STEER : reads velocity command. The steering and velocity changes exactly the same as commanded. -# - DELAY_STEER : reads velocity command. The steering and velocity changes with delay model. -# - DELAY_STEER_ACC : reads acceleration command. The steering and acceleration changes with delay model. -vehicle_model_type: DELAY_STEER_ACC -vel_lim: 50.0 -vel_noise_stddev: 0.0 -vel_time_constant: 0.61 -vel_time_delay: 0.25 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp deleted file mode 100644 index 6bec5522fadbb..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ /dev/null @@ -1,254 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ - -/** - * @file simple_planning_simulator_core.hpp - * @brief vehicle dynamics simulation for autoware - * @author Takamasa Horibe - * @date 2019.08.17 - */ - -#ifndef SIMPLE_PLANNING_SIMULATOR_CORE_H_ -#define SIMPLE_PLANNING_SIMULATOR_CORE_H_ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include "simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" - -class Simulator -{ -public: - /** - * @brief constructor - */ - Simulator(); - - /** - * @brief default destructor - */ - // ~Simulator() = default; - -private: - /* ros system */ - ros::NodeHandle nh_; //!< @brief ros node handle - ros::NodeHandle pnh_; //!< @brief private ros node handle - ros::Publisher pub_pose_; //!< @brief topic ros publisher for current pose - ros::Publisher pub_twist_; //!< @brief topic ros publisher for current twist - ros::Publisher pub_steer_; - ros::Publisher pub_velocity_; - ros::Publisher pub_turn_signal_; - ros::Publisher pub_shift_; - ros::Publisher pub_control_mode_; - - ros::Subscriber sub_vehicle_cmd_; //!< @brief topic subscriber for vehicle_cmd - ros::Subscriber sub_turn_signal_cmd_; //!< @brief topic subscriber for turn_signal_cmd - ros::Subscriber sub_trajectory_; //!< @brief topic subscriber for trajectory used for z ppsition - ros::Subscriber sub_initialpose_; //!< @brief topic subscriber for initialpose topic - ros::Subscriber sub_initialtwist_; //!< @brief topic subscriber for initialtwist topic - ros::Subscriber sub_engage_; //!< @brief topic subscriber for engage topic - ros::Timer timer_simulation_; //!< @brief timer for simulation - - /* tf */ - tf2_ros::Buffer tf_buffer_; - tf2_ros::TransformListener tf_listener_; //!< @brief tf listener - tf2_ros::TransformBroadcaster tf_broadcaster_; //!< @brief tf broadcaster - - /* received & published topics */ - geometry_msgs::PoseStampedConstPtr initial_pose_ptr_; //!< @brief initial vehicle pose - geometry_msgs::PoseWithCovarianceStampedConstPtr - initial_pose_with_cov_ptr_; //!< @brief initial vehicle pose with cov - geometry_msgs::TwistStampedConstPtr initial_twist_ptr_; //!< @brief initial vehicle velocity - geometry_msgs::Pose current_pose_; //!< @brief current vehicle position and angle - geometry_msgs::Twist current_twist_; //!< @brief current vehicle velocity - autoware_vehicle_msgs::VehicleCommandConstPtr - current_vehicle_cmd_ptr_; //!< @brief latest received vehicle_cmd - autoware_planning_msgs::TrajectoryConstPtr - current_trajectory_ptr_; //!< @brief latest received trajectory - double closest_pos_z_; //!< @brief z position on closest trajectory - autoware_vehicle_msgs::TurnSignalConstPtr current_turn_signal_cmd_ptr_; - autoware_vehicle_msgs::ControlMode control_mode_; - - /* frame_id */ - std::string - simulation_frame_id_; //!< @brief vehicle frame id simulated by simple_planning_simulator - std::string map_frame_id_; //!< @brief map frame_id - - /* simple_planning_simulator parameters */ - double loop_rate_; //!< @brief frequency to calculate vehicle model & pubish result - double wheelbase_; //!< @brief wheelbase length to convert angular-velocity & steering - double sim_steering_gear_ratio_; //!< @brief for steering wheel angle calcultion - - /* flags */ - bool is_initialized_; //!< @brief flag to check the initial position is set - bool add_measurement_noise_; //!< @brief flag to add measurement noise - bool simulator_engage_; //!< @brief flag to engage simulator - - /* saved values */ - std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time - - /* vehicle model */ - enum class VehicleModelType { - IDEAL_TWIST = 0, - IDEAL_STEER = 1, - DELAY_TWIST = 2, - DELAY_STEER = 3, - CONST_ACCEL_TWIST = 4, - IDEAL_FORKLIFT_RLS = 5, - DELAY_FORKLIFT_RLS = 6, - IDEAL_ACCEL = 7, - DELAY_STEER_ACC = 8, - } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics - std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer - - /* to generate measurement noise */ - std::shared_ptr rand_engine_ptr_; //!< @brief random engine for measurement noise - std::shared_ptr> - pos_norm_dist_ptr_; //!< @brief Gaussian noise for position - std::shared_ptr> - vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity - std::shared_ptr> - rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw - std::shared_ptr> - angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity - std::shared_ptr> - steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle - - /** - * @brief set current_vehicle_cmd_ptr_ with received message - */ - void callbackVehicleCmd(const autoware_vehicle_msgs::VehicleCommandConstPtr & msg); - - /** - * @brief set current_turn_signal_cmd_ptr with received message - */ - void callbackTurnSignalCmd(const autoware_vehicle_msgs::TurnSignalConstPtr & msg); - - /** - * @brief set current_trajectory_ptr_ with received message - */ - void callbackTrajectory(const autoware_planning_msgs::TrajectoryConstPtr & msg); - - /** - * @brief set initial pose for simulation with received message - */ - void callbackInitialPoseWithCov(const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg); - - /** - * @brief set initial pose with received message - */ - void callbackInitialPoseStamped(const geometry_msgs::PoseStampedConstPtr & msg); - - /** - * @brief set initial twist with received message - */ - void callbackInitialTwistStamped(const geometry_msgs::TwistStampedConstPtr & msg); - - /** - * @brief set simulator engage with received message - */ - void callbackEngage(const std_msgs::BoolConstPtr & msg); - - /** - * @brief get transform from two frame_ids - * @param [in] parent_frame parent frame id - * @param [in] child frame id - * @param [out] transform transform from parent frame to child frame - */ - void getTransformFromTF( - const std::string parent_frame, const std::string child_frame, - geometry_msgs::TransformStamped & transform); - - /** - * @brief timer callback for simulation with loop_rate - */ - void timerCallbackSimulation(const ros::TimerEvent & e); - - /** - * @brief set initial state of simulated vehicle - * @param [in] pose initial position and orientation - * @param [in] twist initial velocity and angular velocity - */ - void setInitialState(const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist); - - /** - * @brief set initial state of simulated vehicle with pose transformation based on frame_id - * @param [in] pose initial position and orientation with header - * @param [in] twist initial velocity and angular velocity - */ - void setInitialStateWithPoseTransform( - const geometry_msgs::PoseStamped & pose, const geometry_msgs::Twist & twist); - - /** - * @brief set initial state of simulated vehicle with pose transformation based on frame_id - * @param [in] pose initial position and orientation with header - * @param [in] twist initial velocity and angular velocity - */ - void setInitialStateWithPoseTransform( - const geometry_msgs::PoseWithCovarianceStamped & pose, const geometry_msgs::Twist & twist); - - /** - * @brief publish pose and twist - * @param [in] pose pose to be published - * @param [in] twist twist to be published - */ - void publishPoseTwist(const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist); - - /** - * @brief publish tf - * @param [in] pose pose used for tf - */ - void publishTF(const geometry_msgs::Pose & pose); - - /** - * @brief update closest pose to calculate pos_z - */ - double getPosZFromTrajectory(const double x, const double y); - - /** - * @brief convert roll-pitch-yaw Eular angle to ros Quaternion - * @param [in] roll roll angle [rad] - * @param [in] pitch pitch angle [rad] - * @param [in] yaw yaw angle [rad] - */ - geometry_msgs::Quaternion getQuaternionFromRPY( - const double & roll, const double & pitch, const double & yaw); -}; - -#endif diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp deleted file mode 100644 index 26bfb4437a29d..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp +++ /dev/null @@ -1,116 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ - -/** - * @file sim_model_constant_acceleration.hpp - * @brief simple planning simulator model with constant acceleration for velocity & steeiring - * @author Takamasa Horibe - * @date 2019.08.17 - */ - -#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_CONSTANT_ACCELERATION_H_ -#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_CONSTANT_ACCELERATION_H_ - -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - -#include -#include -#include - -/** - * @class simple_planning_simulator constant acceleration twist model - * @brief calculate velocity & angular-velocity with constant acceleration - */ -class SimModelConstantAccelTwist : public SimModelInterface -{ -public: - /** - * @brief constructor - * @param [in] vx_lim velocity limit [m/s] - * @param [in] wz_lim angular velocity limit [m/s] - * @param [in] vx_rate acceleration for velocity [m/ss] - * @param [in] wz_rate acceleration for angular velocity [rad/ss] - */ - SimModelConstantAccelTwist(double vx_lim, double wz_lim, double vx_rate, double wz_rate); - - /** - * @brief default destructor - */ - ~SimModelConstantAccelTwist() = default; - -private: - enum IDX { - X = 0, - Y, - YAW, - VX, - WZ, - }; - enum IDX_U { - VX_DES = 0, - WZ_DES, - }; - - const double vx_lim_; //!< @brief velocity limit - const double wz_lim_; //!< @brief angular velocity limit - const double vx_rate_; //!< @brief velocity rate - const double wz_rate_; //!< @brief angular velocity rate - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with constant acceleration - * @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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp deleted file mode 100644 index 25fead30d403d..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp +++ /dev/null @@ -1,257 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ - -/** - * @file sim_model_ideal.hpp - * @brief simple planning simulator ideal velocity model (no dynamics for desired velocity & anguler-velocity or - * steering) - * @author Takamasa Horibe - * @date 2019.08.17 - */ - -#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_IDEAL_H_ -#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_IDEAL_H_ - -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - -#include -#include -#include - -/** - * @class simple_planning_simulator ideal twist model - * @brief calculate ideal twist dynamics - */ -class SimModelIdealTwist : public SimModelInterface -{ -public: - /** - * @brief constructor - */ - SimModelIdealTwist(); - - /** - * @brief destructor - */ - ~SimModelIdealTwist() = default; - -private: - enum IDX { - X = 0, - Y, - YAW, - }; - enum IDX_U { - VX_DES = 0, - WZ_DES, - }; - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with ideal twist 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; -}; - -/** - * @class simple_planning_simulator ideal steering model - * @brief calculate ideal steering dynamics - */ -class SimModelIdealSteer : public SimModelInterface -{ -public: - /** - * @brief constructor - * @param [in] wheelbase vehicle wheelbase length [m] - */ - SimModelIdealSteer(double wheelbase); - - /** - * @brief destructor - */ - ~SimModelIdealSteer() = default; - -private: - enum IDX { - X = 0, - Y, - YAW, - }; - enum IDX_U { - VX_DES = 0, - STEER_DES, - }; - - const double wheelbase_; //!< @brief vehicle wheelbase length - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with ideal 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; -}; - -/** - * @class wf_simulator ideal acceleration and steering model - * @brief calculate ideal steering dynamics - */ -class SimModelIdealAccel : public SimModelInterface -{ -public: - /** - * @brief constructor - * @param [in] wheelbase vehicle wheelbase length [m] - */ - SimModelIdealAccel(double wheelbase); - - /** - * @brief destructor - */ - ~SimModelIdealAccel() = default; - -private: - enum IDX { - X = 0, - Y, - YAW, - VX, - }; - enum IDX_U { - AX_DES = 0, - STEER_DES, - }; - - const double wheelbase_; //!< @brief vehicle wheelbase length - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with ideal 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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp deleted file mode 100644 index a1862275f05c5..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ /dev/null @@ -1,138 +0,0 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ - -/** - * @file sim_model_interface.hpp - * @brief simple planning simulator model interface class - * @author Takamasa Horibe - * @date 2019.08.17 - */ - -#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ -#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ - -#include -#include - -/** - * @class simple_planning_simulator vehicle model class - * @brief calculate vehicle dynamics - */ -class SimModelInterface -{ -protected: - const int dim_x_; //!< @brief dimension of state x - const int dim_u_; //!< @brief dimension of input u - Eigen::VectorXd state_; //!< @brief vehicle state vector - Eigen::VectorXd input_; //!< @brief vehicle input vector - -public: - /** - * @brief constructor - * @param [in] dim_x dimension of state x - * @param [in] dim_u dimension of input u - */ - SimModelInterface(int dim_x, int dim_u); - - /** - * @brief destructor - */ - ~SimModelInterface() = default; - - /** - * @brief get state vector of model - * @param [out] state state vector - */ - void getState(Eigen::VectorXd & state); - - /** - * @brief get input vector of model - * @param [out] input input vector - */ - void getInput(Eigen::VectorXd & input); - - /** - * @brief set state vector of model - * @param [in] state state vector - */ - void setState(const Eigen::VectorXd & state); - - /** - * @brief set input vector of model - * @param [in] input input vector - */ - void setInput(const Eigen::VectorXd & input); - - /** - * @brief update vehicle states with Runge-Kutta methods - * @param [in] dt delta time [s] - * @param [in] input vehicle input - */ - void updateRungeKutta(const double & dt, const Eigen::VectorXd & input); - - /** - * @brief update vehicle states with Euler methods - * @param [in] dt delta time [s] - * @param [in] input vehicle input - */ - void updateEuler(const double & dt, const Eigen::VectorXd & input); - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - virtual void update(const double & dt) = 0; - - /** - * @brief get vehicle position x - */ - virtual double getX() = 0; - - /** - * @brief get vehicle position y - */ - virtual double getY() = 0; - - /** - * @brief get vehicle angle yaw - */ - virtual double getYaw() = 0; - - /** - * @brief get vehicle velocity vx - */ - virtual double getVx() = 0; - - /** - * @brief get vehicle angular-velocity wz - */ - virtual double getWz() = 0; - - /** - * @brief get vehicle steering angle - */ - virtual double getSteer() = 0; - - /** - * @brief calculate derivative of states with vehicle model - * @param [in] state current model state - * @param [in] input input vector to model - */ - virtual Eigen::VectorXd calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) = 0; -}; - -#endif \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp deleted file mode 100644 index 868c10a2a9334..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp +++ /dev/null @@ -1,351 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ - -/** - * @file sim_model_time_delay.hpp - * @brief simple planning simulator model with time delay and 1-dimensional dynamics for velocity & steeiring - * @author Takamasa Horibe, Kim-Ngoc-Khanh Nguyen - * @date 2019.08.17 - */ - -#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_TIME_DELAY_H_ -#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_TIME_DELAY_H_ - -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - -#include -#include -#include -#include -#include - -/** - * @class simple_planning_simulator time delay twist model - * @brief calculate time delay twist dynamics - */ -class SimModelTimeDelayTwist : public SimModelInterface -{ -public: - /** - * @brief constructor - * @param [in] vx_lim velocity limit [m/s] - * @param [in] angvel_lim angular velocity limit [m/s] - * @param [in] vx_rate_lim acceleration limit [m/ss] - * @param [in] wz_rate_lim angular acceleration llimit [rad/ss] - * @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] wx_delay time delay for angular-velocity command [s] - * @param [in] wz_time_constant time constant for 1D model of angular-velocity dynamics - */ - SimModelTimeDelayTwist( - double vx_lim, double angvel_lim, double vx_rate_lim, double wz_rate_lim, double dt, - double vx_delay, double vx_time_constant, double wz_delay, double wz_time_constant); - - /** - * @brief default destructor - */ - ~SimModelTimeDelayTwist() = default; - -private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - - enum IDX { - X = 0, - Y, - YAW, - VX, - WZ, - }; - enum IDX_U { - VX_DES = 0, - WZ_DES, - }; - - const double vx_lim_; //!< @brief velocity limit - const double vx_rate_lim_; //!< @brief acceleration limit - const double wz_lim_; //!< @brief angular velocity limit - const double wz_rate_lim_; //!< @brief angular acceleration limit - - std::deque vx_input_queue_; //!< @brief buffer for velocity command - std::deque wz_input_queue_; //!< @brief buffer for angular velocity command - const double vx_delay_; //!< @brief time delay for velocity command [s] - const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics - const double wz_delay_; //!< @brief time delay for angular-velocity command [s] - const double - wz_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 double & dt); - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with time delay twist 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; -}; - -class SimModelTimeDelaySteer : 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 - */ - SimModelTimeDelaySteer( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant); - - /** - * @brief default destructor - */ - ~SimModelTimeDelaySteer() = default; - -private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - - enum IDX { - X = 0, - Y, - YAW, - VX, - STEER, - }; - enum IDX_U { - VX_DES = 0, - STEER_DES, - }; - - const double vx_lim_; //!< @brief velocity limit [m/s] - const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] - const double steer_lim_; //!< @brief steering limit [rad] - const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const double wheelbase_; //!< @brief vehicle wheelbase length [m] - - std::deque vx_input_queue_; //!< @brief buffer for velocity command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double vx_delay_; //!< @brief time delay for velocity command [s] - const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics - const double steer_delay_; //!< @brief time delay for steering command [s] - const double steer_time_constant_; //!< @brief time constant for 1D model of steering dynamics - - /** - * @brief set queue buffer for input command - * @param [in] dt delta time - */ - void initializeInputQueue(const double & dt); - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with time 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; -}; - -class SimModelTimeDelaySteerAccel : 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] acc_delay time delay for accel command [s] - * @param [in] acc_time_constant time constant for 1D model of accel dynamics - * @param [in] steer_delay time delay for steering command [s] - * @param [in] steer_time_constant time constant for 1D model of steering dynamics - */ - SimModelTimeDelaySteerAccel( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant); - - /** - * @brief default destructor - */ - ~SimModelTimeDelaySteerAccel() = default; - -private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - - enum IDX { - X = 0, - Y, - YAW, - VX, - STEER, - ACCX, - }; - enum IDX_U { - ACCX_DES = 0, - STEER_DES, - DRIVE_SHIFT, - }; - - const double vx_lim_; //!< @brief velocity limit [m/s] - const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] - const double steer_lim_; //!< @brief steering limit [rad] - const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const double wheelbase_; //!< @brief vehicle wheelbase length [m] - - std::deque acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double acc_delay_; //!< @brief time delay for accel command [s] - const double acc_time_constant_; //!< @brief time constant for 1D model of accel dynamics - const double steer_delay_; //!< @brief time delay for steering command [s] - const double steer_time_constant_; //!< @brief time constant for 1D model of steering dynamics - - /** - * @brief set queue buffer for input command - * @param [in] dt delta time - */ - void initializeInputQueue(const double & dt); - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with time 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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch deleted file mode 100644 index 2bae8b588fb7b..0000000000000 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch b/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch deleted file mode 100644 index 2300de112a8db..0000000000000 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch +++ /dev/null @@ -1,82 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml deleted file mode 100644 index 6f8ee2e2fd150..0000000000000 --- a/simulator/simple_planning_simulator/package.xml +++ /dev/null @@ -1,23 +0,0 @@ - - - simple_planning_simulator - 0.1.0 - The simple_planning_simulator package - Takamasa HORIBE - Takamasa HORIBE - - Apache 2 - - catkin - - geometry_msgs - roscpp - rostest - rosunit - std_msgs - tf2 - tf2_ros - autoware_planning_msgs - autoware_control_msgs - autoware_vehicle_msgs - diff --git a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md deleted file mode 100644 index 511852ede903f..0000000000000 --- a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md +++ /dev/null @@ -1,28 +0,0 @@ -# fitParamDelayInputModel.py scripts -## How to use -``` -python fitParamDelayInputModel.py --bag_file [bagfile] (--cutoff_time [cutoff_time] --cutoff_freq [cutoff_freq] --min_delay [min_delay] --max_delay [max_delay] --delay_incr [delay_incr]) -# in round brakets is optional arguments -python fitParamDelayInputModel.py --help # for more information -``` - -## Requirements python packages: -* numpy -* pandas - -## Required topics -* autoware_msgs::VehicleCmd /vehicle_cmd: assuming - * vehicle_cmd/ctrl_cmd/steering_angle is the steering angle input [rad] - * vehicle_cmd/ctrl_cmd/linear_velocity is the vehicle velocity input [m/s] -* autoware_msgs::VehicleStatus /vehicle_status : assuming - * vehicle_status/angle is the measured steering angle [rad] - * vehicle_status/speed is the measured vehicle speed [km/h] - -## Description -* Paramter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input -* Arguments explaining: - * CUTOFF_TIME: Cutoff time[sec]. Rosbag file often was start recording before autoware was run. Time before autoware was run should be cut off. This script will only consider data from t=cutoff_time to the end of the bag file (default is 1.0) - * CUTOFF_FREQ: Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1) - * MIN_DELAY: Min value for searching delay loop (default is 0.1) - * MAX_DELAY: Max value for searching delay loop (default is 1.0) - * DELAY_INCR: Step value for searching delay loop (default is 0.01) diff --git a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py deleted file mode 100644 index b40899e929222..0000000000000 --- a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py +++ /dev/null @@ -1,139 +0,0 @@ -#!/usr/bin/env python -# -*- coding: utf-8 -*- - -import numpy as np -import argparse -import subprocess -import sys -from os import getcwd -from os.path import dirname, basename, splitext, join, exists -try: - import pandas as pd -except ImportError: - print ('Please install pandas. See http://pandas.pydata.org/pandas-docs/stable/') - sys.exit(1) - -FREQ_SAMPLE = 0.001 - -# low pass filter -def lowpass_filter(data, cutoff_freq=2, order=1, dt=FREQ_SAMPLE): - tau = 1.0 / (2 * np.pi * cutoff_freq) - for _ in range(order): - for i in range(1,len(data)): - data[i] = (tau / (tau + dt) * data[i-1] + dt / (tau + dt) * data[i]) - return data - -def rel2abs(path): - ''' - Return absolute path from relative path input - ''' - return join(getcwd(), path) - -def rosbag_to_csv(path, topic_name): - name = splitext(basename(path))[0] - suffix = topic_name.replace('/', '-') - output_path = join(dirname(path), name + '_' + suffix + '.csv') - if exists(output_path): - return output_path - else: - command = "rostopic echo -b {0} -p /{1} | sed -e 's/,/ /g' > {2}".format(path, topic_name, output_path) - print (command) - subprocess.check_call(command, shell=True) - return output_path - -def getActValue(df, speed_type): - tm = np.array(list(df['%time'])) * 1e-9 - # Unit Conversion - if speed_type: - val = np.array(list(df['field'])) / 3.6 - else: - val = np.array(list(df['field'])) - # Calc differential - dval = (val[2:] - val[:-2]) / (tm[2:] - tm[:-2]) - return tm[1:-1], val[1:-1], dval - -def getCmdValueWithDelay(df, delay): - tm = np.array(list(df['%time'])) * 1e-9 - val = np.array(list(df['field'])) - return tm + delay, val - -def getLinearInterpolate(_tm, _val, _index, ti): - tmp_t = _tm[_index] - tmp_nextt = _tm[_index + 1] - tmp_val = _val[_index] - tmp_nextval = _val[_index + 1] - val_i = tmp_val + (tmp_nextval - tmp_val) / (tmp_nextt - tmp_t) * (ti - tmp_t) - return val_i - -def getFittingTimeConstantParam(cmd_data, act_data, \ - delay, args, speed_type = False): - tm_cmd, cmd_delay = getCmdValueWithDelay(cmd_data, delay) - tm_act, act, dact = getActValue(act_data, speed_type) - _t_min = max(tm_cmd[0], tm_act[0]) - _t_max = min(tm_cmd[-1], tm_act[-1]) - tm_cmd = tm_cmd - _t_min - tm_act = tm_act - _t_min - MAX_CNT = int((_t_max - _t_min - args.cutoff_time) / FREQ_SAMPLE) - dact_samp = [None] * MAX_CNT - diff_actcmd_samp = [None] * MAX_CNT - ind_cmd = 0 - ind_act = 0 - for ind in range(MAX_CNT): - ti = ind * FREQ_SAMPLE + args.cutoff_time - while (tm_cmd[ind_cmd + 1] < ti): - ind_cmd += 1 - cmd_delay_i = getLinearInterpolate(tm_cmd, cmd_delay, ind_cmd, ti) - while (tm_act[ind_act + 1] < ti): - ind_act += 1 - act_i = getLinearInterpolate(tm_act, act, ind_act, ti) - dact_i = getLinearInterpolate(tm_act, dact, ind_act, ti) - dact_samp[ind] = dact_i - diff_actcmd_samp[ind] = act_i - cmd_delay_i - dact_samp = np.array(dact_samp) - diff_actcmd_samp = np.array(diff_actcmd_samp) - if args.cutoff_freq > 0: - dact_samp = lowpass_filter(dact_samp, cutoff_freq=args.cutoff_freq) - diff_actcmd_samp = lowpass_filter(diff_actcmd_samp, cutoff_freq=args.cutoff_freq) - dact_samp = dact_samp.reshape(1,-1) - diff_actcmd_samp = diff_actcmd_samp.reshape(1,-1) - tau = -np.dot(diff_actcmd_samp, np.linalg.pinv(dact_samp))[0,0] - error = np.linalg.norm(diff_actcmd_samp + tau * dact_samp) / dact_samp.shape[1] - return tau, error - -def getFittingParam(cmd_data, act_data, args, speed_type = False): - delay_range = int((args.max_delay - args.min_delay) / args.delay_incr) - delays = [args.min_delay + i * args.delay_incr for i in range(delay_range + 1)] - error_min = 1.0e10 - delay_opt = -1 - tau_opt = -1 - for delay in delays: - tau, error = getFittingTimeConstantParam(cmd_data, act_data, delay, args, speed_type=speed_type) - if tau > 0: - if error < error_min: - error_min = error - delay_opt = delay - tau_opt = tau - else: - break - return tau_opt, delay_opt, error_min - -if __name__ == '__main__': - topics = [ 'vehicle_cmd/ctrl_cmd/steering_angle', 'vehicle_status/angle', \ - 'vehicle_cmd/ctrl_cmd/linear_velocity', 'vehicle_status/speed'] - pd_data = [None] * len(topics) - parser = argparse.ArgumentParser(description='Paramter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input') - parser.add_argument('--bag_file', '-b', required=True, type=str, help='rosbag file', metavar='file') - parser.add_argument('--cutoff_time', default=1.0, type=float, help='Cutoff time[sec], Parameter fitting will only consider data from t= cutoff_time to the end of the bag file (default is 1.0)') - parser.add_argument('--cutoff_freq', default=0.1, type=float, help='Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1)') - parser.add_argument('--min_delay', default=0.1, type=float, help='Min value for searching delay loop (default is 0.1)') - parser.add_argument('--max_delay', default=1.0, type=float, help='Max value for searching delay loop (default is 1.0)') - parser.add_argument('--delay_incr', default=0.01, type=float, help='Step value for searching delay loop (default is 0.01)') - args = parser.parse_args() - - for i, topic in enumerate(topics): - csv_log = rosbag_to_csv(rel2abs(args.bag_file), topic) - pd_data[i] = pd.read_csv(csv_log, sep=' ') - tau_opt, delay_opt, error = getFittingParam(pd_data[0], pd_data[1], args, speed_type=False) - print ('Steer angle: tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' %(tau_opt, delay_opt, error)) - tau_opt, delay_opt, error = getFittingParam(pd_data[2], pd_data[3], args, speed_type=True) - print ('Velocity : tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' %(tau_opt, delay_opt, error)) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp deleted file mode 100644 index fdb53fef5d99c..0000000000000 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ /dev/null @@ -1,478 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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/simple_planning_simulator_core.hpp" - -// clang-format off -#define DEBUG_INFO(...) { ROS_INFO(__VA_ARGS__); } - -// clang-format on -namespace -{ -template -T waitForParam(const ros::NodeHandle & nh, const std::string & key) -{ - T value; - ros::Rate rate(1.0); - - while (ros::ok()) { - const auto result = nh.getParam(key, value); - if (result) { - return value; - } - - ROS_WARN("waiting for parameter `%s` ...", key.c_str()); - rate.sleep(); - } - - return {}; -} -} // namespace - -Simulator::Simulator() : nh_(""), pnh_("~"), tf_listener_(tf_buffer_), is_initialized_(false) -{ - /* simple_planning_simulator parameters */ - pnh_.param("loop_rate", loop_rate_, double(50.0)); - wheelbase_ = waitForParam(pnh_, "/vehicle_info/wheel_base"); - pnh_.param("sim_steering_gear_ratio", sim_steering_gear_ratio_, double(15.0)); - pnh_.param("simulation_frame_id", simulation_frame_id_, std::string("base_link")); - pnh_.param("map_frame_id", map_frame_id_, std::string("map")); - pnh_.param("add_measurement_noise", add_measurement_noise_, bool(true)); - - /* set pub sub topic name */ - pub_pose_ = pnh_.advertise("output/current_pose", 1); - pub_twist_ = pnh_.advertise("output/current_twist", 1); - pub_control_mode_ = pnh_.advertise("output/control_mode", 1); - pub_steer_ = nh_.advertise("/vehicle/status/steering", 1); - pub_velocity_ = nh_.advertise("/vehicle/status/velocity", 1); - pub_turn_signal_ = - nh_.advertise("/vehicle/status/turn_signal", 1); - pub_shift_ = nh_.advertise("/vehicle/status/shift", 1); - sub_vehicle_cmd_ = pnh_.subscribe("input/vehicle_cmd", 1, &Simulator::callbackVehicleCmd, this); - sub_turn_signal_cmd_ = - pnh_.subscribe("input/turn_signal_cmd", 1, &Simulator::callbackTurnSignalCmd, this); - sub_initialtwist_ = - pnh_.subscribe("input/initial_twist", 1, &Simulator::callbackInitialTwistStamped, this); - sub_engage_ = pnh_.subscribe("input/engage", 1, &Simulator::callbackEngage, this); - timer_simulation_ = - nh_.createTimer(ros::Duration(1.0 / loop_rate_), &Simulator::timerCallbackSimulation, this); - - bool use_trajectory_for_z_position_source; - pnh_.param( - "use_trajectory_for_z_position_source", use_trajectory_for_z_position_source, bool(true)); - if (use_trajectory_for_z_position_source) { - sub_trajectory_ = nh_.subscribe("base_trajectory", 1, &Simulator::callbackTrajectory, this); - } - - /* set vehicle model parameters */ - double tread_length, angvel_lim, vel_lim, steer_lim, accel_rate, angvel_rate, steer_rate_lim, - vel_time_delay, acc_time_delay, vel_time_constant, steer_time_delay, steer_time_constant, - angvel_time_delay, angvel_time_constant, acc_time_constant; - pnh_.param("tread_length", tread_length, double(1.0)); - pnh_.param("angvel_lim", angvel_lim, double(3.0)); - pnh_.param("vel_lim", vel_lim, double(10.0)); - pnh_.param("steer_lim", steer_lim, double(3.14 / 3.0)); - pnh_.param("accel_rate", accel_rate, double(1.0)); - pnh_.param("angvel_rate", angvel_rate, double(1.0)); - pnh_.param("steer_rate_lim", steer_rate_lim, double(0.3)); - pnh_.param("vel_time_delay", vel_time_delay, double(0.25)); - pnh_.param("vel_time_constant", vel_time_constant, double(0.6197)); - pnh_.param("steer_time_delay", steer_time_delay, double(0.24)); - pnh_.param("steer_time_constant", steer_time_constant, double(0.27)); - pnh_.param("angvel_time_delay", angvel_time_delay, double(0.2)); - pnh_.param("angvel_time_constant", angvel_time_constant, double(0.5)); - pnh_.param("acc_time_delay", acc_time_delay, double(0.3)); - pnh_.param("acc_time_constant", acc_time_constant, double(0.3)); - pnh_.param("initial_engage_state", simulator_engage_, bool(true)); - const double dt = 1.0 / loop_rate_; - - /* set vehicle model type */ - std::string vehicle_model_type_str; - pnh_.param("vehicle_model_type", vehicle_model_type_str, std::string("IDEAL_STEER")); - ROS_INFO("vehicle_model_type = %s", vehicle_model_type_str.c_str()); - if (vehicle_model_type_str == "IDEAL_STEER") { - vehicle_model_type_ = VehicleModelType::IDEAL_STEER; - vehicle_model_ptr_ = std::make_shared(wheelbase_); - } else if (vehicle_model_type_str == "DELAY_STEER") { - vehicle_model_type_ = VehicleModelType::DELAY_STEER; - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, accel_rate, steer_rate_lim, wheelbase_, dt, vel_time_delay, - vel_time_constant, steer_time_delay, steer_time_constant); - } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { - vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, accel_rate, steer_rate_lim, wheelbase_, dt, acc_time_delay, - acc_time_constant, steer_time_delay, steer_time_constant); - } else { - ROS_ERROR("Invalid vehicle_model_type. Initialization failed."); - } - - /* set normal distribution noises */ - int random_seed; - pnh_.param("random_seed", random_seed, -1); - if (random_seed >= 0) { - rand_engine_ptr_ = std::make_shared(random_seed); - } else { - std::random_device seed; - rand_engine_ptr_ = std::make_shared(seed()); - } - double pos_noise_stddev, vel_noise_stddev, rpy_noise_stddev, angvel_noise_stddev, - steer_noise_stddev; - pnh_.param("pos_noise_stddev", pos_noise_stddev, 1e-2); - pnh_.param("vel_noise_stddev", vel_noise_stddev, 1e-2); - pnh_.param("rpy_noise_stddev", rpy_noise_stddev, 1e-4); - pnh_.param("angvel_noise_stddev", angvel_noise_stddev, 1e-3); - pnh_.param("steer_noise_stddev", steer_noise_stddev, 1e-4); - pos_norm_dist_ptr_ = std::make_shared>(0.0, pos_noise_stddev); - vel_norm_dist_ptr_ = std::make_shared>(0.0, vel_noise_stddev); - rpy_norm_dist_ptr_ = std::make_shared>(0.0, rpy_noise_stddev); - angvel_norm_dist_ptr_ = std::make_shared>(0.0, angvel_noise_stddev); - steer_norm_dist_ptr_ = std::make_shared>(0.0, steer_noise_stddev); - - /* set initialize source */ - std::string initialize_source; - pnh_.param("initialize_source", initialize_source, std::string("ORIGIN")); - ROS_INFO_STREAM("initialize_source : " << initialize_source); - if (initialize_source == "RVIZ") { - sub_initialpose_ = - nh_.subscribe("initialpose", 1, &Simulator::callbackInitialPoseWithCov, this); - } else if (initialize_source == "NDT") { - sub_initialpose_ = nh_.subscribe("ndt_pose", 1, &Simulator::callbackInitialPoseStamped, this); - } else if (initialize_source == "GNSS") { - sub_initialpose_ = nh_.subscribe("gnss_pose", 1, &Simulator::callbackInitialPoseStamped, this); - } else if (initialize_source == "ORIGIN") { - geometry_msgs::Pose p; - p.orientation.w = 1.0; // yaw = 0 - geometry_msgs::Twist t; - setInitialState(p, t); // initialize with 0 for all variables - } else { - ROS_WARN("initialize_source is undesired, setting error!!"); - } - current_pose_.orientation.w = 1.0; - - closest_pos_z_ = 0.0; -} - -void Simulator::callbackTrajectory(const autoware_planning_msgs::TrajectoryConstPtr & msg) -{ - current_trajectory_ptr_ = msg; -} -void Simulator::callbackInitialPoseWithCov( - const geometry_msgs::PoseWithCovarianceStampedConstPtr & msg) -{ - geometry_msgs::Twist initial_twist; // initialized with zero for all components - if (initial_twist_ptr_) { - initial_twist = initial_twist_ptr_->twist; - } - //save initial pose - initial_pose_with_cov_ptr_ = msg; - setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist); -} - -void Simulator::callbackInitialPoseStamped(const geometry_msgs::PoseStampedConstPtr & msg) -{ - geometry_msgs::Twist initial_twist; // initialized with zero for all components - if (initial_twist_ptr_) { - initial_twist = initial_twist_ptr_->twist; - } - //save initial pose - initial_pose_ptr_ = msg; - setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist); -} - -void Simulator::callbackInitialTwistStamped(const geometry_msgs::TwistStampedConstPtr & msg) -{ - //save initial pose - initial_twist_ptr_ = msg; - if (initial_pose_ptr_) { - setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist_ptr_->twist); - //input twist to simulator's internal parameter - current_pose_ = initial_pose_ptr_->pose; - current_twist_ = initial_twist_ptr_->twist; - } else if (initial_pose_with_cov_ptr_) { - setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist_ptr_->twist); - } -} - -void Simulator::callbackEngage(const std_msgs::BoolConstPtr & msg) -{ - simulator_engage_ = msg->data; -} - -void Simulator::timerCallbackSimulation(const ros::TimerEvent & e) -{ - if (!is_initialized_) { - ROS_INFO_DELAYED_THROTTLE(3.0, "[simple_planning_simulator] waiting initial position..."); - return; - } - - if (prev_update_time_ptr_ == nullptr) { - prev_update_time_ptr_ = std::make_shared(ros::Time::now()); - } - - /* calculate delta time */ - const double dt = (ros::Time::now() - *prev_update_time_ptr_).toSec(); - *prev_update_time_ptr_ = ros::Time::now(); - - if (simulator_engage_) { - /* update vehicle dynamics when simulator_engage_ is true */ - vehicle_model_ptr_->update(dt); - /* set control mode */ - control_mode_.data = autoware_vehicle_msgs::ControlMode::AUTO; - } else { - /* set control mode */ - control_mode_.data = autoware_vehicle_msgs::ControlMode::MANUAL; - } - - /* save current vehicle pose & twist */ - current_pose_.position.x = vehicle_model_ptr_->getX(); - current_pose_.position.y = vehicle_model_ptr_->getY(); - closest_pos_z_ = getPosZFromTrajectory( - current_pose_.position.x, - current_pose_.position.y); // update vehicle z position from trajectory - current_pose_.position.z = closest_pos_z_; - double roll = 0.0; - double pitch = 0.0; - double yaw = vehicle_model_ptr_->getYaw(); - current_twist_.linear.x = vehicle_model_ptr_->getVx(); - current_twist_.angular.z = vehicle_model_ptr_->getWz(); - - if (simulator_engage_ && add_measurement_noise_) { - current_pose_.position.x += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); - current_pose_.position.y += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); - current_pose_.position.z += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); - roll += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); - pitch += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); - yaw += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); - if (current_twist_.linear.x >= 0.0) { - current_twist_.linear.x += (*vel_norm_dist_ptr_)(*rand_engine_ptr_); - } else { - current_twist_.linear.x -= (*vel_norm_dist_ptr_)(*rand_engine_ptr_); - } - current_twist_.angular.z += (*angvel_norm_dist_ptr_)(*rand_engine_ptr_); - } - - current_pose_.orientation = getQuaternionFromRPY(roll, pitch, yaw); - - /* publish pose & twist */ - publishPoseTwist(current_pose_, current_twist_); - publishTF(current_pose_); - - /* publish steering */ - autoware_vehicle_msgs::Steering steer_msg; - steer_msg.header.frame_id = simulation_frame_id_; - steer_msg.header.stamp = ros::Time::now(); - steer_msg.data = vehicle_model_ptr_->getSteer(); - if (add_measurement_noise_) { - steer_msg.data += (*steer_norm_dist_ptr_)(*rand_engine_ptr_); - } - pub_steer_.publish(steer_msg); - - /* float info publishers */ - std_msgs::Float32 velocity_msg; - velocity_msg.data = current_twist_.linear.x; - pub_velocity_.publish(velocity_msg); - - autoware_vehicle_msgs::TurnSignal turn_signal_msg; - turn_signal_msg.header.frame_id = simulation_frame_id_; - turn_signal_msg.header.stamp = ros::Time::now(); - turn_signal_msg.data = autoware_vehicle_msgs::TurnSignal::NONE; - if (current_turn_signal_cmd_ptr_) { - const auto cmd = current_turn_signal_cmd_ptr_->data; - // ignore invalid data such as cmd=999 - if ( - cmd == autoware_vehicle_msgs::TurnSignal::LEFT || - cmd == autoware_vehicle_msgs::TurnSignal::RIGHT || - cmd == autoware_vehicle_msgs::TurnSignal::HAZARD) { - turn_signal_msg.data = cmd; - } - } - pub_turn_signal_.publish(turn_signal_msg); - - autoware_vehicle_msgs::ShiftStamped shift_msg; - shift_msg.header.frame_id = simulation_frame_id_; - shift_msg.header.stamp = ros::Time::now(); - shift_msg.shift.data = current_twist_.linear.x >= 0.0 ? autoware_vehicle_msgs::Shift::DRIVE - : autoware_vehicle_msgs::Shift::REVERSE; - pub_shift_.publish(shift_msg); - - /* publish control mode */ - pub_control_mode_.publish(control_mode_); -} - -void Simulator::callbackVehicleCmd(const autoware_vehicle_msgs::VehicleCommandConstPtr & msg) -{ - current_vehicle_cmd_ptr_ = msg; - - if ( - vehicle_model_type_ == VehicleModelType::IDEAL_STEER || - vehicle_model_type_ == VehicleModelType::DELAY_STEER) { - Eigen::VectorXd input(2); - input << msg->control.velocity, msg->control.steering_angle; - vehicle_model_ptr_->setInput(input); - } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { - Eigen::VectorXd input(3); - double drive_shift = (msg->shift.data == autoware_vehicle_msgs::Shift::REVERSE) ? -1.0 : 1.0; - input << msg->control.acceleration, msg->control.steering_angle, drive_shift; - vehicle_model_ptr_->setInput(input); - } else { - ROS_WARN("[%s] : invalid vehicle_model_type_ error.", __func__); - } -} -void Simulator::callbackTurnSignalCmd(const autoware_vehicle_msgs::TurnSignalConstPtr & msg) -{ - current_turn_signal_cmd_ptr_ = msg; -} - -void Simulator::setInitialStateWithPoseTransform( - const geometry_msgs::PoseStamped & pose_stamped, const geometry_msgs::Twist & twist) -{ - geometry_msgs::TransformStamped transform; - getTransformFromTF(map_frame_id_, pose_stamped.header.frame_id, transform); - geometry_msgs::Pose pose; - pose.position.x = pose_stamped.pose.position.x + transform.transform.translation.x; - pose.position.y = pose_stamped.pose.position.y + transform.transform.translation.y; - pose.position.z = pose_stamped.pose.position.z + transform.transform.translation.z; - pose.orientation = pose_stamped.pose.orientation; - setInitialState(pose, twist); -} - -void Simulator::setInitialStateWithPoseTransform( - const geometry_msgs::PoseWithCovarianceStamped & pose, const geometry_msgs::Twist & twist) -{ - geometry_msgs::PoseStamped ps; - ps.header = pose.header; - ps.pose = pose.pose.pose; - setInitialStateWithPoseTransform(ps, twist); -} - -void Simulator::setInitialState( - const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist) -{ - const double x = pose.position.x; - const double y = pose.position.y; - const double yaw = tf2::getYaw(pose.orientation); - const double vx = twist.linear.x; - const double steer = 0.0; - const double acc = 0.0; - - if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER) { - Eigen::VectorXd state(3); - state << x, y, yaw; - vehicle_model_ptr_->setState(state); - } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER) { - Eigen::VectorXd state(5); - state << x, y, yaw, vx, steer; - vehicle_model_ptr_->setState(state); - } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { - Eigen::VectorXd state(6); - state << x, y, yaw, vx, steer, acc; - vehicle_model_ptr_->setState(state); - } else { - ROS_WARN("undesired vehicle model type! Initialization failed."); - return; - } - - is_initialized_ = true; -} - -void Simulator::getTransformFromTF( - const std::string parent_frame, const std::string child_frame, - geometry_msgs::TransformStamped & transform) -{ - while (1) { - try { - transform = tf_buffer_.lookupTransform(parent_frame, child_frame, ros::Time(0)); - break; - } catch (tf2::TransformException & ex) { - ROS_ERROR("%s", ex.what()); - ros::Duration(1.0).sleep(); - } - } -} - -void Simulator::publishPoseTwist( - const geometry_msgs::Pose & pose, const geometry_msgs::Twist & twist) -{ - ros::Time current_time = ros::Time::now(); - - // simulatied pose - geometry_msgs::PoseStamped ps; - ps.header.frame_id = map_frame_id_; - ps.header.stamp = current_time; - ps.pose = pose; - pub_pose_.publish(ps); - - geometry_msgs::TwistStamped ts; - ts.header.frame_id = simulation_frame_id_; - ts.header.stamp = current_time; - ts.twist = twist; - pub_twist_.publish(ts); -} - -void Simulator::publishTF(const geometry_msgs::Pose & pose) -{ - ros::Time current_time = ros::Time::now(); - - // send odom transform - geometry_msgs::TransformStamped odom_trans; - odom_trans.header.stamp = current_time; - odom_trans.header.frame_id = map_frame_id_; - odom_trans.child_frame_id = simulation_frame_id_; - odom_trans.transform.translation.x = pose.position.x; - odom_trans.transform.translation.y = pose.position.y; - odom_trans.transform.translation.z = pose.position.z; - odom_trans.transform.rotation = pose.orientation; - tf_broadcaster_.sendTransform(odom_trans); -} - -double Simulator::getPosZFromTrajectory(const double x, const double y) -{ - // calculae cloest point on trajectory - /* - write me... - */ - if (current_trajectory_ptr_ != nullptr) { - const double max_sqrt_dist = 100.0 * 100.0; - double min_sqrt_dist = max_sqrt_dist; - int index; - bool found = false; - for (size_t i = 0; i < current_trajectory_ptr_->points.size(); ++i) { - const double dist_x = (current_trajectory_ptr_->points.at(i).pose.position.x - x); - const double dist_y = (current_trajectory_ptr_->points.at(i).pose.position.y - y); - double sqrt_dist = dist_x * dist_x + dist_y * dist_y; - if (sqrt_dist < min_sqrt_dist) { - min_sqrt_dist = sqrt_dist; - index = i; - found = true; - } - } - if (found) - return current_trajectory_ptr_->points.at(index).pose.position.z; - else - return 0; - } else { - return 0.0; - } -} - -geometry_msgs::Quaternion Simulator::getQuaternionFromRPY( - const double & roll, const double & pitch, const double & yaw) -{ - tf2::Quaternion q; - q.setRPY(roll, pitch, yaw); - return tf2::toMsg(q); -} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp deleted file mode 100644 index 8a7acafa9ecc8..0000000000000 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp +++ /dev/null @@ -1,25 +0,0 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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/simple_planning_simulator_core.hpp" - -int main(int argc, char ** argv) -{ - ros::init(argc, argv, "simple_planning_simulator"); - Simulator obj; - ros::spin(); - return 0; -}; \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp deleted file mode 100644 index 0b04d92c0b97e..0000000000000 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp +++ /dev/null @@ -1,65 +0,0 @@ - -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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_constant_acceleration.hpp" - -SimModelConstantAccelTwist::SimModelConstantAccelTwist( - double vx_lim, double wz_lim, double vx_rate, double wz_rate) -: SimModelInterface(5 /* dim x */, 2 /* dim u */), - vx_lim_(vx_lim), - wz_lim_(wz_lim), - vx_rate_(vx_rate), - wz_rate_(wz_rate){}; - -double SimModelConstantAccelTwist::getX() { return state_(IDX::X); }; -double SimModelConstantAccelTwist::getY() { return state_(IDX::Y); }; -double SimModelConstantAccelTwist::getYaw() { return state_(IDX::YAW); }; -double SimModelConstantAccelTwist::getVx() { return state_(IDX::VX); }; -double SimModelConstantAccelTwist::getWz() { return state_(IDX::WZ); }; -double SimModelConstantAccelTwist::getSteer() { return 0.0; }; -void SimModelConstantAccelTwist::update(const double & dt) { updateRungeKutta(dt, input_); } -Eigen::VectorXd SimModelConstantAccelTwist::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double vel = state(IDX::VX); - const double angvel = state(IDX::WZ); - const double yaw = state(IDX::YAW); - const double vx_des = std::max(std::min(input(IDX_U::VX_DES), vx_lim_), -vx_lim_); - const double wz_des = std::max(std::min(input(IDX_U::WZ_DES), wz_lim_), -wz_lim_); - double vx_rate = 0.0; - double wz_rate = 0.0; - if (vx_des > vel) { - vx_rate = vx_rate_; - } else if (vx_des < vel) { - vx_rate = -vx_rate_; - } - - if (wz_des > angvel) { - wz_rate = wz_rate_; - } else if (wz_des < angvel) { - wz_rate = -wz_rate_; - } - - Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = vel * cos(yaw); - d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = angvel; - d_state(IDX::VX) = vx_rate; - d_state(IDX::WZ) = wz_rate; - - return d_state; -}; \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp deleted file mode 100644 index a955846118520..0000000000000 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp +++ /dev/null @@ -1,107 +0,0 @@ - -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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_ideal.hpp" - -SimModelIdealTwist::SimModelIdealTwist() : SimModelInterface(3 /* dim x */, 2 /* dim u */){}; - -double SimModelIdealTwist::getX() { return state_(IDX::X); }; -double SimModelIdealTwist::getY() { return state_(IDX::Y); }; -double SimModelIdealTwist::getYaw() { return state_(IDX::YAW); }; -double SimModelIdealTwist::getVx() { return input_(IDX_U::VX_DES); }; -double SimModelIdealTwist::getWz() { return input_(IDX_U::WZ_DES); }; -double SimModelIdealTwist::getSteer() { return 0.0; }; -void SimModelIdealTwist::update(const double & dt) { updateRungeKutta(dt, input_); } -Eigen::VectorXd SimModelIdealTwist::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double yaw = state(IDX::YAW); - const double vx = input(IDX_U::VX_DES); - const double wz = input(IDX_U::WZ_DES); - - 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) = wz; - - return d_state; -}; - -SimModelIdealSteer::SimModelIdealSteer(double wheelbase) -: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase){}; - -double SimModelIdealSteer::getX() { return state_(IDX::X); }; -double SimModelIdealSteer::getY() { return state_(IDX::Y); }; -double SimModelIdealSteer::getYaw() { return state_(IDX::YAW); }; -double SimModelIdealSteer::getVx() { return input_(IDX_U::VX_DES); }; -double SimModelIdealSteer::getWz() -{ - return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; -}; -double SimModelIdealSteer::getSteer() { return input_(IDX_U::STEER_DES); }; -void SimModelIdealSteer::update(const double & dt) { updateRungeKutta(dt, input_); } -Eigen::VectorXd SimModelIdealSteer::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double yaw = state(IDX::YAW); - const double vx = input(IDX_U::VX_DES); - const double steer = input(IDX_U::STEER_DES); - - 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_; - - return d_state; -}; - -SimModelIdealAccel::SimModelIdealAccel(double wheelbase) -: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase){}; - -double SimModelIdealAccel::getX() { return state_(IDX::X); }; -double SimModelIdealAccel::getY() { return state_(IDX::Y); }; -double SimModelIdealAccel::getYaw() { return state_(IDX::YAW); }; -double SimModelIdealAccel::getVx() { return state_(IDX::VX); }; -double SimModelIdealAccel::getWz() -{ - return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; -}; -double SimModelIdealAccel::getSteer() { return input_(IDX_U::STEER_DES); }; -void SimModelIdealAccel::update(const double & dt) -{ - updateRungeKutta(dt, input_); - if (state_(IDX::VX) < 0) { - state_(IDX::VX) = 0; - } -} - -Eigen::VectorXd SimModelIdealAccel::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double vx = state(IDX::VX); - const double yaw = state(IDX::YAW); - const double ax = input(IDX_U::AX_DES); - const double steer = input(IDX_U::STEER_DES); - - 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::VX) = ax; - d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; - - return d_state; -}; diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp deleted file mode 100644 index 5a1533bfbda29..0000000000000 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp +++ /dev/null @@ -1,41 +0,0 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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_interface.hpp" - -SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) -{ - state_ = Eigen::VectorXd::Zero(dim_x_); - input_ = Eigen::VectorXd::Zero(dim_u_); -}; - -void SimModelInterface::updateRungeKutta(const double & dt, const Eigen::VectorXd & input) -{ - Eigen::VectorXd k1 = calcModel(state_, input); - Eigen::VectorXd k2 = calcModel(state_ + k1 * 0.5 * dt, input); - Eigen::VectorXd k3 = calcModel(state_ + k2 * 0.5 * dt, input); - Eigen::VectorXd k4 = calcModel(state_ + k3 * dt, input); - - state_ += 1.0 / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4) * dt; -} -void SimModelInterface::updateEuler(const double & dt, const Eigen::VectorXd & input) -{ - state_ += calcModel(state_, input) * dt; -} -void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; }; -void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; }; -void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; }; -void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; }; diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp deleted file mode 100644 index d0208ca839785..0000000000000 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp +++ /dev/null @@ -1,305 +0,0 @@ - -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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_time_delay.hpp" - -/* - * - * SimModelTimeDelayTwist - * - */ - -SimModelTimeDelayTwist::SimModelTimeDelayTwist( - double vx_lim, double wz_lim, double vx_rate_lim, double wz_rate_lim, double dt, double vx_delay, - double vx_time_constant, double wz_delay, double wz_time_constant) -: SimModelInterface(5 /* dim x */, 2 /* dim u */), - MIN_TIME_CONSTANT(0.03), - vx_lim_(vx_lim), - vx_rate_lim_(vx_rate_lim), - wz_lim_(wz_lim), - wz_rate_lim_(wz_rate_lim), - vx_delay_(vx_delay), - vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), - wz_delay_(wz_delay), - wz_time_constant_(std::max(wz_time_constant, MIN_TIME_CONSTANT)) -{ - if (vx_time_constant < MIN_TIME_CONSTANT) { - ROS_WARN("Settings vx_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); - } - if (wz_time_constant < MIN_TIME_CONSTANT) { - ROS_WARN("Settings wz_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); - } - initializeInputQueue(dt); -}; - -double SimModelTimeDelayTwist::getX() { return state_(IDX::X); }; -double SimModelTimeDelayTwist::getY() { return state_(IDX::Y); }; -double SimModelTimeDelayTwist::getYaw() { return state_(IDX::YAW); }; -double SimModelTimeDelayTwist::getVx() { return state_(IDX::VX); }; -double SimModelTimeDelayTwist::getWz() { return state_(IDX::WZ); }; -double SimModelTimeDelayTwist::getSteer() { return 0.0; }; -void SimModelTimeDelayTwist::update(const double & 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(); - wz_input_queue_.push_back(input_(IDX_U::WZ_DES)); - delayed_input(IDX_U::WZ_DES) = wz_input_queue_.front(); - wz_input_queue_.pop_front(); - - updateRungeKutta(dt, delayed_input); -}; -void SimModelTimeDelayTwist::initializeInputQueue(const double & dt) -{ - size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); - for (size_t i = 0; i < vx_input_queue_size; i++) { - vx_input_queue_.push_back(0.0); - } - size_t wz_input_queue_size = static_cast(round(wz_delay_ / dt)); - for (size_t i = 0; i < wz_input_queue_size; i++) { - wz_input_queue_.push_back(0.0); - } -} - -Eigen::VectorXd SimModelTimeDelayTwist::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double vx = state(IDX::VX); - const double wz = state(IDX::WZ); - const double yaw = state(IDX::YAW); - const double delay_input_vx = input(IDX_U::VX_DES); - const double delay_input_wz = input(IDX_U::WZ_DES); - const double delay_vx_des = std::max(std::min(delay_input_vx, vx_lim_), -vx_lim_); - const double delay_wz_des = std::max(std::min(delay_input_wz, wz_lim_), -wz_lim_); - double vx_rate = -(vx - delay_vx_des) / vx_time_constant_; - double wz_rate = -(wz - delay_wz_des) / wz_time_constant_; - vx_rate = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, vx_rate)); - wz_rate = std::min(wz_rate_lim_, std::max(-wz_rate_lim_, wz_rate)); - - 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) = wz; - d_state(IDX::VX) = vx_rate; - d_state(IDX::WZ) = wz_rate; - - return d_state; -}; - -/* - * - * SimModelTimeDelaySteer - * - */ -SimModelTimeDelaySteer::SimModelTimeDelaySteer( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double vx_delay, double vx_time_constant, double steer_delay, - double 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)) -{ - if (vx_time_constant < MIN_TIME_CONSTANT) { - ROS_WARN("Settings vx_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); - } - if (steer_time_constant < MIN_TIME_CONSTANT) { - ROS_WARN("Settings steer_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); - } - - initializeInputQueue(dt); -}; - -double SimModelTimeDelaySteer::getX() { return state_(IDX::X); }; -double SimModelTimeDelaySteer::getY() { return state_(IDX::Y); }; -double SimModelTimeDelaySteer::getYaw() { return state_(IDX::YAW); }; -double SimModelTimeDelaySteer::getVx() { return state_(IDX::VX); }; -double SimModelTimeDelaySteer::getWz() -{ - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; -}; -double SimModelTimeDelaySteer::getSteer() { return state_(IDX::STEER); }; -void SimModelTimeDelaySteer::update(const double & 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(); - - updateRungeKutta(dt, delayed_input); -}; -void SimModelTimeDelaySteer::initializeInputQueue(const double & dt) -{ - size_t vx_input_queue_size = static_cast(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(round(steer_delay_ / dt)); - for (size_t i = 0; i < steer_input_queue_size; i++) { - steer_input_queue_.push_back(0.0); - } -} - -Eigen::VectorXd SimModelTimeDelaySteer::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double vel = state(IDX::VX); - const double yaw = state(IDX::YAW); - const double steer = state(IDX::STEER); - const double delay_input_vel = input(IDX_U::VX_DES); - const double delay_input_steer = input(IDX_U::STEER_DES); - const double delay_vx_des = std::max(std::min(delay_input_vel, vx_lim_), -vx_lim_); - const double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); - double vx_rate = -(vel - delay_vx_des) / vx_time_constant_; - double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; - vx_rate = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, vx_rate)); - steer_rate = std::min(steer_rate_lim_, std::max(-steer_rate_lim_, steer_rate)); - - Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = vel * cos(yaw); - d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; - d_state(IDX::VX) = vx_rate; - d_state(IDX::STEER) = steer_rate; - - return d_state; -}; - -SimModelTimeDelaySteerAccel::SimModelTimeDelaySteerAccel( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant) -: SimModelInterface(6 /* dim x */, 3 /* 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), - acc_delay_(acc_delay), - acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), - steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) -{ - if (acc_time_constant < MIN_TIME_CONSTANT) { - ROS_WARN("Settings acc_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); - } - if (steer_time_constant < MIN_TIME_CONSTANT) { - ROS_WARN("Settings steer_time_constant is too small, replace it by %f", MIN_TIME_CONSTANT); - } - - initializeInputQueue(dt); -}; - -double SimModelTimeDelaySteerAccel::getX() { return state_(IDX::X); }; -double SimModelTimeDelaySteerAccel::getY() { return state_(IDX::Y); }; -double SimModelTimeDelaySteerAccel::getYaw() { return state_(IDX::YAW); }; -double SimModelTimeDelaySteerAccel::getVx() { return state_(IDX::VX); }; -double SimModelTimeDelaySteerAccel::getWz() -{ - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; -}; -double SimModelTimeDelaySteerAccel::getSteer() { return state_(IDX::STEER); }; -void SimModelTimeDelaySteerAccel::update(const double & dt) -{ - Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); - - acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); - delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); - acc_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(); - delayed_input(IDX_U::DRIVE_SHIFT) = input_(IDX_U::DRIVE_SHIFT); - - updateRungeKutta(dt, delayed_input); - // clip velocity and accel - if (delayed_input(IDX_U::DRIVE_SHIFT) >= 0.0) { - state_(IDX::VX) = std::max(0.0, std::min(state_(IDX::VX), vx_lim_)); - if ( - std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || std::abs((state_(IDX::VX) - vx_lim_)) < 10e-9) { - state_(IDX::ACCX) = 0.0; - } - } else { - state_(IDX::VX) = std::min(0.0, std::max(state_(IDX::VX), -vx_lim_)); - if ( - std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || - std::abs((state_(IDX::VX) - (-vx_lim_))) < 10e-9) { - state_(IDX::ACCX) = 0.0; - } - } -} - -void SimModelTimeDelaySteerAccel::initializeInputQueue(const double & dt) -{ - size_t vx_input_queue_size = static_cast(round(acc_delay_ / dt)); - for (size_t i = 0; i < vx_input_queue_size; i++) { - acc_input_queue_.push_back(0.0); - } - size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); - for (size_t i = 0; i < steer_input_queue_size; i++) { - steer_input_queue_.push_back(0.0); - } -} - -Eigen::VectorXd SimModelTimeDelaySteerAccel::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - double vel = state(IDX::VX); - double acc = state(IDX::ACCX); - const double yaw = state(IDX::YAW); - const double steer = state(IDX::STEER); - const double delay_input_acc = input(IDX_U::ACCX_DES); - const double delay_input_steer = input(IDX_U::STEER_DES); - const double drive_shift = input(IDX_U::DRIVE_SHIFT); - double delay_acc_des = std::max(std::min(delay_input_acc, vx_rate_lim_), -vx_rate_lim_); - if (!(drive_shift >= 0.0)) delay_acc_des *= -1; // reverse front-back - double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); - double accx_rate = -(acc - delay_acc_des) / acc_time_constant_; - double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; - acc = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, acc)); - steer_rate = std::min(steer_rate_lim_, std::max(-steer_rate_lim_, steer_rate)); - - if (drive_shift >= 0.0) { - vel = std::max(0.0, std::min(vel, vx_lim_)); - } else { - vel = std::min(0.0, std::max(vel, -vx_lim_)); - } - - Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = vel * cos(yaw); - d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; - d_state(IDX::VX) = acc; - d_state(IDX::STEER) = steer_rate; - d_state(IDX::ACCX) = accx_rate; - - return d_state; -}; From edc7b368123977821e8b694ac4dc4eb179276aac Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Tue, 29 Sep 2020 19:22:55 +0900 Subject: [PATCH 03/52] add sample ros2 packages Signed-off-by: mitsudome-r --- .../simple_planning_simulator/CMakeLists.txt | 68 +++ ...qt_multiplot_simple_planning_simulator.xml | 317 ++++++++++++ .../simple_planning_simulator.param.yaml | 39 ++ .../simple_planning_simulator_core.hpp | 253 ++++++++++ .../sim_model_constant_acceleration.hpp | 116 +++++ .../vehicle_model/sim_model_ideal.hpp | 257 ++++++++++ .../vehicle_model/sim_model_interface.hpp | 138 +++++ .../vehicle_model/sim_model_time_delay.hpp | 359 +++++++++++++ .../vehicle_model/sim_model_util.hpp | 22 + .../launch/simple_planning_simulator.launch | 36 ++ .../simple_planning_simulator.launch.py | 63 +++ .../simple_planning_simulator/package.xml | 30 ++ .../scripts/README_fitParamDelayInputModel.md | 28 ++ .../scripts/fitParamDelayInputModel.py | 139 ++++++ .../src/simple_planning_simulator_core.cpp | 472 ++++++++++++++++++ .../src/simple_planning_simulator_node.cpp | 28 ++ .../sim_model_constant_acceleration.cpp | 65 +++ .../src/vehicle_model/sim_model_ideal.cpp | 107 ++++ .../src/vehicle_model/sim_model_interface.cpp | 41 ++ .../vehicle_model/sim_model_time_delay.cpp | 312 ++++++++++++ .../src/vehicle_model/sim_model_util.cpp | 32 ++ 21 files changed, 2922 insertions(+) create mode 100644 simulator/simple_planning_simulator/CMakeLists.txt create mode 100644 simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml create mode 100644 simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp create mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator.launch create mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py create mode 100644 simulator/simple_planning_simulator/package.xml create mode 100644 simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md create mode 100644 simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp create mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt new file mode 100644 index 0000000000000..277f04af7e3bc --- /dev/null +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 3.5) +project(simple_planning_simulator) + +### Compile options +if(NOT CMAKE_CXX_STANDARD) + set(CMAKE_CXX_STANDARD 14) +endif() +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + # add_compile_options(-Wall -Wextra -Wpedantic) + add_compile_options(-Wno-unused-parameter) +endif() + +### Dependencies +find_package(ament_cmake REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(rclcpp REQUIRED) +find_package(std_msgs REQUIRED) +find_package(tf2 REQUIRED) +find_package(tf2_ros REQUIRED) +find_package(autoware_planning_msgs REQUIRED) +find_package(autoware_control_msgs REQUIRED) +find_package(autoware_vehicle_msgs REQUIRED) + +# Add path of include dir +include_directories(include) + +# Generate exe file +set(SIMPLE_PLANNING_SIMULATOR_SRC + src/simple_planning_simulator_core.cpp + src/vehicle_model/sim_model_interface.cpp + src/vehicle_model/sim_model_ideal.cpp + src/vehicle_model/sim_model_constant_acceleration.cpp + src/vehicle_model/sim_model_time_delay.cpp + src/vehicle_model/sim_model_util.cpp +) +add_executable(simple_planning_simulator_exe src/simple_planning_simulator_node.cpp ${SIMPLE_PLANNING_SIMULATOR_SRC}) + +# Add dependencies. use target_link_libaries() before Crystal +set(SIMPLE_PLANNING_SIMULATOR_DEPENDENCIES + rclcpp + std_msgs + geometry_msgs + tf2 + tf2_ros + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs +) +ament_target_dependencies(simple_planning_simulator_exe ${SIMPLE_PLANNING_SIMULATOR_DEPENDENCIES}) + + +## Install executables and/or libraries +install(TARGETS simple_planning_simulator_exe + ARCHIVE DESTINATION lib/${PROJECT_NAME} + LIBRARY DESTINATION lib/${PROJECT_NAME} + RUNTIME DESTINATION lib/${PROJECT_NAME} +) + +## Install project namespaced headers +install(DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include) + +install(DIRECTORY config launch + DESTINATION share/${PROJECT_NAME} +) + +# set at the end of cmakelists +ament_package() diff --git a/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml b/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml new file mode 100644 index 0000000000000..d75f6f4383a44 --- /dev/null +++ b/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml @@ -0,0 +1,317 @@ + + + + #242424 + #9e9e9e + false + false + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /sim_velocity + geometry_msgs/TwistStamped + + + twist/linear/x + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /sim_velocity + geometry_msgs/TwistStamped + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + sim + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_cmd + autoware_vehicle_msgs/VehicleCommandStamped + + + ctrl_cmd/linear_velocity + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_cmd + autoware_vehicle_msgs/VehicleCommandStamped + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + ctrl_cmd + + + + true + + 30 + ctrl_cmd : vx + + + + + + + + Untitled Axis + 0 + true + + + Untitled Axis + 0 + true + + + + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /sim_vehicle_status + autoware_msgs/VehicleStatus + + + angle + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /sim_vehicle_status + autoware_msgs/VehicleStatus + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + sim + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_cmd + autoware_vehicle_msgs/VehicleCommandStamped + + + ctrl_cmd/steering_angle + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_cmd + autoware_vehicle_msgs/VehicleCommandStamped + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + steer_cmd + + + + + + 1 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_status + autoware_msgs/VehicleStatus + + + angle + 0 + + 1000 + 0 + 0 + -1000 + 0 + + /vehicle_status + autoware_msgs/VehicleStatus + + + + #000000 + 0 + + + 1000 + 15 + 3 + + + 100 + actual + + + + true + + 30 + steer + + + + false +
+ diff --git a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml new file mode 100644 index 0000000000000..43f2426f1ea74 --- /dev/null +++ b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml @@ -0,0 +1,39 @@ +simulation: + simple_planning_simulator: + ros__parameters: + loop_rate: 50.0 + simulation_frame_id: base_link + map_frame_id: map + initialize_source: RVIZ + use_trajectory_for_z_position_source: true + + acc_time_constant: 0.1 + acc_time_delay: 0.1 + accel_rate: 7.0 + add_measurement_noise: true + angvel_lim: 3.0 + angvel_noise_stddev: 0.0 + angvel_rate: 1.0 + angvel_time_constant: 0.5 + angvel_time_delay: 0.2 + initial_engage_state: true + pos_noise_stddev: 0.01 + rpy_noise_stddev: 0.0001 + sim_steering_gear_ratio: 15.0 + steer_lim: 1.0 + steer_noise_stddev: 0.0001 + steer_rate_lim: 5.0 + steer_time_constant: 0.27 + steer_time_delay: 0.24 + tread_length: 1.0 + + # Option for vehicle_model_type: + # - IDEAL_STEER : reads velocity command. The steering and velocity changes exactly the same as commanded. + # - DELAY_STEER : reads velocity command. The steering and elocity changes with delay model. + # - DELAY_STEER_ACC : reads acceleration command. The steering and acceleration changes with delay model. + vehicle_model_type: DELAY_STEER_ACC + vel_lim: 50.0 + vel_noise_stddev: 0.0 + vel_time_constant: 0.61 + vel_time_delay: 0.25 + deadzone_delta_steer: 0.00 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp new file mode 100644 index 0000000000000..197a7ae0b4061 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -0,0 +1,253 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file simple_planning_simulator_core.hpp + * @brief vehicle dynamics simulation for autoware + * @author Takamasa Horibe + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_CORE_H_ +#define SIMPLE_PLANNING_SIMULATOR_CORE_H_ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include "simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" + +class Simulator : public rclcpp::Node +{ +public: + /** + * @brief constructor + */ + explicit Simulator(const std::string & node_name, const rclcpp::NodeOptions & options); + + /** + * @brief default destructor + */ + // ~Simulator() = default; + +private: + /* ros system */ + rclcpp::Publisher::SharedPtr pub_pose_; //!< @brief topic ros publisher for current pose + rclcpp::Publisher::SharedPtr pub_twist_; //!< @brief topic ros publisher for current twist + rclcpp::Publisher::SharedPtr pub_steer_; + rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_turn_signal_; + rclcpp::Publisher::SharedPtr pub_shift_; + rclcpp::Publisher::SharedPtr pub_control_mode_; + + rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; //!< @brief topic subscriber for vehicle_cmd + rclcpp::Subscription::SharedPtr sub_turn_signal_cmd_; //!< @brief topic subscriber for turn_signal_cmd + rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief topic subscriber for trajectory used for z ppsition + rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief topic subscriber for initialpose topic + rclcpp::Subscription::SharedPtr sub_initialtwist_; //!< @brief topic subscriber for initialtwist topic + rclcpp::Subscription::SharedPtr sub_engage_; //!< @brief topic subscriber for engage topic + rclcpp::TimerBase::SharedPtr timer_simulation_; //!< @brief timer for simulation + + /* tf */ + std::shared_ptr tf_broadcaster_; + std::shared_ptr tf_listener_; + std::shared_ptr tf_buffer_; + + /* received & published topics */ + geometry_msgs::msg::PoseStamped::ConstSharedPtr initial_pose_ptr_; //!< @brief initial vehicle pose + geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr + initial_pose_with_cov_ptr_; //!< @brief initial vehicle pose with cov + geometry_msgs::msg::TwistStamped::ConstSharedPtr initial_twist_ptr_; //!< @brief initial vehicle velocity + geometry_msgs::msg::Pose current_pose_; //!< @brief current vehicle position and angle + geometry_msgs::msg::Twist current_twist_; //!< @brief current vehicle velocity + autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr + current_vehicle_cmd_ptr_; //!< @brief latest received vehicle_cmd + autoware_planning_msgs::msg::Trajectory::ConstSharedPtr + current_trajectory_ptr_; //!< @brief latest received trajectory + double closest_pos_z_; //!< @brief z position on closest trajectory + autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr current_turn_signal_cmd_ptr_; + autoware_vehicle_msgs::msg::ControlMode control_mode_; + + /* frame_id */ + std::string + simulation_frame_id_; //!< @brief vehicle frame id simulated by simple_planning_simulator + std::string map_frame_id_; //!< @brief map frame_id + + /* simple_planning_simulator parameters */ + double loop_rate_; //!< @brief frequency to calculate vehicle model & pubish result + double wheelbase_; //!< @brief wheelbase length to convert angular-velocity & steering + double sim_steering_gear_ratio_; //!< @brief for steering wheel angle calcultion + + /* flags */ + bool is_initialized_ = false; //!< @brief flag to check the initial position is set + bool add_measurement_noise_; //!< @brief flag to add measurement noise + bool simulator_engage_; //!< @brief flag to engage simulator + + /* saved values */ + std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time + + /* vehicle model */ + enum class VehicleModelType { + IDEAL_TWIST = 0, + IDEAL_STEER = 1, + DELAY_TWIST = 2, + DELAY_STEER = 3, + CONST_ACCEL_TWIST = 4, + IDEAL_FORKLIFT_RLS = 5, + DELAY_FORKLIFT_RLS = 6, + IDEAL_ACCEL = 7, + DELAY_STEER_ACC = 8, + } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics + std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer + + /* to generate measurement noise */ + std::shared_ptr rand_engine_ptr_; //!< @brief random engine for measurement noise + std::shared_ptr> + pos_norm_dist_ptr_; //!< @brief Gaussian noise for position + std::shared_ptr> + vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity + std::shared_ptr> + rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw + std::shared_ptr> + angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity + std::shared_ptr> + steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle + + /** + * @brief set current_vehicle_cmd_ptr_ with received message + */ + void callbackVehicleCmd(const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg); + + /** + * @brief set current_turn_signal_cmd_ptr with received message + */ + void callbackTurnSignalCmd(const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg); + + /** + * @brief set current_trajectory_ptr_ with received message + */ + void callbackTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + /** + * @brief set initial pose for simulation with received message + */ + void callbackInitialPoseWithCov(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + + /** + * @brief set initial pose with received message + */ + void callbackInitialPoseStamped(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + + /** + * @brief set initial twist with received message + */ + void callbackInitialTwistStamped(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); + + /** + * @brief set simulator engage with received message + */ + void callbackEngage(const std_msgs::msg::Bool::ConstSharedPtr msg); + + /** + * @brief get transform from two frame_ids + * @param [in] parent_frame parent frame id + * @param [in] child frame id + * @param [out] transform transform from parent frame to child frame + */ + void getTransformFromTF( + const std::string parent_frame, const std::string child_frame, + geometry_msgs::msg::TransformStamped & transform); + + /** + * @brief timer callback for simulation with loop_rate + */ + void timerCallbackSimulation(); + + /** + * @brief set initial state of simulated vehicle + * @param [in] pose initial position and orientation + * @param [in] twist initial velocity and angular velocity + */ + void setInitialState(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist); + + /** + * @brief set initial state of simulated vehicle with pose transformation based on frame_id + * @param [in] pose initial position and orientation with header + * @param [in] twist initial velocity and angular velocity + */ + void setInitialStateWithPoseTransform( + const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & twist); + + /** + * @brief set initial state of simulated vehicle with pose transformation based on frame_id + * @param [in] pose initial position and orientation with header + * @param [in] twist initial velocity and angular velocity + */ + void setInitialStateWithPoseTransform( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const geometry_msgs::msg::Twist & twist); + + /** + * @brief publish pose and twist + * @param [in] pose pose to be published + * @param [in] twist twist to be published + */ + void publishPoseTwist(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist); + + /** + * @brief publish tf + * @param [in] pose pose used for tf + */ + void publishTF(const geometry_msgs::msg::Pose & pose); + + /** + * @brief update closest pose to calculate pos_z + */ + double getPosZFromTrajectory(const double x, const double y); + + /** + * @brief convert roll-pitch-yaw Eular angle to ros Quaternion + * @param [in] roll roll angle [rad] + * @param [in] pitch pitch angle [rad] + * @param [in] yaw yaw angle [rad] + */ + geometry_msgs::msg::Quaternion getQuaternionFromRPY( + const double & roll, const double & pitch, const double & yaw); +}; + +#endif diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp new file mode 100644 index 0000000000000..26bfb4437a29d --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp @@ -0,0 +1,116 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file sim_model_constant_acceleration.hpp + * @brief simple planning simulator model with constant acceleration for velocity & steeiring + * @author Takamasa Horibe + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_CONSTANT_ACCELERATION_H_ +#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_CONSTANT_ACCELERATION_H_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include +#include + +/** + * @class simple_planning_simulator constant acceleration twist model + * @brief calculate velocity & angular-velocity with constant acceleration + */ +class SimModelConstantAccelTwist : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] wz_lim angular velocity limit [m/s] + * @param [in] vx_rate acceleration for velocity [m/ss] + * @param [in] wz_rate acceleration for angular velocity [rad/ss] + */ + SimModelConstantAccelTwist(double vx_lim, double wz_lim, double vx_rate, double wz_rate); + + /** + * @brief default destructor + */ + ~SimModelConstantAccelTwist() = default; + +private: + enum IDX { + X = 0, + Y, + YAW, + VX, + WZ, + }; + enum IDX_U { + VX_DES = 0, + WZ_DES, + }; + + const double vx_lim_; //!< @brief velocity limit + const double wz_lim_; //!< @brief angular velocity limit + const double vx_rate_; //!< @brief velocity rate + const double wz_rate_; //!< @brief angular velocity rate + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with constant acceleration + * @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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp new file mode 100644 index 0000000000000..25fead30d403d --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp @@ -0,0 +1,257 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file sim_model_ideal.hpp + * @brief simple planning simulator ideal velocity model (no dynamics for desired velocity & anguler-velocity or + * steering) + * @author Takamasa Horibe + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_IDEAL_H_ +#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_IDEAL_H_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include +#include +#include + +/** + * @class simple_planning_simulator ideal twist model + * @brief calculate ideal twist dynamics + */ +class SimModelIdealTwist : public SimModelInterface +{ +public: + /** + * @brief constructor + */ + SimModelIdealTwist(); + + /** + * @brief destructor + */ + ~SimModelIdealTwist() = default; + +private: + enum IDX { + X = 0, + Y, + YAW, + }; + enum IDX_U { + VX_DES = 0, + WZ_DES, + }; + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with ideal twist 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; +}; + +/** + * @class simple_planning_simulator ideal steering model + * @brief calculate ideal steering dynamics + */ +class SimModelIdealSteer : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + SimModelIdealSteer(double wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealSteer() = default; + +private: + enum IDX { + X = 0, + Y, + YAW, + }; + enum IDX_U { + VX_DES = 0, + STEER_DES, + }; + + const double wheelbase_; //!< @brief vehicle wheelbase length + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with ideal 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; +}; + +/** + * @class wf_simulator ideal acceleration and steering model + * @brief calculate ideal steering dynamics + */ +class SimModelIdealAccel : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + SimModelIdealAccel(double wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealAccel() = default; + +private: + enum IDX { + X = 0, + Y, + YAW, + VX, + }; + enum IDX_U { + AX_DES = 0, + STEER_DES, + }; + + const double wheelbase_; //!< @brief vehicle wheelbase length + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with ideal 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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp new file mode 100644 index 0000000000000..480f521d9548b --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -0,0 +1,138 @@ +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file sim_model_interface.hpp + * @brief simple planning simulator model interface class + * @author Takamasa Horibe + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ +#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ + +#include +#include + +/** + * @class simple_planning_simulator vehicle model class + * @brief calculate vehicle dynamics + */ +class SimModelInterface +{ +protected: + const int dim_x_; //!< @brief dimension of state x + const int dim_u_; //!< @brief dimension of input u + Eigen::VectorXd state_; //!< @brief vehicle state vector + Eigen::VectorXd input_; //!< @brief vehicle input vector + +public: + /** + * @brief constructor + * @param [in] dim_x dimension of state x + * @param [in] dim_u dimension of input u + */ + SimModelInterface(int dim_x, int dim_u); + + /** + * @brief destructor + */ + ~SimModelInterface() = default; + + /** + * @brief get state vector of model + * @param [out] state state vector + */ + void getState(Eigen::VectorXd & state); + + /** + * @brief get input vector of model + * @param [out] input input vector + */ + void getInput(Eigen::VectorXd & input); + + /** + * @brief set state vector of model + * @param [in] state state vector + */ + void setState(const Eigen::VectorXd & state); + + /** + * @brief set input vector of model + * @param [in] input input vector + */ + void setInput(const Eigen::VectorXd & input); + + /** + * @brief update vehicle states with Runge-Kutta methods + * @param [in] dt delta time [s] + * @param [in] input vehicle input + */ + void updateRungeKutta(const double & dt, const Eigen::VectorXd & input); + + /** + * @brief update vehicle states with Euler methods + * @param [in] dt delta time [s] + * @param [in] input vehicle input + */ + void updateEuler(const double & dt, const Eigen::VectorXd & input); + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + virtual void update(const double & dt) = 0; + + /** + * @brief get vehicle position x + */ + virtual double getX() = 0; + + /** + * @brief get vehicle position y + */ + virtual double getY() = 0; + + /** + * @brief get vehicle angle yaw + */ + virtual double getYaw() = 0; + + /** + * @brief get vehicle velocity vx + */ + virtual double getVx() = 0; + + /** + * @brief get vehicle angular-velocity wz + */ + virtual double getWz() = 0; + + /** + * @brief get vehicle steering angle + */ + virtual double getSteer() = 0; + + /** + * @brief calculate derivative of states with vehicle model + * @param [in] state current model state + * @param [in] input input vector to model + */ + virtual Eigen::VectorXd calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) = 0; +}; + +#endif diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp new file mode 100644 index 0000000000000..d38d8e2adc578 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp @@ -0,0 +1,359 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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. + */ + +/** + * @file sim_model_time_delay.hpp + * @brief simple planning simulator model with time delay and 1-dimensional dynamics for velocity & steeiring + * @author Takamasa Horibe, Kim-Ngoc-Khanh Nguyen + * @date 2019.08.17 + */ + +#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_TIME_DELAY_H_ +#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_TIME_DELAY_H_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_util.hpp" + +#include +#include +#include +#include +#include + +/** + * @class simple_planning_simulator time delay twist model + * @brief calculate time delay twist dynamics + */ +class SimModelTimeDelayTwist : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] vx_lim velocity limit [m/s] + * @param [in] angvel_lim angular velocity limit [m/s] + * @param [in] vx_rate_lim acceleration limit [m/ss] + * @param [in] wz_rate_lim angular acceleration llimit [rad/ss] + * @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] wx_delay time delay for angular-velocity command [s] + * @param [in] wz_time_constant time constant for 1D model of angular-velocity dynamics + * @param [in] deadzone_delta_steer deadzone value of steer + */ + SimModelTimeDelayTwist( + double vx_lim, double angvel_lim, double vx_rate_lim, double wz_rate_lim, double dt, + double vx_delay, double vx_time_constant, double wz_delay, double wz_time_constant, + double deadzone_delta_steer); + + /** + * @brief default destructor + */ + ~SimModelTimeDelayTwist() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + WZ, + }; + enum IDX_U { + VX_DES = 0, + WZ_DES, + }; + + const double vx_lim_; //!< @brief velocity limit + const double vx_rate_lim_; //!< @brief acceleration limit + const double wz_lim_; //!< @brief angular velocity limit + const double wz_rate_lim_; //!< @brief angular acceleration limit + + std::deque vx_input_queue_; //!< @brief buffer for velocity command + std::deque wz_input_queue_; //!< @brief buffer for angular velocity command + const double vx_delay_; //!< @brief time delay for velocity command [s] + const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics + const double wz_delay_; //!< @brief time delay for angular-velocity command [s] + const double + wz_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics + const double deadzone_delta_steer_; //!<@ brief deadzone value of steer + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time delay twist 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; +}; + +class SimModelTimeDelaySteer : 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 + * @param [in] deadzone_delta_steer deadzone value of steer + */ + SimModelTimeDelaySteer( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant, double deadzone_delta_steer); + + /** + * @brief default destructor + */ + ~SimModelTimeDelaySteer() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + }; + enum IDX_U { + VX_DES = 0, + STEER_DES, + }; + + const double vx_lim_; //!< @brief velocity limit [m/s] + const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const double steer_lim_; //!< @brief steering limit [rad] + const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_; //!< @brief vehicle wheelbase length [m] + + std::deque vx_input_queue_; //!< @brief buffer for velocity command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double vx_delay_; //!< @brief time delay for velocity command [s] + const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics + const double steer_delay_; //!< @brief time delay for steering command [s] + const double steer_time_constant_; //!< @brief time constant for 1D model of steering dynamics + const double deadzone_delta_steer_; //!<@ brief deadzone value of steer + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time 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; +}; + +class SimModelTimeDelaySteerAccel : 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] acc_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + * @param [in] deadzone_delta_steer deadzone value of steer + */ + SimModelTimeDelaySteerAccel( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double acc_delay, double acc_time_constant, double steer_delay, + double steer_time_constant, double deadzone_delta_steer); + + /** + * @brief default destructor + */ + ~SimModelTimeDelaySteerAccel() = default; + +private: + const double MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U { + ACCX_DES = 0, + STEER_DES, + DRIVE_SHIFT, + }; + + const double vx_lim_; //!< @brief velocity limit [m/s] + const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] + const double steer_lim_; //!< @brief steering limit [rad] + const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] + const double wheelbase_; //!< @brief vehicle wheelbase length [m] + + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const double acc_delay_; //!< @brief time delay for accel command [s] + const double acc_time_constant_; //!< @brief time constant for 1D model of accel dynamics + const double steer_delay_; //!< @brief time delay for steering command [s] + const double steer_time_constant_; //!< @brief time constant for 1D model of steering dynamics + const double deadzone_delta_steer_; //!<@ brief deadzone value of steer + + /** + * @brief set queue buffer for input command + * @param [in] dt delta time + */ + void initializeInputQueue(const double & dt); + + /** + * @brief get vehicle position x + */ + double getX() override; + + /** + * @brief get vehicle position y + */ + double getY() override; + + /** + * @brief get vehicle angle yaw + */ + double getYaw() override; + + /** + * @brief get vehicle velocity vx + */ + double getVx() override; + + /** + * @brief get vehicle angular-velocity wz + */ + double getWz() override; + + /** + * @brief get vehicle steering angle + */ + double getSteer() override; + + /** + * @brief update vehicle states + * @param [in] dt delta time [s] + */ + void update(const double & dt) override; + + /** + * @brief calculate derivative of states with time 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 \ No newline at end of file diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp new file mode 100644 index 0000000000000..2911e9e2dff60 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp @@ -0,0 +1,22 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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 + +namespace sim_model_util +{ +double getDummySteerCommandWithFriction( + const double steer, const double steer_command, const double deadzone_delta_steer); +} diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch new file mode 100644 index 0000000000000..2bae8b588fb7b --- /dev/null +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch @@ -0,0 +1,36 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py new file mode 100644 index 0000000000000..54f89ca61d8f5 --- /dev/null +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -0,0 +1,63 @@ +from launch import LaunchContext +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument +from launch.conditions import IfCondition +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackage +from pathlib import Path + +import os + + + +context = LaunchContext() + + +def find_pack(package_name): + """Return the absolute path to the share directory of the given package.""" + return os.path.join(Path(FindPackage(package_name).perform(context)), 'share', package_name) + +def generate_launch_description(): + + simple_planning_simulator_param_file = os.path.join(find_pack('simple_planning_simulator'), 'config/simple_planning_simulator.param.yaml') + + print(simple_planning_simulator_param_file) + + simple_planning_simulator_param = DeclareLaunchArgument( + 'simple_planning_simulator_param_file', + default_value=simple_planning_simulator_param_file, + description='Path to config file for simple_planning_simulator' + ) + + simple_planning_simulator = Node( + package='simple_planning_simulator', + node_executable='simple_planning_simulator_exe', + node_name='simple_planning_simulator', + node_namespace='simulation', + output='screen', + parameters=[LaunchConfiguration('simple_planning_simulator_param_file'), + { + "/vehicle_info/wheel_base": 4.0, + "random_seed": 1 + } + ], + remappings=[ + ('base_trajectory', '/planning/scenario_planning/trajectory'), + ('output/current_pose', 'current_pose'), + ('output/current_twist', '/vehicle/status/twist'), + ('output/status', '/vehicle/status'), + ('output/control_mode', '/vehicle/status/control_mode'), + ('input/vehicle_cmd', '/control/vehicle_cmd'), + ('input/turn_signal_cmd', '/control/turn_signal_cmd'), + ('input/initial_twist', '/initialtwist'), + ('input/initial_pose', '/initialpose'), + ('input/engage', '/vehicle/engage'), + ] + ) + + + return LaunchDescription([ + simple_planning_simulator_param, + simple_planning_simulator + ]) diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml new file mode 100644 index 0000000000000..407d04d74541d --- /dev/null +++ b/simulator/simple_planning_simulator/package.xml @@ -0,0 +1,30 @@ + + + + simple_planning_simulator + 0.0.2 + simple_planning_simulator as a ROS 2 node + Takamasa HORIBE + Apache License 2.0 + + ament_cmake + rclcpp + rclcpp_components + + std_msgs + geometry_msgs + tf2 + tf2_ros + autoware_planning_msgs + autoware_control_msgs + autoware_vehicle_msgs + + rclcpp + rclcpp_components + launch_ros + rosidl_default_runtime + + + ament_cmake + + diff --git a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md new file mode 100644 index 0000000000000..511852ede903f --- /dev/null +++ b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md @@ -0,0 +1,28 @@ +# fitParamDelayInputModel.py scripts +## How to use +``` +python fitParamDelayInputModel.py --bag_file [bagfile] (--cutoff_time [cutoff_time] --cutoff_freq [cutoff_freq] --min_delay [min_delay] --max_delay [max_delay] --delay_incr [delay_incr]) +# in round brakets is optional arguments +python fitParamDelayInputModel.py --help # for more information +``` + +## Requirements python packages: +* numpy +* pandas + +## Required topics +* autoware_msgs::VehicleCmd /vehicle_cmd: assuming + * vehicle_cmd/ctrl_cmd/steering_angle is the steering angle input [rad] + * vehicle_cmd/ctrl_cmd/linear_velocity is the vehicle velocity input [m/s] +* autoware_msgs::VehicleStatus /vehicle_status : assuming + * vehicle_status/angle is the measured steering angle [rad] + * vehicle_status/speed is the measured vehicle speed [km/h] + +## Description +* Paramter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input +* Arguments explaining: + * CUTOFF_TIME: Cutoff time[sec]. Rosbag file often was start recording before autoware was run. Time before autoware was run should be cut off. This script will only consider data from t=cutoff_time to the end of the bag file (default is 1.0) + * CUTOFF_FREQ: Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1) + * MIN_DELAY: Min value for searching delay loop (default is 0.1) + * MAX_DELAY: Max value for searching delay loop (default is 1.0) + * DELAY_INCR: Step value for searching delay loop (default is 0.01) diff --git a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py new file mode 100644 index 0000000000000..b40899e929222 --- /dev/null +++ b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py @@ -0,0 +1,139 @@ +#!/usr/bin/env python +# -*- coding: utf-8 -*- + +import numpy as np +import argparse +import subprocess +import sys +from os import getcwd +from os.path import dirname, basename, splitext, join, exists +try: + import pandas as pd +except ImportError: + print ('Please install pandas. See http://pandas.pydata.org/pandas-docs/stable/') + sys.exit(1) + +FREQ_SAMPLE = 0.001 + +# low pass filter +def lowpass_filter(data, cutoff_freq=2, order=1, dt=FREQ_SAMPLE): + tau = 1.0 / (2 * np.pi * cutoff_freq) + for _ in range(order): + for i in range(1,len(data)): + data[i] = (tau / (tau + dt) * data[i-1] + dt / (tau + dt) * data[i]) + return data + +def rel2abs(path): + ''' + Return absolute path from relative path input + ''' + return join(getcwd(), path) + +def rosbag_to_csv(path, topic_name): + name = splitext(basename(path))[0] + suffix = topic_name.replace('/', '-') + output_path = join(dirname(path), name + '_' + suffix + '.csv') + if exists(output_path): + return output_path + else: + command = "rostopic echo -b {0} -p /{1} | sed -e 's/,/ /g' > {2}".format(path, topic_name, output_path) + print (command) + subprocess.check_call(command, shell=True) + return output_path + +def getActValue(df, speed_type): + tm = np.array(list(df['%time'])) * 1e-9 + # Unit Conversion + if speed_type: + val = np.array(list(df['field'])) / 3.6 + else: + val = np.array(list(df['field'])) + # Calc differential + dval = (val[2:] - val[:-2]) / (tm[2:] - tm[:-2]) + return tm[1:-1], val[1:-1], dval + +def getCmdValueWithDelay(df, delay): + tm = np.array(list(df['%time'])) * 1e-9 + val = np.array(list(df['field'])) + return tm + delay, val + +def getLinearInterpolate(_tm, _val, _index, ti): + tmp_t = _tm[_index] + tmp_nextt = _tm[_index + 1] + tmp_val = _val[_index] + tmp_nextval = _val[_index + 1] + val_i = tmp_val + (tmp_nextval - tmp_val) / (tmp_nextt - tmp_t) * (ti - tmp_t) + return val_i + +def getFittingTimeConstantParam(cmd_data, act_data, \ + delay, args, speed_type = False): + tm_cmd, cmd_delay = getCmdValueWithDelay(cmd_data, delay) + tm_act, act, dact = getActValue(act_data, speed_type) + _t_min = max(tm_cmd[0], tm_act[0]) + _t_max = min(tm_cmd[-1], tm_act[-1]) + tm_cmd = tm_cmd - _t_min + tm_act = tm_act - _t_min + MAX_CNT = int((_t_max - _t_min - args.cutoff_time) / FREQ_SAMPLE) + dact_samp = [None] * MAX_CNT + diff_actcmd_samp = [None] * MAX_CNT + ind_cmd = 0 + ind_act = 0 + for ind in range(MAX_CNT): + ti = ind * FREQ_SAMPLE + args.cutoff_time + while (tm_cmd[ind_cmd + 1] < ti): + ind_cmd += 1 + cmd_delay_i = getLinearInterpolate(tm_cmd, cmd_delay, ind_cmd, ti) + while (tm_act[ind_act + 1] < ti): + ind_act += 1 + act_i = getLinearInterpolate(tm_act, act, ind_act, ti) + dact_i = getLinearInterpolate(tm_act, dact, ind_act, ti) + dact_samp[ind] = dact_i + diff_actcmd_samp[ind] = act_i - cmd_delay_i + dact_samp = np.array(dact_samp) + diff_actcmd_samp = np.array(diff_actcmd_samp) + if args.cutoff_freq > 0: + dact_samp = lowpass_filter(dact_samp, cutoff_freq=args.cutoff_freq) + diff_actcmd_samp = lowpass_filter(diff_actcmd_samp, cutoff_freq=args.cutoff_freq) + dact_samp = dact_samp.reshape(1,-1) + diff_actcmd_samp = diff_actcmd_samp.reshape(1,-1) + tau = -np.dot(diff_actcmd_samp, np.linalg.pinv(dact_samp))[0,0] + error = np.linalg.norm(diff_actcmd_samp + tau * dact_samp) / dact_samp.shape[1] + return tau, error + +def getFittingParam(cmd_data, act_data, args, speed_type = False): + delay_range = int((args.max_delay - args.min_delay) / args.delay_incr) + delays = [args.min_delay + i * args.delay_incr for i in range(delay_range + 1)] + error_min = 1.0e10 + delay_opt = -1 + tau_opt = -1 + for delay in delays: + tau, error = getFittingTimeConstantParam(cmd_data, act_data, delay, args, speed_type=speed_type) + if tau > 0: + if error < error_min: + error_min = error + delay_opt = delay + tau_opt = tau + else: + break + return tau_opt, delay_opt, error_min + +if __name__ == '__main__': + topics = [ 'vehicle_cmd/ctrl_cmd/steering_angle', 'vehicle_status/angle', \ + 'vehicle_cmd/ctrl_cmd/linear_velocity', 'vehicle_status/speed'] + pd_data = [None] * len(topics) + parser = argparse.ArgumentParser(description='Paramter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input') + parser.add_argument('--bag_file', '-b', required=True, type=str, help='rosbag file', metavar='file') + parser.add_argument('--cutoff_time', default=1.0, type=float, help='Cutoff time[sec], Parameter fitting will only consider data from t= cutoff_time to the end of the bag file (default is 1.0)') + parser.add_argument('--cutoff_freq', default=0.1, type=float, help='Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1)') + parser.add_argument('--min_delay', default=0.1, type=float, help='Min value for searching delay loop (default is 0.1)') + parser.add_argument('--max_delay', default=1.0, type=float, help='Max value for searching delay loop (default is 1.0)') + parser.add_argument('--delay_incr', default=0.01, type=float, help='Step value for searching delay loop (default is 0.01)') + args = parser.parse_args() + + for i, topic in enumerate(topics): + csv_log = rosbag_to_csv(rel2abs(args.bag_file), topic) + pd_data[i] = pd.read_csv(csv_log, sep=' ') + tau_opt, delay_opt, error = getFittingParam(pd_data[0], pd_data[1], args, speed_type=False) + print ('Steer angle: tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' %(tau_opt, delay_opt, error)) + tau_opt, delay_opt, error = getFittingParam(pd_data[2], pd_data[3], args, speed_type=True) + print ('Velocity : tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' %(tau_opt, delay_opt, error)) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp new file mode 100644 index 0000000000000..f24db2604d05b --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -0,0 +1,472 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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/simple_planning_simulator_core.hpp" + +Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & options) +: Node(node_name, options) +{ + /* simple_planning_simulator parameters */ + loop_rate_ = declare_parameter("loop_rate").get(); + wheelbase_ = declare_parameter("/vehicle_info/wheel_base").get(); + sim_steering_gear_ratio_ = declare_parameter("sim_steering_gear_ratio").get(); + simulation_frame_id_ = declare_parameter("simulation_frame_id").get(); + map_frame_id_ = declare_parameter("map_frame_id").get(); + add_measurement_noise_ = declare_parameter("add_measurement_noise").get(); + + /* set pub sub topic name */ + pub_pose_ = + create_publisher("output/current_pose", rclcpp::QoS{1}); + pub_twist_ = + create_publisher("output/current_twist", rclcpp::QoS{1}); + pub_control_mode_ = create_publisher( + "output/control_mode", rclcpp::QoS{1}); + pub_steer_ = create_publisher( + "/vehicle/status/steering", rclcpp::QoS{1}); + pub_velocity_ = + create_publisher("/vehicle/status/velocity", rclcpp::QoS{1}); + pub_turn_signal_ = create_publisher( + "/vehicle/status/turn_signal", rclcpp::QoS{1}); + pub_shift_ = create_publisher( + "/vehicle/status/shift", rclcpp::QoS{1}); + + sub_vehicle_cmd_ = create_subscription( + "input/vehicle_cmd", rclcpp::QoS{1}, + std::bind(&Simulator::callbackVehicleCmd, this, std::placeholders::_1)); + + sub_turn_signal_cmd_ = create_subscription( + "input/turn_signal_cmd", rclcpp::QoS{1}, + [this](const autoware_vehicle_msgs::msg::TurnSignal::SharedPtr msg) { + callbackTurnSignalCmd(msg); + }); + sub_initialtwist_ = create_subscription( + "input/initial_twist", rclcpp::QoS{1}, + std::bind(&Simulator::callbackInitialTwistStamped, this, std::placeholders::_1)); + + sub_engage_ = create_subscription( + "input/engage", rclcpp::QoS{1}, + std::bind(&Simulator::callbackEngage, this, std::placeholders::_1)); + sub_initialpose_ = create_subscription( + "input/initial_pose", rclcpp::QoS{1}, + std::bind(&Simulator::callbackInitialPoseWithCov, this, std::placeholders::_1)); + + const double dt = 1.0 / loop_rate_; + const int dt_ms = static_cast(dt * 1000.0); + timer_simulation_ = create_wall_timer( + std::chrono::milliseconds(dt_ms), std::bind(&Simulator::timerCallbackSimulation, this)); + + bool use_trajectory_for_z_position_source = + declare_parameter("use_trajectory_for_z_position_source").get(); + if (use_trajectory_for_z_position_source) { + sub_trajectory_ = create_subscription( + "base_trajectory", rclcpp::QoS{1}, + std::bind(&Simulator::callbackTrajectory, this, std::placeholders::_1)); + } + + /* tf setting */ + { + tf_buffer_ = std::make_shared(get_clock()); + tf_listener_ = std::make_shared( + *tf_buffer_, std::shared_ptr(this, [](auto) {}), false); + tf_broadcaster_ = std::make_shared( + std::shared_ptr(this, [](auto) {})); + } + + /* set vehicle model parameters */ + { + auto vehicle_model_type_str = declare_parameter("vehicle_model_type").get(); + RCLCPP_INFO(get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); + auto tread_length = declare_parameter("tread_length").get(); + auto angvel_lim = declare_parameter("angvel_lim").get(); + auto vel_lim = declare_parameter("vel_lim").get(); + auto steer_lim = declare_parameter("steer_lim").get(); + auto accel_rate = declare_parameter("accel_rate").get(); + auto angvel_rate = declare_parameter("angvel_rate").get(); + auto steer_rate_lim = declare_parameter("steer_rate_lim").get(); + auto vel_time_delay = declare_parameter("vel_time_delay").get(); + auto vel_time_constant = declare_parameter("vel_time_constant").get(); + auto steer_time_delay = declare_parameter("steer_time_delay").get(); + auto steer_time_constant = declare_parameter("steer_time_constant").get(); + auto angvel_time_delay = declare_parameter("angvel_time_delay").get(); + auto angvel_time_constant = declare_parameter("angvel_time_constant").get(); + auto acc_time_delay = declare_parameter("acc_time_delay").get(); + auto acc_time_constant = declare_parameter("acc_time_constant").get(); + simulator_engage_ = declare_parameter("initial_engage_state").get(); + auto deadzone_delta_steer = declare_parameter("deadzone_delta_steer").get(); + + if (vehicle_model_type_str == "IDEAL_STEER") { + vehicle_model_type_ = VehicleModelType::IDEAL_STEER; + vehicle_model_ptr_ = std::make_shared(wheelbase_); + } else if (vehicle_model_type_str == "DELAY_STEER") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, accel_rate, steer_rate_lim, wheelbase_, dt, vel_time_delay, + vel_time_constant, steer_time_delay, steer_time_constant, deadzone_delta_steer); + } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, accel_rate, steer_rate_lim, wheelbase_, dt, acc_time_delay, + acc_time_constant, steer_time_delay, steer_time_constant, deadzone_delta_steer); + } else { + RCLCPP_ERROR(get_logger(), "Invalid vehicle_model_type. Initialization failed."); + } + } + + /* set normal distribution noises */ + { + int random_seed = declare_parameter("random_seed").get(); + if (random_seed >= 0) { + rand_engine_ptr_ = std::make_shared(random_seed); + } else { + std::random_device seed; + rand_engine_ptr_ = std::make_shared(seed()); + } + auto pos_noise_stddev = declare_parameter("pos_noise_stddev").get(); + auto vel_noise_stddev = declare_parameter("vel_noise_stddev").get(); + auto rpy_noise_stddev = declare_parameter("rpy_noise_stddev").get(); + auto angvel_noise_stddev = declare_parameter("angvel_noise_stddev").get(); + auto steer_noise_stddev = declare_parameter("steer_noise_stddev").get(); + pos_norm_dist_ptr_ = std::make_shared>(0.0, pos_noise_stddev); + vel_norm_dist_ptr_ = std::make_shared>(0.0, vel_noise_stddev); + rpy_norm_dist_ptr_ = std::make_shared>(0.0, rpy_noise_stddev); + angvel_norm_dist_ptr_ = std::make_shared>(0.0, angvel_noise_stddev); + steer_norm_dist_ptr_ = std::make_shared>(0.0, steer_noise_stddev); + } + + current_pose_.orientation.w = 1.0; + + closest_pos_z_ = 0.0; +} + +void Simulator::callbackTrajectory( + const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) +{ + current_trajectory_ptr_ = msg; +} +void Simulator::callbackInitialPoseWithCov( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + geometry_msgs::msg::Twist initial_twist; // initialized with zero for all components + if (initial_twist_ptr_) { + initial_twist = initial_twist_ptr_->twist; + } + //save initial pose + initial_pose_with_cov_ptr_ = msg; + setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist); +} + +void Simulator::callbackInitialPoseStamped( + const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) +{ + geometry_msgs::msg::Twist initial_twist; // initialized with zero for all components + if (initial_twist_ptr_) { + initial_twist = initial_twist_ptr_->twist; + } + //save initial pose + initial_pose_ptr_ = msg; + setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist); +} + +void Simulator::callbackInitialTwistStamped( + const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) +{ + //save initial pose + initial_twist_ptr_ = msg; + if (initial_pose_ptr_) { + setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist_ptr_->twist); + //input twist to simulator's internal parameter + current_pose_ = initial_pose_ptr_->pose; + current_twist_ = initial_twist_ptr_->twist; + } else if (initial_pose_with_cov_ptr_) { + setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist_ptr_->twist); + } +} + +void Simulator::callbackEngage(const std_msgs::msg::Bool::ConstSharedPtr msg) +{ + simulator_engage_ = msg->data; +} + +void Simulator::timerCallbackSimulation() +{ + if (!is_initialized_) { + RCLCPP_INFO(get_logger(), "[simple_planning_simulator] waiting initial position..."); + return; + } + + if (prev_update_time_ptr_ == nullptr) { + prev_update_time_ptr_ = std::make_shared(get_clock()->now()); + } + + /* calculate delta time */ + const double dt = (get_clock()->now() - *prev_update_time_ptr_).seconds(); + *prev_update_time_ptr_ = get_clock()->now(); + + if (simulator_engage_) { + /* update vehicle dynamics when simulator_engage_ is true */ + vehicle_model_ptr_->update(dt); + /* set control mode */ + control_mode_.data = autoware_vehicle_msgs::msg::ControlMode::AUTO; + } else { + /* set control mode */ + control_mode_.data = autoware_vehicle_msgs::msg::ControlMode::MANUAL; + } + + /* save current vehicle pose & twist */ + current_pose_.position.x = vehicle_model_ptr_->getX(); + current_pose_.position.y = vehicle_model_ptr_->getY(); + closest_pos_z_ = getPosZFromTrajectory( + current_pose_.position.x, + current_pose_.position.y); // update vehicle z position from trajectory + current_pose_.position.z = closest_pos_z_; + double roll = 0.0; + double pitch = 0.0; + double yaw = vehicle_model_ptr_->getYaw(); + current_twist_.linear.x = vehicle_model_ptr_->getVx(); + current_twist_.angular.z = vehicle_model_ptr_->getWz(); + + if (simulator_engage_ && add_measurement_noise_) { + current_pose_.position.x += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); + current_pose_.position.y += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); + current_pose_.position.z += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); + roll += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); + pitch += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); + yaw += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); + if (current_twist_.linear.x >= 0.0) { + current_twist_.linear.x += (*vel_norm_dist_ptr_)(*rand_engine_ptr_); + } else { + current_twist_.linear.x -= (*vel_norm_dist_ptr_)(*rand_engine_ptr_); + } + current_twist_.angular.z += (*angvel_norm_dist_ptr_)(*rand_engine_ptr_); + } + + current_pose_.orientation = getQuaternionFromRPY(roll, pitch, yaw); + + /* publish pose & twist */ + publishPoseTwist(current_pose_, current_twist_); + publishTF(current_pose_); + + /* publish steering */ + autoware_vehicle_msgs::msg::Steering steer_msg; + steer_msg.header.frame_id = simulation_frame_id_; + steer_msg.header.stamp = get_clock()->now(); + steer_msg.data = vehicle_model_ptr_->getSteer(); + if (add_measurement_noise_) { + steer_msg.data += (*steer_norm_dist_ptr_)(*rand_engine_ptr_); + } + pub_steer_->publish(steer_msg); + + /* float info publishers */ + std_msgs::msg::Float32 velocity_msg; + velocity_msg.data = current_twist_.linear.x; + pub_velocity_->publish(velocity_msg); + + autoware_vehicle_msgs::msg::TurnSignal turn_signal_msg; + turn_signal_msg.header.frame_id = simulation_frame_id_; + turn_signal_msg.header.stamp = get_clock()->now(); + turn_signal_msg.data = autoware_vehicle_msgs::msg::TurnSignal::NONE; + if (current_turn_signal_cmd_ptr_) { + const auto cmd = current_turn_signal_cmd_ptr_->data; + // ignore invalid data such as cmd=999 + if ( + cmd == autoware_vehicle_msgs::msg::TurnSignal::LEFT || + cmd == autoware_vehicle_msgs::msg::TurnSignal::RIGHT || + cmd == autoware_vehicle_msgs::msg::TurnSignal::HAZARD) { + turn_signal_msg.data = cmd; + } + } + pub_turn_signal_->publish(turn_signal_msg); + + autoware_vehicle_msgs::msg::ShiftStamped shift_msg; + shift_msg.header.frame_id = simulation_frame_id_; + shift_msg.header.stamp = get_clock()->now(); + shift_msg.shift.data = current_twist_.linear.x >= 0.0 + ? autoware_vehicle_msgs::msg::Shift::DRIVE + : autoware_vehicle_msgs::msg::Shift::REVERSE; + pub_shift_->publish(shift_msg); + + /* publish control mode */ + pub_control_mode_->publish(control_mode_); +} + +void Simulator::callbackVehicleCmd( + const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg) +{ + current_vehicle_cmd_ptr_ = msg; + + if ( + vehicle_model_type_ == VehicleModelType::IDEAL_STEER || + vehicle_model_type_ == VehicleModelType::DELAY_STEER) { + Eigen::VectorXd input(2); + input << msg->control.velocity, msg->control.steering_angle; + vehicle_model_ptr_->setInput(input); + } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + Eigen::VectorXd input(3); + double drive_shift = + (msg->shift.data == autoware_vehicle_msgs::msg::Shift::REVERSE) ? -1.0 : 1.0; + input << msg->control.acceleration, msg->control.steering_angle, drive_shift; + vehicle_model_ptr_->setInput(input); + } else { + RCLCPP_WARN(get_logger(), "[%s] : invalid vehicle_model_type_ error.", __func__); + } +} +void Simulator::callbackTurnSignalCmd( + const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg) +{ + current_turn_signal_cmd_ptr_ = msg; +} + +void Simulator::setInitialStateWithPoseTransform( + const geometry_msgs::msg::PoseStamped & pose_stamped, const geometry_msgs::msg::Twist & twist) +{ + geometry_msgs::msg::TransformStamped transform; + getTransformFromTF(map_frame_id_, pose_stamped.header.frame_id, transform); + geometry_msgs::msg::Pose pose; + pose.position.x = pose_stamped.pose.position.x + transform.transform.translation.x; + pose.position.y = pose_stamped.pose.position.y + transform.transform.translation.y; + pose.position.z = pose_stamped.pose.position.z + transform.transform.translation.z; + pose.orientation = pose_stamped.pose.orientation; + setInitialState(pose, twist); +} + +void Simulator::setInitialStateWithPoseTransform( + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::Twist & twist) +{ + geometry_msgs::msg::PoseStamped ps; + ps.header = pose.header; + ps.pose = pose.pose.pose; + setInitialStateWithPoseTransform(ps, twist); +} + +void Simulator::setInitialState( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist) +{ + const double x = pose.position.x; + const double y = pose.position.y; + const double yaw = tf2::getYaw(pose.orientation); + const double vx = twist.linear.x; + const double steer = 0.0; + const double acc = 0.0; + + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER) { + Eigen::VectorXd state(3); + state << x, y, yaw; + vehicle_model_ptr_->setState(state); + } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER) { + Eigen::VectorXd state(5); + state << x, y, yaw, vx, steer; + vehicle_model_ptr_->setState(state); + } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + Eigen::VectorXd state(6); + state << x, y, yaw, vx, steer, acc; + vehicle_model_ptr_->setState(state); + } else { + RCLCPP_WARN(get_logger(), "undesired vehicle model type! Initialization failed."); + return; + } + + is_initialized_ = true; +} + +void Simulator::getTransformFromTF( + const std::string parent_frame, const std::string child_frame, + geometry_msgs::msg::TransformStamped & transform) +{ + while (1) { + try { + const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); + transform = tf_buffer_->lookupTransform( + parent_frame, child_frame, time_point, tf2::durationFromSec(0.0)); + break; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + rclcpp::sleep_for(std::chrono::milliseconds(500)); + } + } +} + +void Simulator::publishPoseTwist( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist) +{ + rclcpp::Time current_time = get_clock()->now(); + // simulatied pose + geometry_msgs::msg::PoseStamped ps; + ps.header.frame_id = map_frame_id_; + ps.header.stamp = current_time; + ps.pose = pose; + pub_pose_->publish(ps); + + geometry_msgs::msg::TwistStamped ts; + ts.header.frame_id = simulation_frame_id_; + ts.header.stamp = current_time; + ts.twist = twist; + pub_twist_->publish(ts); +} + +void Simulator::publishTF(const geometry_msgs::msg::Pose & pose) +{ + rclcpp::Time current_time = get_clock()->now(); + + // send odom transform + geometry_msgs::msg::TransformStamped odom_trans; + odom_trans.header.stamp = current_time; + odom_trans.header.frame_id = map_frame_id_; + odom_trans.child_frame_id = simulation_frame_id_; + odom_trans.transform.translation.x = pose.position.x; + odom_trans.transform.translation.y = pose.position.y; + odom_trans.transform.translation.z = pose.position.z; + odom_trans.transform.rotation = pose.orientation; + tf_broadcaster_->sendTransform(odom_trans); +} + +double Simulator::getPosZFromTrajectory(const double x, const double y) +{ + // calculae cloest point on trajectory + /* + write me... + */ + if (current_trajectory_ptr_ != nullptr) { + const double max_sqrt_dist = 100.0 * 100.0; + double min_sqrt_dist = max_sqrt_dist; + int index; + bool found = false; + for (size_t i = 0; i < current_trajectory_ptr_->points.size(); ++i) { + const double dist_x = (current_trajectory_ptr_->points.at(i).pose.position.x - x); + const double dist_y = (current_trajectory_ptr_->points.at(i).pose.position.y - y); + double sqrt_dist = dist_x * dist_x + dist_y * dist_y; + if (sqrt_dist < min_sqrt_dist) { + min_sqrt_dist = sqrt_dist; + index = i; + found = true; + } + } + if (found) + return current_trajectory_ptr_->points.at(index).pose.position.z; + else + return 0; + } else { + return 0.0; + } +} + +geometry_msgs::msg::Quaternion Simulator::getQuaternionFromRPY( + const double & roll, const double & pitch, const double & yaw) +{ + tf2::Quaternion q; + q.setRPY(roll, pitch, yaw); + return tf2::toMsg(q); +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp new file mode 100644 index 0000000000000..bd097bfd8e013 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp @@ -0,0 +1,28 @@ +/* + * Copyright 2015-2019 Autoware Foundation. All rights reserved. + * + * 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/simple_planning_simulator_core.hpp" + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + rclcpp::NodeOptions options; + const auto node = std::make_shared("simple_planning_simulator", options); + rclcpp::spin(node); + rclcpp::shutdown(); + + return 0; +}; diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp new file mode 100644 index 0000000000000..0b04d92c0b97e --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp @@ -0,0 +1,65 @@ + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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_constant_acceleration.hpp" + +SimModelConstantAccelTwist::SimModelConstantAccelTwist( + double vx_lim, double wz_lim, double vx_rate, double wz_rate) +: SimModelInterface(5 /* dim x */, 2 /* dim u */), + vx_lim_(vx_lim), + wz_lim_(wz_lim), + vx_rate_(vx_rate), + wz_rate_(wz_rate){}; + +double SimModelConstantAccelTwist::getX() { return state_(IDX::X); }; +double SimModelConstantAccelTwist::getY() { return state_(IDX::Y); }; +double SimModelConstantAccelTwist::getYaw() { return state_(IDX::YAW); }; +double SimModelConstantAccelTwist::getVx() { return state_(IDX::VX); }; +double SimModelConstantAccelTwist::getWz() { return state_(IDX::WZ); }; +double SimModelConstantAccelTwist::getSteer() { return 0.0; }; +void SimModelConstantAccelTwist::update(const double & dt) { updateRungeKutta(dt, input_); } +Eigen::VectorXd SimModelConstantAccelTwist::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vel = state(IDX::VX); + const double angvel = state(IDX::WZ); + const double yaw = state(IDX::YAW); + const double vx_des = std::max(std::min(input(IDX_U::VX_DES), vx_lim_), -vx_lim_); + const double wz_des = std::max(std::min(input(IDX_U::WZ_DES), wz_lim_), -wz_lim_); + double vx_rate = 0.0; + double wz_rate = 0.0; + if (vx_des > vel) { + vx_rate = vx_rate_; + } else if (vx_des < vel) { + vx_rate = -vx_rate_; + } + + if (wz_des > angvel) { + wz_rate = wz_rate_; + } else if (wz_des < angvel) { + wz_rate = -wz_rate_; + } + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = angvel; + d_state(IDX::VX) = vx_rate; + d_state(IDX::WZ) = wz_rate; + + return d_state; +}; \ No newline at end of file diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp new file mode 100644 index 0000000000000..a955846118520 --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp @@ -0,0 +1,107 @@ + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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_ideal.hpp" + +SimModelIdealTwist::SimModelIdealTwist() : SimModelInterface(3 /* dim x */, 2 /* dim u */){}; + +double SimModelIdealTwist::getX() { return state_(IDX::X); }; +double SimModelIdealTwist::getY() { return state_(IDX::Y); }; +double SimModelIdealTwist::getYaw() { return state_(IDX::YAW); }; +double SimModelIdealTwist::getVx() { return input_(IDX_U::VX_DES); }; +double SimModelIdealTwist::getWz() { return input_(IDX_U::WZ_DES); }; +double SimModelIdealTwist::getSteer() { return 0.0; }; +void SimModelIdealTwist::update(const double & dt) { updateRungeKutta(dt, input_); } +Eigen::VectorXd SimModelIdealTwist::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double yaw = state(IDX::YAW); + const double vx = input(IDX_U::VX_DES); + const double wz = input(IDX_U::WZ_DES); + + 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) = wz; + + return d_state; +}; + +SimModelIdealSteer::SimModelIdealSteer(double wheelbase) +: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase){}; + +double SimModelIdealSteer::getX() { return state_(IDX::X); }; +double SimModelIdealSteer::getY() { return state_(IDX::Y); }; +double SimModelIdealSteer::getYaw() { return state_(IDX::YAW); }; +double SimModelIdealSteer::getVx() { return input_(IDX_U::VX_DES); }; +double SimModelIdealSteer::getWz() +{ + return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +}; +double SimModelIdealSteer::getSteer() { return input_(IDX_U::STEER_DES); }; +void SimModelIdealSteer::update(const double & dt) { updateRungeKutta(dt, input_); } +Eigen::VectorXd SimModelIdealSteer::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double yaw = state(IDX::YAW); + const double vx = input(IDX_U::VX_DES); + const double steer = input(IDX_U::STEER_DES); + + 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_; + + return d_state; +}; + +SimModelIdealAccel::SimModelIdealAccel(double wheelbase) +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase){}; + +double SimModelIdealAccel::getX() { return state_(IDX::X); }; +double SimModelIdealAccel::getY() { return state_(IDX::Y); }; +double SimModelIdealAccel::getYaw() { return state_(IDX::YAW); }; +double SimModelIdealAccel::getVx() { return state_(IDX::VX); }; +double SimModelIdealAccel::getWz() +{ + return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +}; +double SimModelIdealAccel::getSteer() { return input_(IDX_U::STEER_DES); }; +void SimModelIdealAccel::update(const double & dt) +{ + updateRungeKutta(dt, input_); + if (state_(IDX::VX) < 0) { + state_(IDX::VX) = 0; + } +} + +Eigen::VectorXd SimModelIdealAccel::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vx = state(IDX::VX); + const double yaw = state(IDX::YAW); + const double ax = input(IDX_U::AX_DES); + const double steer = input(IDX_U::STEER_DES); + + 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::VX) = ax; + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + + return d_state; +}; diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp new file mode 100644 index 0000000000000..5a1533bfbda29 --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp @@ -0,0 +1,41 @@ +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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_interface.hpp" + +SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) +{ + state_ = Eigen::VectorXd::Zero(dim_x_); + input_ = Eigen::VectorXd::Zero(dim_u_); +}; + +void SimModelInterface::updateRungeKutta(const double & dt, const Eigen::VectorXd & input) +{ + Eigen::VectorXd k1 = calcModel(state_, input); + Eigen::VectorXd k2 = calcModel(state_ + k1 * 0.5 * dt, input); + Eigen::VectorXd k3 = calcModel(state_ + k2 * 0.5 * dt, input); + Eigen::VectorXd k4 = calcModel(state_ + k3 * dt, input); + + state_ += 1.0 / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4) * dt; +} +void SimModelInterface::updateEuler(const double & dt, const Eigen::VectorXd & input) +{ + state_ += calcModel(state_, input) * dt; +} +void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; }; +void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; }; +void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; }; +void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; }; diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp new file mode 100644 index 0000000000000..8e072c158a99b --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp @@ -0,0 +1,312 @@ + +/* + * Copyright 2018-2019 Autoware Foundation. All rights reserved. + * + * 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_time_delay.hpp" + +/* + * + * SimModelTimeDelayTwist + * + */ + +SimModelTimeDelayTwist::SimModelTimeDelayTwist( + double vx_lim, double wz_lim, double vx_rate_lim, double wz_rate_lim, double dt, double vx_delay, + double vx_time_constant, double wz_delay, double wz_time_constant, double deadzone_delta_steer) +: SimModelInterface(5 /* dim x */, 2 /* dim u */), + MIN_TIME_CONSTANT(0.03), + vx_lim_(vx_lim), + vx_rate_lim_(vx_rate_lim), + wz_lim_(wz_lim), + wz_rate_lim_(wz_rate_lim), + vx_delay_(vx_delay), + vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), + wz_delay_(wz_delay), + wz_time_constant_(std::max(wz_time_constant, MIN_TIME_CONSTANT)), + deadzone_delta_steer_(deadzone_delta_steer) +{ + if (vx_time_constant < MIN_TIME_CONSTANT) { + std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << std::endl; + } + if (wz_time_constant < MIN_TIME_CONSTANT) { + std::cout << "Settings wz_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << std::endl; + } + initializeInputQueue(dt); +}; + +double SimModelTimeDelayTwist::getX() { return state_(IDX::X); }; +double SimModelTimeDelayTwist::getY() { return state_(IDX::Y); }; +double SimModelTimeDelayTwist::getYaw() { return state_(IDX::YAW); }; +double SimModelTimeDelayTwist::getVx() { return state_(IDX::VX); }; +double SimModelTimeDelayTwist::getWz() { return state_(IDX::WZ); }; +double SimModelTimeDelayTwist::getSteer() { return 0.0; }; +void SimModelTimeDelayTwist::update(const double & 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(); + wz_input_queue_.push_back(input_(IDX_U::WZ_DES)); + delayed_input(IDX_U::WZ_DES) = wz_input_queue_.front(); + wz_input_queue_.pop_front(); + // do not use deadzone_delta_steer (Steer IF does not exist in this model) + updateRungeKutta(dt, delayed_input); +}; +void SimModelTimeDelayTwist::initializeInputQueue(const double & dt) +{ + size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); + for (size_t i = 0; i < vx_input_queue_size; i++) { + vx_input_queue_.push_back(0.0); + } + size_t wz_input_queue_size = static_cast(round(wz_delay_ / dt)); + for (size_t i = 0; i < wz_input_queue_size; i++) { + wz_input_queue_.push_back(0.0); + } +} + +Eigen::VectorXd SimModelTimeDelayTwist::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vx = state(IDX::VX); + const double wz = state(IDX::WZ); + const double yaw = state(IDX::YAW); + const double delay_input_vx = input(IDX_U::VX_DES); + const double delay_input_wz = input(IDX_U::WZ_DES); + const double delay_vx_des = std::max(std::min(delay_input_vx, vx_lim_), -vx_lim_); + const double delay_wz_des = std::max(std::min(delay_input_wz, wz_lim_), -wz_lim_); + double vx_rate = -(vx - delay_vx_des) / vx_time_constant_; + double wz_rate = -(wz - delay_wz_des) / wz_time_constant_; + vx_rate = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, vx_rate)); + wz_rate = std::min(wz_rate_lim_, std::max(-wz_rate_lim_, wz_rate)); + + 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) = wz; + d_state(IDX::VX) = vx_rate; + d_state(IDX::WZ) = wz_rate; + + return d_state; +}; + +/* + * + * SimModelTimeDelaySteer + * + */ +SimModelTimeDelaySteer::SimModelTimeDelaySteer( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double vx_delay, double vx_time_constant, double steer_delay, + double steer_time_constant, double deadzone_delta_steer) +: 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)), + deadzone_delta_steer_(deadzone_delta_steer) +{ + if (vx_time_constant < MIN_TIME_CONSTANT) { + std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << std::endl; + } + if (steer_time_constant < MIN_TIME_CONSTANT) { + std::cout << "Settings steer_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << std::endl; + } + + initializeInputQueue(dt); +}; + +double SimModelTimeDelaySteer::getX() { return state_(IDX::X); }; +double SimModelTimeDelaySteer::getY() { return state_(IDX::Y); }; +double SimModelTimeDelaySteer::getYaw() { return state_(IDX::YAW); }; +double SimModelTimeDelaySteer::getVx() { return state_(IDX::VX); }; +double SimModelTimeDelaySteer::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +}; +double SimModelTimeDelaySteer::getSteer() { return state_(IDX::STEER); }; +void SimModelTimeDelaySteer::update(const double & 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)); + const double raw_steer_command = steer_input_queue_.front(); + delayed_input(IDX_U::STEER_DES) = sim_model_util::getDummySteerCommandWithFriction( + getSteer(), raw_steer_command, deadzone_delta_steer_); + steer_input_queue_.pop_front(); + + updateRungeKutta(dt, delayed_input); +}; +void SimModelTimeDelaySteer::initializeInputQueue(const double & dt) +{ + size_t vx_input_queue_size = static_cast(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(round(steer_delay_ / dt)); + for (size_t i = 0; i < steer_input_queue_size; i++) { + steer_input_queue_.push_back(0.0); + } +} + +Eigen::VectorXd SimModelTimeDelaySteer::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const double vel = state(IDX::VX); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + const double delay_input_vel = input(IDX_U::VX_DES); + const double delay_input_steer = input(IDX_U::STEER_DES); + const double delay_vx_des = std::max(std::min(delay_input_vel, vx_lim_), -vx_lim_); + const double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); + double vx_rate = -(vel - delay_vx_des) / vx_time_constant_; + double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; + vx_rate = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, vx_rate)); + steer_rate = std::min(steer_rate_lim_, std::max(-steer_rate_lim_, steer_rate)); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = vx_rate; + d_state(IDX::STEER) = steer_rate; + + return d_state; +}; + +SimModelTimeDelaySteerAccel::SimModelTimeDelaySteerAccel( + double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, + double dt, double acc_delay, double acc_time_constant, double steer_delay, + double steer_time_constant, double deadzone_delta_steer) +: SimModelInterface(6 /* dim x */, 3 /* 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), + acc_delay_(acc_delay), + acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), + deadzone_delta_steer_(deadzone_delta_steer) +{ + if (acc_time_constant < MIN_TIME_CONSTANT) { + std::cout << "Settings acc_time_constant is too small, replace it by" << MIN_TIME_CONSTANT << std::endl; + } + if (steer_time_constant < MIN_TIME_CONSTANT) { + std::cout << "Settings steer_time_constant is too small, replace it by" << MIN_TIME_CONSTANT << std::endl; + } + + initializeInputQueue(dt); +}; + +double SimModelTimeDelaySteerAccel::getX() { return state_(IDX::X); }; +double SimModelTimeDelaySteerAccel::getY() { return state_(IDX::Y); }; +double SimModelTimeDelaySteerAccel::getYaw() { return state_(IDX::YAW); }; +double SimModelTimeDelaySteerAccel::getVx() { return state_(IDX::VX); }; +double SimModelTimeDelaySteerAccel::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +}; +double SimModelTimeDelaySteerAccel::getSteer() { return state_(IDX::STEER); }; +void SimModelTimeDelaySteerAccel::update(const double & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); + delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); + acc_input_queue_.pop_front(); + steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); + const double raw_steer_command = steer_input_queue_.front(); + delayed_input(IDX_U::STEER_DES) = sim_model_util::getDummySteerCommandWithFriction( + getSteer(), raw_steer_command, deadzone_delta_steer_); + steer_input_queue_.pop_front(); + delayed_input(IDX_U::DRIVE_SHIFT) = input_(IDX_U::DRIVE_SHIFT); + + updateRungeKutta(dt, delayed_input); + // clip velocity and accel + if (delayed_input(IDX_U::DRIVE_SHIFT) >= 0.0) { + state_(IDX::VX) = std::max(0.0, std::min(state_(IDX::VX), vx_lim_)); + if ( + std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || std::abs((state_(IDX::VX) - vx_lim_)) < 10e-9) { + state_(IDX::ACCX) = 0.0; + } + } else { + state_(IDX::VX) = std::min(0.0, std::max(state_(IDX::VX), -vx_lim_)); + if ( + std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || + std::abs((state_(IDX::VX) - (-vx_lim_))) < 10e-9) { + state_(IDX::ACCX) = 0.0; + } + } +} + +void SimModelTimeDelaySteerAccel::initializeInputQueue(const double & dt) +{ + size_t vx_input_queue_size = static_cast(round(acc_delay_ / dt)); + for (size_t i = 0; i < vx_input_queue_size; i++) { + acc_input_queue_.push_back(0.0); + } + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + for (size_t i = 0; i < steer_input_queue_size; i++) { + steer_input_queue_.push_back(0.0); + } +} + +Eigen::VectorXd SimModelTimeDelaySteerAccel::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + double vel = state(IDX::VX); + double acc = state(IDX::ACCX); + const double yaw = state(IDX::YAW); + const double steer = state(IDX::STEER); + const double delay_input_acc = input(IDX_U::ACCX_DES); + const double delay_input_steer = input(IDX_U::STEER_DES); + const double drive_shift = input(IDX_U::DRIVE_SHIFT); + double delay_acc_des = std::max(std::min(delay_input_acc, vx_rate_lim_), -vx_rate_lim_); + if (!(drive_shift >= 0.0)) delay_acc_des *= -1; // reverse front-back + double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); + double accx_rate = -(acc - delay_acc_des) / acc_time_constant_; + double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; + acc = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, acc)); + steer_rate = std::min(steer_rate_lim_, std::max(-steer_rate_lim_, steer_rate)); + + if (drive_shift >= 0.0) { + vel = std::max(0.0, std::min(vel, vx_lim_)); + } else { + vel = std::min(0.0, std::max(vel, -vx_lim_)); + } + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = acc; + d_state(IDX::STEER) = steer_rate; + d_state(IDX::ACCX) = accx_rate; + + return d_state; +}; diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp new file mode 100644 index 0000000000000..40721173aa31d --- /dev/null +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp @@ -0,0 +1,32 @@ +/* + * Copyright 2020 Tier IV, Inc. All rights reserved. + * + * 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_util.hpp" + +namespace sim_model_util +{ +double getDummySteerCommandWithFriction( + const double steer, const double steer_command, const double deadzone_delta_steer) +{ + const double delta_steer = std::fabs(steer_command - steer); + // if delta steer is too small, ignore steer command (send current steer as steer command) + if (delta_steer < deadzone_delta_steer) { + return steer; + } + return steer_command; +} + +} // namespace sim_model_util From 5fac7eb3deca883b5100d10521eef4b81275a92c Mon Sep 17 00:00:00 2001 From: mitsudome-r Date: Thu, 8 Oct 2020 17:27:14 +0900 Subject: [PATCH 04/52] add COLCON_IGNORE to ros1 packages Signed-off-by: mitsudome-r --- .../config/simulator_model.yaml | 29 +++++++ ...ple_planning_simulator_dummy_pacmod.launch | 82 +++++++++++++++++++ 2 files changed, 111 insertions(+) create mode 100644 simulator/simple_planning_simulator/config/simulator_model.yaml create mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch diff --git a/simulator/simple_planning_simulator/config/simulator_model.yaml b/simulator/simple_planning_simulator/config/simulator_model.yaml new file mode 100644 index 0000000000000..1b91975334996 --- /dev/null +++ b/simulator/simple_planning_simulator/config/simulator_model.yaml @@ -0,0 +1,29 @@ +acc_time_constant: 0.1 +acc_time_delay: 0.1 +accel_rate: 7.0 +add_measurement_noise: true +angvel_lim: 3.0 +angvel_noise_stddev: 0.0 +angvel_rate: 1.0 +angvel_time_constant: 0.5 +angvel_time_delay: 0.2 +initial_engage_state: true +pos_noise_stddev: 0.01 +rpy_noise_stddev: 0.0001 +sim_steering_gear_ratio: 15.0 +steer_lim: 1.0 +steer_noise_stddev: 0.0001 +steer_rate_lim: 5.0 +steer_time_constant: 0.27 +steer_time_delay: 0.24 +tread_length: 1.0 + +# Option for vehicle_model_type: +# - IDEAL_STEER : reads velocity command. The steering and velocity changes exactly the same as commanded. +# - DELAY_STEER : reads velocity command. The steering and velocity changes with delay model. +# - DELAY_STEER_ACC : reads acceleration command. The steering and acceleration changes with delay model. +vehicle_model_type: DELAY_STEER_ACC +vel_lim: 50.0 +vel_noise_stddev: 0.0 +vel_time_constant: 0.61 +vel_time_delay: 0.25 diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch b/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch new file mode 100644 index 0000000000000..2300de112a8db --- /dev/null +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch @@ -0,0 +1,82 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + From c41b710953f4078a74b0a5efc39ebde32cd31b8e Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Tue, 13 Oct 2020 22:47:30 +0900 Subject: [PATCH 05/52] Fix simple planning simulator (#26) * simple planning simulator: fix params & launch file Signed-off-by: Takamasa Horibe * remove unused file Signed-off-by: Takamasa Horibe * fix timercallback Signed-off-by: Takamasa Horibe --- .../launch/simple_planning_simulator.launch | 36 -------- .../simple_planning_simulator.launch.xml | 37 ++++++++ ...ple_planning_simulator_dummy_pacmod.launch | 82 ----------------- .../src/simple_planning_simulator_core.cpp | 92 +++++++++---------- 4 files changed, 83 insertions(+), 164 deletions(-) delete mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator.launch create mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml delete mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch deleted file mode 100644 index 2bae8b588fb7b..0000000000000 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch +++ /dev/null @@ -1,36 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml new file mode 100644 index 0000000000000..1b8d37b292727 --- /dev/null +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml @@ -0,0 +1,37 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch b/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch deleted file mode 100644 index 2300de112a8db..0000000000000 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator_dummy_pacmod.launch +++ /dev/null @@ -1,82 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index f24db2604d05b..2de0b863351c8 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -20,12 +20,12 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & : Node(node_name, options) { /* simple_planning_simulator parameters */ - loop_rate_ = declare_parameter("loop_rate").get(); - wheelbase_ = declare_parameter("/vehicle_info/wheel_base").get(); - sim_steering_gear_ratio_ = declare_parameter("sim_steering_gear_ratio").get(); - simulation_frame_id_ = declare_parameter("simulation_frame_id").get(); - map_frame_id_ = declare_parameter("map_frame_id").get(); - add_measurement_noise_ = declare_parameter("add_measurement_noise").get(); + loop_rate_ = declare_parameter("loop_rate", 30.0); + wheelbase_ = declare_parameter("vehicle_info.wheel_base", 4.0); + sim_steering_gear_ratio_ = declare_parameter("sim_steering_gear_ratio", 15.0); + simulation_frame_id_ = declare_parameter("simulation_frame_id", "base_link"); + map_frame_id_ = declare_parameter("map_frame_id", "map"); + add_measurement_noise_ = declare_parameter("add_measurement_noise", false); /* set pub sub topic name */ pub_pose_ = @@ -64,16 +64,16 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & std::bind(&Simulator::callbackInitialPoseWithCov, this, std::placeholders::_1)); const double dt = 1.0 / loop_rate_; - const int dt_ms = static_cast(dt * 1000.0); - timer_simulation_ = create_wall_timer( - std::chrono::milliseconds(dt_ms), std::bind(&Simulator::timerCallbackSimulation, this)); - - bool use_trajectory_for_z_position_source = - declare_parameter("use_trajectory_for_z_position_source").get(); - if (use_trajectory_for_z_position_source) { - sub_trajectory_ = create_subscription( - "base_trajectory", rclcpp::QoS{1}, - std::bind(&Simulator::callbackTrajectory, this, std::placeholders::_1)); + + /* Timer */ + { + auto timer_callback = std::bind(&Simulator::timerCallbackSimulation, this); + auto period = std::chrono::duration_cast( + std::chrono::duration(dt)); + timer_simulation_ = std::make_shared>( + this->get_clock(), period, std::move(timer_callback), + this->get_node_base_interface()->get_context()); + this->get_node_timers_interface()->add_timer(timer_simulation_, nullptr); } /* tf setting */ @@ -84,28 +84,28 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & tf_broadcaster_ = std::make_shared( std::shared_ptr(this, [](auto) {})); } - + /* set vehicle model parameters */ { - auto vehicle_model_type_str = declare_parameter("vehicle_model_type").get(); + auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER"); RCLCPP_INFO(get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); - auto tread_length = declare_parameter("tread_length").get(); - auto angvel_lim = declare_parameter("angvel_lim").get(); - auto vel_lim = declare_parameter("vel_lim").get(); - auto steer_lim = declare_parameter("steer_lim").get(); - auto accel_rate = declare_parameter("accel_rate").get(); - auto angvel_rate = declare_parameter("angvel_rate").get(); - auto steer_rate_lim = declare_parameter("steer_rate_lim").get(); - auto vel_time_delay = declare_parameter("vel_time_delay").get(); - auto vel_time_constant = declare_parameter("vel_time_constant").get(); - auto steer_time_delay = declare_parameter("steer_time_delay").get(); - auto steer_time_constant = declare_parameter("steer_time_constant").get(); - auto angvel_time_delay = declare_parameter("angvel_time_delay").get(); - auto angvel_time_constant = declare_parameter("angvel_time_constant").get(); - auto acc_time_delay = declare_parameter("acc_time_delay").get(); - auto acc_time_constant = declare_parameter("acc_time_constant").get(); - simulator_engage_ = declare_parameter("initial_engage_state").get(); - auto deadzone_delta_steer = declare_parameter("deadzone_delta_steer").get(); + auto tread_length = declare_parameter("tread_length", 2.0); + auto angvel_lim = declare_parameter("angvel_lim", 3.0); + auto vel_lim = declare_parameter("vel_lim", 50.0); + auto steer_lim = declare_parameter("steer_lim", 1.0); + auto accel_rate = declare_parameter("accel_rate", 10.0); + auto angvel_rate = declare_parameter("angvel_rate", 1.0); + auto steer_rate_lim = declare_parameter("steer_rate_lim", 5.0); + auto vel_time_delay = declare_parameter("vel_time_delay", 0.25); + auto vel_time_constant = declare_parameter("vel_time_constant", 0.5); + auto steer_time_delay = declare_parameter("steer_time_delay", 0.3); + auto steer_time_constant = declare_parameter("steer_time_constant", 0.3); + auto angvel_time_delay = declare_parameter("angvel_time_delay", 0.3); + auto angvel_time_constant = declare_parameter("angvel_time_constant", 0.3); + auto acc_time_delay = declare_parameter("acc_time_delay", 0.1); + auto acc_time_constant = declare_parameter("acc_time_constant", 0.1); + simulator_engage_ = declare_parameter("initial_engage_state", true); + auto deadzone_delta_steer = declare_parameter("deadzone_delta_steer", 0.0); if (vehicle_model_type_str == "IDEAL_STEER") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER; @@ -124,21 +124,21 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & RCLCPP_ERROR(get_logger(), "Invalid vehicle_model_type. Initialization failed."); } } - + /* set normal distribution noises */ { - int random_seed = declare_parameter("random_seed").get(); + int random_seed = declare_parameter("random_seed", 1); if (random_seed >= 0) { rand_engine_ptr_ = std::make_shared(random_seed); } else { std::random_device seed; rand_engine_ptr_ = std::make_shared(seed()); } - auto pos_noise_stddev = declare_parameter("pos_noise_stddev").get(); - auto vel_noise_stddev = declare_parameter("vel_noise_stddev").get(); - auto rpy_noise_stddev = declare_parameter("rpy_noise_stddev").get(); - auto angvel_noise_stddev = declare_parameter("angvel_noise_stddev").get(); - auto steer_noise_stddev = declare_parameter("steer_noise_stddev").get(); + auto pos_noise_stddev = declare_parameter("pos_noise_stddev", 0.01); + auto vel_noise_stddev = declare_parameter("vel_noise_stddev", 0.0); + auto rpy_noise_stddev = declare_parameter("rpy_noise_stddev", 0.0001); + auto angvel_noise_stddev = declare_parameter("angvel_noise_stddev", 0.0); + auto steer_noise_stddev = declare_parameter("steer_noise_stddev", 0.0001); pos_norm_dist_ptr_ = std::make_shared>(0.0, pos_noise_stddev); vel_norm_dist_ptr_ = std::make_shared>(0.0, vel_noise_stddev); rpy_norm_dist_ptr_ = std::make_shared>(0.0, rpy_noise_stddev); @@ -163,7 +163,7 @@ void Simulator::callbackInitialPoseWithCov( if (initial_twist_ptr_) { initial_twist = initial_twist_ptr_->twist; } - //save initial pose + // save initial pose initial_pose_with_cov_ptr_ = msg; setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist); } @@ -175,7 +175,7 @@ void Simulator::callbackInitialPoseStamped( if (initial_twist_ptr_) { initial_twist = initial_twist_ptr_->twist; } - //save initial pose + // save initial pose initial_pose_ptr_ = msg; setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist); } @@ -183,11 +183,11 @@ void Simulator::callbackInitialPoseStamped( void Simulator::callbackInitialTwistStamped( const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) { - //save initial pose + // save initial pose initial_twist_ptr_ = msg; if (initial_pose_ptr_) { setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist_ptr_->twist); - //input twist to simulator's internal parameter + // input twist to simulator's internal parameter current_pose_ = initial_pose_ptr_->pose; current_twist_ = initial_twist_ptr_->twist; } else if (initial_pose_with_cov_ptr_) { From e14ecad0ea2dfd31cd599b6cc969c6fc0410e5c9 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Tue, 24 Nov 2020 15:17:44 +0900 Subject: [PATCH 06/52] [simple_planning_simulator] add rostopic relay in launch file (#117) * [simple_planning_simulator] add rostopic relay in launch file Signed-off-by: mitsudome-r * add topic_tools as exec_depend Signed-off-by: mitsudome-r --- .../launch/simple_planning_simulator.launch.xml | 7 ++++++- simulator/simple_planning_simulator/package.xml | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml index 1b8d37b292727..1495a6f0d8831 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml @@ -33,5 +33,10 @@ - + + + + + + diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 407d04d74541d..962d1d8ffad2a 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -23,6 +23,7 @@ rclcpp_components launch_ros rosidl_default_runtime + topic_tools ament_cmake From 7406784d83032cc636ca835010e634bd58b5d69a Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Thu, 3 Dec 2020 17:21:16 +0100 Subject: [PATCH 07/52] Adjust copyright notice on 532 out of 699 source files (#143) --- .../simple_planning_simulator_core.hpp | 28 +++++++++---------- .../sim_model_constant_acceleration.hpp | 28 +++++++++---------- .../vehicle_model/sim_model_ideal.hpp | 28 +++++++++---------- .../vehicle_model/sim_model_interface.hpp | 28 +++++++++---------- .../vehicle_model/sim_model_time_delay.hpp | 28 +++++++++---------- .../vehicle_model/sim_model_util.hpp | 28 +++++++++---------- .../src/simple_planning_simulator_core.cpp | 28 +++++++++---------- .../src/simple_planning_simulator_node.cpp | 28 +++++++++---------- .../src/vehicle_model/sim_model_interface.cpp | 28 +++++++++---------- .../src/vehicle_model/sim_model_util.cpp | 28 +++++++++---------- 10 files changed, 130 insertions(+), 150 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 197a7ae0b4061..7f20dae7af42d 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2015-2019 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. /** * @file simple_planning_simulator_core.hpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp index 26bfb4437a29d..ee12986a859e8 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2015-2019 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. /** * @file sim_model_constant_acceleration.hpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp index 25fead30d403d..c799423446f85 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2015-2019 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. /** * @file sim_model_ideal.hpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 480f521d9548b..819c9b77746c4 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2018-2019 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. /** * @file sim_model_interface.hpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp index d38d8e2adc578..080cd8404709a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2015-2019 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. /** * @file sim_model_time_delay.hpp diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp index 2911e9e2dff60..372c32924a887 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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 namespace sim_model_util diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index 2de0b863351c8..466346d7dd460 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2015-2019 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/simple_planning_simulator_core.hpp" diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp index bd097bfd8e013..eea2aafc4230f 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2015-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2015-2019 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/simple_planning_simulator_core.hpp" diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp index 5a1533bfbda29..ab3473ad8e653 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2018-2019 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_interface.hpp" diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp index 40721173aa31d..6323b16006955 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2020 Tier IV, Inc. All rights reserved. - * - * 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. - */ +// Copyright 2020 Tier IV, Inc. +// +// 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_util.hpp" From 7aa33659a11c5badf2398c3b9cbddebd3a17a985 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Mon, 7 Dec 2020 09:52:36 +0100 Subject: [PATCH 08/52] Use quotes for includes where appropriate (#144) * Use quotes for includes where appropriate * Fix lint tests * Make tests pass hopefully --- .../simple_planning_simulator_core.hpp | 42 +++++++++---------- .../sim_model_constant_acceleration.hpp | 4 +- .../vehicle_model/sim_model_ideal.hpp | 4 +- .../vehicle_model/sim_model_interface.hpp | 4 +- .../vehicle_model/sim_model_time_delay.hpp | 4 +- 5 files changed, 29 insertions(+), 29 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 7f20dae7af42d..cd74e055dd4f0 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -22,29 +22,29 @@ #ifndef SIMPLE_PLANNING_SIMULATOR_CORE_H_ #define SIMPLE_PLANNING_SIMULATOR_CORE_H_ -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include +#include "rclcpp/rclcpp.hpp" + +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "std_msgs/msg/bool.hpp" +#include "std_msgs/msg/float32.hpp" +#include "std_msgs/msg/int32.hpp" +#include "std_msgs/msg/string.hpp" +#include "tf2/LinearMath/Quaternion.h" +#include "tf2/utils.h" +#include "tf2_ros/transform_broadcaster.h" +#include "tf2_ros/transform_listener.h" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" #include -#include -#include -#include -#include -#include -#include +#include "autoware_planning_msgs/msg/trajectory.hpp" +#include "autoware_vehicle_msgs/msg/control_mode.hpp" +#include "autoware_vehicle_msgs/msg/shift_stamped.hpp" +#include "autoware_vehicle_msgs/msg/steering.hpp" +#include "autoware_vehicle_msgs/msg/turn_signal.hpp" +#include "autoware_vehicle_msgs/msg/vehicle_command.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp index ee12986a859e8..da4cc6338b12d 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp @@ -24,8 +24,8 @@ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include -#include +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" #include /** diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp index c799423446f85..fdd4875c9fd19 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp @@ -25,8 +25,8 @@ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include -#include +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" #include /** diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 819c9b77746c4..f6b1112c6846b 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -22,8 +22,8 @@ #ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ #define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ -#include -#include +#include "rclcpp/rclcpp.hpp" +#include "eigen3/Eigen/Core" /** * @class simple_planning_simulator vehicle model class diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp index 080cd8404709a..b72a1b59b0688 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp @@ -26,8 +26,8 @@ #include "simple_planning_simulator/vehicle_model/sim_model_util.hpp" #include -#include -#include +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" #include #include From c2ecb3441e07ab0f7adf0dc5fb1806776c5ac2b8 Mon Sep 17 00:00:00 2001 From: Nikolai Morin Date: Tue, 8 Dec 2020 15:10:45 +0100 Subject: [PATCH 09/52] Run uncrustify on the entire Pilot.Auto codebase (#151) * Run uncrustify on the entire Pilot.Auto codebase * Exclude open PRs --- .../simple_planning_simulator_core.hpp | 27 ++++--- .../sim_model_constant_acceleration.hpp | 8 +- .../vehicle_model/sim_model_ideal.hpp | 20 +++-- .../vehicle_model/sim_model_time_delay.hpp | 20 +++-- .../src/simple_planning_simulator_core.cpp | 17 ++-- .../src/simple_planning_simulator_node.cpp | 2 +- .../sim_model_constant_acceleration.cpp | 19 +++-- .../src/vehicle_model/sim_model_ideal.cpp | 54 ++++++------- .../src/vehicle_model/sim_model_interface.cpp | 13 +-- .../vehicle_model/sim_model_time_delay.cpp | 81 ++++++++++--------- 10 files changed, 147 insertions(+), 114 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index cd74e055dd4f0..59e952fdf314b 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -121,7 +121,8 @@ class Simulator : public rclcpp::Node std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time /* vehicle model */ - enum class VehicleModelType { + enum class VehicleModelType + { IDEAL_TWIST = 0, IDEAL_STEER = 1, DELAY_TWIST = 2, @@ -137,15 +138,15 @@ class Simulator : public rclcpp::Node /* to generate measurement noise */ std::shared_ptr rand_engine_ptr_; //!< @brief random engine for measurement noise std::shared_ptr> - pos_norm_dist_ptr_; //!< @brief Gaussian noise for position + pos_norm_dist_ptr_; //!< @brief Gaussian noise for position std::shared_ptr> - vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity + vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity std::shared_ptr> - rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw + rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw std::shared_ptr> - angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity + angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity std::shared_ptr> - steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle + steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle /** * @brief set current_vehicle_cmd_ptr_ with received message @@ -165,7 +166,8 @@ class Simulator : public rclcpp::Node /** * @brief set initial pose for simulation with received message */ - void callbackInitialPoseWithCov(const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + void callbackInitialPoseWithCov( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); /** * @brief set initial pose with received message @@ -202,7 +204,9 @@ class Simulator : public rclcpp::Node * @param [in] pose initial position and orientation * @param [in] twist initial velocity and angular velocity */ - void setInitialState(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist); + void setInitialState( + const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & twist); /** * @brief set initial state of simulated vehicle with pose transformation based on frame_id @@ -218,14 +222,17 @@ class Simulator : public rclcpp::Node * @param [in] twist initial velocity and angular velocity */ void setInitialStateWithPoseTransform( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, const geometry_msgs::msg::Twist & twist); + const geometry_msgs::msg::PoseWithCovarianceStamped & pose, + const geometry_msgs::msg::Twist & twist); /** * @brief publish pose and twist * @param [in] pose pose to be published * @param [in] twist twist to be published */ - void publishPoseTwist(const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist); + void publishPoseTwist( + const geometry_msgs::msg::Pose & pose, + const geometry_msgs::msg::Twist & twist); /** * @brief publish tf diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp index da4cc6338b12d..11985d8601898 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp @@ -50,14 +50,16 @@ class SimModelConstantAccelTwist : public SimModelInterface ~SimModelConstantAccelTwist() = default; private: - enum IDX { + enum IDX + { X = 0, Y, YAW, VX, WZ, }; - enum IDX_U { + enum IDX_U + { VX_DES = 0, WZ_DES, }; @@ -111,4 +113,4 @@ class SimModelConstantAccelTwist : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif \ No newline at end of file +#endif diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp index fdd4875c9fd19..4dc055d36b8a9 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp @@ -47,12 +47,14 @@ class SimModelIdealTwist : public SimModelInterface ~SimModelIdealTwist() = default; private: - enum IDX { + enum IDX + { X = 0, Y, YAW, }; - enum IDX_U { + enum IDX_U + { VX_DES = 0, WZ_DES, }; @@ -120,12 +122,14 @@ class SimModelIdealSteer : public SimModelInterface ~SimModelIdealSteer() = default; private: - enum IDX { + enum IDX + { X = 0, Y, YAW, }; - enum IDX_U { + enum IDX_U + { VX_DES = 0, STEER_DES, }; @@ -195,13 +199,15 @@ class SimModelIdealAccel : public SimModelInterface ~SimModelIdealAccel() = default; private: - enum IDX { + enum IDX + { X = 0, Y, YAW, VX, }; - enum IDX_U { + enum IDX_U + { AX_DES = 0, STEER_DES, }; @@ -252,4 +258,4 @@ class SimModelIdealAccel : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif \ No newline at end of file +#endif diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp index b72a1b59b0688..2513d412ca7ae 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp @@ -64,14 +64,16 @@ class SimModelTimeDelayTwist : public SimModelInterface private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX { + enum IDX + { X = 0, Y, YAW, VX, WZ, }; - enum IDX_U { + enum IDX_U + { VX_DES = 0, WZ_DES, }; @@ -170,14 +172,16 @@ class SimModelTimeDelaySteer : public SimModelInterface private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX { + enum IDX + { X = 0, Y, YAW, VX, STEER, }; - enum IDX_U { + enum IDX_U + { VX_DES = 0, STEER_DES, }; @@ -276,7 +280,8 @@ class SimModelTimeDelaySteerAccel : public SimModelInterface private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX { + enum IDX + { X = 0, Y, YAW, @@ -284,7 +289,8 @@ class SimModelTimeDelaySteerAccel : public SimModelInterface STEER, ACCX, }; - enum IDX_U { + enum IDX_U + { ACCX_DES = 0, STEER_DES, DRIVE_SHIFT, @@ -354,4 +360,4 @@ class SimModelTimeDelaySteerAccel : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif \ No newline at end of file +#endif diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index 466346d7dd460..ff7d9316b4627 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -282,7 +282,8 @@ void Simulator::timerCallbackSimulation() if ( cmd == autoware_vehicle_msgs::msg::TurnSignal::LEFT || cmd == autoware_vehicle_msgs::msg::TurnSignal::RIGHT || - cmd == autoware_vehicle_msgs::msg::TurnSignal::HAZARD) { + cmd == autoware_vehicle_msgs::msg::TurnSignal::HAZARD) + { turn_signal_msg.data = cmd; } } @@ -291,9 +292,9 @@ void Simulator::timerCallbackSimulation() autoware_vehicle_msgs::msg::ShiftStamped shift_msg; shift_msg.header.frame_id = simulation_frame_id_; shift_msg.header.stamp = get_clock()->now(); - shift_msg.shift.data = current_twist_.linear.x >= 0.0 - ? autoware_vehicle_msgs::msg::Shift::DRIVE - : autoware_vehicle_msgs::msg::Shift::REVERSE; + shift_msg.shift.data = current_twist_.linear.x >= 0.0 ? + autoware_vehicle_msgs::msg::Shift::DRIVE : + autoware_vehicle_msgs::msg::Shift::REVERSE; pub_shift_->publish(shift_msg); /* publish control mode */ @@ -307,7 +308,8 @@ void Simulator::callbackVehicleCmd( if ( vehicle_model_type_ == VehicleModelType::IDEAL_STEER || - vehicle_model_type_ == VehicleModelType::DELAY_STEER) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER) + { Eigen::VectorXd input(2); input << msg->control.velocity, msg->control.steering_angle; vehicle_model_ptr_->setInput(input); @@ -452,10 +454,11 @@ double Simulator::getPosZFromTrajectory(const double x, const double y) found = true; } } - if (found) + if (found) { return current_trajectory_ptr_->points.at(index).pose.position.z; - else + } else { return 0; + } } else { return 0.0; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp index eea2aafc4230f..785e523f7e059 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp @@ -23,4 +23,4 @@ int main(int argc, char ** argv) rclcpp::shutdown(); return 0; -}; +} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp index 0b04d92c0b97e..e085e169e0422 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp @@ -1,4 +1,3 @@ - /* * Copyright 2018-2019 Autoware Foundation. All rights reserved. * @@ -23,15 +22,15 @@ SimModelConstantAccelTwist::SimModelConstantAccelTwist( vx_lim_(vx_lim), wz_lim_(wz_lim), vx_rate_(vx_rate), - wz_rate_(wz_rate){}; + wz_rate_(wz_rate) {} -double SimModelConstantAccelTwist::getX() { return state_(IDX::X); }; -double SimModelConstantAccelTwist::getY() { return state_(IDX::Y); }; -double SimModelConstantAccelTwist::getYaw() { return state_(IDX::YAW); }; -double SimModelConstantAccelTwist::getVx() { return state_(IDX::VX); }; -double SimModelConstantAccelTwist::getWz() { return state_(IDX::WZ); }; -double SimModelConstantAccelTwist::getSteer() { return 0.0; }; -void SimModelConstantAccelTwist::update(const double & dt) { updateRungeKutta(dt, input_); } +double SimModelConstantAccelTwist::getX() {return state_(IDX::X);} +double SimModelConstantAccelTwist::getY() {return state_(IDX::Y);} +double SimModelConstantAccelTwist::getYaw() {return state_(IDX::YAW);} +double SimModelConstantAccelTwist::getVx() {return state_(IDX::VX);} +double SimModelConstantAccelTwist::getWz() {return state_(IDX::WZ);} +double SimModelConstantAccelTwist::getSteer() {return 0.0;} +void SimModelConstantAccelTwist::update(const double & dt) {updateRungeKutta(dt, input_);} Eigen::VectorXd SimModelConstantAccelTwist::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { @@ -62,4 +61,4 @@ Eigen::VectorXd SimModelConstantAccelTwist::calcModel( d_state(IDX::WZ) = wz_rate; return d_state; -}; \ No newline at end of file +} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp index a955846118520..0e8d48970a329 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp @@ -1,4 +1,3 @@ - /* * Copyright 2018-2019 Autoware Foundation. All rights reserved. * @@ -17,15 +16,16 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" -SimModelIdealTwist::SimModelIdealTwist() : SimModelInterface(3 /* dim x */, 2 /* dim u */){}; +SimModelIdealTwist::SimModelIdealTwist() +: SimModelInterface(3 /* dim x */, 2 /* dim u */) {} -double SimModelIdealTwist::getX() { return state_(IDX::X); }; -double SimModelIdealTwist::getY() { return state_(IDX::Y); }; -double SimModelIdealTwist::getYaw() { return state_(IDX::YAW); }; -double SimModelIdealTwist::getVx() { return input_(IDX_U::VX_DES); }; -double SimModelIdealTwist::getWz() { return input_(IDX_U::WZ_DES); }; -double SimModelIdealTwist::getSteer() { return 0.0; }; -void SimModelIdealTwist::update(const double & dt) { updateRungeKutta(dt, input_); } +double SimModelIdealTwist::getX() {return state_(IDX::X);} +double SimModelIdealTwist::getY() {return state_(IDX::Y);} +double SimModelIdealTwist::getYaw() {return state_(IDX::YAW);} +double SimModelIdealTwist::getVx() {return input_(IDX_U::VX_DES);} +double SimModelIdealTwist::getWz() {return input_(IDX_U::WZ_DES);} +double SimModelIdealTwist::getSteer() {return 0.0;} +void SimModelIdealTwist::update(const double & dt) {updateRungeKutta(dt, input_);} Eigen::VectorXd SimModelIdealTwist::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { @@ -39,21 +39,21 @@ Eigen::VectorXd SimModelIdealTwist::calcModel( d_state(IDX::YAW) = wz; return d_state; -}; +} SimModelIdealSteer::SimModelIdealSteer(double wheelbase) -: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase){}; +: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} -double SimModelIdealSteer::getX() { return state_(IDX::X); }; -double SimModelIdealSteer::getY() { return state_(IDX::Y); }; -double SimModelIdealSteer::getYaw() { return state_(IDX::YAW); }; -double SimModelIdealSteer::getVx() { return input_(IDX_U::VX_DES); }; +double SimModelIdealSteer::getX() {return state_(IDX::X);} +double SimModelIdealSteer::getY() {return state_(IDX::Y);} +double SimModelIdealSteer::getYaw() {return state_(IDX::YAW);} +double SimModelIdealSteer::getVx() {return input_(IDX_U::VX_DES);} double SimModelIdealSteer::getWz() { return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; -}; -double SimModelIdealSteer::getSteer() { return input_(IDX_U::STEER_DES); }; -void SimModelIdealSteer::update(const double & dt) { updateRungeKutta(dt, input_); } +} +double SimModelIdealSteer::getSteer() {return input_(IDX_U::STEER_DES);} +void SimModelIdealSteer::update(const double & dt) {updateRungeKutta(dt, input_);} Eigen::VectorXd SimModelIdealSteer::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { @@ -67,20 +67,20 @@ Eigen::VectorXd SimModelIdealSteer::calcModel( d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; return d_state; -}; +} SimModelIdealAccel::SimModelIdealAccel(double wheelbase) -: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase){}; +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} -double SimModelIdealAccel::getX() { return state_(IDX::X); }; -double SimModelIdealAccel::getY() { return state_(IDX::Y); }; -double SimModelIdealAccel::getYaw() { return state_(IDX::YAW); }; -double SimModelIdealAccel::getVx() { return state_(IDX::VX); }; +double SimModelIdealAccel::getX() {return state_(IDX::X);} +double SimModelIdealAccel::getY() {return state_(IDX::Y);} +double SimModelIdealAccel::getYaw() {return state_(IDX::YAW);} +double SimModelIdealAccel::getVx() {return state_(IDX::VX);} double SimModelIdealAccel::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; -}; -double SimModelIdealAccel::getSteer() { return input_(IDX_U::STEER_DES); }; +} +double SimModelIdealAccel::getSteer() {return input_(IDX_U::STEER_DES);} void SimModelIdealAccel::update(const double & dt) { updateRungeKutta(dt, input_); @@ -104,4 +104,4 @@ Eigen::VectorXd SimModelIdealAccel::calcModel( d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; return d_state; -}; +} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp index ab3473ad8e653..d39abaddc4c3c 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp @@ -14,11 +14,12 @@ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) +SimModelInterface::SimModelInterface(int dim_x, int dim_u) +: dim_x_(dim_x), dim_u_(dim_u) { state_ = Eigen::VectorXd::Zero(dim_x_); input_ = Eigen::VectorXd::Zero(dim_u_); -}; +} void SimModelInterface::updateRungeKutta(const double & dt, const Eigen::VectorXd & input) { @@ -33,7 +34,7 @@ void SimModelInterface::updateEuler(const double & dt, const Eigen::VectorXd & i { state_ += calcModel(state_, input) * dt; } -void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; }; -void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; }; -void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; }; -void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; }; +void SimModelInterface::getState(Eigen::VectorXd & state) {state = state_;} +void SimModelInterface::getInput(Eigen::VectorXd & input) {input = input_;} +void SimModelInterface::setState(const Eigen::VectorXd & state) {state_ = state;} +void SimModelInterface::setInput(const Eigen::VectorXd & input) {input_ = input;} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp index 8e072c158a99b..334b695d4f341 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp @@ -1,4 +1,3 @@ - /* * Copyright 2018-2019 Autoware Foundation. All rights reserved. * @@ -39,20 +38,22 @@ SimModelTimeDelayTwist::SimModelTimeDelayTwist( deadzone_delta_steer_(deadzone_delta_steer) { if (vx_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << std::endl; + std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << + std::endl; } if (wz_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings wz_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << std::endl; + std::cout << "Settings wz_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << + std::endl; } initializeInputQueue(dt); -}; +} -double SimModelTimeDelayTwist::getX() { return state_(IDX::X); }; -double SimModelTimeDelayTwist::getY() { return state_(IDX::Y); }; -double SimModelTimeDelayTwist::getYaw() { return state_(IDX::YAW); }; -double SimModelTimeDelayTwist::getVx() { return state_(IDX::VX); }; -double SimModelTimeDelayTwist::getWz() { return state_(IDX::WZ); }; -double SimModelTimeDelayTwist::getSteer() { return 0.0; }; +double SimModelTimeDelayTwist::getX() {return state_(IDX::X);} +double SimModelTimeDelayTwist::getY() {return state_(IDX::Y);} +double SimModelTimeDelayTwist::getYaw() {return state_(IDX::YAW);} +double SimModelTimeDelayTwist::getVx() {return state_(IDX::VX);} +double SimModelTimeDelayTwist::getWz() {return state_(IDX::WZ);} +double SimModelTimeDelayTwist::getSteer() {return 0.0;} void SimModelTimeDelayTwist::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -65,7 +66,7 @@ void SimModelTimeDelayTwist::update(const double & dt) wz_input_queue_.pop_front(); // do not use deadzone_delta_steer (Steer IF does not exist in this model) updateRungeKutta(dt, delayed_input); -}; +} void SimModelTimeDelayTwist::initializeInputQueue(const double & dt) { size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); @@ -101,7 +102,7 @@ Eigen::VectorXd SimModelTimeDelayTwist::calcModel( d_state(IDX::WZ) = wz_rate; return d_state; -}; +} /* * @@ -126,24 +127,26 @@ SimModelTimeDelaySteer::SimModelTimeDelaySteer( deadzone_delta_steer_(deadzone_delta_steer) { if (vx_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << std::endl; + std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << + std::endl; } if (steer_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings steer_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << std::endl; + std::cout << "Settings steer_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << + std::endl; } initializeInputQueue(dt); -}; +} -double SimModelTimeDelaySteer::getX() { return state_(IDX::X); }; -double SimModelTimeDelaySteer::getY() { return state_(IDX::Y); }; -double SimModelTimeDelaySteer::getYaw() { return state_(IDX::YAW); }; -double SimModelTimeDelaySteer::getVx() { return state_(IDX::VX); }; +double SimModelTimeDelaySteer::getX() {return state_(IDX::X);} +double SimModelTimeDelaySteer::getY() {return state_(IDX::Y);} +double SimModelTimeDelaySteer::getYaw() {return state_(IDX::YAW);} +double SimModelTimeDelaySteer::getVx() {return state_(IDX::VX);} double SimModelTimeDelaySteer::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; -}; -double SimModelTimeDelaySteer::getSteer() { return state_(IDX::STEER); }; +} +double SimModelTimeDelaySteer::getSteer() {return state_(IDX::STEER);} void SimModelTimeDelaySteer::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -158,7 +161,7 @@ void SimModelTimeDelaySteer::update(const double & dt) steer_input_queue_.pop_front(); updateRungeKutta(dt, delayed_input); -}; +} void SimModelTimeDelaySteer::initializeInputQueue(const double & dt) { size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); @@ -194,7 +197,7 @@ Eigen::VectorXd SimModelTimeDelaySteer::calcModel( d_state(IDX::STEER) = steer_rate; return d_state; -}; +} SimModelTimeDelaySteerAccel::SimModelTimeDelaySteerAccel( double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, @@ -214,24 +217,26 @@ SimModelTimeDelaySteerAccel::SimModelTimeDelaySteerAccel( deadzone_delta_steer_(deadzone_delta_steer) { if (acc_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings acc_time_constant is too small, replace it by" << MIN_TIME_CONSTANT << std::endl; + std::cout << "Settings acc_time_constant is too small, replace it by" << MIN_TIME_CONSTANT << + std::endl; } if (steer_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings steer_time_constant is too small, replace it by" << MIN_TIME_CONSTANT << std::endl; + std::cout << "Settings steer_time_constant is too small, replace it by" << MIN_TIME_CONSTANT << + std::endl; } initializeInputQueue(dt); -}; +} -double SimModelTimeDelaySteerAccel::getX() { return state_(IDX::X); }; -double SimModelTimeDelaySteerAccel::getY() { return state_(IDX::Y); }; -double SimModelTimeDelaySteerAccel::getYaw() { return state_(IDX::YAW); }; -double SimModelTimeDelaySteerAccel::getVx() { return state_(IDX::VX); }; +double SimModelTimeDelaySteerAccel::getX() {return state_(IDX::X);} +double SimModelTimeDelaySteerAccel::getY() {return state_(IDX::Y);} +double SimModelTimeDelaySteerAccel::getYaw() {return state_(IDX::YAW);} +double SimModelTimeDelaySteerAccel::getVx() {return state_(IDX::VX);} double SimModelTimeDelaySteerAccel::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; -}; -double SimModelTimeDelaySteerAccel::getSteer() { return state_(IDX::STEER); }; +} +double SimModelTimeDelaySteerAccel::getSteer() {return state_(IDX::STEER);} void SimModelTimeDelaySteerAccel::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -251,14 +256,16 @@ void SimModelTimeDelaySteerAccel::update(const double & dt) if (delayed_input(IDX_U::DRIVE_SHIFT) >= 0.0) { state_(IDX::VX) = std::max(0.0, std::min(state_(IDX::VX), vx_lim_)); if ( - std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || std::abs((state_(IDX::VX) - vx_lim_)) < 10e-9) { + std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || std::abs((state_(IDX::VX) - vx_lim_)) < 10e-9) + { state_(IDX::ACCX) = 0.0; } } else { state_(IDX::VX) = std::min(0.0, std::max(state_(IDX::VX), -vx_lim_)); if ( std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || - std::abs((state_(IDX::VX) - (-vx_lim_))) < 10e-9) { + std::abs((state_(IDX::VX) - (-vx_lim_))) < 10e-9) + { state_(IDX::ACCX) = 0.0; } } @@ -287,7 +294,9 @@ Eigen::VectorXd SimModelTimeDelaySteerAccel::calcModel( const double delay_input_steer = input(IDX_U::STEER_DES); const double drive_shift = input(IDX_U::DRIVE_SHIFT); double delay_acc_des = std::max(std::min(delay_input_acc, vx_rate_lim_), -vx_rate_lim_); - if (!(drive_shift >= 0.0)) delay_acc_des *= -1; // reverse front-back + if (!(drive_shift >= 0.0)) { + delay_acc_des *= -1; // reverse front-back + } double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); double accx_rate = -(acc - delay_acc_des) / acc_time_constant_; double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; @@ -309,4 +318,4 @@ Eigen::VectorXd SimModelTimeDelaySteerAccel::calcModel( d_state(IDX::ACCX) = accx_rate; return d_state; -}; +} From ffab21db3e5b8867c2537d53bac726d2fc224953 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Wed, 23 Dec 2020 11:49:27 +0900 Subject: [PATCH 10/52] reduce terminal ouput for better error message visibility (#200) * reduce terminal ouput for better error message visibility Signed-off-by: mitsudome-r * [costmap_generator] fix waiting for first transform Signed-off-by: mitsudome-r * fix tests Signed-off-by: mitsudome-r * fix test Signed-off-by: mitsudome-r --- .../src/simple_planning_simulator_core.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index ff7d9316b4627..dfe427ac6f1c7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -201,7 +201,9 @@ void Simulator::callbackEngage(const std_msgs::msg::Bool::ConstSharedPtr msg) void Simulator::timerCallbackSimulation() { if (!is_initialized_) { - RCLCPP_INFO(get_logger(), "[simple_planning_simulator] waiting initial position..."); + RCLCPP_INFO_THROTTLE( + get_logger(), *this->get_clock(), 3000 /*ms*/, + "waiting initial position..."); return; } @@ -386,15 +388,14 @@ void Simulator::getTransformFromTF( const std::string parent_frame, const std::string child_frame, geometry_msgs::msg::TransformStamped & transform) { - while (1) { + while (rclcpp::ok()) { try { const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); transform = tf_buffer_->lookupTransform( - parent_frame, child_frame, time_point, tf2::durationFromSec(0.0)); + parent_frame, child_frame, time_point); break; } catch (tf2::TransformException & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - rclcpp::sleep_for(std::chrono::milliseconds(500)); } } } From fc3691fd361f0ae8e16572aafc4683a091ce9a87 Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Thu, 24 Dec 2020 22:16:22 +0900 Subject: [PATCH 11/52] Use trajectory for z position source (#243) Signed-off-by: wep21 --- .../simple_planning_simulator_core.hpp | 1 + .../src/simple_planning_simulator_core.cpp | 8 +++++++- 2 files changed, 8 insertions(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 59e952fdf314b..b737bf67c11ad 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -116,6 +116,7 @@ class Simulator : public rclcpp::Node bool is_initialized_ = false; //!< @brief flag to check the initial position is set bool add_measurement_noise_; //!< @brief flag to add measurement noise bool simulator_engage_; //!< @brief flag to engage simulator + bool use_trajectory_for_z_position_source_; //!< @brief flag to get z positon from trajectory /* saved values */ std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index dfe427ac6f1c7..75a9b447ffab2 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -24,6 +24,8 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & simulation_frame_id_ = declare_parameter("simulation_frame_id", "base_link"); map_frame_id_ = declare_parameter("map_frame_id", "map"); add_measurement_noise_ = declare_parameter("add_measurement_noise", false); + use_trajectory_for_z_position_source_ = + declare_parameter("use_trajectory_for_z_position_source", true); /* set pub sub topic name */ pub_pose_ = @@ -60,7 +62,11 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & sub_initialpose_ = create_subscription( "input/initial_pose", rclcpp::QoS{1}, std::bind(&Simulator::callbackInitialPoseWithCov, this, std::placeholders::_1)); - + if (use_trajectory_for_z_position_source_) { + sub_trajectory_ = create_subscription( + "base_trajectory", rclcpp::QoS{1}, + std::bind(&Simulator::callbackTrajectory, this, std::placeholders::_1)); + } const double dt = 1.0 / loop_rate_; /* Timer */ From a97a7755a3bb1cb57bfbd9c873864707416ee51c Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Wed, 17 Feb 2021 11:38:54 +0900 Subject: [PATCH 12/52] Ros2 v0.8.0 engage (#342) * [autoware_vehicle_msgs]: Add engage message Signed-off-by: wep21 * [as]: Update message Signed-off-by: wep21 * [awapi_awiv_adapter]: Update message Signed-off-by: wep21 * [web_controller]: Update message Signed-off-by: wep21 * [vehicle_cmd_gate]: Update message Signed-off-by: wep21 * [autoware_state_monitor]: Update message Signed-off-by: wep21 * [autoware_control_msgs]: Remove EngageMode message Signed-off-by: wep21 * [simple_planning_simulator]: Update message Signed-off-by: wep21 --- simulator/simple_planning_simulator/CMakeLists.txt | 2 ++ .../simple_planning_simulator_core.hpp | 12 +++++------- simulator/simple_planning_simulator/package.xml | 1 + .../src/simple_planning_simulator_core.cpp | 13 +++++++------ 4 files changed, 15 insertions(+), 13 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 277f04af7e3bc..60e38f4648062 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -17,6 +17,7 @@ find_package(rclcpp REQUIRED) find_package(std_msgs REQUIRED) find_package(tf2 REQUIRED) find_package(tf2_ros REQUIRED) +find_package(autoware_debug_msgs REQUIRED) find_package(autoware_planning_msgs REQUIRED) find_package(autoware_control_msgs REQUIRED) find_package(autoware_vehicle_msgs REQUIRED) @@ -42,6 +43,7 @@ set(SIMPLE_PLANNING_SIMULATOR_DEPENDENCIES geometry_msgs tf2 tf2_ros + autoware_debug_msgs autoware_planning_msgs autoware_control_msgs autoware_vehicle_msgs diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index b737bf67c11ad..fd32dc8f24072 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -27,10 +27,6 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" -#include "std_msgs/msg/bool.hpp" -#include "std_msgs/msg/float32.hpp" -#include "std_msgs/msg/int32.hpp" -#include "std_msgs/msg/string.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2/utils.h" #include "tf2_ros/transform_broadcaster.h" @@ -39,8 +35,10 @@ #include "eigen3/Eigen/LU" #include +#include "autoware_debug_msgs/msg/float32_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/control_mode.hpp" +#include "autoware_vehicle_msgs/msg/engage.hpp" #include "autoware_vehicle_msgs/msg/shift_stamped.hpp" #include "autoware_vehicle_msgs/msg/steering.hpp" #include "autoware_vehicle_msgs/msg/turn_signal.hpp" @@ -69,7 +67,7 @@ class Simulator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_pose_; //!< @brief topic ros publisher for current pose rclcpp::Publisher::SharedPtr pub_twist_; //!< @brief topic ros publisher for current twist rclcpp::Publisher::SharedPtr pub_steer_; - rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_velocity_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_control_mode_; @@ -79,7 +77,7 @@ class Simulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief topic subscriber for trajectory used for z ppsition rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief topic subscriber for initialpose topic rclcpp::Subscription::SharedPtr sub_initialtwist_; //!< @brief topic subscriber for initialtwist topic - rclcpp::Subscription::SharedPtr sub_engage_; //!< @brief topic subscriber for engage topic + rclcpp::Subscription::SharedPtr sub_engage_; //!< @brief topic subscriber for engage topic rclcpp::TimerBase::SharedPtr timer_simulation_; //!< @brief timer for simulation /* tf */ @@ -183,7 +181,7 @@ class Simulator : public rclcpp::Node /** * @brief set simulator engage with received message */ - void callbackEngage(const std_msgs::msg::Bool::ConstSharedPtr msg); + void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg); /** * @brief get transform from two frame_ids diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 962d1d8ffad2a..c070224994a5f 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -15,6 +15,7 @@ geometry_msgs tf2 tf2_ros + autoware_debug_msgs autoware_planning_msgs autoware_control_msgs autoware_vehicle_msgs diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index 75a9b447ffab2..06f596b63d1c4 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -36,8 +36,8 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & "output/control_mode", rclcpp::QoS{1}); pub_steer_ = create_publisher( "/vehicle/status/steering", rclcpp::QoS{1}); - pub_velocity_ = - create_publisher("/vehicle/status/velocity", rclcpp::QoS{1}); + pub_velocity_ = create_publisher( + "/vehicle/status/velocity", rclcpp::QoS{1}); pub_turn_signal_ = create_publisher( "/vehicle/status/turn_signal", rclcpp::QoS{1}); pub_shift_ = create_publisher( @@ -56,7 +56,7 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & "input/initial_twist", rclcpp::QoS{1}, std::bind(&Simulator::callbackInitialTwistStamped, this, std::placeholders::_1)); - sub_engage_ = create_subscription( + sub_engage_ = create_subscription( "input/engage", rclcpp::QoS{1}, std::bind(&Simulator::callbackEngage, this, std::placeholders::_1)); sub_initialpose_ = create_subscription( @@ -199,9 +199,9 @@ void Simulator::callbackInitialTwistStamped( } } -void Simulator::callbackEngage(const std_msgs::msg::Bool::ConstSharedPtr msg) +void Simulator::callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg) { - simulator_engage_ = msg->data; + simulator_engage_ = msg->engage; } void Simulator::timerCallbackSimulation() @@ -276,7 +276,8 @@ void Simulator::timerCallbackSimulation() pub_steer_->publish(steer_msg); /* float info publishers */ - std_msgs::msg::Float32 velocity_msg; + autoware_debug_msgs::msg::Float32Stamped velocity_msg; + velocity_msg.stamp = this->now(); velocity_msg.data = current_twist_.linear.x; pub_velocity_->publish(velocity_msg); From 4dcfe97c84a261929397e09438fcd59d817c5bdd Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Wed, 24 Feb 2021 15:08:01 +0900 Subject: [PATCH 13/52] Ros2 v0.8.0 fix packages (#351) * add subscription to QoS * add vihicle_param _file to simple_planning_sim * update cmake/packages.xml * comment out unused parameter * apply lint * add vehicle_info_util to lane_change_planner * add vehicle_info_util to vehicle_cmd_gate * fix cmake of simple planning simulator * update cmake/packages.xml of vehicle cmd gate * apply lint * apply lint * add latch option to autoware_state_monitor * delete unused comment --- .../simple_planning_simulator/CMakeLists.txt | 36 ++--- .../simple_planning_simulator_core.hpp | 66 +++++---- .../sim_model_constant_acceleration.hpp | 13 +- .../vehicle_model/sim_model_ideal.hpp | 15 +- .../vehicle_model/sim_model_interface.hpp | 8 +- .../vehicle_model/sim_model_time_delay.hpp | 15 +- .../vehicle_model/sim_model_util.hpp | 8 +- .../simple_planning_simulator.launch.py | 38 +++-- .../simple_planning_simulator.launch.xml | 2 + .../simple_planning_simulator/package.xml | 12 +- .../scripts/fitParamDelayInputModel.py | 133 +++++++++++++----- .../src/simple_planning_simulator_core.cpp | 12 +- .../src/simple_planning_simulator_node.cpp | 3 +- .../sim_model_constant_acceleration.cpp | 30 ++-- .../src/vehicle_model/sim_model_ideal.cpp | 28 ++-- .../src/vehicle_model/sim_model_interface.cpp | 2 +- .../vehicle_model/sim_model_time_delay.cpp | 30 ++-- .../src/vehicle_model/sim_model_util.cpp | 2 +- 18 files changed, 271 insertions(+), 182 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 60e38f4648062..769c9f96fb8d7 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -11,16 +11,8 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() ### Dependencies -find_package(ament_cmake REQUIRED) -find_package(geometry_msgs REQUIRED) -find_package(rclcpp REQUIRED) -find_package(std_msgs REQUIRED) -find_package(tf2 REQUIRED) -find_package(tf2_ros REQUIRED) -find_package(autoware_debug_msgs REQUIRED) -find_package(autoware_planning_msgs REQUIRED) -find_package(autoware_control_msgs REQUIRED) -find_package(autoware_vehicle_msgs REQUIRED) +find_package(ament_cmake_auto REQUIRED) +ament_auto_find_build_dependencies() # Add path of include dir include_directories(include) @@ -34,22 +26,7 @@ set(SIMPLE_PLANNING_SIMULATOR_SRC src/vehicle_model/sim_model_time_delay.cpp src/vehicle_model/sim_model_util.cpp ) -add_executable(simple_planning_simulator_exe src/simple_planning_simulator_node.cpp ${SIMPLE_PLANNING_SIMULATOR_SRC}) - -# Add dependencies. use target_link_libaries() before Crystal -set(SIMPLE_PLANNING_SIMULATOR_DEPENDENCIES - rclcpp - std_msgs - geometry_msgs - tf2 - tf2_ros - autoware_debug_msgs - autoware_planning_msgs - autoware_control_msgs - autoware_vehicle_msgs -) -ament_target_dependencies(simple_planning_simulator_exe ${SIMPLE_PLANNING_SIMULATOR_DEPENDENCIES}) - +ament_auto_add_executable(simple_planning_simulator_exe src/simple_planning_simulator_node.cpp ${SIMPLE_PLANNING_SIMULATOR_SRC}) ## Install executables and/or libraries install(TARGETS simple_planning_simulator_exe @@ -66,5 +43,10 @@ install(DIRECTORY config launch DESTINATION share/${PROJECT_NAME} ) +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + ament_lint_auto_find_test_dependencies() +endif() + # set at the end of cmakelists -ament_package() +ament_auto_package() diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index fd32dc8f24072..a6a6bbf602f43 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -1,4 +1,4 @@ -// Copyright 2015-2019 Autoware Foundation +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,11 +19,17 @@ * @date 2019.08.17 */ -#ifndef SIMPLE_PLANNING_SIMULATOR_CORE_H_ -#define SIMPLE_PLANNING_SIMULATOR_CORE_H_ +#ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ + +#include +#include +#include #include "rclcpp/rclcpp.hpp" +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/twist_stamped.hpp" @@ -31,9 +37,6 @@ #include "tf2/utils.h" #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" -#include #include "autoware_debug_msgs/msg/float32_stamped.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" @@ -48,6 +51,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" +#include "vehicle_info_util/vehicle_info.hpp" class Simulator : public rclcpp::Node { @@ -64,21 +68,29 @@ class Simulator : public rclcpp::Node private: /* ros system */ - rclcpp::Publisher::SharedPtr pub_pose_; //!< @brief topic ros publisher for current pose - rclcpp::Publisher::SharedPtr pub_twist_; //!< @brief topic ros publisher for current twist + rclcpp::Publisher::SharedPtr + pub_pose_; //!< @brief topic ros publisher for current pose + rclcpp::Publisher::SharedPtr + pub_twist_; //!< @brief topic ros publisher for current twist rclcpp::Publisher::SharedPtr pub_steer_; rclcpp::Publisher::SharedPtr pub_velocity_; rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_control_mode_; - rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; //!< @brief topic subscriber for vehicle_cmd - rclcpp::Subscription::SharedPtr sub_turn_signal_cmd_; //!< @brief topic subscriber for turn_signal_cmd - rclcpp::Subscription::SharedPtr sub_trajectory_; //!< @brief topic subscriber for trajectory used for z ppsition - rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief topic subscriber for initialpose topic - rclcpp::Subscription::SharedPtr sub_initialtwist_; //!< @brief topic subscriber for initialtwist topic - rclcpp::Subscription::SharedPtr sub_engage_; //!< @brief topic subscriber for engage topic - rclcpp::TimerBase::SharedPtr timer_simulation_; //!< @brief timer for simulation + rclcpp::Subscription::SharedPtr + sub_vehicle_cmd_; //!< @brief topic subscriber for vehicle_cmd + rclcpp::Subscription::SharedPtr + sub_turn_signal_cmd_; //!< @brief topic subscriber for turn_signal_cmd + rclcpp::Subscription::SharedPtr + sub_trajectory_; //!< @brief topic subscriber for trajectory used for z ppsition + rclcpp::Subscription::SharedPtr + sub_initialpose_; //!< @brief topic subscriber for initialpose topic + rclcpp::Subscription::SharedPtr + sub_initialtwist_; //!< @brief topic subscriber for initialtwist topic + rclcpp::Subscription::SharedPtr + sub_engage_; //!< @brief topic subscriber for engage topic + rclcpp::TimerBase::SharedPtr timer_simulation_; //!< @brief timer for simulation /* tf */ std::shared_ptr tf_broadcaster_; @@ -86,10 +98,12 @@ class Simulator : public rclcpp::Node std::shared_ptr tf_buffer_; /* received & published topics */ - geometry_msgs::msg::PoseStamped::ConstSharedPtr initial_pose_ptr_; //!< @brief initial vehicle pose + geometry_msgs::msg::PoseStamped::ConstSharedPtr + initial_pose_ptr_; //!< @brief initial vehicle pose geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr - initial_pose_with_cov_ptr_; //!< @brief initial vehicle pose with cov - geometry_msgs::msg::TwistStamped::ConstSharedPtr initial_twist_ptr_; //!< @brief initial vehicle velocity + initial_pose_with_cov_ptr_; //!< @brief initial vehicle pose with cov + geometry_msgs::msg::TwistStamped::ConstSharedPtr + initial_twist_ptr_; //!< @brief initial vehicle velocity geometry_msgs::msg::Pose current_pose_; //!< @brief current vehicle position and angle geometry_msgs::msg::Twist current_twist_; //!< @brief current vehicle velocity autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr @@ -111,10 +125,10 @@ class Simulator : public rclcpp::Node double sim_steering_gear_ratio_; //!< @brief for steering wheel angle calcultion /* flags */ - bool is_initialized_ = false; //!< @brief flag to check the initial position is set - bool add_measurement_noise_; //!< @brief flag to add measurement noise - bool simulator_engage_; //!< @brief flag to engage simulator - bool use_trajectory_for_z_position_source_; //!< @brief flag to get z positon from trajectory + bool is_initialized_ = false; //!< @brief flag to check the initial position is set + bool add_measurement_noise_; //!< @brief flag to add measurement noise + bool simulator_engage_; //!< @brief flag to engage simulator + bool use_trajectory_for_z_position_source_; //!< @brief flag to get z positon from trajectory /* saved values */ std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time @@ -204,8 +218,7 @@ class Simulator : public rclcpp::Node * @param [in] twist initial velocity and angular velocity */ void setInitialState( - const geometry_msgs::msg::Pose & pose, - const geometry_msgs::msg::Twist & twist); + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist); /** * @brief set initial state of simulated vehicle with pose transformation based on frame_id @@ -230,8 +243,7 @@ class Simulator : public rclcpp::Node * @param [in] twist twist to be published */ void publishPoseTwist( - const geometry_msgs::msg::Pose & pose, - const geometry_msgs::msg::Twist & twist); + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist); /** * @brief publish tf @@ -254,4 +266,4 @@ class Simulator : public rclcpp::Node const double & roll, const double & pitch, const double & yaw); }; -#endif +#endif // SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp index 11985d8601898..69d30221a1840 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp @@ -1,4 +1,4 @@ -// Copyright 2015-2019 Autoware Foundation +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -14,19 +14,20 @@ /** * @file sim_model_constant_acceleration.hpp - * @brief simple planning simulator model with constant acceleration for velocity & steeiring + * @brief simple planning simulator model with constant acceleration for velocity & steering * @author Takamasa Horibe * @date 2019.08.17 */ -#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_CONSTANT_ACCELERATION_H_ -#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_CONSTANT_ACCELERATION_H_ +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_CONSTANT_ACCELERATION_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_CONSTANT_ACCELERATION_HPP_ + +#include #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include /** * @class simple_planning_simulator constant acceleration twist model @@ -113,4 +114,4 @@ class SimModelConstantAccelTwist : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_CONSTANT_ACCELERATION_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp index 4dc055d36b8a9..3888dcb738ae3 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp @@ -1,4 +1,4 @@ -// Copyright 2015-2019 Autoware Foundation +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -20,14 +20,15 @@ * @date 2019.08.17 */ -#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_IDEAL_H_ -#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_IDEAL_H_ +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_HPP_ + +#include #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include /** * @class simple_planning_simulator ideal twist model @@ -114,7 +115,7 @@ class SimModelIdealSteer : public SimModelInterface * @brief constructor * @param [in] wheelbase vehicle wheelbase length [m] */ - SimModelIdealSteer(double wheelbase); + explicit SimModelIdealSteer(double wheelbase); /** * @brief destructor @@ -191,7 +192,7 @@ class SimModelIdealAccel : public SimModelInterface * @brief constructor * @param [in] wheelbase vehicle wheelbase length [m] */ - SimModelIdealAccel(double wheelbase); + explicit SimModelIdealAccel(double wheelbase); /** * @brief destructor @@ -258,4 +259,4 @@ class SimModelIdealAccel : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index f6b1112c6846b..9fdc2da818707 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,8 +19,8 @@ * @date 2019.08.17 */ -#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ -#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_INTERFACE_H_ +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #include "rclcpp/rclcpp.hpp" #include "eigen3/Eigen/Core" @@ -133,4 +133,4 @@ class SimModelInterface const Eigen::VectorXd & state, const Eigen::VectorXd & input) = 0; }; -#endif +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp index 2513d412ca7ae..9b63f29bb8ff7 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp @@ -1,4 +1,4 @@ -// Copyright 2015-2019 Autoware Foundation +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -19,17 +19,18 @@ * @date 2019.08.17 */ -#ifndef SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_TIME_DELAY_H_ -#define SIMPLE_PLANNING_SIMULATOR_SIM_MODEL_TIME_DELAY_H_ +#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_TIME_DELAY_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_TIME_DELAY_HPP_ + +#include +#include +#include #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_util.hpp" -#include #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include -#include /** * @class simple_planning_simulator time delay twist model @@ -360,4 +361,4 @@ class SimModelTimeDelaySteerAccel : public SimModelInterface Eigen::VectorXd calcModel(const Eigen::VectorXd & state, const Eigen::VectorXd & input) override; }; -#endif +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_TIME_DELAY_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp index 372c32924a887..c91708c5f69e4 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -11,6 +11,10 @@ // 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_UTIL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_UTIL_HPP_ + #include namespace sim_model_util @@ -18,3 +22,5 @@ namespace sim_model_util double getDummySteerCommandWithFriction( const double steer, const double steer_command, const double deadzone_delta_steer); } + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_UTIL_HPP_ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 54f89ca61d8f5..a7593f206cada 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -1,7 +1,20 @@ +# Copyright 2020 Tier IV, Inc. +# +# 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. + from launch import LaunchContext from launch import LaunchDescription from launch.actions import DeclareLaunchArgument -from launch.conditions import IfCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackage @@ -10,17 +23,20 @@ import os - context = LaunchContext() def find_pack(package_name): """Return the absolute path to the share directory of the given package.""" - return os.path.join(Path(FindPackage(package_name).perform(context)), 'share', package_name) + return os.path.join( + Path(FindPackage(package_name).perform(context)), 'share', package_name) + def generate_launch_description(): - simple_planning_simulator_param_file = os.path.join(find_pack('simple_planning_simulator'), 'config/simple_planning_simulator.param.yaml') + simple_planning_simulator_param_file = os.path.join( + find_pack('simple_planning_simulator'), + 'config/simple_planning_simulator.param.yaml') print(simple_planning_simulator_param_file) @@ -29,6 +45,10 @@ def generate_launch_description(): default_value=simple_planning_simulator_param_file, description='Path to config file for simple_planning_simulator' ) + vehicle_param_file_param = DeclareLaunchArgument( + 'vehicle_param_file', + description='Path to config file for vehicle param' + ) simple_planning_simulator = Node( package='simple_planning_simulator', @@ -36,11 +56,9 @@ def generate_launch_description(): node_name='simple_planning_simulator', node_namespace='simulation', output='screen', - parameters=[LaunchConfiguration('simple_planning_simulator_param_file'), - { - "/vehicle_info/wheel_base": 4.0, - "random_seed": 1 - } + parameters=[ + LaunchConfiguration('simple_planning_simulator_param_file'), + LaunchConfiguration('vehicle_param_file'), ], remappings=[ ('base_trajectory', '/planning/scenario_planning/trajectory'), @@ -56,8 +74,8 @@ def generate_launch_description(): ] ) - return LaunchDescription([ simple_planning_simulator_param, + vehicle_param_file_param, simple_planning_simulator ]) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml index 1495a6f0d8831..6fea6471b3aaa 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml @@ -9,6 +9,7 @@ + @@ -24,6 +25,7 @@ + diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index c070224994a5f..5d6e1ece53670 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -7,10 +7,10 @@ Takamasa HORIBE Apache License 2.0 - ament_cmake - rclcpp - rclcpp_components + ament_cmake_auto + rclcpp + rclcpp_components std_msgs geometry_msgs tf2 @@ -19,13 +19,15 @@ autoware_planning_msgs autoware_control_msgs autoware_vehicle_msgs + vehicle_info_util - rclcpp - rclcpp_components launch_ros rosidl_default_runtime topic_tools + ament_lint_auto + ament_lint_common + ament_cmake diff --git a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py index b40899e929222..50b4099ee98d6 100644 --- a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py +++ b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py @@ -1,6 +1,20 @@ #!/usr/bin/env python # -*- coding: utf-8 -*- +# Copyright 2020 Tier IV, Inc. +# +# 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. + import numpy as np import argparse import subprocess @@ -10,25 +24,28 @@ try: import pandas as pd except ImportError: - print ('Please install pandas. See http://pandas.pydata.org/pandas-docs/stable/') + print('Please install pandas. See http://pandas.pydata.org/pandas-docs/stable/') sys.exit(1) FREQ_SAMPLE = 0.001 # low pass filter + + def lowpass_filter(data, cutoff_freq=2, order=1, dt=FREQ_SAMPLE): tau = 1.0 / (2 * np.pi * cutoff_freq) for _ in range(order): - for i in range(1,len(data)): - data[i] = (tau / (tau + dt) * data[i-1] + dt / (tau + dt) * data[i]) + for i in range(1, len(data)): + data[i] = (tau / (tau + dt) * data[i - 1] + + dt / (tau + dt) * data[i]) return data + def rel2abs(path): - ''' - Return absolute path from relative path input - ''' + """Return absolute path from relative path input.""" return join(getcwd(), path) + def rosbag_to_csv(path, topic_name): name = splitext(basename(path))[0] suffix = topic_name.replace('/', '-') @@ -36,11 +53,13 @@ def rosbag_to_csv(path, topic_name): if exists(output_path): return output_path else: - command = "rostopic echo -b {0} -p /{1} | sed -e 's/,/ /g' > {2}".format(path, topic_name, output_path) - print (command) + command = "rostopic echo -b {0} -p /{1} | sed -e 's/,/ /g' > {2}".format( + path, topic_name, output_path) + print(command) subprocess.check_call(command, shell=True) return output_path + def getActValue(df, speed_type): tm = np.array(list(df['%time'])) * 1e-9 # Unit Conversion @@ -52,21 +71,25 @@ def getActValue(df, speed_type): dval = (val[2:] - val[:-2]) / (tm[2:] - tm[:-2]) return tm[1:-1], val[1:-1], dval + def getCmdValueWithDelay(df, delay): tm = np.array(list(df['%time'])) * 1e-9 val = np.array(list(df['field'])) return tm + delay, val + def getLinearInterpolate(_tm, _val, _index, ti): tmp_t = _tm[_index] tmp_nextt = _tm[_index + 1] tmp_val = _val[_index] tmp_nextval = _val[_index + 1] - val_i = tmp_val + (tmp_nextval - tmp_val) / (tmp_nextt - tmp_t) * (ti - tmp_t) + val_i = tmp_val + (tmp_nextval - tmp_val) / \ + (tmp_nextt - tmp_t) * (ti - tmp_t) return val_i -def getFittingTimeConstantParam(cmd_data, act_data, \ - delay, args, speed_type = False): + +def getFittingTimeConstantParam(cmd_data, act_data, + delay, args, speed_type=False): tm_cmd, cmd_delay = getCmdValueWithDelay(cmd_data, delay) tm_act, act, dact = getActValue(act_data, speed_type) _t_min = max(tm_cmd[0], tm_act[0]) @@ -93,21 +116,30 @@ def getFittingTimeConstantParam(cmd_data, act_data, \ diff_actcmd_samp = np.array(diff_actcmd_samp) if args.cutoff_freq > 0: dact_samp = lowpass_filter(dact_samp, cutoff_freq=args.cutoff_freq) - diff_actcmd_samp = lowpass_filter(diff_actcmd_samp, cutoff_freq=args.cutoff_freq) - dact_samp = dact_samp.reshape(1,-1) - diff_actcmd_samp = diff_actcmd_samp.reshape(1,-1) - tau = -np.dot(diff_actcmd_samp, np.linalg.pinv(dact_samp))[0,0] - error = np.linalg.norm(diff_actcmd_samp + tau * dact_samp) / dact_samp.shape[1] + diff_actcmd_samp = lowpass_filter( + diff_actcmd_samp, cutoff_freq=args.cutoff_freq) + dact_samp = dact_samp.reshape(1, -1) + diff_actcmd_samp = diff_actcmd_samp.reshape(1, -1) + tau = -np.dot(diff_actcmd_samp, np.linalg.pinv(dact_samp))[0, 0] + error = np.linalg.norm(diff_actcmd_samp + tau * + dact_samp) / dact_samp.shape[1] return tau, error -def getFittingParam(cmd_data, act_data, args, speed_type = False): + +def getFittingParam(cmd_data, act_data, args, speed_type=False): delay_range = int((args.max_delay - args.min_delay) / args.delay_incr) - delays = [args.min_delay + i * args.delay_incr for i in range(delay_range + 1)] + delays = [ + args.min_delay + + i * + args.delay_incr for i in range( + delay_range + + 1)] error_min = 1.0e10 delay_opt = -1 tau_opt = -1 for delay in delays: - tau, error = getFittingTimeConstantParam(cmd_data, act_data, delay, args, speed_type=speed_type) + tau, error = getFittingTimeConstantParam( + cmd_data, act_data, delay, args, speed_type=speed_type) if tau > 0: if error < error_min: error_min = error @@ -117,23 +149,60 @@ def getFittingParam(cmd_data, act_data, args, speed_type = False): break return tau_opt, delay_opt, error_min + if __name__ == '__main__': - topics = [ 'vehicle_cmd/ctrl_cmd/steering_angle', 'vehicle_status/angle', \ - 'vehicle_cmd/ctrl_cmd/linear_velocity', 'vehicle_status/speed'] + topics = ['vehicle_cmd/ctrl_cmd/steering_angle', 'vehicle_status/angle', + 'vehicle_cmd/ctrl_cmd/linear_velocity', 'vehicle_status/speed'] pd_data = [None] * len(topics) - parser = argparse.ArgumentParser(description='Paramter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input') - parser.add_argument('--bag_file', '-b', required=True, type=str, help='rosbag file', metavar='file') - parser.add_argument('--cutoff_time', default=1.0, type=float, help='Cutoff time[sec], Parameter fitting will only consider data from t= cutoff_time to the end of the bag file (default is 1.0)') - parser.add_argument('--cutoff_freq', default=0.1, type=float, help='Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1)') - parser.add_argument('--min_delay', default=0.1, type=float, help='Min value for searching delay loop (default is 0.1)') - parser.add_argument('--max_delay', default=1.0, type=float, help='Max value for searching delay loop (default is 1.0)') - parser.add_argument('--delay_incr', default=0.01, type=float, help='Step value for searching delay loop (default is 0.01)') + parser = argparse.ArgumentParser( + description='Paramter fitting for Input Delay Model' + ' (First Order System with Dead Time) with rosbag file input') + parser.add_argument( + '--bag_file', + '-b', + required=True, + type=str, + help='rosbag file', + metavar='file') + parser.add_argument( + '--cutoff_time', + default=1.0, + type=float, + help='Cutoff time[sec], Parameter fitting will only consider data ' + 'from t= cutoff_time to the end of the bag file (default is 1.0)') + parser.add_argument( + '--cutoff_freq', + default=0.1, + type=float, + help='Cutoff freq for low-pass filter[Hz], ' + 'negative value will disable low-pass filter (default is 0.1)') + parser.add_argument( + '--min_delay', + default=0.1, + type=float, + help='Min value for searching delay loop (default is 0.1)') + parser.add_argument( + '--max_delay', + default=1.0, + type=float, + help='Max value for searching delay loop (default is 1.0)') + parser.add_argument( + '--delay_incr', + default=0.01, + type=float, + help='Step value for searching delay loop (default is 0.01)') args = parser.parse_args() for i, topic in enumerate(topics): csv_log = rosbag_to_csv(rel2abs(args.bag_file), topic) pd_data[i] = pd.read_csv(csv_log, sep=' ') - tau_opt, delay_opt, error = getFittingParam(pd_data[0], pd_data[1], args, speed_type=False) - print ('Steer angle: tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' %(tau_opt, delay_opt, error)) - tau_opt, delay_opt, error = getFittingParam(pd_data[2], pd_data[3], args, speed_type=True) - print ('Velocity : tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' %(tau_opt, delay_opt, error)) + tau_opt, delay_opt, error = getFittingParam( + pd_data[0], pd_data[1], args, speed_type=False) + print( + 'Steer angle: tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' % + (tau_opt, delay_opt, error)) + tau_opt, delay_opt, error = getFittingParam( + pd_data[2], pd_data[3], args, speed_type=True) + print( + 'Velocity : tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' % + (tau_opt, delay_opt, error)) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index 06f596b63d1c4..998a894f150c3 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -1,4 +1,4 @@ -// Copyright 2015-2019 Autoware Foundation +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,14 +12,17 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include +#include +#include #include "simple_planning_simulator/simple_planning_simulator_core.hpp" Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { /* simple_planning_simulator parameters */ + wheelbase_ = vehicle_info_util::VehicleInfo::create(*this).wheel_base_m_; loop_rate_ = declare_parameter("loop_rate", 30.0); - wheelbase_ = declare_parameter("vehicle_info.wheel_base", 4.0); sim_steering_gear_ratio_ = declare_parameter("sim_steering_gear_ratio", 15.0); simulation_frame_id_ = declare_parameter("simulation_frame_id", "base_link"); map_frame_id_ = declare_parameter("map_frame_id", "map"); @@ -93,19 +96,14 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & { auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER"); RCLCPP_INFO(get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); - auto tread_length = declare_parameter("tread_length", 2.0); - auto angvel_lim = declare_parameter("angvel_lim", 3.0); auto vel_lim = declare_parameter("vel_lim", 50.0); auto steer_lim = declare_parameter("steer_lim", 1.0); auto accel_rate = declare_parameter("accel_rate", 10.0); - auto angvel_rate = declare_parameter("angvel_rate", 1.0); auto steer_rate_lim = declare_parameter("steer_rate_lim", 5.0); auto vel_time_delay = declare_parameter("vel_time_delay", 0.25); auto vel_time_constant = declare_parameter("vel_time_constant", 0.5); auto steer_time_delay = declare_parameter("steer_time_delay", 0.3); auto steer_time_constant = declare_parameter("steer_time_constant", 0.3); - auto angvel_time_delay = declare_parameter("angvel_time_delay", 0.3); - auto angvel_time_constant = declare_parameter("angvel_time_constant", 0.3); auto acc_time_delay = declare_parameter("acc_time_delay", 0.1); auto acc_time_constant = declare_parameter("acc_time_constant", 0.1); simulator_engage_ = declare_parameter("initial_engage_state", true); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp index 785e523f7e059..1f6590c5a17f4 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp @@ -1,4 +1,4 @@ -// Copyright 2015-2019 Autoware Foundation +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. @@ -12,6 +12,7 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "simple_planning_simulator/simple_planning_simulator_core.hpp" int main(int argc, char ** argv) diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp index e085e169e0422..700d7afd3977e 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp @@ -1,18 +1,18 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2015-2020 Autoware Foundation. All rights reserved. +// +// 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 #include "simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp" diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp index 0e8d48970a329..dd9f1cdb66e42 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp @@ -1,18 +1,16 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ +// Copyright 2015-2020 Autoware Foundation. All rights reserved. +// +// 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_ideal.hpp" diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp index d39abaddc4c3c..b0e109028e14d 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp @@ -1,4 +1,4 @@ -// Copyright 2018-2019 Autoware Foundation +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp index 334b695d4f341..05da1f009b19a 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp @@ -1,19 +1,17 @@ -/* - * Copyright 2018-2019 Autoware Foundation. All rights reserved. - * - * 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. - */ - +// Copyright 2015-2020 Autoware Foundation. All rights reserved. +// +// 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 #include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" /* diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp index 6323b16006955..223cd291fe263 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp @@ -1,4 +1,4 @@ -// Copyright 2020 Tier IV, Inc. +// Copyright 2015-2020 Autoware Foundation. All rights reserved. // // Licensed under the Apache License, Version 2.0 (the "License"); // you may not use this file except in compliance with the License. From bdd17918423a1867f5208c49161de36c79f7de18 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 25 Feb 2021 02:29:39 +0900 Subject: [PATCH 14/52] Rename ROS-related .yaml to .param.yaml (#352) * Rename ROS-related .yaml to .param.yaml Signed-off-by: Kenji Miyake * Remove prefix 'default_' of yaml files Signed-off-by: Kenji Miyake * Rename vehicle_info.yaml to vehicle_info.param.yaml Signed-off-by: Kenji Miyake * Rename diagnostic_aggregator's param files Signed-off-by: Kenji Miyake * Fix overlooked parameters Signed-off-by: Kenji Miyake --- .../config/{simulator_model.yaml => simulator_model.param.yaml} | 0 1 file changed, 0 insertions(+), 0 deletions(-) rename simulator/simple_planning_simulator/config/{simulator_model.yaml => simulator_model.param.yaml} (100%) diff --git a/simulator/simple_planning_simulator/config/simulator_model.yaml b/simulator/simple_planning_simulator/config/simulator_model.param.yaml similarity index 100% rename from simulator/simple_planning_simulator/config/simulator_model.yaml rename to simulator/simple_planning_simulator/config/simulator_model.param.yaml From bf134df5cc4f84ab9310ae60cc7d78f622660f96 Mon Sep 17 00:00:00 2001 From: Kazuki Miyahara Date: Tue, 16 Mar 2021 17:43:50 +0900 Subject: [PATCH 15/52] Fix typo in simulator module (#439) --- .../config/simple_planning_simulator.param.yaml | 2 +- .../simple_planning_simulator_core.hpp | 10 +++++----- .../vehicle_model/sim_model_ideal.hpp | 2 +- .../vehicle_model/sim_model_time_delay.hpp | 4 ++-- .../scripts/README_fitParamDelayInputModel.md | 2 +- .../scripts/fitParamDelayInputModel.py | 2 +- .../src/simple_planning_simulator_core.cpp | 4 ++-- 7 files changed, 13 insertions(+), 13 deletions(-) diff --git a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml index 43f2426f1ea74..0070937471bf3 100644 --- a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml +++ b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml @@ -29,7 +29,7 @@ simulation: # Option for vehicle_model_type: # - IDEAL_STEER : reads velocity command. The steering and velocity changes exactly the same as commanded. - # - DELAY_STEER : reads velocity command. The steering and elocity changes with delay model. + # - DELAY_STEER : reads velocity command. The steering and velocity changes with delay model. # - DELAY_STEER_ACC : reads acceleration command. The steering and acceleration changes with delay model. vehicle_model_type: DELAY_STEER_ACC vel_lim: 50.0 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index a6a6bbf602f43..ba07748f24245 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -83,7 +83,7 @@ class Simulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_turn_signal_cmd_; //!< @brief topic subscriber for turn_signal_cmd rclcpp::Subscription::SharedPtr - sub_trajectory_; //!< @brief topic subscriber for trajectory used for z ppsition + sub_trajectory_; //!< @brief topic subscriber for trajectory used for z position rclcpp::Subscription::SharedPtr sub_initialpose_; //!< @brief topic subscriber for initialpose topic rclcpp::Subscription::SharedPtr @@ -120,15 +120,15 @@ class Simulator : public rclcpp::Node std::string map_frame_id_; //!< @brief map frame_id /* simple_planning_simulator parameters */ - double loop_rate_; //!< @brief frequency to calculate vehicle model & pubish result + double loop_rate_; //!< @brief frequency to calculate vehicle model & publish result double wheelbase_; //!< @brief wheelbase length to convert angular-velocity & steering - double sim_steering_gear_ratio_; //!< @brief for steering wheel angle calcultion + double sim_steering_gear_ratio_; //!< @brief for steering wheel angle calculation /* flags */ bool is_initialized_ = false; //!< @brief flag to check the initial position is set bool add_measurement_noise_; //!< @brief flag to add measurement noise bool simulator_engage_; //!< @brief flag to engage simulator - bool use_trajectory_for_z_position_source_; //!< @brief flag to get z positon from trajectory + bool use_trajectory_for_z_position_source_; //!< @brief flag to get z position from trajectory /* saved values */ std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time @@ -257,7 +257,7 @@ class Simulator : public rclcpp::Node double getPosZFromTrajectory(const double x, const double y); /** - * @brief convert roll-pitch-yaw Eular angle to ros Quaternion + * @brief convert roll-pitch-yaw Euler angle to ros Quaternion * @param [in] roll roll angle [rad] * @param [in] pitch pitch angle [rad] * @param [in] yaw yaw angle [rad] diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp index 3888dcb738ae3..8a1297cdcb51a 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp @@ -14,7 +14,7 @@ /** * @file sim_model_ideal.hpp - * @brief simple planning simulator ideal velocity model (no dynamics for desired velocity & anguler-velocity or + * @brief simple planning simulator ideal velocity model (no dynamics for desired velocity & angular-velocity or * steering) * @author Takamasa Horibe * @date 2019.08.17 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp index 9b63f29bb8ff7..26b53bc8fcc9e 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp @@ -14,7 +14,7 @@ /** * @file sim_model_time_delay.hpp - * @brief simple planning simulator model with time delay and 1-dimensional dynamics for velocity & steeiring + * @brief simple planning simulator model with time delay and 1-dimensional dynamics for velocity & steering * @author Takamasa Horibe, Kim-Ngoc-Khanh Nguyen * @date 2019.08.17 */ @@ -44,7 +44,7 @@ class SimModelTimeDelayTwist : public SimModelInterface * @param [in] vx_lim velocity limit [m/s] * @param [in] angvel_lim angular velocity limit [m/s] * @param [in] vx_rate_lim acceleration limit [m/ss] - * @param [in] wz_rate_lim angular acceleration llimit [rad/ss] + * @param [in] wz_rate_lim angular acceleration limit [rad/ss] * @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 diff --git a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md index 511852ede903f..417747a523d7d 100644 --- a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md +++ b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md @@ -19,7 +19,7 @@ python fitParamDelayInputModel.py --help # for more information * vehicle_status/speed is the measured vehicle speed [km/h] ## Description -* Paramter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input +* Parameter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input * Arguments explaining: * CUTOFF_TIME: Cutoff time[sec]. Rosbag file often was start recording before autoware was run. Time before autoware was run should be cut off. This script will only consider data from t=cutoff_time to the end of the bag file (default is 1.0) * CUTOFF_FREQ: Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1) diff --git a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py index 50b4099ee98d6..f9f88d3d90abd 100644 --- a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py +++ b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py @@ -155,7 +155,7 @@ def getFittingParam(cmd_data, act_data, args, speed_type=False): 'vehicle_cmd/ctrl_cmd/linear_velocity', 'vehicle_status/speed'] pd_data = [None] * len(topics) parser = argparse.ArgumentParser( - description='Paramter fitting for Input Delay Model' + description='Parameter fitting for Input Delay Model' ' (First Order System with Dead Time) with rosbag file input') parser.add_argument( '--bag_file', diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index 998a894f150c3..c74449d78a884 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -409,7 +409,7 @@ void Simulator::publishPoseTwist( const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist) { rclcpp::Time current_time = get_clock()->now(); - // simulatied pose + // simulated pose geometry_msgs::msg::PoseStamped ps; ps.header.frame_id = map_frame_id_; ps.header.stamp = current_time; @@ -441,7 +441,7 @@ void Simulator::publishTF(const geometry_msgs::msg::Pose & pose) double Simulator::getPosZFromTrajectory(const double x, const double y) { - // calculae cloest point on trajectory + // calculate closest point on trajectory /* write me... */ From b222d75ce946e090a872f195a2c7c149d99ca460 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Fri, 26 Mar 2021 09:47:00 +0900 Subject: [PATCH 16/52] add use_sim-time option (#454) --- .../launch/simple_planning_simulator.launch.xml | 2 ++ 1 file changed, 2 insertions(+) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml index 6fea6471b3aaa..550a701e309ab 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml @@ -13,6 +13,7 @@ + @@ -36,6 +37,7 @@ + From 4ef120c7e331128bc82a8620c39b1572ca73df18 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 30 Mar 2021 20:16:53 +0900 Subject: [PATCH 17/52] Format launch files (#1219) Signed-off-by: Kenji Miyake --- .../launch/simple_planning_simulator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml index 550a701e309ab..0254f410b2a63 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml @@ -36,7 +36,7 @@ - + From 0ccd6f13ff1262f877b324800c5d72e5ecf36da2 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Mon, 5 Apr 2021 11:05:54 +0900 Subject: [PATCH 18/52] Fix rolling build errors (#1225) * Add missing include files Signed-off-by: Kenji Miyake * Replace rclcpp::Duration Signed-off-by: Kenji Miyake * Use reference for exceptions Signed-off-by: Kenji Miyake * Use from_seconds Signed-off-by: Kenji Miyake --- .../simple_planning_simulator/simple_planning_simulator_core.hpp | 1 + 1 file changed, 1 insertion(+) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index ba07748f24245..5aa3ce2d25e36 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -35,6 +35,7 @@ #include "geometry_msgs/msg/twist_stamped.hpp" #include "tf2/LinearMath/Quaternion.h" #include "tf2/utils.h" +#include "tf2_ros/buffer.h" #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" From 9c2f1761b76494515592f2f716322518faf63f55 Mon Sep 17 00:00:00 2001 From: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com> Date: Mon, 5 Apr 2021 19:34:05 +0900 Subject: [PATCH 19/52] Sync public repo (#1228) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * [simple_planning_simulator] add readme (#424) * add readme of simple_planning_simulator Signed-off-by: Takamasa Horibe * Update simulator/simple_planning_simulator/README.md * set transit_margin_time to intersect. planner (#460) * Fix pose2twist (#462) Signed-off-by: Takagi, Isamu * Ros2 vehicle info param server (#447) * add vehicle_info_param_server * update vehicle info * apply format * fix bug * skip unnecessary search * delete vehicle param file * fix bug * Ros2 fix topic name part2 (#425) * Fix topic name of traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_visualization Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix topic name of traffic_light_map_based_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_recognition Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_classifier Signed-off-by: Takagi, Isamu * Fix lint traffic_light_ssd_fine_detector Signed-off-by: Takagi, Isamu * Fix issues in hdd_reader (#466) * Fix some issues detected by Coverity Scan and Clang-Tidy * Update launch command * Add more `close(new_sock)` * Simplify the definitions of struct * fix: re-construct laneletMapLayer for reindex RTree (#463) * Rviz overlay render fix (#461) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * back to RTD as superclass Signed-off-by: Adam Dabrowski * Rviz overlay render in update (#465) * Moved painiting in SteeringAngle plugin to update() Signed-off-by: Adam Dabrowski * super class now back to MFD Signed-off-by: Adam Dabrowski * uncrustified Signed-off-by: Adam Dabrowski * acquire data in mutex Signed-off-by: Adam Dabrowski * removed unnecessary includes and some dead code Signed-off-by: Adam Dabrowski * Adepted remaining vehicle plugin classes to render-in-update concept. Returned to MFD superclass Signed-off-by: Adam Dabrowski * restored RTD superclass Signed-off-by: Adam Dabrowski Co-authored-by: Takamasa Horibe Co-authored-by: tkimura4 Co-authored-by: Takagi, Isamu <43976882+isamu-takagi@users.noreply.github.com> Co-authored-by: Kazuki Miyahara Co-authored-by: Makoto Tokunaga Co-authored-by: Adam Dąbrowski --- simulator/simple_planning_simulator/README.md | 62 +++++++++++++++++++ .../simple_planning_simulator.launch.py | 6 -- .../simple_planning_simulator.launch.xml | 2 - 3 files changed, 62 insertions(+), 8 deletions(-) create mode 100644 simulator/simple_planning_simulator/README.md diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md new file mode 100644 index 0000000000000..699f8ea78cfde --- /dev/null +++ b/simulator/simple_planning_simulator/README.md @@ -0,0 +1,62 @@ +# Simple Planning Simulator +This node simulates the vehicle motion for a vehicle command in 2D using a simple vehicle model. + +## Interface + +**input** + - /control/vehicle_cmd [`autoware_vehicle_msgs/VehicleCommand`] : target command to drive a vehicle. + - /initialpose [`geometry_msgs/PoseWithCovarianceStamped`] : for initial pose + - /initialtwist [`geometry_msgs/TwistStamped`] : for initial velocity + - /planning/scenario_planning/trajectory [`autoware_planning_msgs/Trajectory`]: for z position + - /vehicle/engage : if true, the vehicle starts to move. if false, stops. + +**output** + - /tf [`tf2_msgs/TFMessage`] : simulated vehicle pose (base_link) + - /vehicle/status/control_mode [`autoware_vehicle_msgs/ControlMode`] : current control mode (Auto/Manual). + - /vehicle/status/shift [`autoware_vehicle_msgs/ShiftStamped`] : current shift (Fwd/Rev) + - /vehicle/status/steering [`autoware_vehicle_msgs/Steering`] : Simulated steering angle + - /vehicle/status/turn_signal [`autoware_vehicle_msgs/TurnSignal`] : current turn signal (just published with NONE status.) + - /vehicle/status/twist [`geometry_msgs/TwistStamped`] : simulated velocity + + + +## Parameter Description + +### Common Parameters + +|Name|Type|Description|Default value| +|:---|:---|:---|:---| +|add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results.| true| +|pos_noise_stddev | double | Standard deviation for position noise | 0.01| +|rpy_noise_stddev | double | Standard deviation for Euler angle noise| 0.0001| +|vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0| +|angvel_noise_stddev | double | Standard deviation for angular velocity noise| 0.0| +|steer_noise_stddev | double | Standard deviation for steering angle noise| 0.0001| +|initial_engage_state | double | If false, the engage command is needed to move the vehicle.| true| + + +### Vehicle Model Parameters + +**vehicle_model_type options** + - `IDEAL_STEER`: uses velocity command. The steering and velocity varies ideally as commanded. The pose is calculated by a bicycle kinematics model. + - `DELAY_STEER` : uses velocity command. The steering and velocity varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. + - `DELAY_STEER_ACC` : uses acceleration command. The steering and acceleration varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. + + + +|Name|Type|Description|IDEAL STEER|DELAY STEER|DELAY STEER　ACC|Default value| unit | +|:---|:---|:---|:---|:---|:---|:---|:---| +|vel_time_delay | double | dead time for the velocity input | x | o | x | 0.25| [s] | +|acc_time_delay | double | dead time for the acceleration input | x | x | o | 0.1 | [s] | +|steer_time_delay | double | dead time for the steering input | x | o | o | 0.24| [s] | +|vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | o | x | 0.61| [s] | +|acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | o | 0.1 | [s] | +|steer_time_constant | double | time constant of the 1st-order steering dynamics | x | o | o | 0.27| [s] | +|vel_lim | double | limit of velocity | x | o | o | 50.0| [m/s] | +|accel_rate | double | limit of acceleration | x | o | o | 7.0 | [m/ss] | +|steer_lim | double | limit of steering angle | x | o | o | 1.0 | [rad] | +|steer_rate_lim | double | limit of steering angle change rate | x | o | o | 5.0 | [rad/s] | +|deadzone_delta_steer | double | dead zone for the steering dynamics | x | o | o | 0.0 | [rad] | + + +*Note*: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a *delay* model. The definition of the *time constant* is the time it takes for the step response to rise up to 63% of its final value. The *deadtime* is a delay in the response to a control input. diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index a7593f206cada..6af0799ef1973 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -45,10 +45,6 @@ def generate_launch_description(): default_value=simple_planning_simulator_param_file, description='Path to config file for simple_planning_simulator' ) - vehicle_param_file_param = DeclareLaunchArgument( - 'vehicle_param_file', - description='Path to config file for vehicle param' - ) simple_planning_simulator = Node( package='simple_planning_simulator', @@ -58,7 +54,6 @@ def generate_launch_description(): output='screen', parameters=[ LaunchConfiguration('simple_planning_simulator_param_file'), - LaunchConfiguration('vehicle_param_file'), ], remappings=[ ('base_trajectory', '/planning/scenario_planning/trajectory'), @@ -76,6 +71,5 @@ def generate_launch_description(): return LaunchDescription([ simple_planning_simulator_param, - vehicle_param_file_param, simple_planning_simulator ]) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml index 0254f410b2a63..3b613dce64eb2 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml @@ -9,7 +9,6 @@ - @@ -26,7 +25,6 @@ - From 2e3c97df04612cdc4f5e76f750c017b4d7eafd6b Mon Sep 17 00:00:00 2001 From: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com> Date: Mon, 26 Apr 2021 15:29:13 +0900 Subject: [PATCH 20/52] Remove use_sim_time for set_parameter (#1260) Signed-off-by: wep21 --- .../launch/simple_planning_simulator.launch.xml | 2 -- 1 file changed, 2 deletions(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml index 3b613dce64eb2..b2d4faa09fa06 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml @@ -12,7 +12,6 @@ - @@ -35,7 +34,6 @@ - From 0289c8fd77e1945df42d04710a34e9f532e957eb Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 11 May 2021 23:28:56 +0900 Subject: [PATCH 21/52] Refactor vehicle info util (#1305) * Update license Signed-off-by: Kenji Miyake * Refactor vehicle_info_util Signed-off-by: Kenji Miyake * Rename and split files Signed-off-by: Kenji Miyake * Fix interfaces Signed-off-by: Kenji Miyake * Fix bug and add error handling Signed-off-by: Kenji Miyake * Add "// namespace" Signed-off-by: Kenji Miyake * Add missing include Signed-off-by: Kenji Miyake --- .../simple_planning_simulator_core.hpp | 2 +- .../src/simple_planning_simulator_core.cpp | 3 ++- 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 5aa3ce2d25e36..ebdfbaee1d9ae 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -52,7 +52,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" -#include "vehicle_info_util/vehicle_info.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" class Simulator : public rclcpp::Node { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index c74449d78a884..6aeb2fd27dd6e 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -21,7 +21,8 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & : Node(node_name, options) { /* simple_planning_simulator parameters */ - wheelbase_ = vehicle_info_util::VehicleInfo::create(*this).wheel_base_m_; + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + wheelbase_ = vehicle_info.wheel_base_m; loop_rate_ = declare_parameter("loop_rate", 30.0); sim_steering_gear_ratio_ = declare_parameter("sim_steering_gear_ratio", 15.0); simulation_frame_id_ = declare_parameter("simulation_frame_id", "base_link"); From c5f49787fb1743af2c2d79d041da185c74fba407 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 25 May 2021 10:35:01 +0900 Subject: [PATCH 22/52] Fix lint errors (#1378) * Fix lint errors Signed-off-by: Kenji Miyake * Fix variable names Signed-off-by: Kenji Miyake --- .../simple_planning_simulator.launch.py | 7 +-- .../scripts/fitParamDelayInputModel.py | 57 ++++++++++--------- 2 files changed, 34 insertions(+), 30 deletions(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 6af0799ef1973..d8a8627b331d2 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -12,16 +12,15 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os +from pathlib import Path + from launch import LaunchContext from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration from launch_ros.actions import Node from launch_ros.substitutions import FindPackage -from pathlib import Path - -import os - context = LaunchContext() diff --git a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py index f9f88d3d90abd..b701425722a30 100644 --- a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py +++ b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py @@ -1,5 +1,4 @@ #!/usr/bin/env python -# -*- coding: utf-8 -*- # Copyright 2020 Tier IV, Inc. # @@ -15,12 +14,18 @@ # See the License for the specific language governing permissions and # limitations under the License. -import numpy as np import argparse +from os import getcwd +from os.path import basename +from os.path import dirname +from os.path import exists +from os.path import join +from os.path import splitext import subprocess import sys -from os import getcwd -from os.path import dirname, basename, splitext, join, exists + +import numpy as np + try: import pandas as pd except ImportError: @@ -68,8 +73,8 @@ def getActValue(df, speed_type): else: val = np.array(list(df['field'])) # Calc differential - dval = (val[2:] - val[:-2]) / (tm[2:] - tm[:-2]) - return tm[1:-1], val[1:-1], dval + d_val = (val[2:] - val[:-2]) / (tm[2:] - tm[:-2]) + return tm[1:-1], val[1:-1], d_val def getCmdValueWithDelay(df, delay): @@ -80,25 +85,25 @@ def getCmdValueWithDelay(df, delay): def getLinearInterpolate(_tm, _val, _index, ti): tmp_t = _tm[_index] - tmp_nextt = _tm[_index + 1] + tmp_next_t = _tm[_index + 1] tmp_val = _val[_index] - tmp_nextval = _val[_index + 1] - val_i = tmp_val + (tmp_nextval - tmp_val) / \ - (tmp_nextt - tmp_t) * (ti - tmp_t) + tmp_next_val = _val[_index + 1] + val_i = tmp_val + (tmp_next_val - tmp_val) / \ + (tmp_next_t - tmp_t) * (ti - tmp_t) return val_i def getFittingTimeConstantParam(cmd_data, act_data, delay, args, speed_type=False): tm_cmd, cmd_delay = getCmdValueWithDelay(cmd_data, delay) - tm_act, act, dact = getActValue(act_data, speed_type) + tm_act, act, d_act = getActValue(act_data, speed_type) _t_min = max(tm_cmd[0], tm_act[0]) _t_max = min(tm_cmd[-1], tm_act[-1]) tm_cmd = tm_cmd - _t_min tm_act = tm_act - _t_min MAX_CNT = int((_t_max - _t_min - args.cutoff_time) / FREQ_SAMPLE) - dact_samp = [None] * MAX_CNT - diff_actcmd_samp = [None] * MAX_CNT + d_act_sample = [None] * MAX_CNT + diff_act_cmd_sample = [None] * MAX_CNT ind_cmd = 0 ind_act = 0 for ind in range(MAX_CNT): @@ -109,20 +114,20 @@ def getFittingTimeConstantParam(cmd_data, act_data, while (tm_act[ind_act + 1] < ti): ind_act += 1 act_i = getLinearInterpolate(tm_act, act, ind_act, ti) - dact_i = getLinearInterpolate(tm_act, dact, ind_act, ti) - dact_samp[ind] = dact_i - diff_actcmd_samp[ind] = act_i - cmd_delay_i - dact_samp = np.array(dact_samp) - diff_actcmd_samp = np.array(diff_actcmd_samp) + d_act_i = getLinearInterpolate(tm_act, d_act, ind_act, ti) + d_act_sample[ind] = d_act_i + diff_act_cmd_sample[ind] = act_i - cmd_delay_i + d_act_sample = np.array(d_act_sample) + diff_act_cmd_sample = np.array(diff_act_cmd_sample) if args.cutoff_freq > 0: - dact_samp = lowpass_filter(dact_samp, cutoff_freq=args.cutoff_freq) - diff_actcmd_samp = lowpass_filter( - diff_actcmd_samp, cutoff_freq=args.cutoff_freq) - dact_samp = dact_samp.reshape(1, -1) - diff_actcmd_samp = diff_actcmd_samp.reshape(1, -1) - tau = -np.dot(diff_actcmd_samp, np.linalg.pinv(dact_samp))[0, 0] - error = np.linalg.norm(diff_actcmd_samp + tau * - dact_samp) / dact_samp.shape[1] + d_act_sample = lowpass_filter(d_act_sample, cutoff_freq=args.cutoff_freq) + diff_act_cmd_sample = lowpass_filter( + diff_act_cmd_sample, cutoff_freq=args.cutoff_freq) + d_act_sample = d_act_sample.reshape(1, -1) + diff_act_cmd_sample = diff_act_cmd_sample.reshape(1, -1) + tau = -np.dot(diff_act_cmd_sample, np.linalg.pinv(d_act_sample))[0, 0] + error = np.linalg.norm(diff_act_cmd_sample + tau * + d_act_sample) / d_act_sample.shape[1] return tau, error From 52b35cf2ae3cf9c236d558851ac579f4ee61a3b1 Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Mon, 19 Jul 2021 21:26:20 +0900 Subject: [PATCH 23/52] Add pre-commit (#1560) * add pre-commit * add pre-commit-config * add additional settings for private repository * use default pre-commit-config * update pre-commit setting * Ignore whitespace for line breaks in markdown * Update .github/workflows/pre-commit.yml Co-authored-by: Kazuki Miyahara * exclude svg * remove pretty-format-json * add double-quote-string-fixer * consider COLCON_IGNORE file when seaching modified package * format file * pre-commit fixes * Update pre-commit.yml * Update .pre-commit-config.yaml Co-authored-by: Kazuki Miyahara Co-authored-by: pre-commit Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> --- .../scripts/README_fitParamDelayInputModel.md | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md index 417747a523d7d..3c0c63b8e6fb7 100644 --- a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md +++ b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md @@ -1,7 +1,7 @@ # fitParamDelayInputModel.py scripts ## How to use ``` -python fitParamDelayInputModel.py --bag_file [bagfile] (--cutoff_time [cutoff_time] --cutoff_freq [cutoff_freq] --min_delay [min_delay] --max_delay [max_delay] --delay_incr [delay_incr]) +python fitParamDelayInputModel.py --bag_file [bagfile] (--cutoff_time [cutoff_time] --cutoff_freq [cutoff_freq] --min_delay [min_delay] --max_delay [max_delay] --delay_incr [delay_incr]) # in round brakets is optional arguments python fitParamDelayInputModel.py --help # for more information ``` @@ -18,7 +18,7 @@ python fitParamDelayInputModel.py --help # for more information * vehicle_status/angle is the measured steering angle [rad] * vehicle_status/speed is the measured vehicle speed [km/h] -## Description +## Description * Parameter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input * Arguments explaining: * CUTOFF_TIME: Cutoff time[sec]. Rosbag file often was start recording before autoware was run. Time before autoware was run should be cut off. This script will only consider data from t=cutoff_time to the end of the bag file (default is 1.0) From 04ccb957e1208fa9d1a12c9a533058dffd04b4e4 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 20 Jul 2021 18:01:14 +0900 Subject: [PATCH 24/52] Add markdownlint and prettier (#1661) * Add markdownlint and prettier Signed-off-by: Kenji Miyake * Ignore .param.yaml Signed-off-by: Kenji Miyake * Apply format Signed-off-by: Kenji Miyake --- simulator/simple_planning_simulator/README.md | 90 +++++++++---------- .../scripts/README_fitParamDelayInputModel.md | 39 ++++---- 2 files changed, 66 insertions(+), 63 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 699f8ea78cfde..3236a78a435e2 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -1,62 +1,60 @@ # Simple Planning Simulator + This node simulates the vehicle motion for a vehicle command in 2D using a simple vehicle model. ## Interface -**input** - - /control/vehicle_cmd [`autoware_vehicle_msgs/VehicleCommand`] : target command to drive a vehicle. - - /initialpose [`geometry_msgs/PoseWithCovarianceStamped`] : for initial pose - - /initialtwist [`geometry_msgs/TwistStamped`] : for initial velocity - - /planning/scenario_planning/trajectory [`autoware_planning_msgs/Trajectory`]: for z position - - /vehicle/engage : if true, the vehicle starts to move. if false, stops. +### input -**output** - - /tf [`tf2_msgs/TFMessage`] : simulated vehicle pose (base_link) - - /vehicle/status/control_mode [`autoware_vehicle_msgs/ControlMode`] : current control mode (Auto/Manual). - - /vehicle/status/shift [`autoware_vehicle_msgs/ShiftStamped`] : current shift (Fwd/Rev) - - /vehicle/status/steering [`autoware_vehicle_msgs/Steering`] : Simulated steering angle - - /vehicle/status/turn_signal [`autoware_vehicle_msgs/TurnSignal`] : current turn signal (just published with NONE status.) - - /vehicle/status/twist [`geometry_msgs/TwistStamped`] : simulated velocity +- /control/vehicle_cmd [`autoware_vehicle_msgs/VehicleCommand`] : target command to drive a vehicle. +- /initialpose [`geometry_msgs/PoseWithCovarianceStamped`] : for initial pose +- /initialtwist [`geometry_msgs/TwistStamped`] : for initial velocity +- /planning/scenario_planning/trajectory [`autoware_planning_msgs/Trajectory`]: for z position +- /vehicle/engage : if true, the vehicle starts to move. if false, stops. +### output +- /tf [`tf2_msgs/TFMessage`] : simulated vehicle pose (base_link) +- /vehicle/status/control_mode [`autoware_vehicle_msgs/ControlMode`] : current control mode (Auto/Manual). +- /vehicle/status/shift [`autoware_vehicle_msgs/ShiftStamped`] : current shift (Fwd/Rev) +- /vehicle/status/steering [`autoware_vehicle_msgs/Steering`] : Simulated steering angle +- /vehicle/status/turn_signal [`autoware_vehicle_msgs/TurnSignal`] : current turn signal (just published with NONE status.) +- /vehicle/status/twist [`geometry_msgs/TwistStamped`] : simulated velocity ## Parameter Description ### Common Parameters -|Name|Type|Description|Default value| -|:---|:---|:---|:---| -|add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results.| true| -|pos_noise_stddev | double | Standard deviation for position noise | 0.01| -|rpy_noise_stddev | double | Standard deviation for Euler angle noise| 0.0001| -|vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0| -|angvel_noise_stddev | double | Standard deviation for angular velocity noise| 0.0| -|steer_noise_stddev | double | Standard deviation for steering angle noise| 0.0001| -|initial_engage_state | double | If false, the engage command is needed to move the vehicle.| true| - +| Name | Type | Description | Default value | +| :-------------------- | :----- | :------------------------------------------------------------- | :------------ | +| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true | +| pos_noise_stddev | double | Standard deviation for position noise | 0.01 | +| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 | +| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 | +| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 | +| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 | +| initial_engage_state | double | If false, the engage command is needed to move the vehicle. | true | ### Vehicle Model Parameters -**vehicle_model_type options** - - `IDEAL_STEER`: uses velocity command. The steering and velocity varies ideally as commanded. The pose is calculated by a bicycle kinematics model. - - `DELAY_STEER` : uses velocity command. The steering and velocity varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. - - `DELAY_STEER_ACC` : uses acceleration command. The steering and acceleration varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. - - - -|Name|Type|Description|IDEAL STEER|DELAY STEER|DELAY STEER　ACC|Default value| unit | -|:---|:---|:---|:---|:---|:---|:---|:---| -|vel_time_delay | double | dead time for the velocity input | x | o | x | 0.25| [s] | -|acc_time_delay | double | dead time for the acceleration input | x | x | o | 0.1 | [s] | -|steer_time_delay | double | dead time for the steering input | x | o | o | 0.24| [s] | -|vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | o | x | 0.61| [s] | -|acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | o | 0.1 | [s] | -|steer_time_constant | double | time constant of the 1st-order steering dynamics | x | o | o | 0.27| [s] | -|vel_lim | double | limit of velocity | x | o | o | 50.0| [m/s] | -|accel_rate | double | limit of acceleration | x | o | o | 7.0 | [m/ss] | -|steer_lim | double | limit of steering angle | x | o | o | 1.0 | [rad] | -|steer_rate_lim | double | limit of steering angle change rate | x | o | o | 5.0 | [rad/s] | -|deadzone_delta_steer | double | dead zone for the steering dynamics | x | o | o | 0.0 | [rad] | - - -*Note*: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a *delay* model. The definition of the *time constant* is the time it takes for the step response to rise up to 63% of its final value. The *deadtime* is a delay in the response to a control input. +#### vehicle_model_type options + +- `IDEAL_STEER`: uses velocity command. The steering and velocity varies ideally as commanded. The pose is calculated by a bicycle kinematics model. +- `DELAY_STEER` : uses velocity command. The steering and velocity varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. +- `DELAY_STEER_ACC` : uses acceleration command. The steering and acceleration varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. + +| Name | Type | Description | IDEAL STEER | DELAY STEER | DELAY STEER 　 ACC | Default value | unit | +| :------------------- | :----- | :--------------------------------------------------- | :---------- | :---------- | :----------------- | :------------ | :------ | +| vel_time_delay | double | dead time for the velocity input | x | o | x | 0.25 | [s] | +| acc_time_delay | double | dead time for the acceleration input | x | x | o | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | o | o | 0.24 | [s] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | o | x | 0.61 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | o | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | o | o | 0.27 | [s] | +| vel_lim | double | limit of velocity | x | o | o | 50.0 | [m/s] | +| accel_rate | double | limit of acceleration | x | o | o | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | o | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | o | o | 5.0 | [rad/s] | +| deadzone_delta_steer | double | dead zone for the steering dynamics | x | o | o | 0.0 | [rad] | + +_Note_: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a _delay_ model. The definition of the _time constant_ is the time it takes for the step response to rise up to 63% of its final value. The _deadtime_ is a delay in the response to a control input. diff --git a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md index 3c0c63b8e6fb7..480894817766c 100644 --- a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md +++ b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md @@ -1,28 +1,33 @@ # fitParamDelayInputModel.py scripts + ## How to use -``` + +```sh python fitParamDelayInputModel.py --bag_file [bagfile] (--cutoff_time [cutoff_time] --cutoff_freq [cutoff_freq] --min_delay [min_delay] --max_delay [max_delay] --delay_incr [delay_incr]) # in round brakets is optional arguments python fitParamDelayInputModel.py --help # for more information ``` -## Requirements python packages: -* numpy -* pandas +## Requirements python packages + +- numpy +- pandas ## Required topics -* autoware_msgs::VehicleCmd /vehicle_cmd: assuming - * vehicle_cmd/ctrl_cmd/steering_angle is the steering angle input [rad] - * vehicle_cmd/ctrl_cmd/linear_velocity is the vehicle velocity input [m/s] -* autoware_msgs::VehicleStatus /vehicle_status : assuming - * vehicle_status/angle is the measured steering angle [rad] - * vehicle_status/speed is the measured vehicle speed [km/h] + +- autoware_msgs::VehicleCmd /vehicle_cmd: assuming + - vehicle_cmd/ctrl_cmd/steering_angle is the steering angle input [rad] + - vehicle_cmd/ctrl_cmd/linear_velocity is the vehicle velocity input [m/s] +- autoware_msgs::VehicleStatus /vehicle_status : assuming + - vehicle_status/angle is the measured steering angle [rad] + - vehicle_status/speed is the measured vehicle speed [km/h] ## Description -* Parameter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input -* Arguments explaining: - * CUTOFF_TIME: Cutoff time[sec]. Rosbag file often was start recording before autoware was run. Time before autoware was run should be cut off. This script will only consider data from t=cutoff_time to the end of the bag file (default is 1.0) - * CUTOFF_FREQ: Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1) - * MIN_DELAY: Min value for searching delay loop (default is 0.1) - * MAX_DELAY: Max value for searching delay loop (default is 1.0) - * DELAY_INCR: Step value for searching delay loop (default is 0.01) + +- Parameter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input +- Arguments explaining: + - CUTOFF_TIME: Cutoff time[sec]. Rosbag file often was start recording before autoware was run. Time before autoware was run should be cut off. This script will only consider data from t=cutoff_time to the end of the bag file (default is 1.0) + - CUTOFF_FREQ: Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1) + - MIN_DELAY: Min value for searching delay loop (default is 0.1) + - MAX_DELAY: Max value for searching delay loop (default is 1.0) + - DELAY_INCR: Step value for searching delay loop (default is 0.01) From 5b7f460b4944c1e1149d8464d32f1a5bd2c3a924 Mon Sep 17 00:00:00 2001 From: Kosuke Murakami Date: Fri, 30 Jul 2021 13:25:19 +0900 Subject: [PATCH 25/52] add cov pub in psim (#1732) --- .../simple_planning_simulator.param.yaml | 2 + .../simple_planning_simulator_core.hpp | 22 +++++- .../simple_planning_simulator.launch.xml | 1 + .../simple_planning_simulator/package.xml | 1 + .../src/simple_planning_simulator_core.cpp | 76 ++++++++++++++++--- 5 files changed, 87 insertions(+), 15 deletions(-) diff --git a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml index 0070937471bf3..3f5568e35c2b7 100644 --- a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml +++ b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml @@ -19,6 +19,8 @@ simulation: initial_engage_state: true pos_noise_stddev: 0.01 rpy_noise_stddev: 0.0001 + x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate + y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate sim_steering_gear_ratio: 15.0 steer_lim: 1.0 steer_noise_stddev: 0.0001 diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index ebdfbaee1d9ae..2450f151360c4 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -25,6 +25,7 @@ #include #include #include +#include #include "rclcpp/rclcpp.hpp" @@ -78,6 +79,7 @@ class Simulator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_turn_signal_; rclcpp::Publisher::SharedPtr pub_shift_; rclcpp::Publisher::SharedPtr pub_control_mode_; + rclcpp::Publisher::SharedPtr pub_cov_; rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; //!< @brief topic subscriber for vehicle_cmd @@ -93,6 +95,10 @@ class Simulator : public rclcpp::Node sub_engage_; //!< @brief topic subscriber for engage topic rclcpp::TimerBase::SharedPtr timer_simulation_; //!< @brief timer for simulation + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult onParameter( + const std::vector & parameters); + /* tf */ std::shared_ptr tf_broadcaster_; std::shared_ptr tf_listener_; @@ -124,6 +130,8 @@ class Simulator : public rclcpp::Node double loop_rate_; //!< @brief frequency to calculate vehicle model & publish result double wheelbase_; //!< @brief wheelbase length to convert angular-velocity & steering double sim_steering_gear_ratio_; //!< @brief for steering wheel angle calculation + double x_stddev_; //!< @brief x standard deviation for dummy covariance in map coordinate + double y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate /* flags */ bool is_initialized_ = false; //!< @brief flag to check the initial position is set @@ -239,12 +247,18 @@ class Simulator : public rclcpp::Node const geometry_msgs::msg::Twist & twist); /** - * @brief publish pose and twist - * @param [in] pose pose to be published + * @brief publish pose_with_covariance + * @param [in] cov pose with covariance to be published + */ + void publishPoseWithCov( + const geometry_msgs::msg::PoseWithCovariance & cov); + + /** + * @brief publish twist * @param [in] twist twist to be published */ - void publishPoseTwist( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist); + void publishTwist( + const geometry_msgs::msg::Twist & twist); /** * @brief publish tf diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml index b2d4faa09fa06..19e972d44b933 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml @@ -17,6 +17,7 @@ + diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 5d6e1ece53670..15a0614867fb4 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -18,6 +18,7 @@ autoware_debug_msgs autoware_planning_msgs autoware_control_msgs + autoware_utils autoware_vehicle_msgs vehicle_info_util diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index 6aeb2fd27dd6e..31130a2acaf13 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -15,11 +15,17 @@ #include #include #include +#include + #include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include "autoware_utils/ros/update_param.hpp" + Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { + using std::placeholders::_1; + /* simple_planning_simulator parameters */ const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); wheelbase_ = vehicle_info.wheel_base_m; @@ -46,6 +52,8 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & "/vehicle/status/turn_signal", rclcpp::QoS{1}); pub_shift_ = create_publisher( "/vehicle/status/shift", rclcpp::QoS{1}); + pub_cov_ = create_publisher( + "output/cov", rclcpp::QoS{1}); sub_vehicle_cmd_ = create_subscription( "input/vehicle_cmd", rclcpp::QoS{1}, @@ -66,6 +74,7 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & sub_initialpose_ = create_subscription( "input/initial_pose", rclcpp::QoS{1}, std::bind(&Simulator::callbackInitialPoseWithCov, this, std::placeholders::_1)); + if (use_trajectory_for_z_position_source_) { sub_trajectory_ = create_subscription( "base_trajectory", rclcpp::QoS{1}, @@ -73,6 +82,13 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & } const double dt = 1.0 / loop_rate_; + /* set param callback */ + set_param_res_ = + this->add_on_set_parameters_callback( + std::bind( + &Simulator::onParameter, this, + _1)); + /* Timer */ { auto timer_callback = std::bind(&Simulator::timerCallbackSimulation, this); @@ -147,6 +163,9 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & rpy_norm_dist_ptr_ = std::make_shared>(0.0, rpy_noise_stddev); angvel_norm_dist_ptr_ = std::make_shared>(0.0, angvel_noise_stddev); steer_norm_dist_ptr_ = std::make_shared>(0.0, steer_noise_stddev); + + x_stddev_ = declare_parameter("x_stddev", 0.0001); + y_stddev_ = declare_parameter("y_stddev", 0.0001); } current_pose_.orientation.w = 1.0; @@ -154,6 +173,24 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & closest_pos_z_ = 0.0; } +rcl_interfaces::msg::SetParametersResult Simulator::onParameter( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + try { + autoware_utils::updateParam(parameters, "x_stddev", x_stddev_); + autoware_utils::updateParam(parameters, "y_stddev", y_stddev_); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + + return result; +} + void Simulator::callbackTrajectory( const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) { @@ -243,6 +280,12 @@ void Simulator::timerCallbackSimulation() current_twist_.linear.x = vehicle_model_ptr_->getVx(); current_twist_.angular.z = vehicle_model_ptr_->getWz(); + /* make current vehicle pose with covariance */ + geometry_msgs::msg::PoseWithCovariance cov; + cov.pose = current_pose_; + cov.covariance[0 * 6 + 0] = x_stddev_; + cov.covariance[1 * 6 + 1] = y_stddev_; + if (simulator_engage_ && add_measurement_noise_) { current_pose_.position.x += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); current_pose_.position.y += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); @@ -260,8 +303,9 @@ void Simulator::timerCallbackSimulation() current_pose_.orientation = getQuaternionFromRPY(roll, pitch, yaw); - /* publish pose & twist */ - publishPoseTwist(current_pose_, current_twist_); + /* publish twist & covariance & tf */ + publishTwist(current_twist_); + publishPoseWithCov(cov); publishTF(current_pose_); /* publish steering */ @@ -406,17 +450,10 @@ void Simulator::getTransformFromTF( } } -void Simulator::publishPoseTwist( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist) +void Simulator::publishTwist( + const geometry_msgs::msg::Twist & twist) { rclcpp::Time current_time = get_clock()->now(); - // simulated pose - geometry_msgs::msg::PoseStamped ps; - ps.header.frame_id = map_frame_id_; - ps.header.stamp = current_time; - ps.pose = pose; - pub_pose_->publish(ps); - geometry_msgs::msg::TwistStamped ts; ts.header.frame_id = simulation_frame_id_; ts.header.stamp = current_time; @@ -424,6 +461,23 @@ void Simulator::publishPoseTwist( pub_twist_->publish(ts); } +void Simulator::publishPoseWithCov( + const geometry_msgs::msg::PoseWithCovariance & cov) +{ + rclcpp::Time current_time = get_clock()->now(); + geometry_msgs::msg::PoseWithCovarianceStamped cs; + cs.header.frame_id = map_frame_id_; + cs.header.stamp = current_time; + cs.pose = cov; + pub_cov_->publish(cs); + + // TODO(kosuke_murakami): remove publishing pose when the v2 scenario sim migration is complete + geometry_msgs::msg::PoseStamped ps; + ps.header = cs.header; + ps.pose = cov.pose; + pub_pose_->publish(ps); +} + void Simulator::publishTF(const geometry_msgs::msg::Pose & pose) { rclcpp::Time current_time = get_clock()->now(); From f74b21287f2a266956b440b716c30a95577c3b26 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Sun, 15 Aug 2021 07:28:48 +0900 Subject: [PATCH 26/52] Fix -Wunused-parameter (#1836) * Fix -Wunused-parameter Signed-off-by: Kenji Miyake * Fix mistake Signed-off-by: Kenji Miyake * fix spell * Fix lint issues Signed-off-by: Kenji Miyake * Ignore flake8 warnings Signed-off-by: Kenji Miyake Co-authored-by: Hiroki OTA --- simulator/simple_planning_simulator/CMakeLists.txt | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 769c9f96fb8d7..4bd10674eb137 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -6,8 +6,7 @@ if(NOT CMAKE_CXX_STANDARD) set(CMAKE_CXX_STANDARD 14) endif() if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - # add_compile_options(-Wall -Wextra -Wpedantic) - add_compile_options(-Wno-unused-parameter) + add_compile_options(-Wall -Wextra -Wpedantic) endif() ### Dependencies From c506ed7326a259a9e84138047e8ef27f61bcdcc2 Mon Sep 17 00:00:00 2001 From: Hiroki OTA Date: Wed, 25 Aug 2021 11:02:32 +0900 Subject: [PATCH 27/52] fix some typos (#1941) * fix some typos * fix typo * Fix typo Signed-off-by: Kenji Miyake Co-authored-by: Kenji Miyake --- .../scripts/README_fitParamDelayInputModel.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md index 480894817766c..fb71c858a2f04 100644 --- a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md +++ b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md @@ -4,7 +4,7 @@ ```sh python fitParamDelayInputModel.py --bag_file [bagfile] (--cutoff_time [cutoff_time] --cutoff_freq [cutoff_freq] --min_delay [min_delay] --max_delay [max_delay] --delay_incr [delay_incr]) -# in round brakets is optional arguments +# in round brackets is optional arguments python fitParamDelayInputModel.py --help # for more information ``` From 4f99ca08dbd6bebd9a689699215f4dab719f013a Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Tue, 31 Aug 2021 17:01:55 +0900 Subject: [PATCH 28/52] Add autoware api (#1979) --- .../simple_planning_simulator_core.hpp | 16 ++++++++++++ .../simple_planning_simulator/package.xml | 2 ++ .../src/simple_planning_simulator_core.cpp | 25 ++++++++++++++++++- 3 files changed, 42 insertions(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 2450f151360c4..fbb7d3f4555a4 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -40,7 +40,9 @@ #include "tf2_ros/transform_broadcaster.h" #include "tf2_ros/transform_listener.h" +#include "autoware_api_utils/autoware_api_utils.hpp" #include "autoware_debug_msgs/msg/float32_stamped.hpp" +#include "autoware_external_api_msgs/srv/initialize_pose.hpp" #include "autoware_planning_msgs/msg/trajectory.hpp" #include "autoware_vehicle_msgs/msg/control_mode.hpp" #include "autoware_vehicle_msgs/msg/engage.hpp" @@ -70,6 +72,10 @@ class Simulator : public rclcpp::Node private: /* ros system */ + rclcpp::CallbackGroup::SharedPtr group_api_service_; + autoware_api_utils::Service::SharedPtr + srv_set_pose_; //!< @brief service to set pose for simulation + rclcpp::Publisher::SharedPtr pub_pose_; //!< @brief topic ros publisher for current pose rclcpp::Publisher::SharedPtr @@ -185,12 +191,15 @@ class Simulator : public rclcpp::Node */ void callbackTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + + // TODO(Takagi, Isamu): deprecated /** * @brief set initial pose for simulation with received message */ void callbackInitialPoseWithCov( const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + // TODO(Takagi, Isamu): deprecated /** * @brief set initial pose with received message */ @@ -206,6 +215,13 @@ class Simulator : public rclcpp::Node */ void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg); + /** + * @brief set pose for simulation with request + */ + void serviceSetPose( + const autoware_external_api_msgs::srv::InitializePose::Request::SharedPtr request, + const autoware_external_api_msgs::srv::InitializePose::Response::SharedPtr response); + /** * @brief get transform from two frame_ids * @param [in] parent_frame parent frame id diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 15a0614867fb4..ef71e3fd3b508 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -15,7 +15,9 @@ geometry_msgs tf2 tf2_ros + autoware_api_utils autoware_debug_msgs + autoware_external_api_msgs autoware_planning_msgs autoware_control_msgs autoware_utils diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index 31130a2acaf13..158adc2980a57 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -24,7 +24,7 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { - using std::placeholders::_1; + using namespace std::placeholders; /* simple_planning_simulator parameters */ const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -37,6 +37,14 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & use_trajectory_for_z_position_source_ = declare_parameter("use_trajectory_for_z_position_source", true); + /* service */ + autoware_api_utils::ServiceProxyNodeInterface proxy(this); + group_api_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_set_pose_ = proxy.create_service( + "/api/simulator/set/pose", + std::bind(&Simulator::serviceSetPose, this, _1, _2), + rmw_qos_profile_services_default, group_api_service_); + /* set pub sub topic name */ pub_pose_ = create_publisher("output/current_pose", rclcpp::QoS{1}); @@ -240,6 +248,21 @@ void Simulator::callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSh simulator_engage_ = msg->engage; } +void Simulator::serviceSetPose( + const autoware_external_api_msgs::srv::InitializePose::Request::SharedPtr request, + const autoware_external_api_msgs::srv::InitializePose::Response::SharedPtr response) +{ + geometry_msgs::msg::Twist initial_twist; // initialized with zero for all components + if (initial_twist_ptr_) { + initial_twist = initial_twist_ptr_->twist; + } + // save initial pose + using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; + initial_pose_with_cov_ptr_ = std::make_shared(request->pose); + setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist); + response->status = autoware_api_utils::response_success(); +} + void Simulator::timerCallbackSimulation() { if (!is_initialized_) { From 7a3670e1d09c6be8b22a88a3ca37d213da0f5b89 Mon Sep 17 00:00:00 2001 From: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com> Date: Thu, 9 Sep 2021 18:44:51 +0900 Subject: [PATCH 29/52] add sort-package-xml hook in pre-commit (#1881) * add sort xml hook in pre-commit * change retval to exit_status * rename * add prettier plugin-xml * use early return * add license note * add tier4 license * restore prettier * change license order * move local hooks to public repo * move prettier-xml to pre-commit-hooks-ros * update version for bug-fix * apply pre-commit --- simulator/simple_planning_simulator/package.xml | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index ef71e3fd3b508..0aaa7fc093cfb 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -9,19 +9,19 @@ ament_cmake_auto - rclcpp - rclcpp_components - std_msgs - geometry_msgs - tf2 - tf2_ros autoware_api_utils + autoware_control_msgs autoware_debug_msgs autoware_external_api_msgs autoware_planning_msgs - autoware_control_msgs autoware_utils autoware_vehicle_msgs + geometry_msgs + rclcpp + rclcpp_components + std_msgs + tf2 + tf2_ros vehicle_info_util launch_ros From 467c390e290fc9cd957b0d45c3cefc1322065d0a Mon Sep 17 00:00:00 2001 From: Makoto Kurihara Date: Thu, 21 Oct 2021 19:27:51 +0900 Subject: [PATCH 30/52] Feature/add ideal accel model interface (#1894) * Add IDEAL_ACCEL model interface for simple planning simulator Signed-off-by: Makoto Kurihara * Add IDEAL_ACCEL model descriptions Signed-off-by: Makoto Kurihara * Fix format Signed-off-by: Makoto Kurihara * Change vehicle model type description at config file Signed-off-by: Makoto Kurihara --- simulator/simple_planning_simulator/README.md | 27 ++++++++++--------- .../simple_planning_simulator.param.yaml | 5 +--- .../config/simulator_model.param.yaml | 5 +--- .../src/simple_planning_simulator_core.cpp | 11 ++++++++ 4 files changed, 27 insertions(+), 21 deletions(-) diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md index 3236a78a435e2..2e2e1301a93bf 100644 --- a/simulator/simple_planning_simulator/README.md +++ b/simulator/simple_planning_simulator/README.md @@ -40,21 +40,22 @@ This node simulates the vehicle motion for a vehicle command in 2D using a simpl #### vehicle_model_type options - `IDEAL_STEER`: uses velocity command. The steering and velocity varies ideally as commanded. The pose is calculated by a bicycle kinematics model. +- `IDEAL_ACCEL`: uses acceleration command. The steering and acceleration varies ideally as commanded. The pose is calculated by a bicycle kinematics model. - `DELAY_STEER` : uses velocity command. The steering and velocity varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. - `DELAY_STEER_ACC` : uses acceleration command. The steering and acceleration varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. -| Name | Type | Description | IDEAL STEER | DELAY STEER | DELAY STEER 　 ACC | Default value | unit | -| :------------------- | :----- | :--------------------------------------------------- | :---------- | :---------- | :----------------- | :------------ | :------ | -| vel_time_delay | double | dead time for the velocity input | x | o | x | 0.25 | [s] | -| acc_time_delay | double | dead time for the acceleration input | x | x | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | o | o | 0.24 | [s] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | o | x | 0.61 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | o | o | 0.27 | [s] | -| vel_lim | double | limit of velocity | x | o | o | 50.0 | [m/s] | -| accel_rate | double | limit of acceleration | x | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | o | o | 5.0 | [rad/s] | -| deadzone_delta_steer | double | dead zone for the steering dynamics | x | o | o | 0.0 | [rad] | +| Name | Type | Description | IDEAL STEER | IDEAL ACCEL | DELAY STEER | DELAY STEER 　 ACC | Default value | unit | +| :------------------- | :----- | :--------------------------------------------------- | :---------- | :---------- | :---------- | :----------------- | :------------ | :------ | +| vel_time_delay | double | dead time for the velocity input | x | x | o | x | 0.25 | [s] | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | o | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | o | o | 0.24 | [s] | +| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | o | x | 0.61 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | o | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | o | o | 0.27 | [s] | +| vel_lim | double | limit of velocity | x | x | o | o | 50.0 | [m/s] | +| accel_rate | double | limit of acceleration | x | x | o | o | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | o | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | o | o | 5.0 | [rad/s] | +| deadzone_delta_steer | double | dead zone for the steering dynamics | x | x | o | o | 0.0 | [rad] | _Note_: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a _delay_ model. The definition of the _time constant_ is the time it takes for the step response to rise up to 63% of its final value. The _deadtime_ is a delay in the response to a control input. diff --git a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml index 3f5568e35c2b7..25ab6c8c52903 100644 --- a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml +++ b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml @@ -29,10 +29,7 @@ simulation: steer_time_delay: 0.24 tread_length: 1.0 - # Option for vehicle_model_type: - # - IDEAL_STEER : reads velocity command. The steering and velocity changes exactly the same as commanded. - # - DELAY_STEER : reads velocity command. The steering and velocity changes with delay model. - # - DELAY_STEER_ACC : reads acceleration command. The steering and acceleration changes with delay model. + # See readme for vehicle model type options vehicle_model_type: DELAY_STEER_ACC vel_lim: 50.0 vel_noise_stddev: 0.0 diff --git a/simulator/simple_planning_simulator/config/simulator_model.param.yaml b/simulator/simple_planning_simulator/config/simulator_model.param.yaml index 1b91975334996..ed8c9d3d0a089 100644 --- a/simulator/simple_planning_simulator/config/simulator_model.param.yaml +++ b/simulator/simple_planning_simulator/config/simulator_model.param.yaml @@ -18,10 +18,7 @@ steer_time_constant: 0.27 steer_time_delay: 0.24 tread_length: 1.0 -# Option for vehicle_model_type: -# - IDEAL_STEER : reads velocity command. The steering and velocity changes exactly the same as commanded. -# - DELAY_STEER : reads velocity command. The steering and velocity changes with delay model. -# - DELAY_STEER_ACC : reads acceleration command. The steering and acceleration changes with delay model. +# See readme for vehicle model type options vehicle_model_type: DELAY_STEER_ACC vel_lim: 50.0 vel_noise_stddev: 0.0 diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index 158adc2980a57..f7dc32f7db476 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -137,6 +137,9 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & if (vehicle_model_type_str == "IDEAL_STEER") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER; vehicle_model_ptr_ = std::make_shared(wheelbase_); + } else if (vehicle_model_type_str == "IDEAL_ACCEL") { + vehicle_model_type_ = VehicleModelType::IDEAL_ACCEL; + vehicle_model_ptr_ = std::make_shared(wheelbase_); } else if (vehicle_model_type_str == "DELAY_STEER") { vehicle_model_type_ = VehicleModelType::DELAY_STEER; vehicle_model_ptr_ = std::make_shared( @@ -388,6 +391,10 @@ void Simulator::callbackVehicleCmd( Eigen::VectorXd input(2); input << msg->control.velocity, msg->control.steering_angle; vehicle_model_ptr_->setInput(input); + } else if (vehicle_model_type_ == VehicleModelType::IDEAL_ACCEL) { + Eigen::VectorXd input(2); + input << msg->control.acceleration, msg->control.steering_angle; + vehicle_model_ptr_->setInput(input); } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { Eigen::VectorXd input(3); double drive_shift = @@ -441,6 +448,10 @@ void Simulator::setInitialState( Eigen::VectorXd state(3); state << x, y, yaw; vehicle_model_ptr_->setState(state); + } else if (vehicle_model_type_ == VehicleModelType::IDEAL_ACCEL) { + Eigen::VectorXd state(4); + state << x, y, yaw, vx; + vehicle_model_ptr_->setState(state); } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER) { Eigen::VectorXd state(5); state << x, y, yaw, vx, steer; From 7a8aa72e4de3ad60a960ba123ccc065840989a06 Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Tue, 2 Nov 2021 21:49:19 +0900 Subject: [PATCH 31/52] Change formatter to clang-format and black (#2332) * Revert "Temporarily comment out pre-commit hooks" This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3. * Replace ament_lint_common with autoware_lint_common Signed-off-by: Kenji Miyake * Remove ament_cmake_uncrustify and ament_clang_format Signed-off-by: Kenji Miyake * Apply Black Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake * Fix for cpplint * Fix include double quotes to angle brackets Signed-off-by: Kenji Miyake * Apply clang-format Signed-off-by: Kenji Miyake * Fix build errors Signed-off-by: Kenji Miyake --- .../simple_planning_simulator_core.hpp | 80 ++++++------ .../sim_model_constant_acceleration.hpp | 14 +-- .../vehicle_model/sim_model_ideal.hpp | 30 ++--- .../vehicle_model/sim_model_interface.hpp | 4 +- .../vehicle_model/sim_model_time_delay.hpp | 33 +++-- .../simple_planning_simulator.launch.py | 50 ++++---- .../simple_planning_simulator/package.xml | 2 +- .../scripts/fitParamDelayInputModel.py | 118 +++++++++--------- .../src/simple_planning_simulator_core.cpp | 53 ++++---- .../src/simple_planning_simulator_node.cpp | 3 +- .../sim_model_constant_acceleration.cpp | 22 ++-- .../src/vehicle_model/sim_model_ideal.cpp | 47 +++---- .../src/vehicle_model/sim_model_interface.cpp | 11 +- .../vehicle_model/sim_model_time_delay.cpp | 67 +++++----- 14 files changed, 253 insertions(+), 281 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index fbb7d3f4555a4..b78af263f8025 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -22,40 +22,40 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ -#include -#include -#include -#include - -#include "rclcpp/rclcpp.hpp" - -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" -#include "tf2/LinearMath/Quaternion.h" -#include "tf2/utils.h" -#include "tf2_ros/buffer.h" -#include "tf2_ros/transform_broadcaster.h" -#include "tf2_ros/transform_listener.h" - -#include "autoware_api_utils/autoware_api_utils.hpp" -#include "autoware_debug_msgs/msg/float32_stamped.hpp" -#include "autoware_external_api_msgs/srv/initialize_pose.hpp" -#include "autoware_planning_msgs/msg/trajectory.hpp" -#include "autoware_vehicle_msgs/msg/control_mode.hpp" -#include "autoware_vehicle_msgs/msg/engage.hpp" -#include "autoware_vehicle_msgs/msg/shift_stamped.hpp" -#include "autoware_vehicle_msgs/msg/steering.hpp" -#include "autoware_vehicle_msgs/msg/turn_signal.hpp" -#include "autoware_vehicle_msgs/msg/vehicle_command.hpp" - #include "simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +#include +#include +#include +#include class Simulator : public rclcpp::Node { @@ -149,8 +149,7 @@ class Simulator : public rclcpp::Node std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time /* vehicle model */ - enum class VehicleModelType - { + enum class VehicleModelType { IDEAL_TWIST = 0, IDEAL_STEER = 1, DELAY_TWIST = 2, @@ -166,15 +165,15 @@ class Simulator : public rclcpp::Node /* to generate measurement noise */ std::shared_ptr rand_engine_ptr_; //!< @brief random engine for measurement noise std::shared_ptr> - pos_norm_dist_ptr_; //!< @brief Gaussian noise for position + pos_norm_dist_ptr_; //!< @brief Gaussian noise for position std::shared_ptr> - vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity + vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity std::shared_ptr> - rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw + rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw std::shared_ptr> - angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity + angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity std::shared_ptr> - steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle + steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle /** * @brief set current_vehicle_cmd_ptr_ with received message @@ -191,7 +190,6 @@ class Simulator : public rclcpp::Node */ void callbackTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); - // TODO(Takagi, Isamu): deprecated /** * @brief set initial pose for simulation with received message @@ -266,15 +264,13 @@ class Simulator : public rclcpp::Node * @brief publish pose_with_covariance * @param [in] cov pose with covariance to be published */ - void publishPoseWithCov( - const geometry_msgs::msg::PoseWithCovariance & cov); + void publishPoseWithCov(const geometry_msgs::msg::PoseWithCovariance & cov); /** * @brief publish twist * @param [in] twist twist to be published */ - void publishTwist( - const geometry_msgs::msg::Twist & twist); + void publishTwist(const geometry_msgs::msg::Twist & twist); /** * @brief publish tf diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp index 69d30221a1840..b1af6152ecddd 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp @@ -22,12 +22,12 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_CONSTANT_ACCELERATION_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_CONSTANT_ACCELERATION_HPP_ -#include - #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" +#include +#include + +#include /** * @class simple_planning_simulator constant acceleration twist model @@ -51,16 +51,14 @@ class SimModelConstantAccelTwist : public SimModelInterface ~SimModelConstantAccelTwist() = default; private: - enum IDX - { + enum IDX { X = 0, Y, YAW, VX, WZ, }; - enum IDX_U - { + enum IDX_U { VX_DES = 0, WZ_DES, }; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp index 8a1297cdcb51a..1cfc43c1e15c3 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp @@ -14,8 +14,8 @@ /** * @file sim_model_ideal.hpp - * @brief simple planning simulator ideal velocity model (no dynamics for desired velocity & angular-velocity or - * steering) + * @brief simple planning simulator ideal velocity model (no dynamics for desired velocity & + * angular-velocity or steering) * @author Takamasa Horibe * @date 2019.08.17 */ @@ -23,12 +23,12 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_HPP_ -#include - #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" +#include +#include + +#include /** * @class simple_planning_simulator ideal twist model @@ -48,14 +48,12 @@ class SimModelIdealTwist : public SimModelInterface ~SimModelIdealTwist() = default; private: - enum IDX - { + enum IDX { X = 0, Y, YAW, }; - enum IDX_U - { + enum IDX_U { VX_DES = 0, WZ_DES, }; @@ -123,14 +121,12 @@ class SimModelIdealSteer : public SimModelInterface ~SimModelIdealSteer() = default; private: - enum IDX - { + enum IDX { X = 0, Y, YAW, }; - enum IDX_U - { + enum IDX_U { VX_DES = 0, STEER_DES, }; @@ -200,15 +196,13 @@ class SimModelIdealAccel : public SimModelInterface ~SimModelIdealAccel() = default; private: - enum IDX - { + enum IDX { X = 0, Y, YAW, VX, }; - enum IDX_U - { + enum IDX_U { AX_DES = 0, STEER_DES, }; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 9fdc2da818707..4250e02b12727 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -22,8 +22,8 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ -#include "rclcpp/rclcpp.hpp" -#include "eigen3/Eigen/Core" +#include +#include /** * @class simple_planning_simulator vehicle model class diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp index 26b53bc8fcc9e..d6193434bcbc8 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp @@ -14,7 +14,8 @@ /** * @file sim_model_time_delay.hpp - * @brief simple planning simulator model with time delay and 1-dimensional dynamics for velocity & steering + * @brief simple planning simulator model with time delay and 1-dimensional dynamics for velocity & + * steering * @author Takamasa Horibe, Kim-Ngoc-Khanh Nguyen * @date 2019.08.17 */ @@ -22,15 +23,15 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_TIME_DELAY_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_TIME_DELAY_HPP_ -#include -#include -#include - #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_util.hpp" -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" +#include +#include + +#include +#include +#include /** * @class simple_planning_simulator time delay twist model @@ -65,16 +66,14 @@ class SimModelTimeDelayTwist : public SimModelInterface private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX - { + enum IDX { X = 0, Y, YAW, VX, WZ, }; - enum IDX_U - { + enum IDX_U { VX_DES = 0, WZ_DES, }; @@ -173,16 +172,14 @@ class SimModelTimeDelaySteer : public SimModelInterface private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX - { + enum IDX { X = 0, Y, YAW, VX, STEER, }; - enum IDX_U - { + enum IDX_U { VX_DES = 0, STEER_DES, }; @@ -281,8 +278,7 @@ class SimModelTimeDelaySteerAccel : public SimModelInterface private: const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX - { + enum IDX { X = 0, Y, YAW, @@ -290,8 +286,7 @@ class SimModelTimeDelaySteerAccel : public SimModelInterface STEER, ACCX, }; - enum IDX_U - { + enum IDX_U { ACCX_DES = 0, STEER_DES, DRIVE_SHIFT, diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index d8a8627b331d2..38cd8365604ae 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -27,48 +27,44 @@ def find_pack(package_name): """Return the absolute path to the share directory of the given package.""" - return os.path.join( - Path(FindPackage(package_name).perform(context)), 'share', package_name) + return os.path.join(Path(FindPackage(package_name).perform(context)), "share", package_name) def generate_launch_description(): simple_planning_simulator_param_file = os.path.join( - find_pack('simple_planning_simulator'), - 'config/simple_planning_simulator.param.yaml') + find_pack("simple_planning_simulator"), "config/simple_planning_simulator.param.yaml" + ) print(simple_planning_simulator_param_file) simple_planning_simulator_param = DeclareLaunchArgument( - 'simple_planning_simulator_param_file', + "simple_planning_simulator_param_file", default_value=simple_planning_simulator_param_file, - description='Path to config file for simple_planning_simulator' + description="Path to config file for simple_planning_simulator", ) simple_planning_simulator = Node( - package='simple_planning_simulator', - node_executable='simple_planning_simulator_exe', - node_name='simple_planning_simulator', - node_namespace='simulation', - output='screen', + package="simple_planning_simulator", + node_executable="simple_planning_simulator_exe", + node_name="simple_planning_simulator", + node_namespace="simulation", + output="screen", parameters=[ - LaunchConfiguration('simple_planning_simulator_param_file'), + LaunchConfiguration("simple_planning_simulator_param_file"), ], remappings=[ - ('base_trajectory', '/planning/scenario_planning/trajectory'), - ('output/current_pose', 'current_pose'), - ('output/current_twist', '/vehicle/status/twist'), - ('output/status', '/vehicle/status'), - ('output/control_mode', '/vehicle/status/control_mode'), - ('input/vehicle_cmd', '/control/vehicle_cmd'), - ('input/turn_signal_cmd', '/control/turn_signal_cmd'), - ('input/initial_twist', '/initialtwist'), - ('input/initial_pose', '/initialpose'), - ('input/engage', '/vehicle/engage'), - ] + ("base_trajectory", "/planning/scenario_planning/trajectory"), + ("output/current_pose", "current_pose"), + ("output/current_twist", "/vehicle/status/twist"), + ("output/status", "/vehicle/status"), + ("output/control_mode", "/vehicle/status/control_mode"), + ("input/vehicle_cmd", "/control/vehicle_cmd"), + ("input/turn_signal_cmd", "/control/turn_signal_cmd"), + ("input/initial_twist", "/initialtwist"), + ("input/initial_pose", "/initialpose"), + ("input/engage", "/vehicle/engage"), + ], ) - return LaunchDescription([ - simple_planning_simulator_param, - simple_planning_simulator - ]) + return LaunchDescription([simple_planning_simulator_param, simple_planning_simulator]) diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 0aaa7fc093cfb..92f1bf0c8df1e 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -29,7 +29,7 @@ topic_tools ament_lint_auto - ament_lint_common + autoware_lint_common ament_cmake diff --git a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py index b701425722a30..a0aaa943fc4e7 100644 --- a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py +++ b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py @@ -29,7 +29,7 @@ try: import pandas as pd except ImportError: - print('Please install pandas. See http://pandas.pydata.org/pandas-docs/stable/') + print("Please install pandas. See http://pandas.pydata.org/pandas-docs/stable/") sys.exit(1) FREQ_SAMPLE = 0.001 @@ -41,8 +41,7 @@ def lowpass_filter(data, cutoff_freq=2, order=1, dt=FREQ_SAMPLE): tau = 1.0 / (2 * np.pi * cutoff_freq) for _ in range(order): for i in range(1, len(data)): - data[i] = (tau / (tau + dt) * data[i - 1] + - dt / (tau + dt) * data[i]) + data[i] = tau / (tau + dt) * data[i - 1] + dt / (tau + dt) * data[i] return data @@ -53,33 +52,34 @@ def rel2abs(path): def rosbag_to_csv(path, topic_name): name = splitext(basename(path))[0] - suffix = topic_name.replace('/', '-') - output_path = join(dirname(path), name + '_' + suffix + '.csv') + suffix = topic_name.replace("/", "-") + output_path = join(dirname(path), name + "_" + suffix + ".csv") if exists(output_path): return output_path else: command = "rostopic echo -b {0} -p /{1} | sed -e 's/,/ /g' > {2}".format( - path, topic_name, output_path) + path, topic_name, output_path + ) print(command) subprocess.check_call(command, shell=True) return output_path def getActValue(df, speed_type): - tm = np.array(list(df['%time'])) * 1e-9 + tm = np.array(list(df["%time"])) * 1e-9 # Unit Conversion if speed_type: - val = np.array(list(df['field'])) / 3.6 + val = np.array(list(df["field"])) / 3.6 else: - val = np.array(list(df['field'])) + val = np.array(list(df["field"])) # Calc differential d_val = (val[2:] - val[:-2]) / (tm[2:] - tm[:-2]) return tm[1:-1], val[1:-1], d_val def getCmdValueWithDelay(df, delay): - tm = np.array(list(df['%time'])) * 1e-9 - val = np.array(list(df['field'])) + tm = np.array(list(df["%time"])) * 1e-9 + val = np.array(list(df["field"])) return tm + delay, val @@ -88,13 +88,11 @@ def getLinearInterpolate(_tm, _val, _index, ti): tmp_next_t = _tm[_index + 1] tmp_val = _val[_index] tmp_next_val = _val[_index + 1] - val_i = tmp_val + (tmp_next_val - tmp_val) / \ - (tmp_next_t - tmp_t) * (ti - tmp_t) + val_i = tmp_val + (tmp_next_val - tmp_val) / (tmp_next_t - tmp_t) * (ti - tmp_t) return val_i -def getFittingTimeConstantParam(cmd_data, act_data, - delay, args, speed_type=False): +def getFittingTimeConstantParam(cmd_data, act_data, delay, args, speed_type=False): tm_cmd, cmd_delay = getCmdValueWithDelay(cmd_data, delay) tm_act, act, d_act = getActValue(act_data, speed_type) _t_min = max(tm_cmd[0], tm_act[0]) @@ -108,10 +106,10 @@ def getFittingTimeConstantParam(cmd_data, act_data, ind_act = 0 for ind in range(MAX_CNT): ti = ind * FREQ_SAMPLE + args.cutoff_time - while (tm_cmd[ind_cmd + 1] < ti): + while tm_cmd[ind_cmd + 1] < ti: ind_cmd += 1 cmd_delay_i = getLinearInterpolate(tm_cmd, cmd_delay, ind_cmd, ti) - while (tm_act[ind_act + 1] < ti): + while tm_act[ind_act + 1] < ti: ind_act += 1 act_i = getLinearInterpolate(tm_act, act, ind_act, ti) d_act_i = getLinearInterpolate(tm_act, d_act, ind_act, ti) @@ -121,30 +119,24 @@ def getFittingTimeConstantParam(cmd_data, act_data, diff_act_cmd_sample = np.array(diff_act_cmd_sample) if args.cutoff_freq > 0: d_act_sample = lowpass_filter(d_act_sample, cutoff_freq=args.cutoff_freq) - diff_act_cmd_sample = lowpass_filter( - diff_act_cmd_sample, cutoff_freq=args.cutoff_freq) + diff_act_cmd_sample = lowpass_filter(diff_act_cmd_sample, cutoff_freq=args.cutoff_freq) d_act_sample = d_act_sample.reshape(1, -1) diff_act_cmd_sample = diff_act_cmd_sample.reshape(1, -1) tau = -np.dot(diff_act_cmd_sample, np.linalg.pinv(d_act_sample))[0, 0] - error = np.linalg.norm(diff_act_cmd_sample + tau * - d_act_sample) / d_act_sample.shape[1] + error = np.linalg.norm(diff_act_cmd_sample + tau * d_act_sample) / d_act_sample.shape[1] return tau, error def getFittingParam(cmd_data, act_data, args, speed_type=False): delay_range = int((args.max_delay - args.min_delay) / args.delay_incr) - delays = [ - args.min_delay + - i * - args.delay_incr for i in range( - delay_range + - 1)] + delays = [args.min_delay + i * args.delay_incr for i in range(delay_range + 1)] error_min = 1.0e10 delay_opt = -1 tau_opt = -1 for delay in delays: tau, error = getFittingTimeConstantParam( - cmd_data, act_data, delay, args, speed_type=speed_type) + cmd_data, act_data, delay, args, speed_type=speed_type + ) if tau > 0: if error < error_min: error_min = error @@ -155,59 +147,65 @@ def getFittingParam(cmd_data, act_data, args, speed_type=False): return tau_opt, delay_opt, error_min -if __name__ == '__main__': - topics = ['vehicle_cmd/ctrl_cmd/steering_angle', 'vehicle_status/angle', - 'vehicle_cmd/ctrl_cmd/linear_velocity', 'vehicle_status/speed'] +if __name__ == "__main__": + topics = [ + "vehicle_cmd/ctrl_cmd/steering_angle", + "vehicle_status/angle", + "vehicle_cmd/ctrl_cmd/linear_velocity", + "vehicle_status/speed", + ] pd_data = [None] * len(topics) parser = argparse.ArgumentParser( - description='Parameter fitting for Input Delay Model' - ' (First Order System with Dead Time) with rosbag file input') + description="Parameter fitting for Input Delay Model" + " (First Order System with Dead Time) with rosbag file input" + ) parser.add_argument( - '--bag_file', - '-b', - required=True, - type=str, - help='rosbag file', - metavar='file') + "--bag_file", "-b", required=True, type=str, help="rosbag file", metavar="file" + ) parser.add_argument( - '--cutoff_time', + "--cutoff_time", default=1.0, type=float, - help='Cutoff time[sec], Parameter fitting will only consider data ' - 'from t= cutoff_time to the end of the bag file (default is 1.0)') + help="Cutoff time[sec], Parameter fitting will only consider data " + "from t= cutoff_time to the end of the bag file (default is 1.0)", + ) parser.add_argument( - '--cutoff_freq', + "--cutoff_freq", default=0.1, type=float, - help='Cutoff freq for low-pass filter[Hz], ' - 'negative value will disable low-pass filter (default is 0.1)') + help="Cutoff freq for low-pass filter[Hz], " + "negative value will disable low-pass filter (default is 0.1)", + ) parser.add_argument( - '--min_delay', + "--min_delay", default=0.1, type=float, - help='Min value for searching delay loop (default is 0.1)') + help="Min value for searching delay loop (default is 0.1)", + ) parser.add_argument( - '--max_delay', + "--max_delay", default=1.0, type=float, - help='Max value for searching delay loop (default is 1.0)') + help="Max value for searching delay loop (default is 1.0)", + ) parser.add_argument( - '--delay_incr', + "--delay_incr", default=0.01, type=float, - help='Step value for searching delay loop (default is 0.01)') + help="Step value for searching delay loop (default is 0.01)", + ) args = parser.parse_args() for i, topic in enumerate(topics): csv_log = rosbag_to_csv(rel2abs(args.bag_file), topic) - pd_data[i] = pd.read_csv(csv_log, sep=' ') - tau_opt, delay_opt, error = getFittingParam( - pd_data[0], pd_data[1], args, speed_type=False) + pd_data[i] = pd.read_csv(csv_log, sep=" ") + tau_opt, delay_opt, error = getFittingParam(pd_data[0], pd_data[1], args, speed_type=False) print( - 'Steer angle: tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' % - (tau_opt, delay_opt, error)) - tau_opt, delay_opt, error = getFittingParam( - pd_data[2], pd_data[3], args, speed_type=True) + "Steer angle: tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e" + % (tau_opt, delay_opt, error) + ) + tau_opt, delay_opt, error = getFittingParam(pd_data[2], pd_data[3], args, speed_type=True) print( - 'Velocity : tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e' % - (tau_opt, delay_opt, error)) + "Velocity : tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e" + % (tau_opt, delay_opt, error) + ) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp index f7dc32f7db476..8a42d96504a1a 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp @@ -12,19 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" + +#include + #include +#include #include #include -#include "simple_planning_simulator/simple_planning_simulator_core.hpp" - -#include "autoware_utils/ros/update_param.hpp" - Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & options) : Node(node_name, options) { - using namespace std::placeholders; + using std::placeholders::_1; + using std::placeholders::_2; /* simple_planning_simulator parameters */ const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); @@ -41,8 +42,7 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & autoware_api_utils::ServiceProxyNodeInterface proxy(this); group_api_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); srv_set_pose_ = proxy.create_service( - "/api/simulator/set/pose", - std::bind(&Simulator::serviceSetPose, this, _1, _2), + "/api/simulator/set/pose", std::bind(&Simulator::serviceSetPose, this, _1, _2), rmw_qos_profile_services_default, group_api_service_); /* set pub sub topic name */ @@ -60,8 +60,8 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & "/vehicle/status/turn_signal", rclcpp::QoS{1}); pub_shift_ = create_publisher( "/vehicle/status/shift", rclcpp::QoS{1}); - pub_cov_ = create_publisher( - "output/cov", rclcpp::QoS{1}); + pub_cov_ = + create_publisher("output/cov", rclcpp::QoS{1}); sub_vehicle_cmd_ = create_subscription( "input/vehicle_cmd", rclcpp::QoS{1}, @@ -92,16 +92,13 @@ Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & /* set param callback */ set_param_res_ = - this->add_on_set_parameters_callback( - std::bind( - &Simulator::onParameter, this, - _1)); + this->add_on_set_parameters_callback(std::bind(&Simulator::onParameter, this, _1)); /* Timer */ { auto timer_callback = std::bind(&Simulator::timerCallbackSimulation, this); - auto period = std::chrono::duration_cast( - std::chrono::duration(dt)); + auto period = + std::chrono::duration_cast(std::chrono::duration(dt)); timer_simulation_ = std::make_shared>( this->get_clock(), period, std::move(timer_callback), this->get_node_base_interface()->get_context()); @@ -270,8 +267,7 @@ void Simulator::timerCallbackSimulation() { if (!is_initialized_) { RCLCPP_INFO_THROTTLE( - get_logger(), *this->get_clock(), 3000 /*ms*/, - "waiting initial position..."); + get_logger(), *this->get_clock(), 3000 /*ms*/, "waiting initial position..."); return; } @@ -360,8 +356,7 @@ void Simulator::timerCallbackSimulation() if ( cmd == autoware_vehicle_msgs::msg::TurnSignal::LEFT || cmd == autoware_vehicle_msgs::msg::TurnSignal::RIGHT || - cmd == autoware_vehicle_msgs::msg::TurnSignal::HAZARD) - { + cmd == autoware_vehicle_msgs::msg::TurnSignal::HAZARD) { turn_signal_msg.data = cmd; } } @@ -370,9 +365,9 @@ void Simulator::timerCallbackSimulation() autoware_vehicle_msgs::msg::ShiftStamped shift_msg; shift_msg.header.frame_id = simulation_frame_id_; shift_msg.header.stamp = get_clock()->now(); - shift_msg.shift.data = current_twist_.linear.x >= 0.0 ? - autoware_vehicle_msgs::msg::Shift::DRIVE : - autoware_vehicle_msgs::msg::Shift::REVERSE; + shift_msg.shift.data = current_twist_.linear.x >= 0.0 + ? autoware_vehicle_msgs::msg::Shift::DRIVE + : autoware_vehicle_msgs::msg::Shift::REVERSE; pub_shift_->publish(shift_msg); /* publish control mode */ @@ -386,8 +381,7 @@ void Simulator::callbackVehicleCmd( if ( vehicle_model_type_ == VehicleModelType::IDEAL_STEER || - vehicle_model_type_ == VehicleModelType::DELAY_STEER) - { + vehicle_model_type_ == VehicleModelType::DELAY_STEER) { Eigen::VectorXd input(2); input << msg->control.velocity, msg->control.steering_angle; vehicle_model_ptr_->setInput(input); @@ -475,8 +469,7 @@ void Simulator::getTransformFromTF( while (rclcpp::ok()) { try { const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); - transform = tf_buffer_->lookupTransform( - parent_frame, child_frame, time_point); + transform = tf_buffer_->lookupTransform(parent_frame, child_frame, time_point); break; } catch (tf2::TransformException & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); @@ -484,8 +477,7 @@ void Simulator::getTransformFromTF( } } -void Simulator::publishTwist( - const geometry_msgs::msg::Twist & twist) +void Simulator::publishTwist(const geometry_msgs::msg::Twist & twist) { rclcpp::Time current_time = get_clock()->now(); geometry_msgs::msg::TwistStamped ts; @@ -495,8 +487,7 @@ void Simulator::publishTwist( pub_twist_->publish(ts); } -void Simulator::publishPoseWithCov( - const geometry_msgs::msg::PoseWithCovariance & cov) +void Simulator::publishPoseWithCov(const geometry_msgs::msg::PoseWithCovariance & cov) { rclcpp::Time current_time = get_clock()->now(); geometry_msgs::msg::PoseWithCovarianceStamped cs; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp index 1f6590c5a17f4..c580d7b8e9d3d 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp @@ -12,9 +12,10 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include + int main(int argc, char ** argv) { rclcpp::init(argc, argv); diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp index 700d7afd3977e..5729ad8d6064b 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp @@ -12,25 +12,27 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp" +#include + SimModelConstantAccelTwist::SimModelConstantAccelTwist( double vx_lim, double wz_lim, double vx_rate, double wz_rate) : SimModelInterface(5 /* dim x */, 2 /* dim u */), vx_lim_(vx_lim), wz_lim_(wz_lim), vx_rate_(vx_rate), - wz_rate_(wz_rate) {} + wz_rate_(wz_rate) +{ +} -double SimModelConstantAccelTwist::getX() {return state_(IDX::X);} -double SimModelConstantAccelTwist::getY() {return state_(IDX::Y);} -double SimModelConstantAccelTwist::getYaw() {return state_(IDX::YAW);} -double SimModelConstantAccelTwist::getVx() {return state_(IDX::VX);} -double SimModelConstantAccelTwist::getWz() {return state_(IDX::WZ);} -double SimModelConstantAccelTwist::getSteer() {return 0.0;} -void SimModelConstantAccelTwist::update(const double & dt) {updateRungeKutta(dt, input_);} +double SimModelConstantAccelTwist::getX() { return state_(IDX::X); } +double SimModelConstantAccelTwist::getY() { return state_(IDX::Y); } +double SimModelConstantAccelTwist::getYaw() { return state_(IDX::YAW); } +double SimModelConstantAccelTwist::getVx() { return state_(IDX::VX); } +double SimModelConstantAccelTwist::getWz() { return state_(IDX::WZ); } +double SimModelConstantAccelTwist::getSteer() { return 0.0; } +void SimModelConstantAccelTwist::update(const double & dt) { updateRungeKutta(dt, input_); } Eigen::VectorXd SimModelConstantAccelTwist::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp index dd9f1cdb66e42..c000e92b674d8 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp @@ -14,16 +14,15 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" -SimModelIdealTwist::SimModelIdealTwist() -: SimModelInterface(3 /* dim x */, 2 /* dim u */) {} +SimModelIdealTwist::SimModelIdealTwist() : SimModelInterface(3 /* dim x */, 2 /* dim u */) {} -double SimModelIdealTwist::getX() {return state_(IDX::X);} -double SimModelIdealTwist::getY() {return state_(IDX::Y);} -double SimModelIdealTwist::getYaw() {return state_(IDX::YAW);} -double SimModelIdealTwist::getVx() {return input_(IDX_U::VX_DES);} -double SimModelIdealTwist::getWz() {return input_(IDX_U::WZ_DES);} -double SimModelIdealTwist::getSteer() {return 0.0;} -void SimModelIdealTwist::update(const double & dt) {updateRungeKutta(dt, input_);} +double SimModelIdealTwist::getX() { return state_(IDX::X); } +double SimModelIdealTwist::getY() { return state_(IDX::Y); } +double SimModelIdealTwist::getYaw() { return state_(IDX::YAW); } +double SimModelIdealTwist::getVx() { return input_(IDX_U::VX_DES); } +double SimModelIdealTwist::getWz() { return input_(IDX_U::WZ_DES); } +double SimModelIdealTwist::getSteer() { return 0.0; } +void SimModelIdealTwist::update(const double & dt) { updateRungeKutta(dt, input_); } Eigen::VectorXd SimModelIdealTwist::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { @@ -40,18 +39,20 @@ Eigen::VectorXd SimModelIdealTwist::calcModel( } SimModelIdealSteer::SimModelIdealSteer(double wheelbase) -: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} +: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) +{ +} -double SimModelIdealSteer::getX() {return state_(IDX::X);} -double SimModelIdealSteer::getY() {return state_(IDX::Y);} -double SimModelIdealSteer::getYaw() {return state_(IDX::YAW);} -double SimModelIdealSteer::getVx() {return input_(IDX_U::VX_DES);} +double SimModelIdealSteer::getX() { return state_(IDX::X); } +double SimModelIdealSteer::getY() { return state_(IDX::Y); } +double SimModelIdealSteer::getYaw() { return state_(IDX::YAW); } +double SimModelIdealSteer::getVx() { return input_(IDX_U::VX_DES); } double SimModelIdealSteer::getWz() { return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -double SimModelIdealSteer::getSteer() {return input_(IDX_U::STEER_DES);} -void SimModelIdealSteer::update(const double & dt) {updateRungeKutta(dt, input_);} +double SimModelIdealSteer::getSteer() { return input_(IDX_U::STEER_DES); } +void SimModelIdealSteer::update(const double & dt) { updateRungeKutta(dt, input_); } Eigen::VectorXd SimModelIdealSteer::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) { @@ -68,17 +69,19 @@ Eigen::VectorXd SimModelIdealSteer::calcModel( } SimModelIdealAccel::SimModelIdealAccel(double wheelbase) -: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) +{ +} -double SimModelIdealAccel::getX() {return state_(IDX::X);} -double SimModelIdealAccel::getY() {return state_(IDX::Y);} -double SimModelIdealAccel::getYaw() {return state_(IDX::YAW);} -double SimModelIdealAccel::getVx() {return state_(IDX::VX);} +double SimModelIdealAccel::getX() { return state_(IDX::X); } +double SimModelIdealAccel::getY() { return state_(IDX::Y); } +double SimModelIdealAccel::getYaw() { return state_(IDX::YAW); } +double SimModelIdealAccel::getVx() { return state_(IDX::VX); } double SimModelIdealAccel::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -double SimModelIdealAccel::getSteer() {return input_(IDX_U::STEER_DES);} +double SimModelIdealAccel::getSteer() { return input_(IDX_U::STEER_DES); } void SimModelIdealAccel::update(const double & dt) { updateRungeKutta(dt, input_); diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp index b0e109028e14d..60c3e422a3da6 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp @@ -14,8 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -SimModelInterface::SimModelInterface(int dim_x, int dim_u) -: dim_x_(dim_x), dim_u_(dim_u) +SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) { state_ = Eigen::VectorXd::Zero(dim_x_); input_ = Eigen::VectorXd::Zero(dim_u_); @@ -34,7 +33,7 @@ void SimModelInterface::updateEuler(const double & dt, const Eigen::VectorXd & i { state_ += calcModel(state_, input) * dt; } -void SimModelInterface::getState(Eigen::VectorXd & state) {state = state_;} -void SimModelInterface::getInput(Eigen::VectorXd & input) {input = input_;} -void SimModelInterface::setState(const Eigen::VectorXd & state) {state_ = state;} -void SimModelInterface::setInput(const Eigen::VectorXd & input) {input_ = input;} +void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; } +void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; } +void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; } +void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; } diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp index 05da1f009b19a..e6e28dc7873ec 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp +++ b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp @@ -11,9 +11,10 @@ // 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 #include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" +#include + /* * * SimModelTimeDelayTwist @@ -36,22 +37,22 @@ SimModelTimeDelayTwist::SimModelTimeDelayTwist( deadzone_delta_steer_(deadzone_delta_steer) { if (vx_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << - std::endl; + std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT + << std::endl; } if (wz_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings wz_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << - std::endl; + std::cout << "Settings wz_time_constant is too small, replace it by " << MIN_TIME_CONSTANT + << std::endl; } initializeInputQueue(dt); } -double SimModelTimeDelayTwist::getX() {return state_(IDX::X);} -double SimModelTimeDelayTwist::getY() {return state_(IDX::Y);} -double SimModelTimeDelayTwist::getYaw() {return state_(IDX::YAW);} -double SimModelTimeDelayTwist::getVx() {return state_(IDX::VX);} -double SimModelTimeDelayTwist::getWz() {return state_(IDX::WZ);} -double SimModelTimeDelayTwist::getSteer() {return 0.0;} +double SimModelTimeDelayTwist::getX() { return state_(IDX::X); } +double SimModelTimeDelayTwist::getY() { return state_(IDX::Y); } +double SimModelTimeDelayTwist::getYaw() { return state_(IDX::YAW); } +double SimModelTimeDelayTwist::getVx() { return state_(IDX::VX); } +double SimModelTimeDelayTwist::getWz() { return state_(IDX::WZ); } +double SimModelTimeDelayTwist::getSteer() { return 0.0; } void SimModelTimeDelayTwist::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -125,26 +126,26 @@ SimModelTimeDelaySteer::SimModelTimeDelaySteer( deadzone_delta_steer_(deadzone_delta_steer) { if (vx_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << - std::endl; + std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT + << std::endl; } if (steer_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings steer_time_constant is too small, replace it by " << MIN_TIME_CONSTANT << - std::endl; + std::cout << "Settings steer_time_constant is too small, replace it by " << MIN_TIME_CONSTANT + << std::endl; } initializeInputQueue(dt); } -double SimModelTimeDelaySteer::getX() {return state_(IDX::X);} -double SimModelTimeDelaySteer::getY() {return state_(IDX::Y);} -double SimModelTimeDelaySteer::getYaw() {return state_(IDX::YAW);} -double SimModelTimeDelaySteer::getVx() {return state_(IDX::VX);} +double SimModelTimeDelaySteer::getX() { return state_(IDX::X); } +double SimModelTimeDelaySteer::getY() { return state_(IDX::Y); } +double SimModelTimeDelaySteer::getYaw() { return state_(IDX::YAW); } +double SimModelTimeDelaySteer::getVx() { return state_(IDX::VX); } double SimModelTimeDelaySteer::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -double SimModelTimeDelaySteer::getSteer() {return state_(IDX::STEER);} +double SimModelTimeDelaySteer::getSteer() { return state_(IDX::STEER); } void SimModelTimeDelaySteer::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -215,26 +216,26 @@ SimModelTimeDelaySteerAccel::SimModelTimeDelaySteerAccel( deadzone_delta_steer_(deadzone_delta_steer) { if (acc_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings acc_time_constant is too small, replace it by" << MIN_TIME_CONSTANT << - std::endl; + std::cout << "Settings acc_time_constant is too small, replace it by" << MIN_TIME_CONSTANT + << std::endl; } if (steer_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings steer_time_constant is too small, replace it by" << MIN_TIME_CONSTANT << - std::endl; + std::cout << "Settings steer_time_constant is too small, replace it by" << MIN_TIME_CONSTANT + << std::endl; } initializeInputQueue(dt); } -double SimModelTimeDelaySteerAccel::getX() {return state_(IDX::X);} -double SimModelTimeDelaySteerAccel::getY() {return state_(IDX::Y);} -double SimModelTimeDelaySteerAccel::getYaw() {return state_(IDX::YAW);} -double SimModelTimeDelaySteerAccel::getVx() {return state_(IDX::VX);} +double SimModelTimeDelaySteerAccel::getX() { return state_(IDX::X); } +double SimModelTimeDelaySteerAccel::getY() { return state_(IDX::Y); } +double SimModelTimeDelaySteerAccel::getYaw() { return state_(IDX::YAW); } +double SimModelTimeDelaySteerAccel::getVx() { return state_(IDX::VX); } double SimModelTimeDelaySteerAccel::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -double SimModelTimeDelaySteerAccel::getSteer() {return state_(IDX::STEER);} +double SimModelTimeDelaySteerAccel::getSteer() { return state_(IDX::STEER); } void SimModelTimeDelaySteerAccel::update(const double & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -254,16 +255,14 @@ void SimModelTimeDelaySteerAccel::update(const double & dt) if (delayed_input(IDX_U::DRIVE_SHIFT) >= 0.0) { state_(IDX::VX) = std::max(0.0, std::min(state_(IDX::VX), vx_lim_)); if ( - std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || std::abs((state_(IDX::VX) - vx_lim_)) < 10e-9) - { + std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || std::abs((state_(IDX::VX) - vx_lim_)) < 10e-9) { state_(IDX::ACCX) = 0.0; } } else { state_(IDX::VX) = std::min(0.0, std::max(state_(IDX::VX), -vx_lim_)); if ( std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || - std::abs((state_(IDX::VX) - (-vx_lim_))) < 10e-9) - { + std::abs((state_(IDX::VX) - (-vx_lim_))) < 10e-9) { state_(IDX::ACCX) = 0.0; } } @@ -293,7 +292,7 @@ Eigen::VectorXd SimModelTimeDelaySteerAccel::calcModel( const double drive_shift = input(IDX_U::DRIVE_SHIFT); double delay_acc_des = std::max(std::min(delay_input_acc, vx_rate_lim_), -vx_rate_lim_); if (!(drive_shift >= 0.0)) { - delay_acc_des *= -1; // reverse front-back + delay_acc_des *= -1; // reverse front-back } double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); double accx_rate = -(acc - delay_acc_des) / acc_time_constant_; From b96e68b1123480debda52e65f206fdf0c71dfd9d Mon Sep 17 00:00:00 2001 From: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com> Date: Thu, 4 Nov 2021 19:32:37 +0900 Subject: [PATCH 32/52] Add COLCON_IGNORE (#500) Signed-off-by: Kenji Miyake --- simulator/simple_planning_simulator/COLCON_IGNORE | 0 1 file changed, 0 insertions(+), 0 deletions(-) create mode 100644 simulator/simple_planning_simulator/COLCON_IGNORE diff --git a/simulator/simple_planning_simulator/COLCON_IGNORE b/simulator/simple_planning_simulator/COLCON_IGNORE new file mode 100644 index 0000000000000..e69de29bb2d1d From 789899e72d6269d5287523e8757a1f2ebf744114 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Thu, 11 Nov 2021 10:57:43 +0900 Subject: [PATCH 33/52] Back port .auto control packages (#571) * Implement Lateral and Longitudinal Control Muxer * [#570] Porting wf_simulator * [#1189] Deactivate flaky test in 'trajectory_follower_nodes' * [#1189] Fix flacky test in 'trajectory_follower_nodes/latlon_muxer' * [#1057] Add osqp_interface package * [#1057] Add library code for MPC-based lateral control * [#1271] Use std::abs instead of abs * [#1057] Implement Lateral Controller for Cargo ODD * [#1246] Resolve "Test case names currently use snake_case but should be CamelCase" * [#1325] Deactivate flaky smoke test in 'trajectory_follower_nodes' * [#1058] Add library code of longitudinal controller * Fix build error for trajectory follower Signed-off-by: wep21 * Fix build error for trajectory follower nodes Signed-off-by: wep21 * [#1272] Add AckermannControlCommand support to simple_planning_simulator * [#1058] Add Longitudinal Controller node * [#1058] Rename velocity_controller -> longitudinal_controller * [#1058] Update CMakeLists.txt for the longitudinal_controller_node * [#1058] Add smoke test python launch file * [#1058] Use LowPassFilter1d from trajectory_follower * [#1058] Use autoware_auto_msgs * [#1058] Changes for .auto (debug msg tmp fix, common func, tf listener) * [#1058] Remove unused parameters * [#1058] Fix ros test * [#1058] Rm default params from declare_parameters + use autoware types * [#1058] Use default param file to setup NodeOptions in the ros test * [#1058] Fix docstring * [#1058] Replace receiving a Twist with a VehicleKinematicState * [#1058] Change class variables format to m_ prefix * [#1058] Fix plugin name of LongitudinalController in CMakeLists.txt * [#1058] Fix copyright dates * [#1058] Reorder includes * [#1058] Add some tests (~89% coverage without disabling flaky tests) * [#1058] Add more tests (90+% coverage without disabling flaky tests) * [#1058] Use Float32MultiArrayDiagnostic message for debug and slope * [#1058] Calculate wheel_base value from vehicle parameters * [#1058] Cleanup redundant logger setting in tests * [#1058] Set ROS_DOMAIN_ID when running tests to prevent CI failures * [#1058] Remove TF listener and use published vehicle state instead * [#1058] Change smoke tests to use autoware_testing * [#1058] Add plotjuggler cfg for both lateral and longitudinal control * [#1058] Improve design documents * [#1058] Disable flaky test * [#1058] Properly transform vehicle state in longitudinal node * [#1058] Fix TF buffer of lateral controller * [#1058] Tuning of lateral controller for LGSVL * [#1058] Fix formating * [#1058] Fix /tf_static sub to be transient_local * [#1058] Fix yaw recalculation of reverse trajs in the lateral controller * modify trajectory_follower for galactic build Signed-off-by: Takamasa Horibe * [#1379] Update trajectory_follower * [#1379] Update simple_planning_simulator * [#1379] Update trajectory_follower_nodes * apply trajectory msg modification in control Signed-off-by: Takamasa Horibe * move directory Signed-off-by: Takamasa Horibe * remote control/trajectory_follower level dorectpry Signed-off-by: Takamasa Horibe * remove .iv trajectory follower Signed-off-by: Takamasa Horibe * use .auto trajectory_follower Signed-off-by: Takamasa Horibe * remove .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * use .iv simple_planning_simulator & osqp_interface Signed-off-by: Takamasa Horibe * add tmp_autoware_auto_dependencies Signed-off-by: Takamasa Horibe * tmporally add autoware_auto_msgs Signed-off-by: Takamasa Horibe * apply .auto message split Signed-off-by: Takamasa Horibe * fix build depend Signed-off-by: Takamasa Horibe * fix packages using osqp * fix autoware_auto_geometry * ignore lint of some packages * ignore ament_lint of some packages * ignore lint/pre-commit of trajectory_follower_nodes * disable unit tests of some packages Co-authored-by: Maxime CLEMENT Co-authored-by: Joshua Whitley Co-authored-by: Igor Bogoslavskyi Co-authored-by: MIURA Yasuyuki Co-authored-by: wep21 Co-authored-by: tomoya.kimura --- .../simple_planning_simulator/CMakeLists.txt | 70 +-- .../simple_planning_simulator/COLCON_IGNORE | 0 simulator/simple_planning_simulator/README.md | 61 -- ...qt_multiplot_simple_planning_simulator.xml | 317 ---------- .../simple_planning_simulator.param.yaml | 38 -- .../config/simulator_model.param.yaml | 26 - .../simple_planning_simulator-design.md | 122 ++++ .../simple_planning_simulator_core.hpp | 348 +++++------ .../vehicle_model/sim_model.hpp | 26 + .../sim_model_constant_acceleration.hpp | 115 ---- .../sim_model_delay_steer_acc.hpp | 145 +++++ .../sim_model_delay_steer_acc_geared.hpp | 152 +++++ .../vehicle_model/sim_model_ideal.hpp | 256 -------- .../sim_model_ideal_steer_acc.hpp | 113 ++++ .../sim_model_ideal_steer_acc_geared.hpp | 121 ++++ .../sim_model_ideal_steer_vel.hpp | 113 ++++ .../vehicle_model/sim_model_interface.hpp | 71 ++- .../vehicle_model/sim_model_time_delay.hpp | 359 ----------- .../vehicle_model/sim_model_util.hpp | 26 - .../visibility_control.hpp | 40 ++ .../simple_planning_simulator.launch.py | 88 +-- .../simple_planning_simulator.launch.xml | 43 -- .../simple_planning_simulator/package.xml | 32 +- ...mple_planning_simulator_default.param.yaml | 19 + .../param/vehicle_characteristics.param.yaml | 12 + .../scripts/README_fitParamDelayInputModel.md | 33 -- .../scripts/fitParamDelayInputModel.py | 211 ------- .../simple_planning_simulator_core.cpp | 387 ++++++++++++ .../sim_model_delay_steer_acc.cpp | 99 ++++ .../sim_model_delay_steer_acc_geared.cpp | 131 ++++ .../sim_model_ideal_steer_acc.cpp | 52 ++ .../sim_model_ideal_steer_acc_geared.cpp | 82 +++ .../sim_model_ideal_steer_vel.cpp | 52 ++ .../vehicle_model/sim_model_interface.cpp | 20 +- .../src/simple_planning_simulator_core.cpp | 559 ------------------ .../src/simple_planning_simulator_node.cpp | 28 - .../sim_model_constant_acceleration.cpp | 66 --- .../src/vehicle_model/sim_model_ideal.cpp | 108 ---- .../vehicle_model/sim_model_time_delay.cpp | 318 ---------- .../src/vehicle_model/sim_model_util.cpp | 30 - .../test/test_simple_planning_simulator.cpp | 387 ++++++++++++ 41 files changed, 2354 insertions(+), 2922 deletions(-) delete mode 100644 simulator/simple_planning_simulator/COLCON_IGNORE delete mode 100644 simulator/simple_planning_simulator/README.md delete mode 100644 simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml delete mode 100644 simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml delete mode 100644 simulator/simple_planning_simulator/config/simulator_model.param.yaml create mode 100644 simulator/simple_planning_simulator/design/simple_planning_simulator-design.md create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp delete mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp create mode 100644 simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp delete mode 100644 simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml create mode 100644 simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml create mode 100644 simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml delete mode 100644 simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md delete mode 100644 simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp create mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp rename simulator/simple_planning_simulator/src/{ => simple_planning_simulator}/vehicle_model/sim_model_interface.cpp (64%) delete mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp delete mode 100644 simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp delete mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp delete mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp delete mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp delete mode 100644 simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp create mode 100644 simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 4bd10674eb137..50b5630d60b5c 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -1,51 +1,53 @@ -cmake_minimum_required(VERSION 3.5) -project(simple_planning_simulator) +cmake_minimum_required(VERSION 3.6) -### Compile options -if(NOT CMAKE_CXX_STANDARD) - set(CMAKE_CXX_STANDARD 14) -endif() -if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") - add_compile_options(-Wall -Wextra -Wpedantic) -endif() +project(simple_planning_simulator) ### Dependencies find_package(ament_cmake_auto REQUIRED) ament_auto_find_build_dependencies() -# Add path of include dir -include_directories(include) - -# Generate exe file -set(SIMPLE_PLANNING_SIMULATOR_SRC - src/simple_planning_simulator_core.cpp - src/vehicle_model/sim_model_interface.cpp - src/vehicle_model/sim_model_ideal.cpp - src/vehicle_model/sim_model_constant_acceleration.cpp - src/vehicle_model/sim_model_time_delay.cpp - src/vehicle_model/sim_model_util.cpp -) -ament_auto_add_executable(simple_planning_simulator_exe src/simple_planning_simulator_node.cpp ${SIMPLE_PLANNING_SIMULATOR_SRC}) -## Install executables and/or libraries -install(TARGETS simple_planning_simulator_exe - ARCHIVE DESTINATION lib/${PROJECT_NAME} - LIBRARY DESTINATION lib/${PROJECT_NAME} - RUNTIME DESTINATION lib/${PROJECT_NAME} +# Component +ament_auto_add_library(${PROJECT_NAME} SHARED + include/simple_planning_simulator/simple_planning_simulator_core.hpp + include/simple_planning_simulator/visibility_control.hpp + src/simple_planning_simulator/simple_planning_simulator_core.cpp + src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp + src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp + src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp + src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp + src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp + src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp ) +target_include_directories(${PROJECT_NAME} SYSTEM PUBLIC ${tf2_INCLUDE_DIRS}) +autoware_set_compile_options(${PROJECT_NAME}) + +target_compile_options(${PROJECT_NAME} PRIVATE -Wno-old-style-cast) # RCLCPP_ERROR_THROTTLE() has built-in old-style casts. -## Install project namespaced headers -install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION include) -install(DIRECTORY config launch - DESTINATION share/${PROJECT_NAME} +# Node executable +rclcpp_components_register_node(${PROJECT_NAME} + PLUGIN "simulation::simple_planning_simulator::SimplePlanningSimulator" + EXECUTABLE ${PROJECT_NAME}_exe ) + +### Test if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() + + # Unit test + ament_add_gtest( + simple_planning_simulator_unit_tests + test/test_simple_planning_simulator.cpp + TIMEOUT 120) + autoware_set_compile_options(simple_planning_simulator_unit_tests) + target_link_libraries(simple_planning_simulator_unit_tests ${PROJECT_NAME}) + target_include_directories(simple_planning_simulator_unit_tests PRIVATE "include") endif() -# set at the end of cmakelists -ament_auto_package() + + + +ament_auto_package(INSTALL_TO_SHARE param launch) diff --git a/simulator/simple_planning_simulator/COLCON_IGNORE b/simulator/simple_planning_simulator/COLCON_IGNORE deleted file mode 100644 index e69de29bb2d1d..0000000000000 diff --git a/simulator/simple_planning_simulator/README.md b/simulator/simple_planning_simulator/README.md deleted file mode 100644 index 2e2e1301a93bf..0000000000000 --- a/simulator/simple_planning_simulator/README.md +++ /dev/null @@ -1,61 +0,0 @@ -# Simple Planning Simulator - -This node simulates the vehicle motion for a vehicle command in 2D using a simple vehicle model. - -## Interface - -### input - -- /control/vehicle_cmd [`autoware_vehicle_msgs/VehicleCommand`] : target command to drive a vehicle. -- /initialpose [`geometry_msgs/PoseWithCovarianceStamped`] : for initial pose -- /initialtwist [`geometry_msgs/TwistStamped`] : for initial velocity -- /planning/scenario_planning/trajectory [`autoware_planning_msgs/Trajectory`]: for z position -- /vehicle/engage : if true, the vehicle starts to move. if false, stops. - -### output - -- /tf [`tf2_msgs/TFMessage`] : simulated vehicle pose (base_link) -- /vehicle/status/control_mode [`autoware_vehicle_msgs/ControlMode`] : current control mode (Auto/Manual). -- /vehicle/status/shift [`autoware_vehicle_msgs/ShiftStamped`] : current shift (Fwd/Rev) -- /vehicle/status/steering [`autoware_vehicle_msgs/Steering`] : Simulated steering angle -- /vehicle/status/turn_signal [`autoware_vehicle_msgs/TurnSignal`] : current turn signal (just published with NONE status.) -- /vehicle/status/twist [`geometry_msgs/TwistStamped`] : simulated velocity - -## Parameter Description - -### Common Parameters - -| Name | Type | Description | Default value | -| :-------------------- | :----- | :------------------------------------------------------------- | :------------ | -| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true | -| pos_noise_stddev | double | Standard deviation for position noise | 0.01 | -| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 | -| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 | -| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 | -| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 | -| initial_engage_state | double | If false, the engage command is needed to move the vehicle. | true | - -### Vehicle Model Parameters - -#### vehicle_model_type options - -- `IDEAL_STEER`: uses velocity command. The steering and velocity varies ideally as commanded. The pose is calculated by a bicycle kinematics model. -- `IDEAL_ACCEL`: uses acceleration command. The steering and acceleration varies ideally as commanded. The pose is calculated by a bicycle kinematics model. -- `DELAY_STEER` : uses velocity command. The steering and velocity varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. -- `DELAY_STEER_ACC` : uses acceleration command. The steering and acceleration varies following a 1st-order delay model. The pose is calculated by a bicycle kinematics model. - -| Name | Type | Description | IDEAL STEER | IDEAL ACCEL | DELAY STEER | DELAY STEER 　 ACC | Default value | unit | -| :------------------- | :----- | :--------------------------------------------------- | :---------- | :---------- | :---------- | :----------------- | :------------ | :------ | -| vel_time_delay | double | dead time for the velocity input | x | x | o | x | 0.25 | [s] | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | o | o | 0.24 | [s] | -| vel_time_constant | double | time constant of the 1st-order velocity dynamics | x | x | o | x | 0.61 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | o | o | 0.27 | [s] | -| vel_lim | double | limit of velocity | x | x | o | o | 50.0 | [m/s] | -| accel_rate | double | limit of acceleration | x | x | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | o | o | 5.0 | [rad/s] | -| deadzone_delta_steer | double | dead zone for the steering dynamics | x | x | o | o | 0.0 | [rad] | - -_Note_: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a _delay_ model. The definition of the _time constant_ is the time it takes for the step response to rise up to 63% of its final value. The _deadtime_ is a delay in the response to a control input. diff --git a/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml b/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml deleted file mode 100644 index d75f6f4383a44..0000000000000 --- a/simulator/simple_planning_simulator/config/rqt_multiplot_simple_planning_simulator.xml +++ /dev/null @@ -1,317 +0,0 @@ - - - - #242424 - #9e9e9e - false - false - - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /sim_velocity - geometry_msgs/TwistStamped - - - twist/linear/x - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /sim_velocity - geometry_msgs/TwistStamped - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - sim - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_cmd - autoware_vehicle_msgs/VehicleCommandStamped - - - ctrl_cmd/linear_velocity - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_cmd - autoware_vehicle_msgs/VehicleCommandStamped - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - ctrl_cmd - - - - true - - 30 - ctrl_cmd : vx - - - - - - - - Untitled Axis - 0 - true - - - Untitled Axis - 0 - true - - - - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /sim_vehicle_status - autoware_msgs/VehicleStatus - - - angle - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /sim_vehicle_status - autoware_msgs/VehicleStatus - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - sim - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_cmd - autoware_vehicle_msgs/VehicleCommandStamped - - - ctrl_cmd/steering_angle - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_cmd - autoware_vehicle_msgs/VehicleCommandStamped - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - steer_cmd - - - - - - 1 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_status - autoware_msgs/VehicleStatus - - - angle - 0 - - 1000 - 0 - 0 - -1000 - 0 - - /vehicle_status - autoware_msgs/VehicleStatus - - - - #000000 - 0 - - - 1000 - 15 - 3 - - - 100 - actual - - - - true - - 30 - steer - - - - false -
- diff --git a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml b/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml deleted file mode 100644 index 25ab6c8c52903..0000000000000 --- a/simulator/simple_planning_simulator/config/simple_planning_simulator.param.yaml +++ /dev/null @@ -1,38 +0,0 @@ -simulation: - simple_planning_simulator: - ros__parameters: - loop_rate: 50.0 - simulation_frame_id: base_link - map_frame_id: map - initialize_source: RVIZ - use_trajectory_for_z_position_source: true - - acc_time_constant: 0.1 - acc_time_delay: 0.1 - accel_rate: 7.0 - add_measurement_noise: true - angvel_lim: 3.0 - angvel_noise_stddev: 0.0 - angvel_rate: 1.0 - angvel_time_constant: 0.5 - angvel_time_delay: 0.2 - initial_engage_state: true - pos_noise_stddev: 0.01 - rpy_noise_stddev: 0.0001 - x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate - y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate - sim_steering_gear_ratio: 15.0 - steer_lim: 1.0 - steer_noise_stddev: 0.0001 - steer_rate_lim: 5.0 - steer_time_constant: 0.27 - steer_time_delay: 0.24 - tread_length: 1.0 - - # See readme for vehicle model type options - vehicle_model_type: DELAY_STEER_ACC - vel_lim: 50.0 - vel_noise_stddev: 0.0 - vel_time_constant: 0.61 - vel_time_delay: 0.25 - deadzone_delta_steer: 0.00 diff --git a/simulator/simple_planning_simulator/config/simulator_model.param.yaml b/simulator/simple_planning_simulator/config/simulator_model.param.yaml deleted file mode 100644 index ed8c9d3d0a089..0000000000000 --- a/simulator/simple_planning_simulator/config/simulator_model.param.yaml +++ /dev/null @@ -1,26 +0,0 @@ -acc_time_constant: 0.1 -acc_time_delay: 0.1 -accel_rate: 7.0 -add_measurement_noise: true -angvel_lim: 3.0 -angvel_noise_stddev: 0.0 -angvel_rate: 1.0 -angvel_time_constant: 0.5 -angvel_time_delay: 0.2 -initial_engage_state: true -pos_noise_stddev: 0.01 -rpy_noise_stddev: 0.0001 -sim_steering_gear_ratio: 15.0 -steer_lim: 1.0 -steer_noise_stddev: 0.0001 -steer_rate_lim: 5.0 -steer_time_constant: 0.27 -steer_time_delay: 0.24 -tread_length: 1.0 - -# See readme for vehicle model type options -vehicle_model_type: DELAY_STEER_ACC -vel_lim: 50.0 -vel_noise_stddev: 0.0 -vel_time_constant: 0.61 -vel_time_delay: 0.25 diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md new file mode 100644 index 0000000000000..a2e34cf6c7cc6 --- /dev/null +++ b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md @@ -0,0 +1,122 @@ +simple_planning_simulator {#simple_planning_simulator-package-design} +=========== + + +# Purpose / Use cases + +This node simulates the vehicle motion for a vehicle command in 2D using a simple vehicle model. + + + +# Design + + +The purpose of this simulator is for the integration test of planning and control modules. This does not simulate sensing or perception, but is implemented in pure c++ only and works without GPU. + +## Assumptions / Known limits + + - It simulates only in 2D motion. + - It does not perform physical operations such as collision and sensing, but only calculates the integral results of vehicle dynamics. + + +## Inputs / Outputs / API + + +**input** + - input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle. + - input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle. + - input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear). + - /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose + +**output** + - /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link) + - /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM) + - /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.) + + +## Inner-workings / Algorithms + +### Common Parameters + +|Name|Type|Description|Default value| +|:---|:---|:---|:---| +|simulated_frame_id | string | set to the child_frame_id in output tf |"base_link"| +|origin_frame_id | string | set to the frame_id in output tf |"odom"| +|initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | "INITIAL_POSE_TOPIC" | +|add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results.| true| +|pos_noise_stddev | double | Standard deviation for position noise | 0.01| +|rpy_noise_stddev | double | Standard deviation for Euler angle noise| 0.0001| +|vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0| +|angvel_noise_stddev | double | Standard deviation for angular velocity noise| 0.0| +|steer_noise_stddev | double | Standard deviation for steering angle noise| 0.0001| + + + +### Vehicle Model Parameters + + +**vehicle_model_type options** + + - `IDEAL_STEER_VEL` + - `IDEAL_STEER_ACC` + - `IDEAL_STEER_ACC_GEARED` + - `DELAY_STEER_ACC` + - `DELAY_STEER_ACC_GEARED` + +The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. + +The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). + + +|Name|Type|Description|I_ST_V|I_ST_A|I_ST_A_G|D_ST_A|D_ST_A_G|Default value| unit | +|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---| +|acc_time_delay | double | dead time for the acceleration input | x | x | x | o | o | 0.1 | [s] | +|steer_time_delay | double | dead time for the steering input | x | x | x | o | o | 0.24| [s] | +|acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | o | o | 0.1 | [s] | +|steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | 0.27| [s] | +|vel_lim | double | limit of velocity | x | x | x | o | o | 50.0| [m/s] | +|vel_rate_lim | double | limit of acceleration | x | x | x | o | o | 7.0 | [m/ss] | +|steer_lim | double | limit of steering angle | x | x | x | o | o | 1.0 | [rad] | +|steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | 5.0 | [rad/s] | + + + +*Note*: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a *delay* model. The definition of the *time constant* is the time it takes for the step response to rise up to 63% of its final value. The *deadtime* is a delay in the response to a control input. + + +### Default TF configuration + +Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. +In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. + + +## Error detection and handling + +The only validation on inputs being done is testing for a valid vehicle model type. + + +# Security considerations + + + + +# References / External links +This is originally developed in the Autoware.AI. See the link below. + +https://github.com/Autoware-AI/simulation/tree/master/wf_simulator + +# Future extensions / Unimplemented parts + + - Improving the accuracy of vehicle models (e.g., adding steering dead zones and slip behavior) + - Cooperation with modules that output pseudo pointcloud or pseudo perception results + + +# Related issues + + - #1142: Follow-up to #570 - Integrate simple_planning_simulator Into CI diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index b78af263f8025..bf6caff5f0131 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -1,10 +1,10 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. +// 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 +//    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, @@ -12,285 +12,227 @@ // See the License for the specific language governing permissions and // limitations under the License. -/** - * @file simple_planning_simulator_core.hpp - * @brief vehicle dynamics simulation for autoware - * @author Takamasa Horibe - * @date 2019.08.17 - */ - #ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ -#include "simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp" - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include + #include -#include #include #include -#include #include -#include +#include + +#include "rclcpp/rclcpp.hpp" + +#include "simple_planning_simulator/visibility_control.hpp" + +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/twist_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" -class Simulator : public rclcpp::Node +#include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_state_report.hpp" +#include "autoware_auto_geometry_msgs/msg/complex32.hpp" +#include "common/types.hpp" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + + +namespace simulation +{ +namespace simple_planning_simulator +{ +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; + +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_vehicle_msgs::msg::VehicleKinematicState; +using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; +using autoware_auto_vehicle_msgs::msg::VehicleStateReport; +using autoware_auto_vehicle_msgs::msg::VehicleStateCommand; +using geometry_msgs::msg::TransformStamped; +using geometry_msgs::msg::PoseWithCovarianceStamped; +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::Pose; +using geometry_msgs::msg::Twist; +using autoware_auto_geometry_msgs::msg::Complex32; + +class DeltaTime { public: - /** - * @brief constructor - */ - explicit Simulator(const std::string & node_name, const rclcpp::NodeOptions & options); + DeltaTime() + : prev_updated_time_ptr_(nullptr) {} + float64_t get_dt(const rclcpp::Time & now) + { + if (prev_updated_time_ptr_ == nullptr) { + prev_updated_time_ptr_ = std::make_shared(now); + return 0.0; + } + const float64_t dt = (now - *prev_updated_time_ptr_).seconds(); + *prev_updated_time_ptr_ = now; + return dt; + } - /** - * @brief default destructor - */ - // ~Simulator() = default; +private: + std::shared_ptr prev_updated_time_ptr_; +}; + +class MeasurementNoiseGenerator +{ +public: + MeasurementNoiseGenerator() {} + + std::shared_ptr rand_engine_; + std::shared_ptr> pos_dist_; + std::shared_ptr> vel_dist_; + std::shared_ptr> rpy_dist_; + std::shared_ptr> steer_dist_; +}; + +class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node +{ +public: + explicit SimplePlanningSimulator(const rclcpp::NodeOptions & options); private: /* ros system */ - rclcpp::CallbackGroup::SharedPtr group_api_service_; - autoware_api_utils::Service::SharedPtr - srv_set_pose_; //!< @brief service to set pose for simulation - - rclcpp::Publisher::SharedPtr - pub_pose_; //!< @brief topic ros publisher for current pose - rclcpp::Publisher::SharedPtr - pub_twist_; //!< @brief topic ros publisher for current twist - rclcpp::Publisher::SharedPtr pub_steer_; - rclcpp::Publisher::SharedPtr pub_velocity_; - rclcpp::Publisher::SharedPtr pub_turn_signal_; - rclcpp::Publisher::SharedPtr pub_shift_; - rclcpp::Publisher::SharedPtr pub_control_mode_; - rclcpp::Publisher::SharedPtr pub_cov_; - - rclcpp::Subscription::SharedPtr - sub_vehicle_cmd_; //!< @brief topic subscriber for vehicle_cmd - rclcpp::Subscription::SharedPtr - sub_turn_signal_cmd_; //!< @brief topic subscriber for turn_signal_cmd - rclcpp::Subscription::SharedPtr - sub_trajectory_; //!< @brief topic subscriber for trajectory used for z position - rclcpp::Subscription::SharedPtr - sub_initialpose_; //!< @brief topic subscriber for initialpose topic - rclcpp::Subscription::SharedPtr - sub_initialtwist_; //!< @brief topic subscriber for initialtwist topic - rclcpp::Subscription::SharedPtr - sub_engage_; //!< @brief topic subscriber for engage topic - rclcpp::TimerBase::SharedPtr timer_simulation_; //!< @brief timer for simulation - - OnSetParametersCallbackHandle::SharedPtr set_param_res_; - rcl_interfaces::msg::SetParametersResult onParameter( - const std::vector & parameters); + rclcpp::Publisher::SharedPtr pub_kinematic_state_; + rclcpp::Publisher::SharedPtr pub_state_report_; + rclcpp::Publisher::SharedPtr pub_tf_; + rclcpp::Publisher::SharedPtr pub_current_pose_; + + rclcpp::Subscription::SharedPtr sub_state_cmd_; + rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; + rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; + rclcpp::Subscription::SharedPtr sub_init_pose_; + + uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time + rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation /* tf */ - std::shared_ptr tf_broadcaster_; - std::shared_ptr tf_listener_; - std::shared_ptr tf_buffer_; + tf2_ros::Buffer tf_buffer_; + tf2_ros::TransformListener tf_listener_; /* received & published topics */ - geometry_msgs::msg::PoseStamped::ConstSharedPtr - initial_pose_ptr_; //!< @brief initial vehicle pose - geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr - initial_pose_with_cov_ptr_; //!< @brief initial vehicle pose with cov - geometry_msgs::msg::TwistStamped::ConstSharedPtr - initial_twist_ptr_; //!< @brief initial vehicle velocity - geometry_msgs::msg::Pose current_pose_; //!< @brief current vehicle position and angle - geometry_msgs::msg::Twist current_twist_; //!< @brief current vehicle velocity - autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr - current_vehicle_cmd_ptr_; //!< @brief latest received vehicle_cmd - autoware_planning_msgs::msg::Trajectory::ConstSharedPtr - current_trajectory_ptr_; //!< @brief latest received trajectory - double closest_pos_z_; //!< @brief z position on closest trajectory - autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr current_turn_signal_cmd_ptr_; - autoware_vehicle_msgs::msg::ControlMode control_mode_; + VehicleKinematicState current_kinematic_state_; + VehicleControlCommand::ConstSharedPtr current_vehicle_cmd_ptr_; + AckermannControlCommand::ConstSharedPtr current_ackermann_cmd_ptr_; + VehicleStateCommand::ConstSharedPtr current_vehicle_state_cmd_ptr_; /* frame_id */ - std::string - simulation_frame_id_; //!< @brief vehicle frame id simulated by simple_planning_simulator - std::string map_frame_id_; //!< @brief map frame_id - - /* simple_planning_simulator parameters */ - double loop_rate_; //!< @brief frequency to calculate vehicle model & publish result - double wheelbase_; //!< @brief wheelbase length to convert angular-velocity & steering - double sim_steering_gear_ratio_; //!< @brief for steering wheel angle calculation - double x_stddev_; //!< @brief x standard deviation for dummy covariance in map coordinate - double y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate + std::string simulated_frame_id_; //!< @brief simulated vehicle frame id + std::string origin_frame_id_; //!< @brief map frame_id /* flags */ - bool is_initialized_ = false; //!< @brief flag to check the initial position is set - bool add_measurement_noise_; //!< @brief flag to add measurement noise - bool simulator_engage_; //!< @brief flag to engage simulator - bool use_trajectory_for_z_position_source_; //!< @brief flag to get z position from trajectory + bool8_t is_initialized_; //!< @brief flag to check the initial position is set + bool8_t add_measurement_noise_; //!< @brief flag to add measurement noise + + DeltaTime delta_time_; //!< @brief to calculate delta time + + MeasurementNoiseGenerator measurement_noise_; //!< @brief for measurement noise + + float32_t cg_to_rear_m_; //!< @brief length from baselink to CoM - /* saved values */ - std::shared_ptr prev_update_time_ptr_; //!< @brief previously updated time /* vehicle model */ - enum class VehicleModelType { - IDEAL_TWIST = 0, - IDEAL_STEER = 1, - DELAY_TWIST = 2, - DELAY_STEER = 3, - CONST_ACCEL_TWIST = 4, - IDEAL_FORKLIFT_RLS = 5, - DELAY_FORKLIFT_RLS = 6, - IDEAL_ACCEL = 7, - DELAY_STEER_ACC = 8, + enum class VehicleModelType + { + IDEAL_STEER_ACC = 0, + IDEAL_STEER_ACC_GEARED = 1, + DELAY_STEER_ACC = 2, + DELAY_STEER_ACC_GEARED = 3, + IDEAL_STEER_VEL = 4, } vehicle_model_type_; //!< @brief vehicle model type to decide the model dynamics std::shared_ptr vehicle_model_ptr_; //!< @brief vehicle model pointer - /* to generate measurement noise */ - std::shared_ptr rand_engine_ptr_; //!< @brief random engine for measurement noise - std::shared_ptr> - pos_norm_dist_ptr_; //!< @brief Gaussian noise for position - std::shared_ptr> - vel_norm_dist_ptr_; //!< @brief Gaussian noise for velocity - std::shared_ptr> - rpy_norm_dist_ptr_; //!< @brief Gaussian noise for roll-pitch-yaw - std::shared_ptr> - angvel_norm_dist_ptr_; //!< @brief Gaussian noise for angular velocity - std::shared_ptr> - steer_norm_dist_ptr_; //!< @brief Gaussian noise for steering angle - /** * @brief set current_vehicle_cmd_ptr_ with received message */ - void callbackVehicleCmd(const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg); + void on_vehicle_cmd(const VehicleControlCommand::ConstSharedPtr msg); /** - * @brief set current_turn_signal_cmd_ptr with received message + * @brief set current_ackermann_cmd_ptr_ with received message */ - void callbackTurnSignalCmd(const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg); + void on_ackermann_cmd(const AckermannControlCommand::ConstSharedPtr msg); /** - * @brief set current_trajectory_ptr_ with received message + * @brief set input steering, velocity, and acceleration of the vehicle model */ - void callbackTrajectory(const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg); + void set_input(const float steer, const float vel, const float accel); - // TODO(Takagi, Isamu): deprecated /** - * @brief set initial pose for simulation with received message + * @brief set current_vehicle_state_ with received message */ - void callbackInitialPoseWithCov( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg); + void on_state_cmd(const VehicleStateCommand::ConstSharedPtr msg); - // TODO(Takagi, Isamu): deprecated /** - * @brief set initial pose with received message + * @brief set initial pose for simulation with received message */ - void callbackInitialPoseStamped(const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg); + void on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg); /** - * @brief set initial twist with received message + * @brief get transform from two frame_ids + * @param [in] parent_frame parent frame id + * @param [in] child_frame child frame id + * @return transform from parent frame to child frame */ - void callbackInitialTwistStamped(const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg); + TransformStamped get_transform_msg(const std::string parent_frame, const std::string child_frame); /** - * @brief set simulator engage with received message + * @brief timer callback for simulation with loop_rate */ - void callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg); + void on_timer(); /** - * @brief set pose for simulation with request + * @brief initialize vehicle_model_ptr */ - void serviceSetPose( - const autoware_external_api_msgs::srv::InitializePose::Request::SharedPtr request, - const autoware_external_api_msgs::srv::InitializePose::Response::SharedPtr response); + void initialize_vehicle_model(); /** - * @brief get transform from two frame_ids - * @param [in] parent_frame parent frame id - * @param [in] child frame id - * @param [out] transform transform from parent frame to child frame + * @brief add measurement noise */ - void getTransformFromTF( - const std::string parent_frame, const std::string child_frame, - geometry_msgs::msg::TransformStamped & transform); - - /** - * @brief timer callback for simulation with loop_rate - */ - void timerCallbackSimulation(); + void add_measurement_noise(VehicleKinematicState & state) const; /** * @brief set initial state of simulated vehicle * @param [in] pose initial position and orientation * @param [in] twist initial velocity and angular velocity */ - void setInitialState( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist); - - /** - * @brief set initial state of simulated vehicle with pose transformation based on frame_id - * @param [in] pose initial position and orientation with header - * @param [in] twist initial velocity and angular velocity - */ - void setInitialStateWithPoseTransform( - const geometry_msgs::msg::PoseStamped & pose, const geometry_msgs::msg::Twist & twist); + void set_initial_state(const Pose & pose, const Twist & twist); /** * @brief set initial state of simulated vehicle with pose transformation based on frame_id * @param [in] pose initial position and orientation with header * @param [in] twist initial velocity and angular velocity */ - void setInitialStateWithPoseTransform( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::Twist & twist); + void set_initial_state_with_transform(const PoseStamped & pose, const Twist & twist); /** - * @brief publish pose_with_covariance - * @param [in] cov pose with covariance to be published + * @brief publish pose and twist + * @param [in] state The kinematic state to publish */ - void publishPoseWithCov(const geometry_msgs::msg::PoseWithCovariance & cov); + void publish_kinematic_state(const VehicleKinematicState & state); /** - * @brief publish twist - * @param [in] twist twist to be published + * @brief publish vehicle state report */ - void publishTwist(const geometry_msgs::msg::Twist & twist); + void publish_state_report(); /** * @brief publish tf - * @param [in] pose pose used for tf - */ - void publishTF(const geometry_msgs::msg::Pose & pose); - - /** - * @brief update closest pose to calculate pos_z - */ - double getPosZFromTrajectory(const double x, const double y); - - /** - * @brief convert roll-pitch-yaw Euler angle to ros Quaternion - * @param [in] roll roll angle [rad] - * @param [in] pitch pitch angle [rad] - * @param [in] yaw yaw angle [rad] + * @param [in] state The kinematic state to publish as a TF */ - geometry_msgs::msg::Quaternion getQuaternionFromRPY( - const double & roll, const double & pitch, const double & yaw); + void publish_tf(const VehicleKinematicState & state); }; +} // namespace simple_planning_simulator +} // namespace simulation #endif // SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp new file mode 100644 index 0000000000000..ef72b9e1f2c2c --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -0,0 +1,26 @@ +// 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_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" + + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp deleted file mode 100644 index b1af6152ecddd..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_constant_acceleration.hpp +++ /dev/null @@ -1,115 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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. - -/** - * @file sim_model_constant_acceleration.hpp - * @brief simple planning simulator model with constant acceleration for velocity & steering - * @author Takamasa Horibe - * @date 2019.08.17 - */ - -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_CONSTANT_ACCELERATION_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_CONSTANT_ACCELERATION_HPP_ - -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - -#include -#include - -#include - -/** - * @class simple_planning_simulator constant acceleration twist model - * @brief calculate velocity & angular-velocity with constant acceleration - */ -class SimModelConstantAccelTwist : public SimModelInterface -{ -public: - /** - * @brief constructor - * @param [in] vx_lim velocity limit [m/s] - * @param [in] wz_lim angular velocity limit [m/s] - * @param [in] vx_rate acceleration for velocity [m/ss] - * @param [in] wz_rate acceleration for angular velocity [rad/ss] - */ - SimModelConstantAccelTwist(double vx_lim, double wz_lim, double vx_rate, double wz_rate); - - /** - * @brief default destructor - */ - ~SimModelConstantAccelTwist() = default; - -private: - enum IDX { - X = 0, - Y, - YAW, - VX, - WZ, - }; - enum IDX_U { - VX_DES = 0, - WZ_DES, - }; - - const double vx_lim_; //!< @brief velocity limit - const double wz_lim_; //!< @brief angular velocity limit - const double vx_rate_; //!< @brief velocity rate - const double wz_rate_; //!< @brief angular velocity rate - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with constant acceleration - * @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_CONSTANT_ACCELERATION_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp new file mode 100644 index 0000000000000..f0a610c54eb0f --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -0,0 +1,145 @@ +// 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_ACC_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ + +#include +#include +#include + +#include "eigen3/Eigen/LU" +#include "eigen3/Eigen/Core" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +class SimModelDelaySteerAcc : 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] acc_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + */ + SimModelDelaySteerAcc( + 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 acc_delay, float64_t acc_time_constant, + float64_t steer_delay, float64_t steer_time_constant); + + /** + * @brief default destructor + */ + ~SimModelDelaySteerAcc() = default; + +private: + const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX + { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U + { + ACCX_DES = 0, + STEER_DES, + DRIVE_SHIFT, + }; + + const float64_t vx_lim_; //!< @brief velocity limit [m/s] + const float64_t vx_rate_lim_; //!< @brief acceleration limit [m/ss] + 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] + + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const float64_t acc_delay_; //!< @brief time delay for accel command [s] + const float64_t acc_time_constant_; //!< @brief time constant for accel dynamics + const float64_t steer_delay_; //!< @brief time delay for steering command [s] + const float64_t steer_time_constant_; //!< @brief time constant for steering 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 velocity vx + */ + float64_t getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + float64_t getVy() override; + + /** + * @brief get vehicle longiudinal 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 time 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_ACC_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp new file mode 100644 index 0000000000000..c9bcf1bb32e6f --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -0,0 +1,152 @@ +// 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_ACC_GEARED_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ + +#include +#include +#include + +#include "eigen3/Eigen/LU" +#include "eigen3/Eigen/Core" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +class SimModelDelaySteerAccGeared : 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] acc_delay time delay for accel command [s] + * @param [in] acc_time_constant time constant for 1D model of accel dynamics + * @param [in] steer_delay time delay for steering command [s] + * @param [in] steer_time_constant time constant for 1D model of steering dynamics + */ + SimModelDelaySteerAccGeared( + 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 acc_delay, float64_t acc_time_constant, + float64_t steer_delay, float64_t steer_time_constant); + + /** + * @brief default destructor + */ + ~SimModelDelaySteerAccGeared() = default; + +private: + const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant + + enum IDX + { + X = 0, + Y, + YAW, + VX, + STEER, + ACCX, + }; + enum IDX_U + { + ACCX_DES = 0, + STEER_DES, + DRIVE_SHIFT, + }; + + const float64_t vx_lim_; //!< @brief velocity limit [m/s] + const float64_t vx_rate_lim_; //!< @brief acceleration limit [m/ss] + 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] + + std::deque acc_input_queue_; //!< @brief buffer for accel command + std::deque steer_input_queue_; //!< @brief buffer for steering command + const float64_t acc_delay_; //!< @brief time delay for accel command [s] + const float64_t acc_time_constant_; //!< @brief time constant for accel dynamics + const float64_t steer_delay_; //!< @brief time delay for steering command [s] + const float64_t steer_time_constant_; //!< @brief time constant for steering 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 velocity vx + */ + float64_t getVx() override; + + /** + * @brief get vehicle lateral velocity + */ + float64_t getVy() override; + + /** + * @brief get vehicle longiudinal 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 time 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; + + /** + * @brief calculate velocity with considering current velocity and gear + * @param [in] state current state + * @param [in] gear current gear (defined in autoware_auto_msgs/VehicleStateCommand) + */ + float64_t calcVelocityWithGear(const Eigen::VectorXd & state, const uint8_t gear) const; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp deleted file mode 100644 index 1cfc43c1e15c3..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal.hpp +++ /dev/null @@ -1,256 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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. - -/** - * @file sim_model_ideal.hpp - * @brief simple planning simulator ideal velocity model (no dynamics for desired velocity & - * angular-velocity or steering) - * @author Takamasa Horibe - * @date 2019.08.17 - */ - -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_HPP_ - -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - -#include -#include - -#include - -/** - * @class simple_planning_simulator ideal twist model - * @brief calculate ideal twist dynamics - */ -class SimModelIdealTwist : public SimModelInterface -{ -public: - /** - * @brief constructor - */ - SimModelIdealTwist(); - - /** - * @brief destructor - */ - ~SimModelIdealTwist() = default; - -private: - enum IDX { - X = 0, - Y, - YAW, - }; - enum IDX_U { - VX_DES = 0, - WZ_DES, - }; - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with ideal twist 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; -}; - -/** - * @class simple_planning_simulator ideal steering model - * @brief calculate ideal steering dynamics - */ -class SimModelIdealSteer : public SimModelInterface -{ -public: - /** - * @brief constructor - * @param [in] wheelbase vehicle wheelbase length [m] - */ - explicit SimModelIdealSteer(double wheelbase); - - /** - * @brief destructor - */ - ~SimModelIdealSteer() = default; - -private: - enum IDX { - X = 0, - Y, - YAW, - }; - enum IDX_U { - VX_DES = 0, - STEER_DES, - }; - - const double wheelbase_; //!< @brief vehicle wheelbase length - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with ideal 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; -}; - -/** - * @class wf_simulator ideal acceleration and steering model - * @brief calculate ideal steering dynamics - */ -class SimModelIdealAccel : public SimModelInterface -{ -public: - /** - * @brief constructor - * @param [in] wheelbase vehicle wheelbase length [m] - */ - explicit SimModelIdealAccel(double wheelbase); - - /** - * @brief destructor - */ - ~SimModelIdealAccel() = default; - -private: - enum IDX { - X = 0, - Y, - YAW, - VX, - }; - enum IDX_U { - AX_DES = 0, - STEER_DES, - }; - - const double wheelbase_; //!< @brief vehicle wheelbase length - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with ideal 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_IDEAL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp new file mode 100644 index 0000000000000..c4aca56e5bd51 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp @@ -0,0 +1,113 @@ +// 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_IDEAL_STEER_ACC_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +/** + * @class SimModelIdealSteerAcc + * @brief calculate ideal steering dynamics + */ +class SimModelIdealSteerAcc : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + explicit SimModelIdealSteerAcc(float64_t wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealSteerAcc() = default; + +private: + enum IDX + { + X = 0, + Y, + YAW, + VX + }; + enum IDX_U + { + AX_DES = 0, + STEER_DES, + }; + + const float64_t wheelbase_; //!< @brief vehicle wheelbase length + + /** + * @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 longiudinal 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 ideal 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_IDEAL_STEER_ACC_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp new file mode 100644 index 0000000000000..8b74c5f1baf19 --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -0,0 +1,121 @@ +// 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_IDEAL_STEER_ACC_GEARED_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +/** + * @class SimModelIdealSteerAccGeared + * @brief calculate ideal steering dynamics + */ +class SimModelIdealSteerAccGeared : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + explicit SimModelIdealSteerAccGeared(float64_t wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealSteerAccGeared() = default; + +private: + enum IDX + { + X = 0, + Y, + YAW, + VX + }; + enum IDX_U + { + AX_DES = 0, + STEER_DES, + }; + + const float64_t wheelbase_; //!< @brief vehicle wheelbase length + float64_t current_acc_; //!< @brief current_acc with gear consideration + + /** + * @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 longiudinal 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 ideal 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; + + /** + * @brief calculate velocity with considering current velocity and gear + * @param [in] state current state + * @param [in] gear current gear (defined in autoware_auto_msgs/VehicleStateCommand) + */ + float64_t calcVelocityWithGear(const Eigen::VectorXd & state, const uint8_t gear) const; +}; + +#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp new file mode 100644 index 0000000000000..dae5f0fab633c --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp @@ -0,0 +1,113 @@ +// 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_IDEAL_STEER_VEL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ + +#include + +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +/** + * @class SimModelIdealSteerVel + * @brief calculate ideal steering dynamics + */ +class SimModelIdealSteerVel : public SimModelInterface +{ +public: + /** + * @brief constructor + * @param [in] wheelbase vehicle wheelbase length [m] + */ + explicit SimModelIdealSteerVel(float64_t wheelbase); + + /** + * @brief destructor + */ + ~SimModelIdealSteerVel() = default; + +private: + enum IDX + { + X = 0, + Y, + YAW + }; + enum IDX_U + { + VX_DES = 0, + STEER_DES, + }; + + const float64_t wheelbase_; //!< @brief vehicle wheelbase length + float64_t prev_vx_ = 0.0; + float64_t current_ax_ = 0.0; + + /** + * @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 longiudinal 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 ideal 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_IDEAL_STEER_VEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index 4250e02b12727..bdcafdda14de5 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -1,10 +1,10 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. +// 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 +//    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, @@ -12,22 +12,20 @@ // See the License for the specific language governing permissions and // limitations under the License. -/** - * @file sim_model_interface.hpp - * @brief simple planning simulator model interface class - * @author Takamasa Horibe - * @date 2019.08.17 - */ - #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ -#include -#include +#include "eigen3/Eigen/Core" +#include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" +#include "common/types.hpp" + +using autoware::common::types::float32_t; +using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; /** - * @class simple_planning_simulator vehicle model class - * @brief calculate vehicle dynamics + * @class SimModelInterface + * @brief simple_planning_simulator vehicle model class to calculate vehicle dynamics */ class SimModelInterface { @@ -37,6 +35,9 @@ class SimModelInterface Eigen::VectorXd state_; //!< @brief vehicle state vector Eigen::VectorXd input_; //!< @brief vehicle input vector + //!< @brief gear command defined in autoware_auto_msgs/VehicleStateCommand + uint8_t gear_ = autoware_auto_vehicle_msgs::msg::VehicleStateCommand::GEAR_DRIVE; + public: /** * @brief constructor @@ -74,55 +75,81 @@ class SimModelInterface */ void setInput(const Eigen::VectorXd & input); + /** + * @brief set gear + * @param [in] gear gear command defined in autoware_auto_msgs/VehicleStateCommand + */ + void setGear(const uint8_t gear); + /** * @brief update vehicle states with Runge-Kutta methods * @param [in] dt delta time [s] * @param [in] input vehicle input */ - void updateRungeKutta(const double & dt, const Eigen::VectorXd & input); + void updateRungeKutta(const float64_t & dt, const Eigen::VectorXd & input); /** * @brief update vehicle states with Euler methods * @param [in] dt delta time [s] * @param [in] input vehicle input */ - void updateEuler(const double & dt, const Eigen::VectorXd & input); + void updateEuler(const float64_t & dt, const Eigen::VectorXd & input); /** * @brief update vehicle states * @param [in] dt delta time [s] */ - virtual void update(const double & dt) = 0; + virtual void update(const float64_t & dt) = 0; /** * @brief get vehicle position x */ - virtual double getX() = 0; + virtual float64_t getX() = 0; /** * @brief get vehicle position y */ - virtual double getY() = 0; + virtual float64_t getY() = 0; /** * @brief get vehicle angle yaw */ - virtual double getYaw() = 0; + virtual float64_t getYaw() = 0; /** * @brief get vehicle velocity vx */ - virtual double getVx() = 0; + virtual float64_t getVx() = 0; + + /** + * @brief get vehicle lateral velocity + */ + virtual float64_t getVy() = 0; + + /** + * @brief get vehicle longiudinal acceleration + */ + virtual float64_t getAx() = 0; /** * @brief get vehicle angular-velocity wz */ - virtual double getWz() = 0; + virtual float64_t getWz() = 0; /** * @brief get vehicle steering angle */ - virtual double getSteer() = 0; + virtual float64_t getSteer() = 0; + + /** + * @brief get state vector dimension + */ + inline int getDimX() {return dim_x_;} + + /** + * @brief get input vector demension + */ + inline int getDimU() {return dim_u_;} /** * @brief calculate derivative of states with vehicle model diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp deleted file mode 100644 index d6193434bcbc8..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_time_delay.hpp +++ /dev/null @@ -1,359 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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. - -/** - * @file sim_model_time_delay.hpp - * @brief simple planning simulator model with time delay and 1-dimensional dynamics for velocity & - * steering - * @author Takamasa Horibe, Kim-Ngoc-Khanh Nguyen - * @date 2019.08.17 - */ - -#ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_TIME_DELAY_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_TIME_DELAY_HPP_ - -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_util.hpp" - -#include -#include - -#include -#include -#include - -/** - * @class simple_planning_simulator time delay twist model - * @brief calculate time delay twist dynamics - */ -class SimModelTimeDelayTwist : public SimModelInterface -{ -public: - /** - * @brief constructor - * @param [in] vx_lim velocity limit [m/s] - * @param [in] angvel_lim angular velocity limit [m/s] - * @param [in] vx_rate_lim acceleration limit [m/ss] - * @param [in] wz_rate_lim angular acceleration limit [rad/ss] - * @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] wx_delay time delay for angular-velocity command [s] - * @param [in] wz_time_constant time constant for 1D model of angular-velocity dynamics - * @param [in] deadzone_delta_steer deadzone value of steer - */ - SimModelTimeDelayTwist( - double vx_lim, double angvel_lim, double vx_rate_lim, double wz_rate_lim, double dt, - double vx_delay, double vx_time_constant, double wz_delay, double wz_time_constant, - double deadzone_delta_steer); - - /** - * @brief default destructor - */ - ~SimModelTimeDelayTwist() = default; - -private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - - enum IDX { - X = 0, - Y, - YAW, - VX, - WZ, - }; - enum IDX_U { - VX_DES = 0, - WZ_DES, - }; - - const double vx_lim_; //!< @brief velocity limit - const double vx_rate_lim_; //!< @brief acceleration limit - const double wz_lim_; //!< @brief angular velocity limit - const double wz_rate_lim_; //!< @brief angular acceleration limit - - std::deque vx_input_queue_; //!< @brief buffer for velocity command - std::deque wz_input_queue_; //!< @brief buffer for angular velocity command - const double vx_delay_; //!< @brief time delay for velocity command [s] - const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics - const double wz_delay_; //!< @brief time delay for angular-velocity command [s] - const double - wz_time_constant_; //!< @brief time constant for 1D model of angular-velocity dynamics - const double deadzone_delta_steer_; //!<@ brief deadzone value of steer - - /** - * @brief set queue buffer for input command - * @param [in] dt delta time - */ - void initializeInputQueue(const double & dt); - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with time delay twist 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; -}; - -class SimModelTimeDelaySteer : 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 - * @param [in] deadzone_delta_steer deadzone value of steer - */ - SimModelTimeDelaySteer( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double deadzone_delta_steer); - - /** - * @brief default destructor - */ - ~SimModelTimeDelaySteer() = default; - -private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - - enum IDX { - X = 0, - Y, - YAW, - VX, - STEER, - }; - enum IDX_U { - VX_DES = 0, - STEER_DES, - }; - - const double vx_lim_; //!< @brief velocity limit [m/s] - const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] - const double steer_lim_; //!< @brief steering limit [rad] - const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const double wheelbase_; //!< @brief vehicle wheelbase length [m] - - std::deque vx_input_queue_; //!< @brief buffer for velocity command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double vx_delay_; //!< @brief time delay for velocity command [s] - const double vx_time_constant_; //!< @brief time constant for 1D model of velocity dynamics - const double steer_delay_; //!< @brief time delay for steering command [s] - const double steer_time_constant_; //!< @brief time constant for 1D model of steering dynamics - const double deadzone_delta_steer_; //!<@ brief deadzone value of steer - - /** - * @brief set queue buffer for input command - * @param [in] dt delta time - */ - void initializeInputQueue(const double & dt); - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with time 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; -}; - -class SimModelTimeDelaySteerAccel : 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] acc_delay time delay for accel command [s] - * @param [in] acc_time_constant time constant for 1D model of accel dynamics - * @param [in] steer_delay time delay for steering command [s] - * @param [in] steer_time_constant time constant for 1D model of steering dynamics - * @param [in] deadzone_delta_steer deadzone value of steer - */ - SimModelTimeDelaySteerAccel( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double deadzone_delta_steer); - - /** - * @brief default destructor - */ - ~SimModelTimeDelaySteerAccel() = default; - -private: - const double MIN_TIME_CONSTANT; //!< @brief minimum time constant - - enum IDX { - X = 0, - Y, - YAW, - VX, - STEER, - ACCX, - }; - enum IDX_U { - ACCX_DES = 0, - STEER_DES, - DRIVE_SHIFT, - }; - - const double vx_lim_; //!< @brief velocity limit [m/s] - const double vx_rate_lim_; //!< @brief acceleration limit [m/ss] - const double steer_lim_; //!< @brief steering limit [rad] - const double steer_rate_lim_; //!< @brief steering angular velocity limit [rad/s] - const double wheelbase_; //!< @brief vehicle wheelbase length [m] - - std::deque acc_input_queue_; //!< @brief buffer for accel command - std::deque steer_input_queue_; //!< @brief buffer for steering command - const double acc_delay_; //!< @brief time delay for accel command [s] - const double acc_time_constant_; //!< @brief time constant for 1D model of accel dynamics - const double steer_delay_; //!< @brief time delay for steering command [s] - const double steer_time_constant_; //!< @brief time constant for 1D model of steering dynamics - const double deadzone_delta_steer_; //!<@ brief deadzone value of steer - - /** - * @brief set queue buffer for input command - * @param [in] dt delta time - */ - void initializeInputQueue(const double & dt); - - /** - * @brief get vehicle position x - */ - double getX() override; - - /** - * @brief get vehicle position y - */ - double getY() override; - - /** - * @brief get vehicle angle yaw - */ - double getYaw() override; - - /** - * @brief get vehicle velocity vx - */ - double getVx() override; - - /** - * @brief get vehicle angular-velocity wz - */ - double getWz() override; - - /** - * @brief get vehicle steering angle - */ - double getSteer() override; - - /** - * @brief update vehicle states - * @param [in] dt delta time [s] - */ - void update(const double & dt) override; - - /** - * @brief calculate derivative of states with time 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_TIME_DELAY_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp deleted file mode 100644 index c91708c5f69e4..0000000000000 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_util.hpp +++ /dev/null @@ -1,26 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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_UTIL_HPP_ -#define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_UTIL_HPP_ - -#include - -namespace sim_model_util -{ -double getDummySteerCommandWithFriction( - const double steer, const double steer_command, const double deadzone_delta_steer); -} - -#endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_UTIL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp new file mode 100644 index 0000000000000..d1179bbcc833d --- /dev/null +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp @@ -0,0 +1,40 @@ +// 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__VISIBILITY_CONTROL_HPP_ +#define SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ + +#if defined(__WIN32) +#if defined(PLANNING_SIMULATOR_BUILDING_DLL) || \ + defined(PLANNING_SIMULATOR_EXPORTS) +#define PLANNING_SIMULATOR_PUBLIC __declspec(dllexport) +#define PLANNING_SIMULATOR_LOCAL +#else // defined(PLANNING_SIMULATOR_BUILDING_DLL) || + // defined(PLANNING_SIMULATOR_EXPORTS) +#define PLANNING_SIMULATOR_PUBLIC __declspec(dllimport) +#define PLANNING_SIMULATOR_LOCAL +#endif // defined(PLANNING_SIMULATOR_BUILDING_DLL) || + // defined(PLANNING_SIMULATOR_EXPORTS) +#elif defined(__linux__) +#define PLANNING_SIMULATOR_PUBLIC __attribute__((visibility("default"))) +#define PLANNING_SIMULATOR_LOCAL __attribute__((visibility("hidden"))) +#elif defined(__APPLE__) +#define PLANNING_SIMULATOR_PUBLIC __attribute__((visibility("default"))) +#define PLANNING_SIMULATOR_LOCAL __attribute__((visibility("hidden"))) +#else // defined(_LINUX) +#error "Unsupported Build Configuration" +#endif // defined(_WINDOWS) + +#endif // SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 38cd8365604ae..de991b4b70bc5 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -1,10 +1,10 @@ -# Copyright 2020 Tier IV, Inc. +# 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 +#    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, @@ -12,59 +12,63 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os -from pathlib import Path +import ament_index_python +import launch +import launch_ros.actions -from launch import LaunchContext -from launch import LaunchDescription from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from launch_ros.actions import Node -from launch_ros.substitutions import FindPackage - -context = LaunchContext() +from ament_index_python import get_package_share_directory - -def find_pack(package_name): - """Return the absolute path to the share directory of the given package.""" - return os.path.join(Path(FindPackage(package_name).perform(context)), "share", package_name) +import os def generate_launch_description(): - simple_planning_simulator_param_file = os.path.join( - find_pack("simple_planning_simulator"), "config/simple_planning_simulator.param.yaml" - ) - - print(simple_planning_simulator_param_file) + default_vehicle_characteristics_param = os.path.join( + get_package_share_directory('simple_planning_simulator'), + 'param/vehicle_characteristics.param.param.yaml') - simple_planning_simulator_param = DeclareLaunchArgument( - "simple_planning_simulator_param_file", - default_value=simple_planning_simulator_param_file, - description="Path to config file for simple_planning_simulator", + vehicle_characteristics_param = DeclareLaunchArgument( + 'vehicle_characteristics_param_file', + default_value=default_vehicle_characteristics_param, + description='Path to config file for vehicle characteristics' ) - simple_planning_simulator = Node( - package="simple_planning_simulator", - node_executable="simple_planning_simulator_exe", - node_name="simple_planning_simulator", - node_namespace="simulation", - output="screen", + simple_planning_simulator_node = launch_ros.actions.Node( + package='simple_planning_simulator', + executable='simple_planning_simulator_exe', + name='simple_planning_simulator', + namespace='simulation', + output='screen', parameters=[ - LaunchConfiguration("simple_planning_simulator_param_file"), + "{}/param/simple_planning_simulator_default.param.yaml".format( + ament_index_python.get_package_share_directory( + "simple_planning_simulator" + ) + ), + LaunchConfiguration('vehicle_characteristics_param_file'), ], remappings=[ - ("base_trajectory", "/planning/scenario_planning/trajectory"), - ("output/current_pose", "current_pose"), - ("output/current_twist", "/vehicle/status/twist"), - ("output/status", "/vehicle/status"), - ("output/control_mode", "/vehicle/status/control_mode"), - ("input/vehicle_cmd", "/control/vehicle_cmd"), - ("input/turn_signal_cmd", "/control/turn_signal_cmd"), - ("input/initial_twist", "/initialtwist"), - ("input/initial_pose", "/initialpose"), - ("input/engage", "/vehicle/engage"), - ], + ('input/vehicle_control_command', '/vehicle/vehicle_command'), + ('input/ackermann_control_command', '/vehicle/ackermann_vehicle_command'), + ('input/vehicle_state_command', '/vehicle/state_command'), + ('output/kinematic_state', '/vehicle/vehicle_kinematic_state'), + ('output/vehicle_state_report', '/vehicle/state_report'), + ('/initialpose', '/localization/initialpose'), + ] ) - return LaunchDescription([simple_planning_simulator_param, simple_planning_simulator]) + map_to_odom_tf_publisher = launch_ros.actions.Node( + package='tf2_ros', + executable='static_transform_publisher', + name='static_map_to_odom_tf_publisher', + output='screen', + arguments=['0.0', '0.0', '0.0', '0', '0', '0', 'map', 'odom']) + + ld = launch.LaunchDescription([ + vehicle_characteristics_param, + simple_planning_simulator_node, + map_to_odom_tf_publisher + ]) + return ld diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml deleted file mode 100644 index 19e972d44b933..0000000000000 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.xml +++ /dev/null @@ -1,43 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 92f1bf0c8df1e..6bb93b9a55774 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -2,36 +2,32 @@ simple_planning_simulator - 0.0.2 + 1.0.0 simple_planning_simulator as a ROS 2 node Takamasa HORIBE Apache License 2.0 ament_cmake_auto + autoware_auto_cmake - autoware_api_utils - autoware_control_msgs - autoware_debug_msgs - autoware_external_api_msgs - autoware_planning_msgs - autoware_utils - autoware_vehicle_msgs + autoware_auto_planning_msgs + autoware_auto_control_msgs + autoware_auto_vehicle_msgs + autoware_auto_tf2 + autoware_auto_common geometry_msgs + + motion_common + rclcpp - rclcpp_components - std_msgs tf2 tf2_ros - vehicle_info_util launch_ros - rosidl_default_runtime - topic_tools - ament_lint_auto - autoware_lint_common + ament_cmake_gtest + + - - ament_cmake - + ament_cmake diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml new file mode 100644 index 0000000000000..cac6b5822e573 --- /dev/null +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -0,0 +1,19 @@ +simulation: + simple_planning_simulator: + ros__parameters: + simulated_frame_id: "base_link" + origin_frame_id: "odom" + vehicle_model_type: "IDEAL_STEER_VEL" + initialize_source: "INITIAL_POSE_TOPIC" + timer_sampling_time_ms: 25 + add_measurement_noise: False + vel_lim: 30.0 + vel_rate_lim: 30.0 + steer_lim: 0.6 + steer_rate_lim: 6.28 + acc_time_delay: 0.1 + acc_time_constant: 0.1 + steer_time_delay: 0.1 + steer_time_constant: 0.1 + +# Note: vehicle characteristics parameters (e.g. wheelbase) are difined in a separate file. diff --git a/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml b/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml new file mode 100644 index 0000000000000..12b6efa79d1cd --- /dev/null +++ b/simulator/simple_planning_simulator/param/vehicle_characteristics.param.yaml @@ -0,0 +1,12 @@ +/**: + ros__parameters: + vehicle: + cg_to_front_m: 1.228 + cg_to_rear_m: 1.5618 + front_corner_stiffness: 17000.0 + rear_corner_stiffness: 20000.0 + mass_kg: 1460.0 + yaw_inertia_kgm2: 2170.0 + width_m: 2.0 + front_overhang_m: 0.5 + rear_overhang_m: 0.5 diff --git a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md b/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md deleted file mode 100644 index fb71c858a2f04..0000000000000 --- a/simulator/simple_planning_simulator/scripts/README_fitParamDelayInputModel.md +++ /dev/null @@ -1,33 +0,0 @@ -# fitParamDelayInputModel.py scripts - -## How to use - -```sh -python fitParamDelayInputModel.py --bag_file [bagfile] (--cutoff_time [cutoff_time] --cutoff_freq [cutoff_freq] --min_delay [min_delay] --max_delay [max_delay] --delay_incr [delay_incr]) -# in round brackets is optional arguments -python fitParamDelayInputModel.py --help # for more information -``` - -## Requirements python packages - -- numpy -- pandas - -## Required topics - -- autoware_msgs::VehicleCmd /vehicle_cmd: assuming - - vehicle_cmd/ctrl_cmd/steering_angle is the steering angle input [rad] - - vehicle_cmd/ctrl_cmd/linear_velocity is the vehicle velocity input [m/s] -- autoware_msgs::VehicleStatus /vehicle_status : assuming - - vehicle_status/angle is the measured steering angle [rad] - - vehicle_status/speed is the measured vehicle speed [km/h] - -## Description - -- Parameter fitting for Input Delay Model (First Order System with Dead Time) with rosbag file input -- Arguments explaining: - - CUTOFF_TIME: Cutoff time[sec]. Rosbag file often was start recording before autoware was run. Time before autoware was run should be cut off. This script will only consider data from t=cutoff_time to the end of the bag file (default is 1.0) - - CUTOFF_FREQ: Cutoff freq for low-pass filter[Hz], negative value will disable low-pass filter (default is 0.1) - - MIN_DELAY: Min value for searching delay loop (default is 0.1) - - MAX_DELAY: Max value for searching delay loop (default is 1.0) - - DELAY_INCR: Step value for searching delay loop (default is 0.01) diff --git a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py b/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py deleted file mode 100644 index a0aaa943fc4e7..0000000000000 --- a/simulator/simple_planning_simulator/scripts/fitParamDelayInputModel.py +++ /dev/null @@ -1,211 +0,0 @@ -#!/usr/bin/env python - -# Copyright 2020 Tier IV, Inc. -# -# 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. - -import argparse -from os import getcwd -from os.path import basename -from os.path import dirname -from os.path import exists -from os.path import join -from os.path import splitext -import subprocess -import sys - -import numpy as np - -try: - import pandas as pd -except ImportError: - print("Please install pandas. See http://pandas.pydata.org/pandas-docs/stable/") - sys.exit(1) - -FREQ_SAMPLE = 0.001 - -# low pass filter - - -def lowpass_filter(data, cutoff_freq=2, order=1, dt=FREQ_SAMPLE): - tau = 1.0 / (2 * np.pi * cutoff_freq) - for _ in range(order): - for i in range(1, len(data)): - data[i] = tau / (tau + dt) * data[i - 1] + dt / (tau + dt) * data[i] - return data - - -def rel2abs(path): - """Return absolute path from relative path input.""" - return join(getcwd(), path) - - -def rosbag_to_csv(path, topic_name): - name = splitext(basename(path))[0] - suffix = topic_name.replace("/", "-") - output_path = join(dirname(path), name + "_" + suffix + ".csv") - if exists(output_path): - return output_path - else: - command = "rostopic echo -b {0} -p /{1} | sed -e 's/,/ /g' > {2}".format( - path, topic_name, output_path - ) - print(command) - subprocess.check_call(command, shell=True) - return output_path - - -def getActValue(df, speed_type): - tm = np.array(list(df["%time"])) * 1e-9 - # Unit Conversion - if speed_type: - val = np.array(list(df["field"])) / 3.6 - else: - val = np.array(list(df["field"])) - # Calc differential - d_val = (val[2:] - val[:-2]) / (tm[2:] - tm[:-2]) - return tm[1:-1], val[1:-1], d_val - - -def getCmdValueWithDelay(df, delay): - tm = np.array(list(df["%time"])) * 1e-9 - val = np.array(list(df["field"])) - return tm + delay, val - - -def getLinearInterpolate(_tm, _val, _index, ti): - tmp_t = _tm[_index] - tmp_next_t = _tm[_index + 1] - tmp_val = _val[_index] - tmp_next_val = _val[_index + 1] - val_i = tmp_val + (tmp_next_val - tmp_val) / (tmp_next_t - tmp_t) * (ti - tmp_t) - return val_i - - -def getFittingTimeConstantParam(cmd_data, act_data, delay, args, speed_type=False): - tm_cmd, cmd_delay = getCmdValueWithDelay(cmd_data, delay) - tm_act, act, d_act = getActValue(act_data, speed_type) - _t_min = max(tm_cmd[0], tm_act[0]) - _t_max = min(tm_cmd[-1], tm_act[-1]) - tm_cmd = tm_cmd - _t_min - tm_act = tm_act - _t_min - MAX_CNT = int((_t_max - _t_min - args.cutoff_time) / FREQ_SAMPLE) - d_act_sample = [None] * MAX_CNT - diff_act_cmd_sample = [None] * MAX_CNT - ind_cmd = 0 - ind_act = 0 - for ind in range(MAX_CNT): - ti = ind * FREQ_SAMPLE + args.cutoff_time - while tm_cmd[ind_cmd + 1] < ti: - ind_cmd += 1 - cmd_delay_i = getLinearInterpolate(tm_cmd, cmd_delay, ind_cmd, ti) - while tm_act[ind_act + 1] < ti: - ind_act += 1 - act_i = getLinearInterpolate(tm_act, act, ind_act, ti) - d_act_i = getLinearInterpolate(tm_act, d_act, ind_act, ti) - d_act_sample[ind] = d_act_i - diff_act_cmd_sample[ind] = act_i - cmd_delay_i - d_act_sample = np.array(d_act_sample) - diff_act_cmd_sample = np.array(diff_act_cmd_sample) - if args.cutoff_freq > 0: - d_act_sample = lowpass_filter(d_act_sample, cutoff_freq=args.cutoff_freq) - diff_act_cmd_sample = lowpass_filter(diff_act_cmd_sample, cutoff_freq=args.cutoff_freq) - d_act_sample = d_act_sample.reshape(1, -1) - diff_act_cmd_sample = diff_act_cmd_sample.reshape(1, -1) - tau = -np.dot(diff_act_cmd_sample, np.linalg.pinv(d_act_sample))[0, 0] - error = np.linalg.norm(diff_act_cmd_sample + tau * d_act_sample) / d_act_sample.shape[1] - return tau, error - - -def getFittingParam(cmd_data, act_data, args, speed_type=False): - delay_range = int((args.max_delay - args.min_delay) / args.delay_incr) - delays = [args.min_delay + i * args.delay_incr for i in range(delay_range + 1)] - error_min = 1.0e10 - delay_opt = -1 - tau_opt = -1 - for delay in delays: - tau, error = getFittingTimeConstantParam( - cmd_data, act_data, delay, args, speed_type=speed_type - ) - if tau > 0: - if error < error_min: - error_min = error - delay_opt = delay - tau_opt = tau - else: - break - return tau_opt, delay_opt, error_min - - -if __name__ == "__main__": - topics = [ - "vehicle_cmd/ctrl_cmd/steering_angle", - "vehicle_status/angle", - "vehicle_cmd/ctrl_cmd/linear_velocity", - "vehicle_status/speed", - ] - pd_data = [None] * len(topics) - parser = argparse.ArgumentParser( - description="Parameter fitting for Input Delay Model" - " (First Order System with Dead Time) with rosbag file input" - ) - parser.add_argument( - "--bag_file", "-b", required=True, type=str, help="rosbag file", metavar="file" - ) - parser.add_argument( - "--cutoff_time", - default=1.0, - type=float, - help="Cutoff time[sec], Parameter fitting will only consider data " - "from t= cutoff_time to the end of the bag file (default is 1.0)", - ) - parser.add_argument( - "--cutoff_freq", - default=0.1, - type=float, - help="Cutoff freq for low-pass filter[Hz], " - "negative value will disable low-pass filter (default is 0.1)", - ) - parser.add_argument( - "--min_delay", - default=0.1, - type=float, - help="Min value for searching delay loop (default is 0.1)", - ) - parser.add_argument( - "--max_delay", - default=1.0, - type=float, - help="Max value for searching delay loop (default is 1.0)", - ) - parser.add_argument( - "--delay_incr", - default=0.01, - type=float, - help="Step value for searching delay loop (default is 0.01)", - ) - args = parser.parse_args() - - for i, topic in enumerate(topics): - csv_log = rosbag_to_csv(rel2abs(args.bag_file), topic) - pd_data[i] = pd.read_csv(csv_log, sep=" ") - tau_opt, delay_opt, error = getFittingParam(pd_data[0], pd_data[1], args, speed_type=False) - print( - "Steer angle: tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e" - % (tau_opt, delay_opt, error) - ) - tau_opt, delay_opt, error = getFittingParam(pd_data[2], pd_data[3], args, speed_type=True) - print( - "Velocity : tau_opt = %2.4f, delay_opt = %2.4f, error = %2.4e" - % (tau_opt, delay_opt, error) - ) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp new file mode 100644 index 0000000000000..1909cacae724e --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -0,0 +1,387 @@ +// 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 + +#include +#include +#include +#include +#include +#include + +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" + +#include "common/types.hpp" +#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "motion_common/motion_common.hpp" + +#include "rclcpp_components/register_node_macro.hpp" + +using namespace std::chrono_literals; + +namespace +{ +autoware_auto_vehicle_msgs::msg::VehicleKinematicState convert_baselink_to_com( + const autoware_auto_vehicle_msgs::msg::VehicleKinematicState & in, const float32_t baselink_to_com) +{ + autoware_auto_vehicle_msgs::msg::VehicleKinematicState out = in; + + // TODO(Horibe) convert to CoM for vehicle_kinematic_state msg. + const auto yaw = motion::motion_common::to_angle(out.state.pose.orientation); + out.state.pose.position.x += static_cast(std::cos(yaw) * baselink_to_com); + out.state.pose.position.y += static_cast(std::sin(yaw) * baselink_to_com); + + return out; +} + +autoware_auto_vehicle_msgs::msg::VehicleKinematicState to_kinematic_state( + const std::shared_ptr vehicle_model_ptr) +{ + autoware_auto_vehicle_msgs::msg::VehicleKinematicState s; + s.state.pose.position.x = vehicle_model_ptr->getX(); + s.state.pose.position.y = vehicle_model_ptr->getY(); + s.state.pose.orientation = motion::motion_common::from_angle(vehicle_model_ptr->getYaw()); + s.state.longitudinal_velocity_mps = + static_cast(vehicle_model_ptr->getVx()); + s.state.lateral_velocity_mps = 0.0; + s.state.acceleration_mps2 = static_cast(vehicle_model_ptr->getAx()); + s.state.heading_rate_rps = static_cast(vehicle_model_ptr->getWz()); + s.state.front_wheel_angle_rad = + static_cast(vehicle_model_ptr->getSteer()); + s.state.rear_wheel_angle_rad = 0.0; + return s; +} +} // namespace + +namespace simulation +{ +namespace simple_planning_simulator +{ + +SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & options) +: Node("simple_planning_simulator", options), + tf_buffer_(get_clock()), + tf_listener_(tf_buffer_, std::shared_ptr(this, [](auto) {}), false) +{ + simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link"); + origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); + add_measurement_noise_ = declare_parameter("add_measurement_noise", false); + cg_to_rear_m_ = static_cast(declare_parameter("vehicle.cg_to_rear_m", 1.5)); + + using rclcpp::QoS; + using std::placeholders::_1; + + sub_init_pose_ = create_subscription( + "/initialpose", QoS{1}, + std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); + sub_vehicle_cmd_ = create_subscription( + "input/vehicle_control_command", QoS{1}, + std::bind(&SimplePlanningSimulator::on_vehicle_cmd, this, _1)); + sub_ackermann_cmd_ = create_subscription( + "input/ackermann_control_command", QoS{1}, + std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1)); + sub_state_cmd_ = create_subscription( + "input/vehicle_state_command", QoS{1}, + std::bind(&SimplePlanningSimulator::on_state_cmd, this, _1)); + + pub_state_report_ = create_publisher("output/vehicle_state_report", QoS{1}); + pub_current_pose_ = create_publisher("/current_pose", QoS{1}); + pub_kinematic_state_ = create_publisher("output/kinematic_state", QoS{1}); + pub_tf_ = create_publisher("/tf", QoS{1}); + + timer_sampling_time_ms_ = static_cast(declare_parameter("timer_sampling_time_ms", 25)); + on_timer_ = create_wall_timer( + std::chrono::milliseconds(timer_sampling_time_ms_), + std::bind(&SimplePlanningSimulator::on_timer, this)); + + + // set vehicle model type + initialize_vehicle_model(); + + // set initialize source + const auto initialize_source = declare_parameter("initialize_source", "INITIAL_POSE_TOPIC"); + RCLCPP_INFO(this->get_logger(), "initialize_source : %s", initialize_source.c_str()); + if (initialize_source == "ORIGIN") { + geometry_msgs::msg::Pose p; + p.orientation.w = 1.0; // yaw = 0 + set_initial_state(p, geometry_msgs::msg::Twist{}); // initialize with 0 for all variables + } else if (initialize_source == "INITIAL_POSE_TOPIC") { + // initialpose sub already exists. Do nothing. + } + + + // measurement noise + { + std::random_device seed; + auto & m = measurement_noise_; + m.rand_engine_ = std::make_shared(seed()); + float64_t pos_noise_stddev = declare_parameter("pos_noise_stddev", 1e-2); + float64_t vel_noise_stddev = declare_parameter("vel_noise_stddev", 1e-2); + float64_t rpy_noise_stddev = declare_parameter("rpy_noise_stddev", 1e-4); + float64_t steer_noise_stddev = declare_parameter("steer_noise_stddev", 1e-4); + m.pos_dist_ = std::make_shared>(0.0, pos_noise_stddev); + m.vel_dist_ = std::make_shared>(0.0, vel_noise_stddev); + m.rpy_dist_ = std::make_shared>(0.0, rpy_noise_stddev); + m.steer_dist_ = std::make_shared>(0.0, steer_noise_stddev); + } +} + +void SimplePlanningSimulator::initialize_vehicle_model() +{ + const auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER_VEL"); + + RCLCPP_INFO(this->get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); + + const float64_t cg_to_front_m = declare_parameter("vehicle.cg_to_front_m", 1.5); + const float64_t wheelbase = cg_to_front_m + static_cast(cg_to_rear_m_); + const float64_t vel_lim = declare_parameter("vel_lim", 50.0); + const float64_t vel_rate_lim = declare_parameter("vel_rate_lim", 7.0); + const float64_t steer_lim = declare_parameter("steer_lim", 1.0); + const float64_t steer_rate_lim = declare_parameter("steer_rate_lim", 5.0); + const float64_t acc_time_delay = declare_parameter("acc_time_delay", 0.1); + const float64_t acc_time_constant = declare_parameter("acc_time_constant", 0.1); + const float64_t steer_time_delay = declare_parameter("steer_time_delay", 0.24); + const float64_t steer_time_constant = declare_parameter("steer_time_constant", 0.27); + + if (vehicle_model_type_str == "IDEAL_STEER_VEL") { + vehicle_model_type_ = VehicleModelType::IDEAL_STEER_VEL; + vehicle_model_ptr_ = std::make_shared(wheelbase); + } else if (vehicle_model_type_str == "IDEAL_STEER_ACC") { + vehicle_model_type_ = VehicleModelType::IDEAL_STEER_ACC; + vehicle_model_ptr_ = std::make_shared(wheelbase); + } else if (vehicle_model_type_str == "IDEAL_STEER_ACC_GEARED") { + vehicle_model_type_ = VehicleModelType::IDEAL_STEER_ACC_GEARED; + vehicle_model_ptr_ = std::make_shared(wheelbase); + } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, + steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, + steer_time_constant); + } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { + vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; + vehicle_model_ptr_ = std::make_shared( + vel_lim, steer_lim, vel_rate_lim, + steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, + steer_time_constant); + } else { + throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); + } +} + +void SimplePlanningSimulator::on_timer() +{ + if (!is_initialized_) { + RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting initialization..."); + return; + } + + // update vehicle dynamics + { + const float64_t dt = delta_time_.get_dt(get_clock()->now()); + vehicle_model_ptr_->update(dt); + } + + // set current kinematic state + current_kinematic_state_ = to_kinematic_state(vehicle_model_ptr_); + + if (add_measurement_noise_) { + add_measurement_noise(current_kinematic_state_); + } + + // publish vehicle state + publish_kinematic_state(convert_baselink_to_com(current_kinematic_state_, cg_to_rear_m_)); + publish_state_report(); + publish_tf(current_kinematic_state_); +} + +void SimplePlanningSimulator::on_initialpose( + const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) +{ + // save initial pose + geometry_msgs::msg::Twist initial_twist; + geometry_msgs::msg::PoseStamped initial_pose; + initial_pose.header = msg->header; + initial_pose.pose = msg->pose.pose; + set_initial_state_with_transform(initial_pose, initial_twist); +} + +void SimplePlanningSimulator::on_vehicle_cmd( + const autoware_auto_vehicle_msgs::msg::VehicleControlCommand::ConstSharedPtr msg) +{ + current_vehicle_cmd_ptr_ = msg; + set_input(msg->front_wheel_angle_rad, msg->velocity_mps, msg->long_accel_mps2); +} + +void SimplePlanningSimulator::on_ackermann_cmd( + const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) +{ + current_ackermann_cmd_ptr_ = msg; + set_input( + msg->lateral.steering_tire_angle, msg->longitudinal.speed, + msg->longitudinal.acceleration); +} + +void SimplePlanningSimulator::set_input(const float steer, const float vel, const float accel) +{ + Eigen::VectorXd input(vehicle_model_ptr_->getDimU()); + + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL) { + input << vel, steer; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) + { + input << accel, steer; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { + input << accel, steer; + } + vehicle_model_ptr_->setInput(input); +} + +void SimplePlanningSimulator::on_state_cmd( + const autoware_auto_vehicle_msgs::msg::VehicleStateCommand::ConstSharedPtr msg) +{ + current_vehicle_state_cmd_ptr_ = msg; + + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { + vehicle_model_ptr_->setGear(current_vehicle_state_cmd_ptr_->gear); + } +} + +void SimplePlanningSimulator::add_measurement_noise(VehicleKinematicState & state) const +{ + auto & n = measurement_noise_; + state.state.pose.position.x += (*n.pos_dist_)(*n.rand_engine_); + state.state.pose.position.y += (*n.pos_dist_)(*n.rand_engine_); + state.state.longitudinal_velocity_mps += static_cast((*n.vel_dist_)(*n.rand_engine_)); + state.state.front_wheel_angle_rad += static_cast((*n.steer_dist_)(*n.rand_engine_)); + + float32_t yaw = motion::motion_common::to_angle(state.state.pose.orientation); + yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); + state.state.pose.orientation = motion::motion_common::from_angle(yaw); +} + + +void SimplePlanningSimulator::set_initial_state_with_transform( + const geometry_msgs::msg::PoseStamped & pose_stamped, const geometry_msgs::msg::Twist & twist) +{ + auto transform = get_transform_msg(origin_frame_id_, pose_stamped.header.frame_id); + geometry_msgs::msg::Pose pose; + pose.position.x = pose_stamped.pose.position.x + transform.transform.translation.x; + pose.position.y = pose_stamped.pose.position.y + transform.transform.translation.y; + pose.position.z = pose_stamped.pose.position.z + transform.transform.translation.z; + pose.orientation = pose_stamped.pose.orientation; + set_initial_state(pose, twist); +} + +void SimplePlanningSimulator::set_initial_state( + const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist) +{ + const float64_t x = pose.position.x; + const float64_t y = pose.position.y; + const float64_t yaw = ::motion::motion_common::to_angle(pose.orientation); + const float64_t vx = twist.linear.x; + const float64_t steer = 0.0; + const float64_t accx = 0.0; + + Eigen::VectorXd state(vehicle_model_ptr_->getDimX()); + + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL) { + state << x, y, yaw; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) + { + state << x, y, yaw, vx; + } else if ( // NOLINT + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { + state << x, y, yaw, vx, steer, accx; + } + vehicle_model_ptr_->setState(state); + + is_initialized_ = true; +} + +geometry_msgs::msg::TransformStamped SimplePlanningSimulator::get_transform_msg( + const std::string parent_frame, const std::string child_frame) +{ + geometry_msgs::msg::TransformStamped transform; + while (true) { + try { + const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); + transform = tf_buffer_.lookupTransform( + parent_frame, child_frame, time_point, tf2::durationFromSec( + 0.0)); + break; + } catch (tf2::TransformException & ex) { + RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); + rclcpp::sleep_for(std::chrono::milliseconds(500)); + } + } + return transform; +} + +void SimplePlanningSimulator::publish_kinematic_state( + const VehicleKinematicState & state) +{ + VehicleKinematicState msg = state; + msg.header.frame_id = origin_frame_id_; + msg.header.stamp = get_clock()->now(); + + pub_kinematic_state_->publish(msg); +} + +void SimplePlanningSimulator::publish_state_report() +{ + VehicleStateReport msg; + msg.stamp = get_clock()->now(); + msg.mode = VehicleStateReport::MODE_AUTONOMOUS; + if (current_vehicle_state_cmd_ptr_) { + msg.gear = current_vehicle_state_cmd_ptr_->gear; + } + pub_state_report_->publish(msg); +} + +void SimplePlanningSimulator::publish_tf(const VehicleKinematicState & state) +{ + geometry_msgs::msg::TransformStamped tf; + tf.header.stamp = get_clock()->now(); + tf.header.frame_id = origin_frame_id_; + tf.child_frame_id = simulated_frame_id_; + tf.transform.translation.x = state.state.pose.position.x; + tf.transform.translation.y = state.state.pose.position.y; + tf.transform.translation.z = 0.0; + tf.transform.rotation = state.state.pose.orientation; + + tf2_msgs::msg::TFMessage tf_msg{}; + tf_msg.transforms.emplace_back(std::move(tf)); + pub_tf_->publish(tf_msg); +} +} // namespace simple_planning_simulator +} // namespace simulation + +RCLCPP_COMPONENTS_REGISTER_NODE(simulation::simple_planning_simulator::SimplePlanningSimulator) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp new file mode 100644 index 0000000000000..d2fff30b202ce --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -0,0 +1,99 @@ +// 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_acc.hpp" + +#include + +SimModelDelaySteerAcc::SimModelDelaySteerAcc( + 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 acc_delay, float64_t acc_time_constant, + float64_t steer_delay, float64_t steer_time_constant) +: SimModelInterface(6 /* 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), + acc_delay_(acc_delay), + acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) +{ + initializeInputQueue(dt); +} + +float64_t SimModelDelaySteerAcc::getX() {return state_(IDX::X);} +float64_t SimModelDelaySteerAcc::getY() {return state_(IDX::Y);} +float64_t SimModelDelaySteerAcc::getYaw() {return state_(IDX::YAW);} +float64_t SimModelDelaySteerAcc::getVx() {return state_(IDX::VX);} +float64_t SimModelDelaySteerAcc::getVy() {return 0.0;} +float64_t SimModelDelaySteerAcc::getAx() {return state_(IDX::ACCX);} +float64_t SimModelDelaySteerAcc::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +float64_t SimModelDelaySteerAcc::getSteer() {return state_(IDX::STEER);} +void SimModelDelaySteerAcc::update(const float64_t & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); + delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); + acc_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(); + + updateRungeKutta(dt, delayed_input); + + state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); +} + +void SimModelDelaySteerAcc::initializeInputQueue(const float64_t & dt) +{ + size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); + acc_input_queue_.resize(acc_input_queue_size); + std::fill(acc_input_queue_.begin(), acc_input_queue_.end(), 0.0); + + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + steer_input_queue_.resize(steer_input_queue_size); + std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); +} + +Eigen::VectorXd SimModelDelaySteerAcc::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 vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); + const float64_t yaw = state(IDX::YAW); + const float64_t steer = state(IDX::STEER); + const float64_t acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); + const float64_t steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + float64_t steer_rate = -(steer - steer_des) / steer_time_constant_; + steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = acc; + d_state(IDX::STEER) = steer_rate; + d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; + + return d_state; +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp new file mode 100644 index 0000000000000..37492012c641c --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -0,0 +1,131 @@ +// 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 + +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" + +SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( + 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 acc_delay, float64_t acc_time_constant, + float64_t steer_delay, float64_t steer_time_constant) +: SimModelInterface(6 /* 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), + acc_delay_(acc_delay), + acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), + steer_delay_(steer_delay), + steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)) +{ + initializeInputQueue(dt); +} + +float64_t SimModelDelaySteerAccGeared::getX() {return state_(IDX::X);} +float64_t SimModelDelaySteerAccGeared::getY() {return state_(IDX::Y);} +float64_t SimModelDelaySteerAccGeared::getYaw() {return state_(IDX::YAW);} +float64_t SimModelDelaySteerAccGeared::getVx() {return state_(IDX::VX);} +float64_t SimModelDelaySteerAccGeared::getVy() {return 0.0;} +float64_t SimModelDelaySteerAccGeared::getAx() {return state_(IDX::ACCX);} +float64_t SimModelDelaySteerAccGeared::getWz() +{ + return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; +} +float64_t SimModelDelaySteerAccGeared::getSteer() {return state_(IDX::STEER);} +void SimModelDelaySteerAccGeared::update(const float64_t & dt) +{ + Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); + + acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); + delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); + acc_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(); + + const auto prev_vx = state_(IDX::VX); + + updateRungeKutta(dt, delayed_input); + + // take velocity limit explicitly + state_(IDX::VX) = std::max(-vx_lim_, std::min(state_(IDX::VX), vx_lim_)); + + state_(IDX::VX) = calcVelocityWithGear(state_, gear_); + + // calc acc directly after gear considerataion + state_(IDX::ACCX) = (state_(IDX::VX) - prev_vx) / std::max(dt, 1.0e-5); +} + +void SimModelDelaySteerAccGeared::initializeInputQueue(const float64_t & dt) +{ + size_t acc_input_queue_size = static_cast(round(acc_delay_ / dt)); + acc_input_queue_.resize(acc_input_queue_size); + std::fill(acc_input_queue_.begin(), acc_input_queue_.end(), 0.0); + + size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); + steer_input_queue_.resize(steer_input_queue_size); + std::fill(steer_input_queue_.begin(), steer_input_queue_.end(), 0.0); +} + +Eigen::VectorXd SimModelDelaySteerAccGeared::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 vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); + const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); + const float64_t yaw = state(IDX::YAW); + const float64_t steer = state(IDX::STEER); + const float64_t acc_des = sat(input(IDX_U::ACCX_DES), vx_rate_lim_, -vx_rate_lim_); + const float64_t steer_des = sat(input(IDX_U::STEER_DES), steer_lim_, -steer_lim_); + float64_t steer_rate = -(steer - steer_des) / steer_time_constant_; + steer_rate = sat(steer_rate, steer_rate_lim_, -steer_rate_lim_); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vel * cos(yaw); + d_state(IDX::Y) = vel * sin(yaw); + d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; + d_state(IDX::VX) = acc; + d_state(IDX::STEER) = steer_rate; + d_state(IDX::ACCX) = -(acc - acc_des) / acc_time_constant_; + + return d_state; +} + + +float64_t SimModelDelaySteerAccGeared::calcVelocityWithGear( + const Eigen::VectorXd & state, const uint8_t gear) const +{ + using autoware_auto_vehicle_msgs::msg::VehicleStateCommand; + if (gear == VehicleStateCommand::GEAR_DRIVE || + gear == VehicleStateCommand::GEAR_LOW || + gear == VehicleStateCommand::GEAR_NEUTRAL) + { + if (state(IDX::VX) < 0.0) { + return 0.0; + } + } else if (gear == VehicleStateCommand::GEAR_REVERSE) { + if (state(IDX::VX) > 0.0) { + return 0.0; + } + } else if (gear == VehicleStateCommand::GEAR_PARK) { + return 0.0; + } + + return state(IDX::VX); +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp new file mode 100644 index 0000000000000..ee74b645850cf --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp @@ -0,0 +1,52 @@ +// 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_ideal_steer_acc.hpp" + + +SimModelIdealSteerAcc::SimModelIdealSteerAcc(float64_t wheelbase) +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} + +float64_t SimModelIdealSteerAcc::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerAcc::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerAcc::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerAcc::getVx() {return state_(IDX::VX);} +float64_t SimModelIdealSteerAcc::getVy() {return 0.0;} +float64_t SimModelIdealSteerAcc::getAx() {return input_(IDX_U::AX_DES);} +float64_t SimModelIdealSteerAcc::getWz() +{ + return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +} +float64_t SimModelIdealSteerAcc::getSteer() {return input_(IDX_U::STEER_DES);} +void SimModelIdealSteerAcc::update(const float64_t & dt) +{ + updateRungeKutta(dt, input_); +} + +Eigen::VectorXd SimModelIdealSteerAcc::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const float64_t vx = state(IDX::VX); + const float64_t yaw = state(IDX::YAW); + const float64_t ax = input(IDX_U::AX_DES); + const float64_t steer = input(IDX_U::STEER_DES); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * std::cos(yaw); + d_state(IDX::Y) = vx * std::sin(yaw); + d_state(IDX::VX) = ax; + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + + return d_state; +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp new file mode 100644 index 0000000000000..b22926872d4b2 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -0,0 +1,82 @@ +// 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 + +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" +#include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" + +SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(float64_t wheelbase) +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) {} + +float64_t SimModelIdealSteerAccGeared::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerAccGeared::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerAccGeared::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerAccGeared::getVx() {return state_(IDX::VX);} +float64_t SimModelIdealSteerAccGeared::getVy() {return 0.0;} +float64_t SimModelIdealSteerAccGeared::getAx() {return current_acc_;} +float64_t SimModelIdealSteerAccGeared::getWz() +{ + return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +} +float64_t SimModelIdealSteerAccGeared::getSteer() {return input_(IDX_U::STEER_DES);} +void SimModelIdealSteerAccGeared::update(const float64_t & dt) +{ + const auto prev_vx = state_(IDX::VX); + + updateRungeKutta(dt, input_); + + state_(IDX::VX) = calcVelocityWithGear(state_, gear_); + + current_acc_ = (state_(IDX::VX) - prev_vx) / std::max(dt, 1.0e-5); +} + +Eigen::VectorXd SimModelIdealSteerAccGeared::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const float64_t vx = state(IDX::VX); + const float64_t yaw = state(IDX::YAW); + const float64_t ax = input(IDX_U::AX_DES); + const float64_t steer = input(IDX_U::STEER_DES); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * std::cos(yaw); + d_state(IDX::Y) = vx * std::sin(yaw); + d_state(IDX::VX) = ax; + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + + return d_state; +} + +float64_t SimModelIdealSteerAccGeared::calcVelocityWithGear( + const Eigen::VectorXd & state, const uint8_t gear) const +{ + using autoware_auto_vehicle_msgs::msg::VehicleStateCommand; + if (gear == VehicleStateCommand::GEAR_DRIVE || + gear == VehicleStateCommand::GEAR_LOW || + gear == VehicleStateCommand::GEAR_NEUTRAL) + { + if (state(IDX::VX) < 0.0) { + return 0.0; + } + } else if (gear == VehicleStateCommand::GEAR_REVERSE) { + if (state(IDX::VX) > 0.0) { + return 0.0; + } + } else if (gear == VehicleStateCommand::GEAR_PARK) { + return 0.0; + } + + return state(IDX::VX); +} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp new file mode 100644 index 0000000000000..ea40f647e2379 --- /dev/null +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp @@ -0,0 +1,52 @@ +// 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_ideal_steer_vel.hpp" + + +SimModelIdealSteerVel::SimModelIdealSteerVel(float64_t wheelbase) +: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} + +float64_t SimModelIdealSteerVel::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerVel::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerVel::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerVel::getVx() {return input_(IDX_U::VX_DES);} +float64_t SimModelIdealSteerVel::getVy() {return 0.0;} +float64_t SimModelIdealSteerVel::getAx() {return current_ax_;} +float64_t SimModelIdealSteerVel::getWz() +{ + return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; +} +float64_t SimModelIdealSteerVel::getSteer() {return input_(IDX_U::STEER_DES);} +void SimModelIdealSteerVel::update(const float64_t & dt) +{ + updateRungeKutta(dt, input_); + current_ax_ = (input_(IDX_U::VX_DES) - prev_vx_) / dt; + prev_vx_ = input_(IDX_U::VX_DES); +} + +Eigen::VectorXd SimModelIdealSteerVel::calcModel( + const Eigen::VectorXd & state, const Eigen::VectorXd & input) +{ + const float64_t yaw = state(IDX::YAW); + const float64_t vx = input(IDX_U::VX_DES); + const float64_t steer = input(IDX_U::STEER_DES); + + Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); + d_state(IDX::X) = vx * std::cos(yaw); + d_state(IDX::Y) = vx * std::sin(yaw); + d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; + + return d_state; +} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp similarity index 64% rename from simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp rename to simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp index 60c3e422a3da6..13bf38fc11f72 100644 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp @@ -1,10 +1,10 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. +// 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 +//    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, @@ -14,13 +14,14 @@ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) +SimModelInterface::SimModelInterface(int dim_x, int dim_u) +: dim_x_(dim_x), dim_u_(dim_u) { state_ = Eigen::VectorXd::Zero(dim_x_); input_ = Eigen::VectorXd::Zero(dim_u_); } -void SimModelInterface::updateRungeKutta(const double & dt, const Eigen::VectorXd & input) +void SimModelInterface::updateRungeKutta(const float64_t & dt, const Eigen::VectorXd & input) { Eigen::VectorXd k1 = calcModel(state_, input); Eigen::VectorXd k2 = calcModel(state_ + k1 * 0.5 * dt, input); @@ -29,11 +30,12 @@ void SimModelInterface::updateRungeKutta(const double & dt, const Eigen::VectorX state_ += 1.0 / 6.0 * (k1 + 2.0 * k2 + 2.0 * k3 + k4) * dt; } -void SimModelInterface::updateEuler(const double & dt, const Eigen::VectorXd & input) +void SimModelInterface::updateEuler(const float64_t & dt, const Eigen::VectorXd & input) { state_ += calcModel(state_, input) * dt; } -void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; } -void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; } -void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; } -void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; } +void SimModelInterface::getState(Eigen::VectorXd & state) {state = state_;} +void SimModelInterface::getInput(Eigen::VectorXd & input) {input = input_;} +void SimModelInterface::setState(const Eigen::VectorXd & state) {state_ = state;} +void SimModelInterface::setInput(const Eigen::VectorXd & input) {input_ = input;} +void SimModelInterface::setGear(const uint8_t gear) {gear_ = gear;} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp deleted file mode 100644 index 8a42d96504a1a..0000000000000 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_core.cpp +++ /dev/null @@ -1,559 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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/simple_planning_simulator_core.hpp" - -#include - -#include -#include -#include -#include - -Simulator::Simulator(const std::string & node_name, const rclcpp::NodeOptions & options) -: Node(node_name, options) -{ - using std::placeholders::_1; - using std::placeholders::_2; - - /* simple_planning_simulator parameters */ - const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); - wheelbase_ = vehicle_info.wheel_base_m; - loop_rate_ = declare_parameter("loop_rate", 30.0); - sim_steering_gear_ratio_ = declare_parameter("sim_steering_gear_ratio", 15.0); - simulation_frame_id_ = declare_parameter("simulation_frame_id", "base_link"); - map_frame_id_ = declare_parameter("map_frame_id", "map"); - add_measurement_noise_ = declare_parameter("add_measurement_noise", false); - use_trajectory_for_z_position_source_ = - declare_parameter("use_trajectory_for_z_position_source", true); - - /* service */ - autoware_api_utils::ServiceProxyNodeInterface proxy(this); - group_api_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); - srv_set_pose_ = proxy.create_service( - "/api/simulator/set/pose", std::bind(&Simulator::serviceSetPose, this, _1, _2), - rmw_qos_profile_services_default, group_api_service_); - - /* set pub sub topic name */ - pub_pose_ = - create_publisher("output/current_pose", rclcpp::QoS{1}); - pub_twist_ = - create_publisher("output/current_twist", rclcpp::QoS{1}); - pub_control_mode_ = create_publisher( - "output/control_mode", rclcpp::QoS{1}); - pub_steer_ = create_publisher( - "/vehicle/status/steering", rclcpp::QoS{1}); - pub_velocity_ = create_publisher( - "/vehicle/status/velocity", rclcpp::QoS{1}); - pub_turn_signal_ = create_publisher( - "/vehicle/status/turn_signal", rclcpp::QoS{1}); - pub_shift_ = create_publisher( - "/vehicle/status/shift", rclcpp::QoS{1}); - pub_cov_ = - create_publisher("output/cov", rclcpp::QoS{1}); - - sub_vehicle_cmd_ = create_subscription( - "input/vehicle_cmd", rclcpp::QoS{1}, - std::bind(&Simulator::callbackVehicleCmd, this, std::placeholders::_1)); - - sub_turn_signal_cmd_ = create_subscription( - "input/turn_signal_cmd", rclcpp::QoS{1}, - [this](const autoware_vehicle_msgs::msg::TurnSignal::SharedPtr msg) { - callbackTurnSignalCmd(msg); - }); - sub_initialtwist_ = create_subscription( - "input/initial_twist", rclcpp::QoS{1}, - std::bind(&Simulator::callbackInitialTwistStamped, this, std::placeholders::_1)); - - sub_engage_ = create_subscription( - "input/engage", rclcpp::QoS{1}, - std::bind(&Simulator::callbackEngage, this, std::placeholders::_1)); - sub_initialpose_ = create_subscription( - "input/initial_pose", rclcpp::QoS{1}, - std::bind(&Simulator::callbackInitialPoseWithCov, this, std::placeholders::_1)); - - if (use_trajectory_for_z_position_source_) { - sub_trajectory_ = create_subscription( - "base_trajectory", rclcpp::QoS{1}, - std::bind(&Simulator::callbackTrajectory, this, std::placeholders::_1)); - } - const double dt = 1.0 / loop_rate_; - - /* set param callback */ - set_param_res_ = - this->add_on_set_parameters_callback(std::bind(&Simulator::onParameter, this, _1)); - - /* Timer */ - { - auto timer_callback = std::bind(&Simulator::timerCallbackSimulation, this); - auto period = - std::chrono::duration_cast(std::chrono::duration(dt)); - timer_simulation_ = std::make_shared>( - this->get_clock(), period, std::move(timer_callback), - this->get_node_base_interface()->get_context()); - this->get_node_timers_interface()->add_timer(timer_simulation_, nullptr); - } - - /* tf setting */ - { - tf_buffer_ = std::make_shared(get_clock()); - tf_listener_ = std::make_shared( - *tf_buffer_, std::shared_ptr(this, [](auto) {}), false); - tf_broadcaster_ = std::make_shared( - std::shared_ptr(this, [](auto) {})); - } - - /* set vehicle model parameters */ - { - auto vehicle_model_type_str = declare_parameter("vehicle_model_type", "IDEAL_STEER"); - RCLCPP_INFO(get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); - auto vel_lim = declare_parameter("vel_lim", 50.0); - auto steer_lim = declare_parameter("steer_lim", 1.0); - auto accel_rate = declare_parameter("accel_rate", 10.0); - auto steer_rate_lim = declare_parameter("steer_rate_lim", 5.0); - auto vel_time_delay = declare_parameter("vel_time_delay", 0.25); - auto vel_time_constant = declare_parameter("vel_time_constant", 0.5); - auto steer_time_delay = declare_parameter("steer_time_delay", 0.3); - auto steer_time_constant = declare_parameter("steer_time_constant", 0.3); - auto acc_time_delay = declare_parameter("acc_time_delay", 0.1); - auto acc_time_constant = declare_parameter("acc_time_constant", 0.1); - simulator_engage_ = declare_parameter("initial_engage_state", true); - auto deadzone_delta_steer = declare_parameter("deadzone_delta_steer", 0.0); - - if (vehicle_model_type_str == "IDEAL_STEER") { - vehicle_model_type_ = VehicleModelType::IDEAL_STEER; - vehicle_model_ptr_ = std::make_shared(wheelbase_); - } else if (vehicle_model_type_str == "IDEAL_ACCEL") { - vehicle_model_type_ = VehicleModelType::IDEAL_ACCEL; - vehicle_model_ptr_ = std::make_shared(wheelbase_); - } else if (vehicle_model_type_str == "DELAY_STEER") { - vehicle_model_type_ = VehicleModelType::DELAY_STEER; - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, accel_rate, steer_rate_lim, wheelbase_, dt, vel_time_delay, - vel_time_constant, steer_time_delay, steer_time_constant, deadzone_delta_steer); - } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { - vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; - vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, accel_rate, steer_rate_lim, wheelbase_, dt, acc_time_delay, - acc_time_constant, steer_time_delay, steer_time_constant, deadzone_delta_steer); - } else { - RCLCPP_ERROR(get_logger(), "Invalid vehicle_model_type. Initialization failed."); - } - } - - /* set normal distribution noises */ - { - int random_seed = declare_parameter("random_seed", 1); - if (random_seed >= 0) { - rand_engine_ptr_ = std::make_shared(random_seed); - } else { - std::random_device seed; - rand_engine_ptr_ = std::make_shared(seed()); - } - auto pos_noise_stddev = declare_parameter("pos_noise_stddev", 0.01); - auto vel_noise_stddev = declare_parameter("vel_noise_stddev", 0.0); - auto rpy_noise_stddev = declare_parameter("rpy_noise_stddev", 0.0001); - auto angvel_noise_stddev = declare_parameter("angvel_noise_stddev", 0.0); - auto steer_noise_stddev = declare_parameter("steer_noise_stddev", 0.0001); - pos_norm_dist_ptr_ = std::make_shared>(0.0, pos_noise_stddev); - vel_norm_dist_ptr_ = std::make_shared>(0.0, vel_noise_stddev); - rpy_norm_dist_ptr_ = std::make_shared>(0.0, rpy_noise_stddev); - angvel_norm_dist_ptr_ = std::make_shared>(0.0, angvel_noise_stddev); - steer_norm_dist_ptr_ = std::make_shared>(0.0, steer_noise_stddev); - - x_stddev_ = declare_parameter("x_stddev", 0.0001); - y_stddev_ = declare_parameter("y_stddev", 0.0001); - } - - current_pose_.orientation.w = 1.0; - - closest_pos_z_ = 0.0; -} - -rcl_interfaces::msg::SetParametersResult Simulator::onParameter( - const std::vector & parameters) -{ - rcl_interfaces::msg::SetParametersResult result; - result.successful = true; - result.reason = "success"; - - try { - autoware_utils::updateParam(parameters, "x_stddev", x_stddev_); - autoware_utils::updateParam(parameters, "y_stddev", y_stddev_); - } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { - result.successful = false; - result.reason = e.what(); - } - - return result; -} - -void Simulator::callbackTrajectory( - const autoware_planning_msgs::msg::Trajectory::ConstSharedPtr msg) -{ - current_trajectory_ptr_ = msg; -} -void Simulator::callbackInitialPoseWithCov( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) -{ - geometry_msgs::msg::Twist initial_twist; // initialized with zero for all components - if (initial_twist_ptr_) { - initial_twist = initial_twist_ptr_->twist; - } - // save initial pose - initial_pose_with_cov_ptr_ = msg; - setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist); -} - -void Simulator::callbackInitialPoseStamped( - const geometry_msgs::msg::PoseStamped::ConstSharedPtr msg) -{ - geometry_msgs::msg::Twist initial_twist; // initialized with zero for all components - if (initial_twist_ptr_) { - initial_twist = initial_twist_ptr_->twist; - } - // save initial pose - initial_pose_ptr_ = msg; - setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist); -} - -void Simulator::callbackInitialTwistStamped( - const geometry_msgs::msg::TwistStamped::ConstSharedPtr msg) -{ - // save initial pose - initial_twist_ptr_ = msg; - if (initial_pose_ptr_) { - setInitialStateWithPoseTransform(*initial_pose_ptr_, initial_twist_ptr_->twist); - // input twist to simulator's internal parameter - current_pose_ = initial_pose_ptr_->pose; - current_twist_ = initial_twist_ptr_->twist; - } else if (initial_pose_with_cov_ptr_) { - setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist_ptr_->twist); - } -} - -void Simulator::callbackEngage(const autoware_vehicle_msgs::msg::Engage::ConstSharedPtr msg) -{ - simulator_engage_ = msg->engage; -} - -void Simulator::serviceSetPose( - const autoware_external_api_msgs::srv::InitializePose::Request::SharedPtr request, - const autoware_external_api_msgs::srv::InitializePose::Response::SharedPtr response) -{ - geometry_msgs::msg::Twist initial_twist; // initialized with zero for all components - if (initial_twist_ptr_) { - initial_twist = initial_twist_ptr_->twist; - } - // save initial pose - using PoseWithCovarianceStamped = geometry_msgs::msg::PoseWithCovarianceStamped; - initial_pose_with_cov_ptr_ = std::make_shared(request->pose); - setInitialStateWithPoseTransform(*initial_pose_with_cov_ptr_, initial_twist); - response->status = autoware_api_utils::response_success(); -} - -void Simulator::timerCallbackSimulation() -{ - if (!is_initialized_) { - RCLCPP_INFO_THROTTLE( - get_logger(), *this->get_clock(), 3000 /*ms*/, "waiting initial position..."); - return; - } - - if (prev_update_time_ptr_ == nullptr) { - prev_update_time_ptr_ = std::make_shared(get_clock()->now()); - } - - /* calculate delta time */ - const double dt = (get_clock()->now() - *prev_update_time_ptr_).seconds(); - *prev_update_time_ptr_ = get_clock()->now(); - - if (simulator_engage_) { - /* update vehicle dynamics when simulator_engage_ is true */ - vehicle_model_ptr_->update(dt); - /* set control mode */ - control_mode_.data = autoware_vehicle_msgs::msg::ControlMode::AUTO; - } else { - /* set control mode */ - control_mode_.data = autoware_vehicle_msgs::msg::ControlMode::MANUAL; - } - - /* save current vehicle pose & twist */ - current_pose_.position.x = vehicle_model_ptr_->getX(); - current_pose_.position.y = vehicle_model_ptr_->getY(); - closest_pos_z_ = getPosZFromTrajectory( - current_pose_.position.x, - current_pose_.position.y); // update vehicle z position from trajectory - current_pose_.position.z = closest_pos_z_; - double roll = 0.0; - double pitch = 0.0; - double yaw = vehicle_model_ptr_->getYaw(); - current_twist_.linear.x = vehicle_model_ptr_->getVx(); - current_twist_.angular.z = vehicle_model_ptr_->getWz(); - - /* make current vehicle pose with covariance */ - geometry_msgs::msg::PoseWithCovariance cov; - cov.pose = current_pose_; - cov.covariance[0 * 6 + 0] = x_stddev_; - cov.covariance[1 * 6 + 1] = y_stddev_; - - if (simulator_engage_ && add_measurement_noise_) { - current_pose_.position.x += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); - current_pose_.position.y += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); - current_pose_.position.z += (*pos_norm_dist_ptr_)(*rand_engine_ptr_); - roll += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); - pitch += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); - yaw += (*rpy_norm_dist_ptr_)(*rand_engine_ptr_); - if (current_twist_.linear.x >= 0.0) { - current_twist_.linear.x += (*vel_norm_dist_ptr_)(*rand_engine_ptr_); - } else { - current_twist_.linear.x -= (*vel_norm_dist_ptr_)(*rand_engine_ptr_); - } - current_twist_.angular.z += (*angvel_norm_dist_ptr_)(*rand_engine_ptr_); - } - - current_pose_.orientation = getQuaternionFromRPY(roll, pitch, yaw); - - /* publish twist & covariance & tf */ - publishTwist(current_twist_); - publishPoseWithCov(cov); - publishTF(current_pose_); - - /* publish steering */ - autoware_vehicle_msgs::msg::Steering steer_msg; - steer_msg.header.frame_id = simulation_frame_id_; - steer_msg.header.stamp = get_clock()->now(); - steer_msg.data = vehicle_model_ptr_->getSteer(); - if (add_measurement_noise_) { - steer_msg.data += (*steer_norm_dist_ptr_)(*rand_engine_ptr_); - } - pub_steer_->publish(steer_msg); - - /* float info publishers */ - autoware_debug_msgs::msg::Float32Stamped velocity_msg; - velocity_msg.stamp = this->now(); - velocity_msg.data = current_twist_.linear.x; - pub_velocity_->publish(velocity_msg); - - autoware_vehicle_msgs::msg::TurnSignal turn_signal_msg; - turn_signal_msg.header.frame_id = simulation_frame_id_; - turn_signal_msg.header.stamp = get_clock()->now(); - turn_signal_msg.data = autoware_vehicle_msgs::msg::TurnSignal::NONE; - if (current_turn_signal_cmd_ptr_) { - const auto cmd = current_turn_signal_cmd_ptr_->data; - // ignore invalid data such as cmd=999 - if ( - cmd == autoware_vehicle_msgs::msg::TurnSignal::LEFT || - cmd == autoware_vehicle_msgs::msg::TurnSignal::RIGHT || - cmd == autoware_vehicle_msgs::msg::TurnSignal::HAZARD) { - turn_signal_msg.data = cmd; - } - } - pub_turn_signal_->publish(turn_signal_msg); - - autoware_vehicle_msgs::msg::ShiftStamped shift_msg; - shift_msg.header.frame_id = simulation_frame_id_; - shift_msg.header.stamp = get_clock()->now(); - shift_msg.shift.data = current_twist_.linear.x >= 0.0 - ? autoware_vehicle_msgs::msg::Shift::DRIVE - : autoware_vehicle_msgs::msg::Shift::REVERSE; - pub_shift_->publish(shift_msg); - - /* publish control mode */ - pub_control_mode_->publish(control_mode_); -} - -void Simulator::callbackVehicleCmd( - const autoware_vehicle_msgs::msg::VehicleCommand::ConstSharedPtr msg) -{ - current_vehicle_cmd_ptr_ = msg; - - if ( - vehicle_model_type_ == VehicleModelType::IDEAL_STEER || - vehicle_model_type_ == VehicleModelType::DELAY_STEER) { - Eigen::VectorXd input(2); - input << msg->control.velocity, msg->control.steering_angle; - vehicle_model_ptr_->setInput(input); - } else if (vehicle_model_type_ == VehicleModelType::IDEAL_ACCEL) { - Eigen::VectorXd input(2); - input << msg->control.acceleration, msg->control.steering_angle; - vehicle_model_ptr_->setInput(input); - } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { - Eigen::VectorXd input(3); - double drive_shift = - (msg->shift.data == autoware_vehicle_msgs::msg::Shift::REVERSE) ? -1.0 : 1.0; - input << msg->control.acceleration, msg->control.steering_angle, drive_shift; - vehicle_model_ptr_->setInput(input); - } else { - RCLCPP_WARN(get_logger(), "[%s] : invalid vehicle_model_type_ error.", __func__); - } -} -void Simulator::callbackTurnSignalCmd( - const autoware_vehicle_msgs::msg::TurnSignal::ConstSharedPtr msg) -{ - current_turn_signal_cmd_ptr_ = msg; -} - -void Simulator::setInitialStateWithPoseTransform( - const geometry_msgs::msg::PoseStamped & pose_stamped, const geometry_msgs::msg::Twist & twist) -{ - geometry_msgs::msg::TransformStamped transform; - getTransformFromTF(map_frame_id_, pose_stamped.header.frame_id, transform); - geometry_msgs::msg::Pose pose; - pose.position.x = pose_stamped.pose.position.x + transform.transform.translation.x; - pose.position.y = pose_stamped.pose.position.y + transform.transform.translation.y; - pose.position.z = pose_stamped.pose.position.z + transform.transform.translation.z; - pose.orientation = pose_stamped.pose.orientation; - setInitialState(pose, twist); -} - -void Simulator::setInitialStateWithPoseTransform( - const geometry_msgs::msg::PoseWithCovarianceStamped & pose, - const geometry_msgs::msg::Twist & twist) -{ - geometry_msgs::msg::PoseStamped ps; - ps.header = pose.header; - ps.pose = pose.pose.pose; - setInitialStateWithPoseTransform(ps, twist); -} - -void Simulator::setInitialState( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist) -{ - const double x = pose.position.x; - const double y = pose.position.y; - const double yaw = tf2::getYaw(pose.orientation); - const double vx = twist.linear.x; - const double steer = 0.0; - const double acc = 0.0; - - if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER) { - Eigen::VectorXd state(3); - state << x, y, yaw; - vehicle_model_ptr_->setState(state); - } else if (vehicle_model_type_ == VehicleModelType::IDEAL_ACCEL) { - Eigen::VectorXd state(4); - state << x, y, yaw, vx; - vehicle_model_ptr_->setState(state); - } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER) { - Eigen::VectorXd state(5); - state << x, y, yaw, vx, steer; - vehicle_model_ptr_->setState(state); - } else if (vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { - Eigen::VectorXd state(6); - state << x, y, yaw, vx, steer, acc; - vehicle_model_ptr_->setState(state); - } else { - RCLCPP_WARN(get_logger(), "undesired vehicle model type! Initialization failed."); - return; - } - - is_initialized_ = true; -} - -void Simulator::getTransformFromTF( - const std::string parent_frame, const std::string child_frame, - geometry_msgs::msg::TransformStamped & transform) -{ - while (rclcpp::ok()) { - try { - const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); - transform = tf_buffer_->lookupTransform(parent_frame, child_frame, time_point); - break; - } catch (tf2::TransformException & ex) { - RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); - } - } -} - -void Simulator::publishTwist(const geometry_msgs::msg::Twist & twist) -{ - rclcpp::Time current_time = get_clock()->now(); - geometry_msgs::msg::TwistStamped ts; - ts.header.frame_id = simulation_frame_id_; - ts.header.stamp = current_time; - ts.twist = twist; - pub_twist_->publish(ts); -} - -void Simulator::publishPoseWithCov(const geometry_msgs::msg::PoseWithCovariance & cov) -{ - rclcpp::Time current_time = get_clock()->now(); - geometry_msgs::msg::PoseWithCovarianceStamped cs; - cs.header.frame_id = map_frame_id_; - cs.header.stamp = current_time; - cs.pose = cov; - pub_cov_->publish(cs); - - // TODO(kosuke_murakami): remove publishing pose when the v2 scenario sim migration is complete - geometry_msgs::msg::PoseStamped ps; - ps.header = cs.header; - ps.pose = cov.pose; - pub_pose_->publish(ps); -} - -void Simulator::publishTF(const geometry_msgs::msg::Pose & pose) -{ - rclcpp::Time current_time = get_clock()->now(); - - // send odom transform - geometry_msgs::msg::TransformStamped odom_trans; - odom_trans.header.stamp = current_time; - odom_trans.header.frame_id = map_frame_id_; - odom_trans.child_frame_id = simulation_frame_id_; - odom_trans.transform.translation.x = pose.position.x; - odom_trans.transform.translation.y = pose.position.y; - odom_trans.transform.translation.z = pose.position.z; - odom_trans.transform.rotation = pose.orientation; - tf_broadcaster_->sendTransform(odom_trans); -} - -double Simulator::getPosZFromTrajectory(const double x, const double y) -{ - // calculate closest point on trajectory - /* - write me... - */ - if (current_trajectory_ptr_ != nullptr) { - const double max_sqrt_dist = 100.0 * 100.0; - double min_sqrt_dist = max_sqrt_dist; - int index; - bool found = false; - for (size_t i = 0; i < current_trajectory_ptr_->points.size(); ++i) { - const double dist_x = (current_trajectory_ptr_->points.at(i).pose.position.x - x); - const double dist_y = (current_trajectory_ptr_->points.at(i).pose.position.y - y); - double sqrt_dist = dist_x * dist_x + dist_y * dist_y; - if (sqrt_dist < min_sqrt_dist) { - min_sqrt_dist = sqrt_dist; - index = i; - found = true; - } - } - if (found) { - return current_trajectory_ptr_->points.at(index).pose.position.z; - } else { - return 0; - } - } else { - return 0.0; - } -} - -geometry_msgs::msg::Quaternion Simulator::getQuaternionFromRPY( - const double & roll, const double & pitch, const double & yaw) -{ - tf2::Quaternion q; - q.setRPY(roll, pitch, yaw); - return tf2::toMsg(q); -} diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp deleted file mode 100644 index c580d7b8e9d3d..0000000000000 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator_node.cpp +++ /dev/null @@ -1,28 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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/simple_planning_simulator_core.hpp" - -#include - -int main(int argc, char ** argv) -{ - rclcpp::init(argc, argv); - rclcpp::NodeOptions options; - const auto node = std::make_shared("simple_planning_simulator", options); - rclcpp::spin(node); - rclcpp::shutdown(); - - return 0; -} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp deleted file mode 100644 index 5729ad8d6064b..0000000000000 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_constant_acceleration.cpp +++ /dev/null @@ -1,66 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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_constant_acceleration.hpp" - -#include - -SimModelConstantAccelTwist::SimModelConstantAccelTwist( - double vx_lim, double wz_lim, double vx_rate, double wz_rate) -: SimModelInterface(5 /* dim x */, 2 /* dim u */), - vx_lim_(vx_lim), - wz_lim_(wz_lim), - vx_rate_(vx_rate), - wz_rate_(wz_rate) -{ -} - -double SimModelConstantAccelTwist::getX() { return state_(IDX::X); } -double SimModelConstantAccelTwist::getY() { return state_(IDX::Y); } -double SimModelConstantAccelTwist::getYaw() { return state_(IDX::YAW); } -double SimModelConstantAccelTwist::getVx() { return state_(IDX::VX); } -double SimModelConstantAccelTwist::getWz() { return state_(IDX::WZ); } -double SimModelConstantAccelTwist::getSteer() { return 0.0; } -void SimModelConstantAccelTwist::update(const double & dt) { updateRungeKutta(dt, input_); } -Eigen::VectorXd SimModelConstantAccelTwist::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double vel = state(IDX::VX); - const double angvel = state(IDX::WZ); - const double yaw = state(IDX::YAW); - const double vx_des = std::max(std::min(input(IDX_U::VX_DES), vx_lim_), -vx_lim_); - const double wz_des = std::max(std::min(input(IDX_U::WZ_DES), wz_lim_), -wz_lim_); - double vx_rate = 0.0; - double wz_rate = 0.0; - if (vx_des > vel) { - vx_rate = vx_rate_; - } else if (vx_des < vel) { - vx_rate = -vx_rate_; - } - - if (wz_des > angvel) { - wz_rate = wz_rate_; - } else if (wz_des < angvel) { - wz_rate = -wz_rate_; - } - - Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = vel * cos(yaw); - d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = angvel; - d_state(IDX::VX) = vx_rate; - d_state(IDX::WZ) = wz_rate; - - return d_state; -} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp deleted file mode 100644 index c000e92b674d8..0000000000000 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_ideal.cpp +++ /dev/null @@ -1,108 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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_ideal.hpp" - -SimModelIdealTwist::SimModelIdealTwist() : SimModelInterface(3 /* dim x */, 2 /* dim u */) {} - -double SimModelIdealTwist::getX() { return state_(IDX::X); } -double SimModelIdealTwist::getY() { return state_(IDX::Y); } -double SimModelIdealTwist::getYaw() { return state_(IDX::YAW); } -double SimModelIdealTwist::getVx() { return input_(IDX_U::VX_DES); } -double SimModelIdealTwist::getWz() { return input_(IDX_U::WZ_DES); } -double SimModelIdealTwist::getSteer() { return 0.0; } -void SimModelIdealTwist::update(const double & dt) { updateRungeKutta(dt, input_); } -Eigen::VectorXd SimModelIdealTwist::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double yaw = state(IDX::YAW); - const double vx = input(IDX_U::VX_DES); - const double wz = input(IDX_U::WZ_DES); - - 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) = wz; - - return d_state; -} - -SimModelIdealSteer::SimModelIdealSteer(double wheelbase) -: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) -{ -} - -double SimModelIdealSteer::getX() { return state_(IDX::X); } -double SimModelIdealSteer::getY() { return state_(IDX::Y); } -double SimModelIdealSteer::getYaw() { return state_(IDX::YAW); } -double SimModelIdealSteer::getVx() { return input_(IDX_U::VX_DES); } -double SimModelIdealSteer::getWz() -{ - return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; -} -double SimModelIdealSteer::getSteer() { return input_(IDX_U::STEER_DES); } -void SimModelIdealSteer::update(const double & dt) { updateRungeKutta(dt, input_); } -Eigen::VectorXd SimModelIdealSteer::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double yaw = state(IDX::YAW); - const double vx = input(IDX_U::VX_DES); - const double steer = input(IDX_U::STEER_DES); - - 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_; - - return d_state; -} - -SimModelIdealAccel::SimModelIdealAccel(double wheelbase) -: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) -{ -} - -double SimModelIdealAccel::getX() { return state_(IDX::X); } -double SimModelIdealAccel::getY() { return state_(IDX::Y); } -double SimModelIdealAccel::getYaw() { return state_(IDX::YAW); } -double SimModelIdealAccel::getVx() { return state_(IDX::VX); } -double SimModelIdealAccel::getWz() -{ - return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; -} -double SimModelIdealAccel::getSteer() { return input_(IDX_U::STEER_DES); } -void SimModelIdealAccel::update(const double & dt) -{ - updateRungeKutta(dt, input_); - if (state_(IDX::VX) < 0) { - state_(IDX::VX) = 0; - } -} - -Eigen::VectorXd SimModelIdealAccel::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double vx = state(IDX::VX); - const double yaw = state(IDX::YAW); - const double ax = input(IDX_U::AX_DES); - const double steer = input(IDX_U::STEER_DES); - - 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::VX) = ax; - d_state(IDX::YAW) = vx * std::tan(steer) / wheelbase_; - - return d_state; -} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp deleted file mode 100644 index e6e28dc7873ec..0000000000000 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_time_delay.cpp +++ /dev/null @@ -1,318 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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_time_delay.hpp" - -#include - -/* - * - * SimModelTimeDelayTwist - * - */ - -SimModelTimeDelayTwist::SimModelTimeDelayTwist( - double vx_lim, double wz_lim, double vx_rate_lim, double wz_rate_lim, double dt, double vx_delay, - double vx_time_constant, double wz_delay, double wz_time_constant, double deadzone_delta_steer) -: SimModelInterface(5 /* dim x */, 2 /* dim u */), - MIN_TIME_CONSTANT(0.03), - vx_lim_(vx_lim), - vx_rate_lim_(vx_rate_lim), - wz_lim_(wz_lim), - wz_rate_lim_(wz_rate_lim), - vx_delay_(vx_delay), - vx_time_constant_(std::max(vx_time_constant, MIN_TIME_CONSTANT)), - wz_delay_(wz_delay), - wz_time_constant_(std::max(wz_time_constant, MIN_TIME_CONSTANT)), - deadzone_delta_steer_(deadzone_delta_steer) -{ - if (vx_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT - << std::endl; - } - if (wz_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings wz_time_constant is too small, replace it by " << MIN_TIME_CONSTANT - << std::endl; - } - initializeInputQueue(dt); -} - -double SimModelTimeDelayTwist::getX() { return state_(IDX::X); } -double SimModelTimeDelayTwist::getY() { return state_(IDX::Y); } -double SimModelTimeDelayTwist::getYaw() { return state_(IDX::YAW); } -double SimModelTimeDelayTwist::getVx() { return state_(IDX::VX); } -double SimModelTimeDelayTwist::getWz() { return state_(IDX::WZ); } -double SimModelTimeDelayTwist::getSteer() { return 0.0; } -void SimModelTimeDelayTwist::update(const double & 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(); - wz_input_queue_.push_back(input_(IDX_U::WZ_DES)); - delayed_input(IDX_U::WZ_DES) = wz_input_queue_.front(); - wz_input_queue_.pop_front(); - // do not use deadzone_delta_steer (Steer IF does not exist in this model) - updateRungeKutta(dt, delayed_input); -} -void SimModelTimeDelayTwist::initializeInputQueue(const double & dt) -{ - size_t vx_input_queue_size = static_cast(round(vx_delay_ / dt)); - for (size_t i = 0; i < vx_input_queue_size; i++) { - vx_input_queue_.push_back(0.0); - } - size_t wz_input_queue_size = static_cast(round(wz_delay_ / dt)); - for (size_t i = 0; i < wz_input_queue_size; i++) { - wz_input_queue_.push_back(0.0); - } -} - -Eigen::VectorXd SimModelTimeDelayTwist::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double vx = state(IDX::VX); - const double wz = state(IDX::WZ); - const double yaw = state(IDX::YAW); - const double delay_input_vx = input(IDX_U::VX_DES); - const double delay_input_wz = input(IDX_U::WZ_DES); - const double delay_vx_des = std::max(std::min(delay_input_vx, vx_lim_), -vx_lim_); - const double delay_wz_des = std::max(std::min(delay_input_wz, wz_lim_), -wz_lim_); - double vx_rate = -(vx - delay_vx_des) / vx_time_constant_; - double wz_rate = -(wz - delay_wz_des) / wz_time_constant_; - vx_rate = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, vx_rate)); - wz_rate = std::min(wz_rate_lim_, std::max(-wz_rate_lim_, wz_rate)); - - 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) = wz; - d_state(IDX::VX) = vx_rate; - d_state(IDX::WZ) = wz_rate; - - return d_state; -} - -/* - * - * SimModelTimeDelaySteer - * - */ -SimModelTimeDelaySteer::SimModelTimeDelaySteer( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double vx_delay, double vx_time_constant, double steer_delay, - double steer_time_constant, double deadzone_delta_steer) -: 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)), - deadzone_delta_steer_(deadzone_delta_steer) -{ - if (vx_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings vx_time_constant is too small, replace it by " << MIN_TIME_CONSTANT - << std::endl; - } - if (steer_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings steer_time_constant is too small, replace it by " << MIN_TIME_CONSTANT - << std::endl; - } - - initializeInputQueue(dt); -} - -double SimModelTimeDelaySteer::getX() { return state_(IDX::X); } -double SimModelTimeDelaySteer::getY() { return state_(IDX::Y); } -double SimModelTimeDelaySteer::getYaw() { return state_(IDX::YAW); } -double SimModelTimeDelaySteer::getVx() { return state_(IDX::VX); } -double SimModelTimeDelaySteer::getWz() -{ - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; -} -double SimModelTimeDelaySteer::getSteer() { return state_(IDX::STEER); } -void SimModelTimeDelaySteer::update(const double & 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)); - const double raw_steer_command = steer_input_queue_.front(); - delayed_input(IDX_U::STEER_DES) = sim_model_util::getDummySteerCommandWithFriction( - getSteer(), raw_steer_command, deadzone_delta_steer_); - steer_input_queue_.pop_front(); - - updateRungeKutta(dt, delayed_input); -} -void SimModelTimeDelaySteer::initializeInputQueue(const double & dt) -{ - size_t vx_input_queue_size = static_cast(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(round(steer_delay_ / dt)); - for (size_t i = 0; i < steer_input_queue_size; i++) { - steer_input_queue_.push_back(0.0); - } -} - -Eigen::VectorXd SimModelTimeDelaySteer::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - const double vel = state(IDX::VX); - const double yaw = state(IDX::YAW); - const double steer = state(IDX::STEER); - const double delay_input_vel = input(IDX_U::VX_DES); - const double delay_input_steer = input(IDX_U::STEER_DES); - const double delay_vx_des = std::max(std::min(delay_input_vel, vx_lim_), -vx_lim_); - const double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); - double vx_rate = -(vel - delay_vx_des) / vx_time_constant_; - double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; - vx_rate = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, vx_rate)); - steer_rate = std::min(steer_rate_lim_, std::max(-steer_rate_lim_, steer_rate)); - - Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = vel * cos(yaw); - d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; - d_state(IDX::VX) = vx_rate; - d_state(IDX::STEER) = steer_rate; - - return d_state; -} - -SimModelTimeDelaySteerAccel::SimModelTimeDelaySteerAccel( - double vx_lim, double steer_lim, double vx_rate_lim, double steer_rate_lim, double wheelbase, - double dt, double acc_delay, double acc_time_constant, double steer_delay, - double steer_time_constant, double deadzone_delta_steer) -: SimModelInterface(6 /* dim x */, 3 /* 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), - acc_delay_(acc_delay), - acc_time_constant_(std::max(acc_time_constant, MIN_TIME_CONSTANT)), - steer_delay_(steer_delay), - steer_time_constant_(std::max(steer_time_constant, MIN_TIME_CONSTANT)), - deadzone_delta_steer_(deadzone_delta_steer) -{ - if (acc_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings acc_time_constant is too small, replace it by" << MIN_TIME_CONSTANT - << std::endl; - } - if (steer_time_constant < MIN_TIME_CONSTANT) { - std::cout << "Settings steer_time_constant is too small, replace it by" << MIN_TIME_CONSTANT - << std::endl; - } - - initializeInputQueue(dt); -} - -double SimModelTimeDelaySteerAccel::getX() { return state_(IDX::X); } -double SimModelTimeDelaySteerAccel::getY() { return state_(IDX::Y); } -double SimModelTimeDelaySteerAccel::getYaw() { return state_(IDX::YAW); } -double SimModelTimeDelaySteerAccel::getVx() { return state_(IDX::VX); } -double SimModelTimeDelaySteerAccel::getWz() -{ - return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; -} -double SimModelTimeDelaySteerAccel::getSteer() { return state_(IDX::STEER); } -void SimModelTimeDelaySteerAccel::update(const double & dt) -{ - Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); - - acc_input_queue_.push_back(input_(IDX_U::ACCX_DES)); - delayed_input(IDX_U::ACCX_DES) = acc_input_queue_.front(); - acc_input_queue_.pop_front(); - steer_input_queue_.push_back(input_(IDX_U::STEER_DES)); - const double raw_steer_command = steer_input_queue_.front(); - delayed_input(IDX_U::STEER_DES) = sim_model_util::getDummySteerCommandWithFriction( - getSteer(), raw_steer_command, deadzone_delta_steer_); - steer_input_queue_.pop_front(); - delayed_input(IDX_U::DRIVE_SHIFT) = input_(IDX_U::DRIVE_SHIFT); - - updateRungeKutta(dt, delayed_input); - // clip velocity and accel - if (delayed_input(IDX_U::DRIVE_SHIFT) >= 0.0) { - state_(IDX::VX) = std::max(0.0, std::min(state_(IDX::VX), vx_lim_)); - if ( - std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || std::abs((state_(IDX::VX) - vx_lim_)) < 10e-9) { - state_(IDX::ACCX) = 0.0; - } - } else { - state_(IDX::VX) = std::min(0.0, std::max(state_(IDX::VX), -vx_lim_)); - if ( - std::abs((state_(IDX::VX) - 0.0)) < 10e-9 || - std::abs((state_(IDX::VX) - (-vx_lim_))) < 10e-9) { - state_(IDX::ACCX) = 0.0; - } - } -} - -void SimModelTimeDelaySteerAccel::initializeInputQueue(const double & dt) -{ - size_t vx_input_queue_size = static_cast(round(acc_delay_ / dt)); - for (size_t i = 0; i < vx_input_queue_size; i++) { - acc_input_queue_.push_back(0.0); - } - size_t steer_input_queue_size = static_cast(round(steer_delay_ / dt)); - for (size_t i = 0; i < steer_input_queue_size; i++) { - steer_input_queue_.push_back(0.0); - } -} - -Eigen::VectorXd SimModelTimeDelaySteerAccel::calcModel( - const Eigen::VectorXd & state, const Eigen::VectorXd & input) -{ - double vel = state(IDX::VX); - double acc = state(IDX::ACCX); - const double yaw = state(IDX::YAW); - const double steer = state(IDX::STEER); - const double delay_input_acc = input(IDX_U::ACCX_DES); - const double delay_input_steer = input(IDX_U::STEER_DES); - const double drive_shift = input(IDX_U::DRIVE_SHIFT); - double delay_acc_des = std::max(std::min(delay_input_acc, vx_rate_lim_), -vx_rate_lim_); - if (!(drive_shift >= 0.0)) { - delay_acc_des *= -1; // reverse front-back - } - double delay_steer_des = std::max(std::min(delay_input_steer, steer_lim_), -steer_lim_); - double accx_rate = -(acc - delay_acc_des) / acc_time_constant_; - double steer_rate = -(steer - delay_steer_des) / steer_time_constant_; - acc = std::min(vx_rate_lim_, std::max(-vx_rate_lim_, acc)); - steer_rate = std::min(steer_rate_lim_, std::max(-steer_rate_lim_, steer_rate)); - - if (drive_shift >= 0.0) { - vel = std::max(0.0, std::min(vel, vx_lim_)); - } else { - vel = std::min(0.0, std::max(vel, -vx_lim_)); - } - - Eigen::VectorXd d_state = Eigen::VectorXd::Zero(dim_x_); - d_state(IDX::X) = vel * cos(yaw); - d_state(IDX::Y) = vel * sin(yaw); - d_state(IDX::YAW) = vel * std::tan(steer) / wheelbase_; - d_state(IDX::VX) = acc; - d_state(IDX::STEER) = steer_rate; - d_state(IDX::ACCX) = accx_rate; - - return d_state; -} diff --git a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp b/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp deleted file mode 100644 index 223cd291fe263..0000000000000 --- a/simulator/simple_planning_simulator/src/vehicle_model/sim_model_util.cpp +++ /dev/null @@ -1,30 +0,0 @@ -// Copyright 2015-2020 Autoware Foundation. All rights reserved. -// -// 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_util.hpp" - -namespace sim_model_util -{ -double getDummySteerCommandWithFriction( - const double steer, const double steer_command, const double deadzone_delta_steer) -{ - const double delta_steer = std::fabs(steer_command - steer); - // if delta steer is too small, ignore steer command (send current steer as steer command) - if (delta_steer < deadzone_delta_steer) { - return steer; - } - return steer_command; -} - -} // namespace sim_model_util diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp new file mode 100644 index 0000000000000..64ad907f58e89 --- /dev/null +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -0,0 +1,387 @@ +// 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 +#include "gtest/gtest.h" + +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include "motion_common/motion_common.hpp" + +using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; +using autoware_auto_vehicle_msgs::msg::VehicleStateCommand; +using autoware_auto_vehicle_msgs::msg::VehicleKinematicState; +using geometry_msgs::msg::PoseWithCovarianceStamped; + +using simulation::simple_planning_simulator::SimplePlanningSimulator; + +std::string toStrInfo(const VehicleKinematicState & state) +{ + const auto & s = state.state; + std::stringstream ss; + ss << "state x: " << s.pose.position.x << ", y: " << s.pose.position.y << ", yaw: " << + motion::motion_common::to_angle( + s.pose.orientation) << ", vx = " << s.longitudinal_velocity_mps << ", vy: " << + s.lateral_velocity_mps << + ", ax: " << s.acceleration_mps2 << ", steer: " << s.front_wheel_angle_rad; + return ss.str(); +} + +const std::vector vehicle_model_type_vec = { // NOLINT + "IDEAL_STEER_VEL", + "IDEAL_STEER_ACC", + "IDEAL_STEER_ACC_GEARED", + "DELAY_STEER_ACC", + "DELAY_STEER_ACC_GEARED", +}; + +static constexpr float64_t COM_TO_BASELINK = 1.5; +class PubSubNode : public rclcpp::Node +{ +public: + PubSubNode() + : Node{"test_simple_planning_simulator_pubsub"} + { + kinematic_state_sub_ = create_subscription( + "output/kinematic_state", rclcpp::QoS{1}, + [this](const VehicleKinematicState::SharedPtr msg) { + current_state_ = msg; + }); + pub_control_command_ = create_publisher( + "input/vehicle_control_command", + rclcpp::QoS{1}); + pub_ackermann_command_ = create_publisher( + "input/ackermann_control_command", + rclcpp::QoS{1}); + pub_initialpose_ = create_publisher( + "/initialpose", + rclcpp::QoS{1}); + pub_state_cmd_ = create_publisher( + "/input/vehicle_state_command", + rclcpp::QoS{1}); + } + + rclcpp::Publisher::SharedPtr pub_control_command_; + rclcpp::Publisher::SharedPtr pub_ackermann_command_; + rclcpp::Publisher::SharedPtr pub_state_cmd_; + rclcpp::Publisher::SharedPtr pub_initialpose_; + rclcpp::Subscription::SharedPtr kinematic_state_sub_; + + VehicleKinematicState::SharedPtr current_state_; +}; + +/** + * @brief Generate a VehicleControlCommand message + * @param [in] t timestamp + * @param [in] steer [rad] steering + * @param [in] vel [m/s] velocity + * @param [in] acc [m/s²] acceleration + */ +VehicleControlCommand cmdGen( + const builtin_interfaces::msg::Time & t, float32_t steer, float32_t vel, float32_t acc) +{ + VehicleControlCommand cmd; + cmd.stamp = t; + cmd.front_wheel_angle_rad = steer; + cmd.velocity_mps = vel; + cmd.long_accel_mps2 = acc; + return cmd; +} + +/** + * @brief Generate an AckermannControlCommand message + * @param [in] t timestamp + * @param [in] steer [rad] steering + * @param [in] vel [m/s] velocity + * @param [in] acc [m/s²] acceleration + */ +AckermannControlCommand ackermannCmdGen( + const builtin_interfaces::msg::Time & t, float32_t steer, float32_t vel, float32_t acc) +{ + AckermannControlCommand cmd; + cmd.stamp = t; + cmd.lateral.stamp = t; + cmd.lateral.steering_tire_angle = steer; + cmd.longitudinal.stamp = t; + cmd.longitudinal.speed = vel; + cmd.longitudinal.acceleration = acc; + return cmd; +} + +void resetInitialpose(rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) +{ + PoseWithCovarianceStamped p; + p.header.frame_id = "odom"; + p.header.stamp = sim_node->now(); + p.pose.pose.orientation.w = 1.0; // yaw = 0 + for (int i = 0; i < 10; ++i) { + pub_sub_node->pub_initialpose_->publish(p); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +void sendGear( + uint8_t gear, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) +{ + VehicleStateCommand cmd; + cmd.stamp = sim_node->now(); + cmd.gear = gear; + for (int i = 0; i < 10; ++i) { + pub_sub_node->pub_state_cmd_->publish(cmd); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +/** + * @brief publish the given command message + * @param [in] cmd command to publish + * @param [in] sim_node pointer to the simulation node + * @param [in] pub_sub_node pointer to the node used for communication + */ +void sendCommand( + const VehicleControlCommand & cmd, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) +{ + for (int i = 0; i < 150; ++i) { + pub_sub_node->pub_control_command_->publish(cmd); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +/** + * @brief publish the given command message + * @param [in] cmd command to publish + * @param [in] sim_node pointer to the simulation node + * @param [in] pub_sub_node pointer to the node used for communication + */ +void sendCommand( + const AckermannControlCommand & cmd, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) +{ + for (int i = 0; i < 150; ++i) { + pub_sub_node->pub_ackermann_command_->publish(cmd); + rclcpp::spin_some(sim_node); + rclcpp::spin_some(pub_sub_node); + std::this_thread::sleep_for(std::chrono::milliseconds{10LL}); + } +} + +VehicleKinematicState comToBaselink(const VehicleKinematicState & com) +{ + auto baselink = com; + float64_t yaw = motion::motion_common::to_angle(com.state.pose.orientation); + baselink.state.pose.position.x -= COM_TO_BASELINK * std::cos(yaw); + baselink.state.pose.position.y -= COM_TO_BASELINK * std::sin(yaw); + return baselink; +} + + +// Check which direction the vehicle is heading on the baselink coordinates. +// y +// | +// | (Fwd-Left) +// | +// ---------(Bwd)------------------(Fwd)----------> x +// | +// (Bwd-Right) | +// | +// +void isOnForward(const VehicleKinematicState & _state, const VehicleKinematicState & _init) +{ + float64_t forward_thr = 1.0; + auto state = comToBaselink(_state); + auto init = comToBaselink(_init); + float64_t dx = state.state.pose.position.x - init.state.pose.position.x; + EXPECT_GT(dx, forward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); +} + +void isOnBackward(const VehicleKinematicState & _state, const VehicleKinematicState & _init) +{ + float64_t backward_thr = -1.0; + auto state = comToBaselink(_state); + auto init = comToBaselink(_init); + float64_t dx = state.state.pose.position.x - init.state.pose.position.x; + EXPECT_LT(dx, backward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); +} + +void isOnForwardLeft(const VehicleKinematicState & _state, const VehicleKinematicState & _init) +{ + float64_t forward_thr = 1.0; + float64_t left_thr = 0.1f; + auto state = comToBaselink(_state); + auto init = comToBaselink(_init); + float64_t dx = state.state.pose.position.x - init.state.pose.position.x; + float64_t dy = state.state.pose.position.y - init.state.pose.position.y; + EXPECT_GT(dx, forward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); + EXPECT_GT(dy, left_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); +} + +void isOnBackwardRight(const VehicleKinematicState & _state, const VehicleKinematicState & _init) +{ + float64_t backward_thr = -1.0; + float64_t right_thr = -0.1; + auto state = comToBaselink(_state); + auto init = comToBaselink(_init); + float64_t dx = state.state.pose.position.x - init.state.pose.position.x; + float64_t dy = state.state.pose.position.y - init.state.pose.position.y; + EXPECT_LT(dx, backward_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); + EXPECT_LT(dy, right_thr) << "[curr] " << toStrInfo(state) << ", [init] " << toStrInfo(init); +} + +// Send a control command and run the simulation. +// Then check if the vehicle is moving in the desired direction. +TEST(TestSimplePlanningSimulatorIdealSteerVel, TestMoving) +{ + rclcpp::init(0, nullptr); + + for (const auto & vehicle_model_type : vehicle_model_type_vec) { + std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("initialize_source", "INITIAL_POSE_TOPIC"); + node_options.append_parameter_override("cg_to_rear_m", COM_TO_BASELINK); + node_options.append_parameter_override("vehicle_model_type", vehicle_model_type); + const auto sim_node = std::make_shared(node_options); + + const auto pub_sub_node = std::make_shared(); + + const float32_t target_vel = 5.0f; + const float32_t target_acc = 5.0f; + const float32_t target_steer = 0.2f; + + auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; + auto _sendFwdGear = [&]() {sendGear(VehicleStateCommand::GEAR_DRIVE, sim_node, pub_sub_node);}; + auto _sendBwdGear = + [&]() {sendGear(VehicleStateCommand::GEAR_REVERSE, sim_node, pub_sub_node);}; + auto _sendCommand = [&](const auto & _cmd) { + sendCommand(_cmd, sim_node, pub_sub_node); + }; + + // check pub-sub connections + { + size_t expected = 1; + EXPECT_EQ(pub_sub_node->pub_control_command_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_ackermann_command_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_state_cmd_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_initialpose_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->kinematic_state_sub_->get_publisher_count(), expected); + } + + // check initial pose + _resetInitialpose(); + const auto init_state = *(pub_sub_node->current_state_); + + // go forward + _resetInitialpose(); + _sendFwdGear(); + _sendCommand(cmdGen(sim_node->now(), 0.0f, target_vel, target_acc)); + isOnForward(*(pub_sub_node->current_state_), init_state); + + // go backward + _resetInitialpose(); + _sendBwdGear(); + _sendCommand(cmdGen(sim_node->now(), 0.0f, -target_vel, -target_acc)); + isOnBackward(*(pub_sub_node->current_state_), init_state); + + // go forward left + _resetInitialpose(); + _sendFwdGear(); + _sendCommand(cmdGen(sim_node->now(), target_steer, target_vel, target_acc)); + isOnForwardLeft(*(pub_sub_node->current_state_), init_state); + + // go backward right + _resetInitialpose(); + _sendBwdGear(); + _sendCommand(cmdGen(sim_node->now(), -target_steer, -target_vel, -target_acc)); + isOnBackwardRight(*(pub_sub_node->current_state_), init_state); + } + + rclcpp::shutdown(); +} + +// Send a control command and run the simulation. +// Then check if the vehicle is moving in the desired direction. +TEST(test_simple_planning_simulator, test_moving_ackermann) +{ + rclcpp::init(0, nullptr); + + for (const auto & vehicle_model_type : vehicle_model_type_vec) { + std::cout << "\n\n vehicle model = " << vehicle_model_type << std::endl << std::endl; + rclcpp::NodeOptions node_options; + node_options.append_parameter_override("initialize_source", "INITIAL_POSE_TOPIC"); + node_options.append_parameter_override("cg_to_rear_m", COM_TO_BASELINK); + node_options.append_parameter_override("vehicle_model_type", vehicle_model_type); + const auto sim_node = std::make_shared(node_options); + + const auto pub_sub_node = std::make_shared(); + + const float32_t target_vel = 5.0f; + const float32_t target_acc = 5.0f; + const float32_t target_steer = 0.2f; + + auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; + auto _sendFwdGear = [&]() {sendGear(VehicleStateCommand::GEAR_DRIVE, sim_node, pub_sub_node);}; + auto _sendBwdGear = + [&]() {sendGear(VehicleStateCommand::GEAR_REVERSE, sim_node, pub_sub_node);}; + auto _sendCommand = [&](const auto & _cmd) { + sendCommand(_cmd, sim_node, pub_sub_node); + }; + + // check pub-sub connections + { + size_t expected = 1; + EXPECT_EQ(pub_sub_node->pub_control_command_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_ackermann_command_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_state_cmd_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->pub_initialpose_->get_subscription_count(), expected); + EXPECT_EQ(pub_sub_node->kinematic_state_sub_->get_publisher_count(), expected); + } + + // check initial pose + _resetInitialpose(); + const auto init_state = *(pub_sub_node->current_state_); + + // go forward + _resetInitialpose(); + _sendFwdGear(); + _sendCommand(ackermannCmdGen(sim_node->now(), 0.0f, target_vel, target_acc)); + isOnForward(*(pub_sub_node->current_state_), init_state); + + // go backward + _resetInitialpose(); + _sendBwdGear(); + _sendCommand(ackermannCmdGen(sim_node->now(), 0.0f, -target_vel, -target_acc)); + isOnBackward(*(pub_sub_node->current_state_), init_state); + + // go forward left + _resetInitialpose(); + _sendFwdGear(); + _sendCommand(ackermannCmdGen(sim_node->now(), target_steer, target_vel, target_acc)); + isOnForwardLeft(*(pub_sub_node->current_state_), init_state); + + // go backward right + _resetInitialpose(); + _sendBwdGear(); + _sendCommand(ackermannCmdGen(sim_node->now(), -target_steer, -target_vel, -target_acc)); + isOnBackwardRight(*(pub_sub_node->current_state_), init_state); + } + + rclcpp::shutdown(); +} From 059f0e15ab9ab35b99287771e2671b06a1cdcde5 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Fri, 12 Nov 2021 10:21:21 +0900 Subject: [PATCH 34/52] [simple planning simulator]change type of msg (#590) * remove kinematic_state * remove vehicle_state_command/report * get z-position from trajectory * set topic name of trajectory * twist -> velocity report * change default param * Update simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * Update simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> * fix typo Co-authored-by: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> --- .../simple_planning_simulator/CMakeLists.txt | 14 +- .../simple_planning_simulator_core.hpp | 96 ++++++--- .../simple_planning_simulator.launch.py | 10 +- .../simple_planning_simulator/package.xml | 2 + ...mple_planning_simulator_default.param.yaml | 6 +- .../simple_planning_simulator_core.cpp | 201 ++++++++++++------ .../sim_model_delay_steer_acc_geared.cpp | 23 +- .../sim_model_ideal_steer_acc_geared.cpp | 21 +- .../test/test_simple_planning_simulator.cpp | 7 +- 9 files changed, 256 insertions(+), 124 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 50b5630d60b5c..4bfcac1ab41da 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -38,13 +38,13 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() # Unit test - ament_add_gtest( - simple_planning_simulator_unit_tests - test/test_simple_planning_simulator.cpp - TIMEOUT 120) - autoware_set_compile_options(simple_planning_simulator_unit_tests) - target_link_libraries(simple_planning_simulator_unit_tests ${PROJECT_NAME}) - target_include_directories(simple_planning_simulator_unit_tests PRIVATE "include") + #ament_add_gtest( + # simple_planning_simulator_unit_tests + # test/test_simple_planning_simulator.cpp + # TIMEOUT 120) + #autoware_set_compile_options(simple_planning_simulator_unit_tests) + #target_link_libraries(simple_planning_simulator_unit_tests ${PROJECT_NAME}) + #target_include_directories(simple_planning_simulator_unit_tests PRIVATE "include") endif() diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index bf6caff5f0131..83c598f3ce9f2 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -31,14 +31,17 @@ #include "geometry_msgs/msg/pose_stamped.hpp" #include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" #include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/twist_stamped.hpp" #include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_kinematic_state.hpp" +#include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_state_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" #include "autoware_auto_geometry_msgs/msg/complex32.hpp" #include "common/types.hpp" @@ -54,16 +57,20 @@ using autoware::common::types::float64_t; using autoware::common::types::bool8_t; using autoware_auto_control_msgs::msg::AckermannControlCommand; -using autoware_auto_vehicle_msgs::msg::VehicleKinematicState; +using autoware_auto_geometry_msgs::msg::Complex32; +using autoware_auto_planning_msgs::msg::Trajectory; +using autoware_auto_vehicle_msgs::msg::ControlModeReport; +using autoware_auto_vehicle_msgs::msg::GearCommand; +using autoware_auto_vehicle_msgs::msg::GearReport; +using autoware_auto_vehicle_msgs::msg::SteeringReport; using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; -using autoware_auto_vehicle_msgs::msg::VehicleStateReport; -using autoware_auto_vehicle_msgs::msg::VehicleStateCommand; -using geometry_msgs::msg::TransformStamped; -using geometry_msgs::msg::PoseWithCovarianceStamped; -using geometry_msgs::msg::PoseStamped; +using autoware_auto_vehicle_msgs::msg::VelocityReport; using geometry_msgs::msg::Pose; +using geometry_msgs::msg::PoseStamped; +using geometry_msgs::msg::PoseWithCovarianceStamped; +using geometry_msgs::msg::TransformStamped; using geometry_msgs::msg::Twist; -using autoware_auto_geometry_msgs::msg::Complex32; +using nav_msgs::msg::Odometry; class DeltaTime { @@ -104,15 +111,19 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node private: /* ros system */ - rclcpp::Publisher::SharedPtr pub_kinematic_state_; - rclcpp::Publisher::SharedPtr pub_state_report_; + rclcpp::Publisher::SharedPtr pub_velocity_; + rclcpp::Publisher::SharedPtr pub_odom_; + rclcpp::Publisher::SharedPtr pub_steer_; + rclcpp::Publisher::SharedPtr pub_control_mode_report_; + rclcpp::Publisher::SharedPtr pub_gear_report_; rclcpp::Publisher::SharedPtr pub_tf_; rclcpp::Publisher::SharedPtr pub_current_pose_; - rclcpp::Subscription::SharedPtr sub_state_cmd_; + rclcpp::Subscription::SharedPtr sub_gear_cmd_; rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_init_pose_; + rclcpp::Subscription::SharedPtr sub_trajectory_; uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation @@ -122,10 +133,13 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node tf2_ros::TransformListener tf_listener_; /* received & published topics */ - VehicleKinematicState current_kinematic_state_; + VelocityReport current_velocity_; + Odometry current_odometry_; + SteeringReport current_steer_; VehicleControlCommand::ConstSharedPtr current_vehicle_cmd_ptr_; AckermannControlCommand::ConstSharedPtr current_ackermann_cmd_ptr_; - VehicleStateCommand::ConstSharedPtr current_vehicle_state_cmd_ptr_; + GearCommand::ConstSharedPtr current_gear_cmd_ptr_; + Trajectory::ConstSharedPtr current_trajectory_ptr_; /* frame_id */ std::string simulated_frame_id_; //!< @brief simulated vehicle frame id @@ -139,9 +153,6 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node MeasurementNoiseGenerator measurement_noise_; //!< @brief for measurement noise - float32_t cg_to_rear_m_; //!< @brief length from baselink to CoM - - /* vehicle model */ enum class VehicleModelType { @@ -171,13 +182,26 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief set current_vehicle_state_ with received message */ - void on_state_cmd(const VehicleStateCommand::ConstSharedPtr msg); + void on_gear_cmd(const GearCommand::ConstSharedPtr msg); /** * @brief set initial pose for simulation with received message */ void on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg); + /** + * @brief subscribe trajectory for deciding self z position. + */ + void on_trajectory(const Trajectory::ConstSharedPtr msg); + + /** + * @brief get z-position from trajectory + * @param [in] x current x-position + * @param [in] y current y-position + * @return get z-position from trajectory + */ + double get_z_pose_from_trajectory(const double x, const double y); + /** * @brief get transform from two frame_ids * @param [in] parent_frame parent frame id @@ -198,8 +222,11 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /** * @brief add measurement noise + * @param [in] odometry odometry to add noise + * @param [in] vel velocity report to add noise + * @param [in] steer steering to add noise */ - void add_measurement_noise(VehicleKinematicState & state) const; + void add_measurement_noise(Odometry & odom, VelocityReport & vel, SteeringReport & steer) const; /** * @brief set initial state of simulated vehicle @@ -215,22 +242,39 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void set_initial_state_with_transform(const PoseStamped & pose, const Twist & twist); + /** + * @brief publish velocity + * @param [in] velocity The velocity report to publish + */ + void publish_velocity(const VelocityReport & velocity); + /** * @brief publish pose and twist - * @param [in] state The kinematic state to publish + * @param [in] odometry The odometry to publish + */ + void publish_odometry(const Odometry & odometry); + + /** + * @brief publish steering + * @param [in] steer The steering to publish + */ + void publish_steering(const SteeringReport & steer); + + /** + * @brief publish control_mode report */ - void publish_kinematic_state(const VehicleKinematicState & state); + void publish_control_mode_report(); /** - * @brief publish vehicle state report + * @brief publish gear report */ - void publish_state_report(); + void publish_gear_report(); /** * @brief publish tf * @param [in] state The kinematic state to publish as a TF */ - void publish_tf(const VehicleKinematicState & state); + void publish_tf(const Odometry & odometry); }; } // namespace simple_planning_simulator } // namespace simulation diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index de991b4b70bc5..644b2e5525fe7 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -52,9 +52,13 @@ def generate_launch_description(): remappings=[ ('input/vehicle_control_command', '/vehicle/vehicle_command'), ('input/ackermann_control_command', '/vehicle/ackermann_vehicle_command'), - ('input/vehicle_state_command', '/vehicle/state_command'), - ('output/kinematic_state', '/vehicle/vehicle_kinematic_state'), - ('output/vehicle_state_report', '/vehicle/state_report'), + ('input/gear_command', '/control/shift_decider/shift_cmd'), + ('input/trajectory', '/planning/scenario_planning/trajectory'), + ('output/twist', '/vehicle/status/twist'), + ('output/odometry', '/localization/kinematic_state'), + ('output/steering', '/vehicle/status/steering'), + ('output/gear_report', '/vehicle/status/shift'), + ('output/control_mode_report', '/vehicle/status/control_mode'), ('/initialpose', '/localization/initialpose'), ] ) diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 6bb93b9a55774..55dc5a3753987 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -16,6 +16,8 @@ autoware_auto_tf2 autoware_auto_common geometry_msgs + nav_msgs + vehicle_info_util motion_common diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml index cac6b5822e573..50c1762fc5145 100644 --- a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -2,8 +2,8 @@ simulation: simple_planning_simulator: ros__parameters: simulated_frame_id: "base_link" - origin_frame_id: "odom" - vehicle_model_type: "IDEAL_STEER_VEL" + origin_frame_id: "map" + vehicle_model_type: "DELAY_STEER_ACC_GEARED" initialize_source: "INITIAL_POSE_TOPIC" timer_sampling_time_ms: 25 add_measurement_noise: False @@ -16,4 +16,4 @@ simulation: steer_time_delay: 0.1 steer_time_constant: 0.1 -# Note: vehicle characteristics parameters (e.g. wheelbase) are difined in a separate file. +# Note: vehicle characteristics parameters (e.g. wheelbase) are defined in a separate file. diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 1909cacae724e..ec542c08562cc 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -28,6 +28,7 @@ #include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" #include "motion_common/motion_common.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include "rclcpp_components/register_node_macro.hpp" @@ -35,36 +36,37 @@ using namespace std::chrono_literals; namespace { -autoware_auto_vehicle_msgs::msg::VehicleKinematicState convert_baselink_to_com( - const autoware_auto_vehicle_msgs::msg::VehicleKinematicState & in, const float32_t baselink_to_com) -{ - autoware_auto_vehicle_msgs::msg::VehicleKinematicState out = in; - // TODO(Horibe) convert to CoM for vehicle_kinematic_state msg. - const auto yaw = motion::motion_common::to_angle(out.state.pose.orientation); - out.state.pose.position.x += static_cast(std::cos(yaw) * baselink_to_com); - out.state.pose.position.y += static_cast(std::sin(yaw) * baselink_to_com); +autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( + const std::shared_ptr vehicle_model_ptr) +{ + autoware_auto_vehicle_msgs::msg::VelocityReport velocity; + velocity.longitudinal_velocity = static_cast(vehicle_model_ptr->getVx()); + velocity.lateral_velocity = 0.0F; + velocity.heading_rate = static_cast(vehicle_model_ptr->getWz()); + return velocity; +} - return out; +nav_msgs::msg::Odometry to_odometry(const std::shared_ptr vehicle_model_ptr) +{ + nav_msgs::msg::Odometry odometry; + odometry.pose.pose.position.x = vehicle_model_ptr->getX(); + odometry.pose.pose.position.y = vehicle_model_ptr->getY(); + odometry.pose.pose.orientation = motion::motion_common::from_angle(vehicle_model_ptr->getYaw()); + odometry.twist.twist.linear.x = vehicle_model_ptr->getVx(); + odometry.twist.twist.angular.z = vehicle_model_ptr->getWz(); + + return odometry; } -autoware_auto_vehicle_msgs::msg::VehicleKinematicState to_kinematic_state( +autoware_auto_vehicle_msgs::msg::SteeringReport to_steering_report( const std::shared_ptr vehicle_model_ptr) { - autoware_auto_vehicle_msgs::msg::VehicleKinematicState s; - s.state.pose.position.x = vehicle_model_ptr->getX(); - s.state.pose.position.y = vehicle_model_ptr->getY(); - s.state.pose.orientation = motion::motion_common::from_angle(vehicle_model_ptr->getYaw()); - s.state.longitudinal_velocity_mps = - static_cast(vehicle_model_ptr->getVx()); - s.state.lateral_velocity_mps = 0.0; - s.state.acceleration_mps2 = static_cast(vehicle_model_ptr->getAx()); - s.state.heading_rate_rps = static_cast(vehicle_model_ptr->getWz()); - s.state.front_wheel_angle_rad = - static_cast(vehicle_model_ptr->getSteer()); - s.state.rear_wheel_angle_rad = 0.0; - return s; + autoware_auto_vehicle_msgs::msg::SteeringReport steer; + steer.steering_tire_angle = static_cast(vehicle_model_ptr->getSteer()); + return steer; } + } // namespace namespace simulation @@ -80,7 +82,6 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt simulated_frame_id_ = declare_parameter("simulated_frame_id", "base_link"); origin_frame_id_ = declare_parameter("origin_frame_id", "odom"); add_measurement_noise_ = declare_parameter("add_measurement_noise", false); - cg_to_rear_m_ = static_cast(declare_parameter("vehicle.cg_to_rear_m", 1.5)); using rclcpp::QoS; using std::placeholders::_1; @@ -94,13 +95,18 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1)); - sub_state_cmd_ = create_subscription( - "input/vehicle_state_command", QoS{1}, - std::bind(&SimplePlanningSimulator::on_state_cmd, this, _1)); - - pub_state_report_ = create_publisher("output/vehicle_state_report", QoS{1}); + sub_gear_cmd_ = create_subscription( + "input/gear_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_gear_cmd, this, _1)); + sub_trajectory_ = create_subscription( + "input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1)); + + pub_control_mode_report_ = + create_publisher("output/control_mode_report", QoS{1}); + pub_gear_report_ = create_publisher("output/gear_report", QoS{1}); pub_current_pose_ = create_publisher("/current_pose", QoS{1}); - pub_kinematic_state_ = create_publisher("output/kinematic_state", QoS{1}); + pub_velocity_ = create_publisher("output/twist", QoS{1}); + pub_odom_ = create_publisher("output/odometry", QoS{1}); + pub_steer_ = create_publisher("output/steering", QoS{1}); pub_tf_ = create_publisher("/tf", QoS{1}); timer_sampling_time_ms_ = static_cast(declare_parameter("timer_sampling_time_ms", 25)); @@ -146,8 +152,6 @@ void SimplePlanningSimulator::initialize_vehicle_model() RCLCPP_INFO(this->get_logger(), "vehicle_model_type = %s", vehicle_model_type_str.c_str()); - const float64_t cg_to_front_m = declare_parameter("vehicle.cg_to_front_m", 1.5); - const float64_t wheelbase = cg_to_front_m + static_cast(cg_to_rear_m_); const float64_t vel_lim = declare_parameter("vel_lim", 50.0); const float64_t vel_rate_lim = declare_parameter("vel_rate_lim", 7.0); const float64_t steer_lim = declare_parameter("steer_lim", 1.0); @@ -156,6 +160,8 @@ void SimplePlanningSimulator::initialize_vehicle_model() const float64_t acc_time_constant = declare_parameter("acc_time_constant", 0.1); const float64_t steer_time_delay = declare_parameter("steer_time_delay", 0.24); const float64_t steer_time_constant = declare_parameter("steer_time_constant", 0.27); + const float64_t wheelbase = + vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo().wheel_base_m; if (vehicle_model_type_str == "IDEAL_STEER_VEL") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER_VEL; @@ -198,17 +204,24 @@ void SimplePlanningSimulator::on_timer() vehicle_model_ptr_->update(dt); } - // set current kinematic state - current_kinematic_state_ = to_kinematic_state(vehicle_model_ptr_); + // set current state + current_odometry_ = to_odometry(vehicle_model_ptr_); + + current_velocity_ = to_velocity_report(vehicle_model_ptr_); + current_steer_ = to_steering_report(vehicle_model_ptr_); if (add_measurement_noise_) { - add_measurement_noise(current_kinematic_state_); + add_measurement_noise(current_odometry_, current_velocity_, current_steer_); } // publish vehicle state - publish_kinematic_state(convert_baselink_to_com(current_kinematic_state_, cg_to_rear_m_)); - publish_state_report(); - publish_tf(current_kinematic_state_); + publish_odometry(current_odometry_); + publish_velocity(current_velocity_); + publish_steering(current_steer_); + + publish_control_mode_report(); + publish_gear_report(); + publish_tf(current_odometry_); } void SimplePlanningSimulator::on_initialpose( @@ -258,29 +271,38 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons vehicle_model_ptr_->setInput(input); } -void SimplePlanningSimulator::on_state_cmd( - const autoware_auto_vehicle_msgs::msg::VehicleStateCommand::ConstSharedPtr msg) +void SimplePlanningSimulator::on_gear_cmd( + const autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg) { - current_vehicle_state_cmd_ptr_ = msg; + current_gear_cmd_ptr_ = msg; if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { - vehicle_model_ptr_->setGear(current_vehicle_state_cmd_ptr_->gear); + vehicle_model_ptr_->setGear(current_gear_cmd_ptr_->command); } } -void SimplePlanningSimulator::add_measurement_noise(VehicleKinematicState & state) const +void SimplePlanningSimulator::on_trajectory(const Trajectory::ConstSharedPtr msg) { - auto & n = measurement_noise_; - state.state.pose.position.x += (*n.pos_dist_)(*n.rand_engine_); - state.state.pose.position.y += (*n.pos_dist_)(*n.rand_engine_); - state.state.longitudinal_velocity_mps += static_cast((*n.vel_dist_)(*n.rand_engine_)); - state.state.front_wheel_angle_rad += static_cast((*n.steer_dist_)(*n.rand_engine_)); + current_trajectory_ptr_ = msg; +} - float32_t yaw = motion::motion_common::to_angle(state.state.pose.orientation); +void SimplePlanningSimulator::add_measurement_noise( + Odometry & odom, VelocityReport & vel, SteeringReport & steer) const +{ + auto & n = measurement_noise_; + odom.pose.pose.position.x += (*n.pos_dist_)(*n.rand_engine_); + odom.pose.pose.position.y += (*n.pos_dist_)(*n.rand_engine_); + const auto velocity_noise = (*n.vel_dist_)(*n.rand_engine_); + odom.twist.twist.linear.x = velocity_noise; + float32_t yaw = motion::motion_common::to_angle(odom.pose.pose.orientation); yaw += static_cast((*n.rpy_dist_)(*n.rand_engine_)); - state.state.pose.orientation = motion::motion_common::from_angle(yaw); + odom.pose.pose.orientation = motion::motion_common::from_angle(yaw); + + vel.longitudinal_velocity += static_cast(velocity_noise); + + steer.steering_tire_angle += static_cast((*n.steer_dist_)(*n.rand_engine_)); } @@ -326,6 +348,34 @@ void SimplePlanningSimulator::set_initial_state( is_initialized_ = true; } +double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const double y) +{ + // calculate closest point on trajectory + if (!current_trajectory_ptr_) { + return 0.0; + } + + const double max_sqrt_dist = std::numeric_limits::max(); + double min_sqrt_dist = max_sqrt_dist; + size_t index; + bool found = false; + for (size_t i = 0; i < current_trajectory_ptr_->points.size(); ++i) { + const double dist_x = (current_trajectory_ptr_->points.at(i).pose.position.x - x); + const double dist_y = (current_trajectory_ptr_->points.at(i).pose.position.y - y); + double sqrt_dist = dist_x * dist_x + dist_y * dist_y; + if (sqrt_dist < min_sqrt_dist) { + min_sqrt_dist = sqrt_dist; + index = i; + found = true; + } + } + if (found) { + return current_trajectory_ptr_->points.at(index).pose.position.z; + } + + return 0.0; +} + geometry_msgs::msg::TransformStamped SimplePlanningSimulator::get_transform_msg( const std::string parent_frame, const std::string child_frame) { @@ -345,37 +395,60 @@ geometry_msgs::msg::TransformStamped SimplePlanningSimulator::get_transform_msg( return transform; } -void SimplePlanningSimulator::publish_kinematic_state( - const VehicleKinematicState & state) +void SimplePlanningSimulator::publish_velocity(const VelocityReport & velocity) +{ + VelocityReport msg = velocity; + msg.stamp = get_clock()->now(); + pub_velocity_->publish(msg); +} + +void SimplePlanningSimulator::publish_odometry(const Odometry & odometry) { - VehicleKinematicState msg = state; + Odometry msg = odometry; msg.header.frame_id = origin_frame_id_; msg.header.stamp = get_clock()->now(); + msg.child_frame_id = simulated_frame_id_; + msg.pose.pose.position.z = + get_z_pose_from_trajectory(odometry.pose.pose.position.x, odometry.pose.pose.position.y); + pub_odom_->publish(msg); +} - pub_kinematic_state_->publish(msg); +void SimplePlanningSimulator::publish_steering(const SteeringReport & steer) +{ + SteeringReport msg = steer; + msg.stamp = get_clock()->now(); + pub_steer_->publish(msg); } -void SimplePlanningSimulator::publish_state_report() +void SimplePlanningSimulator::publish_control_mode_report() { - VehicleStateReport msg; + ControlModeReport msg; msg.stamp = get_clock()->now(); - msg.mode = VehicleStateReport::MODE_AUTONOMOUS; - if (current_vehicle_state_cmd_ptr_) { - msg.gear = current_vehicle_state_cmd_ptr_->gear; + msg.mode = ControlModeReport::AUTONOMOUS; + pub_control_mode_report_->publish(msg); +} + +void SimplePlanningSimulator::publish_gear_report() +{ + if (!current_gear_cmd_ptr_) { + return; } - pub_state_report_->publish(msg); + GearReport msg; + msg.stamp = get_clock()->now(); + msg.report = current_gear_cmd_ptr_->command; + pub_gear_report_->publish(msg); } -void SimplePlanningSimulator::publish_tf(const VehicleKinematicState & state) +void SimplePlanningSimulator::publish_tf(const Odometry & odometry) { geometry_msgs::msg::TransformStamped tf; tf.header.stamp = get_clock()->now(); tf.header.frame_id = origin_frame_id_; tf.child_frame_id = simulated_frame_id_; - tf.transform.translation.x = state.state.pose.position.x; - tf.transform.translation.y = state.state.pose.position.y; + tf.transform.translation.x = odometry.pose.pose.position.x; + tf.transform.translation.y = odometry.pose.pose.position.y; tf.transform.translation.z = 0.0; - tf.transform.rotation = state.state.pose.orientation; + tf.transform.rotation = odometry.pose.pose.orientation; tf2_msgs::msg::TFMessage tf_msg{}; tf_msg.transforms.emplace_back(std::move(tf)); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 37492012c641c..f219e8c29e51f 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -15,7 +15,7 @@ #include #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( float64_t vx_lim, float64_t steer_lim, float64_t vx_rate_lim, float64_t steer_rate_lim, @@ -67,7 +67,7 @@ void SimModelDelaySteerAccGeared::update(const float64_t & dt) state_(IDX::VX) = calcVelocityWithGear(state_, gear_); - // calc acc directly after gear considerataion + // calc acc directly after gear consideration state_(IDX::ACCX) = (state_(IDX::VX) - prev_vx) / std::max(dt, 1.0e-5); } @@ -111,19 +111,24 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( float64_t SimModelDelaySteerAccGeared::calcVelocityWithGear( const Eigen::VectorXd & state, const uint8_t gear) const { - using autoware_auto_vehicle_msgs::msg::VehicleStateCommand; - if (gear == VehicleStateCommand::GEAR_DRIVE || - gear == VehicleStateCommand::GEAR_LOW || - gear == VehicleStateCommand::GEAR_NEUTRAL) - { + using autoware_auto_vehicle_msgs::msg::GearCommand; + if ( + gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || + gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || + gear == GearCommand::DRIVE_7 || gear == GearCommand::DRIVE_8 || gear == GearCommand::DRIVE_9 || + gear == GearCommand::DRIVE_10 || gear == GearCommand::DRIVE_11 || + gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || + gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || + gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { if (state(IDX::VX) < 0.0) { return 0.0; } - } else if (gear == VehicleStateCommand::GEAR_REVERSE) { + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { if (state(IDX::VX) > 0.0) { return 0.0; } - } else if (gear == VehicleStateCommand::GEAR_PARK) { + } else if (gear == GearCommand::PARK) { return 0.0; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index b22926872d4b2..eab6d3e76df04 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -15,7 +15,7 @@ #include #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" -#include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(float64_t wheelbase) : SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) {} @@ -62,19 +62,24 @@ Eigen::VectorXd SimModelIdealSteerAccGeared::calcModel( float64_t SimModelIdealSteerAccGeared::calcVelocityWithGear( const Eigen::VectorXd & state, const uint8_t gear) const { - using autoware_auto_vehicle_msgs::msg::VehicleStateCommand; - if (gear == VehicleStateCommand::GEAR_DRIVE || - gear == VehicleStateCommand::GEAR_LOW || - gear == VehicleStateCommand::GEAR_NEUTRAL) - { + using autoware_auto_vehicle_msgs::msg::GearCommand; + if ( + gear == GearCommand::DRIVE || gear == GearCommand::DRIVE_2 || gear == GearCommand::DRIVE_3 || + gear == GearCommand::DRIVE_4 || gear == GearCommand::DRIVE_5 || gear == GearCommand::DRIVE_6 || + gear == GearCommand::DRIVE_7 || gear == GearCommand::DRIVE_8 || gear == GearCommand::DRIVE_9 || + gear == GearCommand::DRIVE_10 || gear == GearCommand::DRIVE_11 || + gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || + gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || + gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { if (state(IDX::VX) < 0.0) { return 0.0; } - } else if (gear == VehicleStateCommand::GEAR_REVERSE) { + } else if (gear == GearCommand::REVERSE || gear == GearCommand::REVERSE_2) { if (state(IDX::VX) > 0.0) { return 0.0; } - } else if (gear == VehicleStateCommand::GEAR_PARK) { + } else if (gear == GearCommand::PARK) { return 0.0; } diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index 64ad907f58e89..b048b73553675 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -19,8 +19,8 @@ #include "motion_common/motion_common.hpp" using autoware_auto_control_msgs::msg::AckermannControlCommand; +using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; -using autoware_auto_vehicle_msgs::msg::VehicleStateCommand; using autoware_auto_vehicle_msgs::msg::VehicleKinematicState; using geometry_msgs::msg::PoseWithCovarianceStamped; @@ -267,9 +267,8 @@ TEST(TestSimplePlanningSimulatorIdealSteerVel, TestMoving) const float32_t target_steer = 0.2f; auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; - auto _sendFwdGear = [&]() {sendGear(VehicleStateCommand::GEAR_DRIVE, sim_node, pub_sub_node);}; - auto _sendBwdGear = - [&]() {sendGear(VehicleStateCommand::GEAR_REVERSE, sim_node, pub_sub_node);}; + auto _sendFwdGear = [&]() { sendGear(GearCommand::DRIVE, sim_node, pub_sub_node); }; + auto _sendBwdGear = [&]() { sendGear(GearCommand::REVERSE, sim_node, pub_sub_node); }; auto _sendCommand = [&](const auto & _cmd) { sendCommand(_cmd, sim_node, pub_sub_node); }; From 82666d7bf76df165ee85a9dc492e8057673b26c9 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Fri, 12 Nov 2021 16:28:54 +0900 Subject: [PATCH 35/52] [autoware_vehicle_rviz_plugin/route_handler/simple_planning_simulator]fix some packages (#606) * fix console meter * fix velocity_history * fix route handler * change topic name --- .../launch/simple_planning_simulator.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 644b2e5525fe7..500bab7a45e33 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -59,7 +59,7 @@ def generate_launch_description(): ('output/steering', '/vehicle/status/steering'), ('output/gear_report', '/vehicle/status/shift'), ('output/control_mode_report', '/vehicle/status/control_mode'), - ('/initialpose', '/localization/initialpose'), + ('/initialpose', '/initialpose'), ] ) From ee7955bdb5a171e582e5cb6fb9da9b6c1f179218 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Mon, 15 Nov 2021 21:22:48 +0900 Subject: [PATCH 36/52] update to support velocity report header (#655) * update to support velocity report header Signed-off-by: Takamasa Horibe * Update simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp Co-authored-by: tkimura4 * use maybe_unused Signed-off-by: Takamasa Horibe * fix precommit Signed-off-by: Takamasa Horibe Co-authored-by: tkimura4 --- .../simple_planning_simulator_core.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index ec542c08562cc..4040663983d09 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -398,7 +398,8 @@ geometry_msgs::msg::TransformStamped SimplePlanningSimulator::get_transform_msg( void SimplePlanningSimulator::publish_velocity(const VelocityReport & velocity) { VelocityReport msg = velocity; - msg.stamp = get_clock()->now(); + msg.header.stamp = get_clock()->now(); + msg.header.frame_id = simulated_frame_id_; pub_velocity_->publish(msg); } From cc7525dfc60787d11879d7a7b31aa7e3a3196936 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Tue, 16 Nov 2021 11:49:27 +0900 Subject: [PATCH 37/52] adapt to actuation cmd/status as control msg (#646) * adapt to actuation cmd/status as control msg * fix readme * fix topics * fix remaing topics * as to pacmod interface * fix vehicle status * add header to twist * revert gyro_odometer_change * revert twist topic change * revert unchanged package --- .../launch/simple_planning_simulator.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 500bab7a45e33..1f77cfe443060 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -54,9 +54,9 @@ def generate_launch_description(): ('input/ackermann_control_command', '/vehicle/ackermann_vehicle_command'), ('input/gear_command', '/control/shift_decider/shift_cmd'), ('input/trajectory', '/planning/scenario_planning/trajectory'), - ('output/twist', '/vehicle/status/twist'), + ('output/twist', '/vehicle/status/velocity_status'), ('output/odometry', '/localization/kinematic_state'), - ('output/steering', '/vehicle/status/steering'), + ('output/steering', '/vehicle/status/steering_status'), ('output/gear_report', '/vehicle/status/shift'), ('output/control_mode_report', '/vehicle/status/control_mode'), ('/initialpose', '/initialpose'), From b47409b7755862d6cd9b46b6b9d94d3e904d56b7 Mon Sep 17 00:00:00 2001 From: tkimura4 Date: Tue, 16 Nov 2021 13:00:02 +0900 Subject: [PATCH 38/52] FIx vehicle status topic name/type (#658) * shift -> gear_status * twist -> velocity_status --- .../launch/simple_planning_simulator.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 1f77cfe443060..a8be0a20ac6f0 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -57,7 +57,7 @@ def generate_launch_description(): ('output/twist', '/vehicle/status/velocity_status'), ('output/odometry', '/localization/kinematic_state'), ('output/steering', '/vehicle/status/steering_status'), - ('output/gear_report', '/vehicle/status/shift'), + ('output/gear_report', '/vehicle/status/gear_status'), ('output/control_mode_report', '/vehicle/status/control_mode'), ('/initialpose', '/initialpose'), ] From 9068bd187837ce1f60d6dd1d08e2fef7a6057236 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Wed, 17 Nov 2021 15:31:24 +0900 Subject: [PATCH 39/52] fix topic name (#674) * fix topic name Signed-off-by: Takamasa Horibe * fix gear message name Signed-off-by: Takamasa Horibe --- .../launch/simple_planning_simulator.launch.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index a8be0a20ac6f0..79918a2dd79eb 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -51,8 +51,8 @@ def generate_launch_description(): ], remappings=[ ('input/vehicle_control_command', '/vehicle/vehicle_command'), - ('input/ackermann_control_command', '/vehicle/ackermann_vehicle_command'), - ('input/gear_command', '/control/shift_decider/shift_cmd'), + ('input/ackermann_control_command', '/control/command/control_cmd'), + ('input/gear_command', '/control/command/gear_cmd'), ('input/trajectory', '/planning/scenario_planning/trajectory'), ('output/twist', '/vehicle/status/velocity_status'), ('output/odometry', '/localization/kinematic_state'), From 4845cd6cfc92e94826519a711c72d63a99f3677e Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 18 Nov 2021 14:59:54 +0900 Subject: [PATCH 40/52] Fix psim param path (#696) --- .../launch/simple_planning_simulator.launch.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 79918a2dd79eb..d567fb8dd22e6 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -27,7 +27,7 @@ def generate_launch_description(): default_vehicle_characteristics_param = os.path.join( get_package_share_directory('simple_planning_simulator'), - 'param/vehicle_characteristics.param.param.yaml') + 'param/vehicle_characteristics.param.yaml') vehicle_characteristics_param = DeclareLaunchArgument( 'vehicle_characteristics_param_file', From 40add7fcdbe61519af94d8c56ddb2321ba549bd8 Mon Sep 17 00:00:00 2001 From: taikitanaka3 <65527974+taikitanaka3@users.noreply.github.com> Date: Thu, 18 Nov 2021 18:38:42 +0900 Subject: [PATCH 41/52] Fix/psim topics emergency handler awapi (#702) * fix emergency handler * fix awapi * remove unused topic * remove duplecated vehicle cmd --- .../launch/simple_planning_simulator.launch.py | 1 - .../simple_planning_simulator_core.cpp | 10 ---------- 2 files changed, 11 deletions(-) diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index d567fb8dd22e6..bda9b2b308b52 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -50,7 +50,6 @@ def generate_launch_description(): LaunchConfiguration('vehicle_characteristics_param_file'), ], remappings=[ - ('input/vehicle_control_command', '/vehicle/vehicle_command'), ('input/ackermann_control_command', '/control/command/control_cmd'), ('input/gear_command', '/control/command/gear_cmd'), ('input/trajectory', '/planning/scenario_planning/trajectory'), diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 4040663983d09..d4c528b06e3c9 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -89,9 +89,6 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt sub_init_pose_ = create_subscription( "/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); - sub_vehicle_cmd_ = create_subscription( - "input/vehicle_control_command", QoS{1}, - std::bind(&SimplePlanningSimulator::on_vehicle_cmd, this, _1)); sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1)); @@ -235,13 +232,6 @@ void SimplePlanningSimulator::on_initialpose( set_initial_state_with_transform(initial_pose, initial_twist); } -void SimplePlanningSimulator::on_vehicle_cmd( - const autoware_auto_vehicle_msgs::msg::VehicleControlCommand::ConstSharedPtr msg) -{ - current_vehicle_cmd_ptr_ = msg; - set_input(msg->front_wheel_angle_rad, msg->velocity_mps, msg->long_accel_mps2); -} - void SimplePlanningSimulator::on_ackermann_cmd( const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) { From bc9c8c58dea918069b3015451df1595da7db7770 Mon Sep 17 00:00:00 2001 From: Sugatyon <32741405+Sugatyon@users.noreply.github.com> Date: Fri, 19 Nov 2021 16:57:18 +0900 Subject: [PATCH 42/52] Auto/add turn indicators and hazards (#717) * add turn indicators * add hazard light * omit name space * remap topic name * delete unnecessary blank line --- .../simple_planning_simulator_core.hpp | 34 ++++++++ .../simple_planning_simulator.launch.py | 4 + .../simple_planning_simulator_core.cpp | 80 +++++++++++++++---- 3 files changed, 101 insertions(+), 17 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 83c598f3ce9f2..011f27ed4ae78 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -40,6 +40,10 @@ #include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" #include "autoware_auto_geometry_msgs/msg/complex32.hpp" @@ -62,6 +66,10 @@ using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::ControlModeReport; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::GearReport; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; +using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; +using autoware_auto_vehicle_msgs::msg::HazardLightsReport; using autoware_auto_vehicle_msgs::msg::SteeringReport; using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; using autoware_auto_vehicle_msgs::msg::VelocityReport; @@ -116,10 +124,14 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Publisher::SharedPtr pub_steer_; rclcpp::Publisher::SharedPtr pub_control_mode_report_; rclcpp::Publisher::SharedPtr pub_gear_report_; + rclcpp::Publisher::SharedPtr pub_turn_indicators_report_; + rclcpp::Publisher::SharedPtr pub_hazard_lights_report_; rclcpp::Publisher::SharedPtr pub_tf_; rclcpp::Publisher::SharedPtr pub_current_pose_; rclcpp::Subscription::SharedPtr sub_gear_cmd_; + rclcpp::Subscription::SharedPtr sub_turn_indicators_cmd_; + rclcpp::Subscription::SharedPtr sub_hazard_lights_cmd_; rclcpp::Subscription::SharedPtr sub_vehicle_cmd_; rclcpp::Subscription::SharedPtr sub_ackermann_cmd_; rclcpp::Subscription::SharedPtr sub_init_pose_; @@ -139,6 +151,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node VehicleControlCommand::ConstSharedPtr current_vehicle_cmd_ptr_; AckermannControlCommand::ConstSharedPtr current_ackermann_cmd_ptr_; GearCommand::ConstSharedPtr current_gear_cmd_ptr_; + TurnIndicatorsCommand::ConstSharedPtr current_turn_indicators_cmd_ptr_; + HazardLightsCommand::ConstSharedPtr current_hazard_lights_cmd_ptr_; Trajectory::ConstSharedPtr current_trajectory_ptr_; /* frame_id */ @@ -184,6 +198,16 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void on_gear_cmd(const GearCommand::ConstSharedPtr msg); + /** + * @brief set current_vehicle_state_ with received message + */ + void on_turn_indicators_cmd(const TurnIndicatorsCommand::ConstSharedPtr msg); + + /** + * @brief set current_vehicle_state_ with received message + */ + void on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg); + /** * @brief set initial pose for simulation with received message */ @@ -270,6 +294,16 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void publish_gear_report(); + /** + * @brief publish turn indicators report + */ + void publish_turn_indicators_report(); + + /** + * @brief publish hazard lights report + */ + void publish_hazard_lights_report(); + /** * @brief publish tf * @param [in] state The kinematic state to publish as a TF diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index bda9b2b308b52..72ce051a55d83 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -52,11 +52,15 @@ def generate_launch_description(): remappings=[ ('input/ackermann_control_command', '/control/command/control_cmd'), ('input/gear_command', '/control/command/gear_cmd'), + ('input/turn_indicators_command', '/control/command/turn_indicators_cmd'), + ('input/hazard_lights_command', '/control/command/hazard_lights_cmd'), ('input/trajectory', '/planning/scenario_planning/trajectory'), ('output/twist', '/vehicle/status/velocity_status'), ('output/odometry', '/localization/kinematic_state'), ('output/steering', '/vehicle/status/steering_status'), ('output/gear_report', '/vehicle/status/gear_status'), + ('output/turn_indicators_report', '/vehicle/status/turn_indicators_status'), + ('output/hazard_lights_report', '/vehicle/status/hazard_lights_status'), ('output/control_mode_report', '/vehicle/status/control_mode'), ('/initialpose', '/initialpose'), ] diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index d4c528b06e3c9..00fd1ff902792 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -94,13 +94,23 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1)); sub_gear_cmd_ = create_subscription( "input/gear_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_gear_cmd, this, _1)); + sub_turn_indicators_cmd_ = create_subscription( + "input/turn_indicators_command", QoS{1}, + std::bind(&SimplePlanningSimulator::on_turn_indicators_cmd, this, _1)); + sub_hazard_lights_cmd_ = create_subscription( + "input/hazard_lights_command", QoS{1}, + std::bind(&SimplePlanningSimulator::on_hazard_lights_cmd, this, _1)); sub_trajectory_ = create_subscription( "input/trajectory", QoS{1}, std::bind(&SimplePlanningSimulator::on_trajectory, this, _1)); pub_control_mode_report_ = create_publisher("output/control_mode_report", QoS{1}); pub_gear_report_ = create_publisher("output/gear_report", QoS{1}); - pub_current_pose_ = create_publisher("/current_pose", QoS{1}); + pub_turn_indicators_report_ = + create_publisher("output/turn_indicators_report", QoS{1}); + pub_hazard_lights_report_ = + create_publisher("output/hazard_lights_report", QoS{1}); + pub_current_pose_ = create_publisher("/current_pose", QoS{1}); pub_velocity_ = create_publisher("output/twist", QoS{1}); pub_odom_ = create_publisher("output/odometry", QoS{1}); pub_steer_ = create_publisher("output/steering", QoS{1}); @@ -119,9 +129,9 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt const auto initialize_source = declare_parameter("initialize_source", "INITIAL_POSE_TOPIC"); RCLCPP_INFO(this->get_logger(), "initialize_source : %s", initialize_source.c_str()); if (initialize_source == "ORIGIN") { - geometry_msgs::msg::Pose p; + Pose p; p.orientation.w = 1.0; // yaw = 0 - set_initial_state(p, geometry_msgs::msg::Twist{}); // initialize with 0 for all variables + set_initial_state(p, Twist{}); // initialize with 0 for all variables } else if (initialize_source == "INITIAL_POSE_TOPIC") { // initialpose sub already exists. Do nothing. } @@ -218,15 +228,17 @@ void SimplePlanningSimulator::on_timer() publish_control_mode_report(); publish_gear_report(); + publish_turn_indicators_report(); + publish_hazard_lights_report(); publish_tf(current_odometry_); } void SimplePlanningSimulator::on_initialpose( - const geometry_msgs::msg::PoseWithCovarianceStamped::ConstSharedPtr msg) + const PoseWithCovarianceStamped::ConstSharedPtr msg) { // save initial pose - geometry_msgs::msg::Twist initial_twist; - geometry_msgs::msg::PoseStamped initial_pose; + Twist initial_twist; + PoseStamped initial_pose; initial_pose.header = msg->header; initial_pose.pose = msg->pose.pose; set_initial_state_with_transform(initial_pose, initial_twist); @@ -262,7 +274,7 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons } void SimplePlanningSimulator::on_gear_cmd( - const autoware_auto_vehicle_msgs::msg::GearCommand::ConstSharedPtr msg) + const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ptr_ = msg; @@ -273,6 +285,18 @@ void SimplePlanningSimulator::on_gear_cmd( } } +void SimplePlanningSimulator::on_turn_indicators_cmd( + const TurnIndicatorsCommand::ConstSharedPtr msg) +{ + current_turn_indicators_cmd_ptr_ = msg; +} + +void SimplePlanningSimulator::on_hazard_lights_cmd( + const HazardLightsCommand::ConstSharedPtr msg) +{ + current_hazard_lights_cmd_ptr_ = msg; +} + void SimplePlanningSimulator::on_trajectory(const Trajectory::ConstSharedPtr msg) { current_trajectory_ptr_ = msg; @@ -297,10 +321,10 @@ void SimplePlanningSimulator::add_measurement_noise( void SimplePlanningSimulator::set_initial_state_with_transform( - const geometry_msgs::msg::PoseStamped & pose_stamped, const geometry_msgs::msg::Twist & twist) + const PoseStamped & pose_stamped, const Twist & twist) { auto transform = get_transform_msg(origin_frame_id_, pose_stamped.header.frame_id); - geometry_msgs::msg::Pose pose; + Pose pose; pose.position.x = pose_stamped.pose.position.x + transform.transform.translation.x; pose.position.y = pose_stamped.pose.position.y + transform.transform.translation.y; pose.position.z = pose_stamped.pose.position.z + transform.transform.translation.z; @@ -309,7 +333,7 @@ void SimplePlanningSimulator::set_initial_state_with_transform( } void SimplePlanningSimulator::set_initial_state( - const geometry_msgs::msg::Pose & pose, const geometry_msgs::msg::Twist & twist) + const Pose & pose, const Twist & twist) { const float64_t x = pose.position.x; const float64_t y = pose.position.y; @@ -359,17 +383,17 @@ double SimplePlanningSimulator::get_z_pose_from_trajectory(const double x, const found = true; } } - if (found) { - return current_trajectory_ptr_->points.at(index).pose.position.z; - } + if (found) { + return current_trajectory_ptr_->points.at(index).pose.position.z; + } - return 0.0; + return 0.0; } -geometry_msgs::msg::TransformStamped SimplePlanningSimulator::get_transform_msg( +TransformStamped SimplePlanningSimulator::get_transform_msg( const std::string parent_frame, const std::string child_frame) { - geometry_msgs::msg::TransformStamped transform; + TransformStamped transform; while (true) { try { const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); @@ -430,9 +454,31 @@ void SimplePlanningSimulator::publish_gear_report() pub_gear_report_->publish(msg); } +void SimplePlanningSimulator::publish_turn_indicators_report() +{ + if (!current_turn_indicators_cmd_ptr_) { + return; + } + TurnIndicatorsReport msg; + msg.stamp = get_clock()->now(); + msg.report = current_turn_indicators_cmd_ptr_->command; + pub_turn_indicators_report_->publish(msg); +} + +void SimplePlanningSimulator::publish_hazard_lights_report() +{ + if (!current_hazard_lights_cmd_ptr_) { + return; + } + HazardLightsReport msg; + msg.stamp = get_clock()->now(); + msg.report = current_hazard_lights_cmd_ptr_->command; + pub_hazard_lights_report_->publish(msg); +} + void SimplePlanningSimulator::publish_tf(const Odometry & odometry) { - geometry_msgs::msg::TransformStamped tf; + TransformStamped tf; tf.header.stamp = get_clock()->now(); tf.header.frame_id = origin_frame_id_; tf.child_frame_id = simulated_frame_id_; From 336c5f9b3962a418f912dfc10dc75c34d3417291 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 24 Nov 2021 19:16:22 +0900 Subject: [PATCH 43/52] [simple_planning_simulator]fix bug (#727) * input z-axis of trajectory to pose(tf/odometry) * output 0 velocity when invalid gear is input --- .../simple_planning_simulator_core.cpp | 6 +++--- .../vehicle_model/sim_model_delay_steer_acc_geared.cpp | 2 +- .../vehicle_model/sim_model_ideal_steer_acc_geared.cpp | 2 +- 3 files changed, 5 insertions(+), 5 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 00fd1ff902792..29b6c81436fbd 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -213,6 +213,8 @@ void SimplePlanningSimulator::on_timer() // set current state current_odometry_ = to_odometry(vehicle_model_ptr_); + current_odometry_.pose.pose.position.z = get_z_pose_from_trajectory( + current_odometry_.pose.pose.position.x, current_odometry_.pose.pose.position.y); current_velocity_ = to_velocity_report(vehicle_model_ptr_); current_steer_ = to_steering_report(vehicle_model_ptr_); @@ -423,8 +425,6 @@ void SimplePlanningSimulator::publish_odometry(const Odometry & odometry) msg.header.frame_id = origin_frame_id_; msg.header.stamp = get_clock()->now(); msg.child_frame_id = simulated_frame_id_; - msg.pose.pose.position.z = - get_z_pose_from_trajectory(odometry.pose.pose.position.x, odometry.pose.pose.position.y); pub_odom_->publish(msg); } @@ -484,7 +484,7 @@ void SimplePlanningSimulator::publish_tf(const Odometry & odometry) tf.child_frame_id = simulated_frame_id_; tf.transform.translation.x = odometry.pose.pose.position.x; tf.transform.translation.y = odometry.pose.pose.position.y; - tf.transform.translation.z = 0.0; + tf.transform.translation.z = odometry.pose.pose.position.z; tf.transform.rotation = odometry.pose.pose.orientation; tf2_msgs::msg::TFMessage tf_msg{}; diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index f219e8c29e51f..5856650f72c29 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -132,5 +132,5 @@ float64_t SimModelDelaySteerAccGeared::calcVelocityWithGear( return 0.0; } - return state(IDX::VX); + return 0.0; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index eab6d3e76df04..6fb5cfe6f3553 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -83,5 +83,5 @@ float64_t SimModelIdealSteerAccGeared::calcVelocityWithGear( return 0.0; } - return state(IDX::VX); + return 0.0; } From 530a622006507b0489f80fd18053b87f40cd8068 Mon Sep 17 00:00:00 2001 From: Tomoya Kimura Date: Wed, 24 Nov 2021 19:59:24 +0900 Subject: [PATCH 44/52] fix gear process in sim (#728) --- .../vehicle_model/sim_model_delay_steer_acc_geared.cpp | 4 +++- .../vehicle_model/sim_model_ideal_steer_acc_geared.cpp | 4 +++- 2 files changed, 6 insertions(+), 2 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 5856650f72c29..dda3048387e07 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -130,7 +130,9 @@ float64_t SimModelDelaySteerAccGeared::calcVelocityWithGear( } } else if (gear == GearCommand::PARK) { return 0.0; + } else { + return 0.0; } - return 0.0; + return state(IDX::VX); } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 6fb5cfe6f3553..a3c7c42428606 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -81,7 +81,9 @@ float64_t SimModelIdealSteerAccGeared::calcVelocityWithGear( } } else if (gear == GearCommand::PARK) { return 0.0; + } else { + return 0.0; } - return 0.0; + return state(IDX::VX); } From 8168271a85987724c5c0d5effd7a61de0dd31ec6 Mon Sep 17 00:00:00 2001 From: "Takagi, Isamu" <43976882+isamu-takagi@users.noreply.github.com> Date: Thu, 25 Nov 2021 18:18:02 +0900 Subject: [PATCH 45/52] Fix for integration test (#732) * Add backward compatibility of autoware state * Add simulator initial pose service * Fix pre-commit * Fix pre-commit --- .../simple_planning_simulator_core.hpp | 14 ++++++++++++++ .../simple_planning_simulator/package.xml | 2 ++ .../simple_planning_simulator_core.cpp | 19 +++++++++++++++++++ 3 files changed, 35 insertions(+) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 011f27ed4ae78..792a7cff7141f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -49,6 +49,9 @@ #include "autoware_auto_geometry_msgs/msg/complex32.hpp" #include "common/types.hpp" +#include "autoware_api_utils/autoware_api_utils.hpp" +#include "autoware_external_api_msgs/srv/initialize_pose.hpp" + #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" @@ -73,6 +76,7 @@ using autoware_auto_vehicle_msgs::msg::HazardLightsReport; using autoware_auto_vehicle_msgs::msg::SteeringReport; using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; using autoware_auto_vehicle_msgs::msg::VelocityReport; +using autoware_external_api_msgs::srv::InitializePose; using geometry_msgs::msg::Pose; using geometry_msgs::msg::PoseStamped; using geometry_msgs::msg::PoseWithCovarianceStamped; @@ -137,6 +141,9 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_init_pose_; rclcpp::Subscription::SharedPtr sub_trajectory_; + rclcpp::CallbackGroup::SharedPtr group_api_service_; + autoware_api_utils::Service::SharedPtr srv_set_pose_; + uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation @@ -213,6 +220,13 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node */ void on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg); + /** + * @brief set initial pose for simulation with received request + */ + void on_set_pose( + const InitializePose::Request::ConstSharedPtr request, + const InitializePose::Response::SharedPtr response); + /** * @brief subscribe trajectory for deciding self z position. */ diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 55dc5a3753987..5c2166db65d0e 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -10,11 +10,13 @@ ament_cmake_auto autoware_auto_cmake + autoware_api_utils autoware_auto_planning_msgs autoware_auto_control_msgs autoware_auto_vehicle_msgs autoware_auto_tf2 autoware_auto_common + autoware_external_api_msgs geometry_msgs nav_msgs vehicle_info_util diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 29b6c81436fbd..789f79981be31 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -85,6 +85,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt using rclcpp::QoS; using std::placeholders::_1; + using std::placeholders::_2; sub_init_pose_ = create_subscription( "/initialpose", QoS{1}, @@ -121,6 +122,11 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt std::chrono::milliseconds(timer_sampling_time_ms_), std::bind(&SimplePlanningSimulator::on_timer, this)); + autoware_api_utils::ServiceProxyNodeInterface proxy(this); + group_api_service_ = create_callback_group(rclcpp::CallbackGroupType::MutuallyExclusive); + srv_set_pose_ = proxy.create_service( + "/api/simulator/set/pose", std::bind(&SimplePlanningSimulator::on_set_pose, this, _1, _2), + rmw_qos_profile_services_default, group_api_service_); // set vehicle model type initialize_vehicle_model(); @@ -246,6 +252,19 @@ void SimplePlanningSimulator::on_initialpose( set_initial_state_with_transform(initial_pose, initial_twist); } +void SimplePlanningSimulator::on_set_pose( + const InitializePose::Request::ConstSharedPtr request, + const InitializePose::Response::SharedPtr response) +{ + // save initial pose + Twist initial_twist; + PoseStamped initial_pose; + initial_pose.header = request->pose.header; + initial_pose.pose = request->pose.pose.pose; + set_initial_state_with_transform(initial_pose, initial_twist); + response->status = autoware_api_utils::response_success(); +} + void SimplePlanningSimulator::on_ackermann_cmd( const autoware_auto_control_msgs::msg::AckermannControlCommand::ConstSharedPtr msg) { From 69ad7afad037dbdacafbc570f28c87a7de8f80f7 Mon Sep 17 00:00:00 2001 From: Takamasa Horibe Date: Fri, 26 Nov 2021 14:06:02 +0900 Subject: [PATCH 46/52] Simple planning simulator update for latest develop (#735) * Refactor vehicle info util (#1305) Signed-off-by: Takamasa Horibe * add cov pub in psim (#1732) Signed-off-by: Takamasa Horibe * remove pose_with_covariance publisher and add covariance information in Odometry Signed-off-by: Takamasa Horibe --- .../simple_planning_simulator_core.hpp | 7 ++++ .../simple_planning_simulator/package.xml | 1 + ...mple_planning_simulator_default.param.yaml | 2 ++ .../simple_planning_simulator_core.cpp | 36 +++++++++++++++++-- 4 files changed, 44 insertions(+), 2 deletions(-) diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 792a7cff7141f..5cfa6371eee66 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -147,6 +147,10 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation + OnSetParametersCallbackHandle::SharedPtr set_param_res_; + rcl_interfaces::msg::SetParametersResult on_parameter( + const std::vector & parameters); + /* tf */ tf2_ros::Buffer tf_buffer_; tf2_ros::TransformListener tf_listener_; @@ -174,6 +178,9 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node MeasurementNoiseGenerator measurement_noise_; //!< @brief for measurement noise + float64_t x_stddev_; //!< @brief x standard deviation for dummy covariance in map coordinate + float64_t y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate + /* vehicle model */ enum class VehicleModelType { diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 5c2166db65d0e..032ad95f4bf0f 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -20,6 +20,7 @@ geometry_msgs nav_msgs vehicle_info_util + autoware_utils motion_common diff --git a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml index 50c1762fc5145..52f2d67f5bc50 100644 --- a/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml +++ b/simulator/simple_planning_simulator/param/simple_planning_simulator_default.param.yaml @@ -15,5 +15,7 @@ simulation: acc_time_constant: 0.1 steer_time_delay: 0.1 steer_time_constant: 0.1 + x_stddev: 0.0001 # x standard deviation for dummy covariance in map coordinate + y_stddev: 0.0001 # y standard deviation for dummy covariance in map coordinate # Note: vehicle characteristics parameters (e.g. wheelbase) are defined in a separate file. diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 789f79981be31..276c0e7186457 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -25,6 +25,7 @@ #include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "common/types.hpp" +#include "autoware_utils/ros/update_param.hpp" #include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" #include "simple_planning_simulator/vehicle_model/sim_model.hpp" #include "motion_common/motion_common.hpp" @@ -117,6 +118,10 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt pub_steer_ = create_publisher("output/steering", QoS{1}); pub_tf_ = create_publisher("/tf", QoS{1}); + /* set param callback */ + set_param_res_ = + this->add_on_set_parameters_callback(std::bind(&SimplePlanningSimulator::on_parameter, this, _1)); + timer_sampling_time_ms_ = static_cast(declare_parameter("timer_sampling_time_ms", 25)); on_timer_ = create_wall_timer( std::chrono::milliseconds(timer_sampling_time_ms_), @@ -156,6 +161,9 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt m.vel_dist_ = std::make_shared>(0.0, vel_noise_stddev); m.rpy_dist_ = std::make_shared>(0.0, rpy_noise_stddev); m.steer_dist_ = std::make_shared>(0.0, steer_noise_stddev); + + x_stddev_ = declare_parameter("x_stddev", 0.0001); + y_stddev_ = declare_parameter("y_stddev", 0.0001); } } @@ -173,8 +181,8 @@ void SimplePlanningSimulator::initialize_vehicle_model() const float64_t acc_time_constant = declare_parameter("acc_time_constant", 0.1); const float64_t steer_time_delay = declare_parameter("steer_time_delay", 0.24); const float64_t steer_time_constant = declare_parameter("steer_time_constant", 0.27); - const float64_t wheelbase = - vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo().wheel_base_m; + const auto vehicle_info = vehicle_info_util::VehicleInfoUtil(*this).getVehicleInfo(); + const float64_t wheelbase = vehicle_info.wheel_base_m; if (vehicle_model_type_str == "IDEAL_STEER_VEL") { vehicle_model_type_ = VehicleModelType::IDEAL_STEER_VEL; @@ -204,6 +212,24 @@ void SimplePlanningSimulator::initialize_vehicle_model() } } +rcl_interfaces::msg::SetParametersResult SimplePlanningSimulator::on_parameter( + const std::vector & parameters) +{ + rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = "success"; + + try { + autoware_utils::updateParam(parameters, "x_stddev", x_stddev_); + autoware_utils::updateParam(parameters, "y_stddev", y_stddev_); + } catch (const rclcpp::exceptions::InvalidParameterTypeException & e) { + result.successful = false; + result.reason = e.what(); + } + + return result; +} + void SimplePlanningSimulator::on_timer() { if (!is_initialized_) { @@ -229,6 +255,12 @@ void SimplePlanningSimulator::on_timer() add_measurement_noise(current_odometry_, current_velocity_, current_steer_); } + // add estimate covariance + { + current_odometry_.pose.covariance[0 * 6 + 0] = x_stddev_; + current_odometry_.pose.covariance[1 * 6 + 1] = y_stddev_; + } + // publish vehicle state publish_odometry(current_odometry_); publish_velocity(current_velocity_); From 4acd06b983e3c04e44e28a0968413f1b8f117341 Mon Sep 17 00:00:00 2001 From: Fumiya Watanabe Date: Mon, 29 Nov 2021 11:12:26 +0900 Subject: [PATCH 47/52] Fix acceleration for reverse (#737) * Fix acceleration for reverse * Fix acceleration in set_input * remove unused using * Fix code --- .../simple_planning_simulator_core.cpp | 14 ++++++++++++-- 1 file changed, 12 insertions(+), 2 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index 276c0e7186457..fef5e432c452f 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -308,20 +308,30 @@ void SimplePlanningSimulator::on_ackermann_cmd( void SimplePlanningSimulator::set_input(const float steer, const float vel, const float accel) { + using autoware_auto_vehicle_msgs::msg::GearCommand; Eigen::VectorXd input(vehicle_model_ptr_->getDimU()); + // TODO (Watanabe): The definition of the sign of acceleration in REVERSE mode is different + // between .auto and proposal.iv, and will be discussed later. + float acc = accel; + if(!current_gear_cmd_ptr_) { + acc = 0.0; + } else if (current_gear_cmd_ptr_->command == GearCommand::REVERSE || current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) { + acc = -accel; + } + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_VEL) { input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { - input << accel, steer; + input << acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { - input << accel, steer; + input << acc, steer; } vehicle_model_ptr_->setInput(input); } From 7731f6240e23376819705690df2d2b58f5c007b8 Mon Sep 17 00:00:00 2001 From: "pre-commit-ci[bot]" <66853113+pre-commit-ci[bot]@users.noreply.github.com> Date: Thu, 2 Dec 2021 01:55:20 +0000 Subject: [PATCH 48/52] ci(pre-commit): autofix --- .../simple_planning_simulator-design.md | 104 ++++++++---------- .../simple_planning_simulator_core.hpp | 70 ++++++------ .../vehicle_model/sim_model.hpp | 9 +- .../sim_model_delay_steer_acc.hpp | 15 +-- .../sim_model_delay_steer_acc_geared.hpp | 15 +-- .../sim_model_ideal_steer_acc.hpp | 16 +-- .../sim_model_ideal_steer_acc_geared.hpp | 18 +-- .../sim_model_ideal_steer_vel.hpp | 15 +-- .../vehicle_model/sim_model_interface.hpp | 9 +- .../visibility_control.hpp | 4 +- .../simple_planning_simulator.launch.py | 81 +++++++------- .../simple_planning_simulator/package.xml | 16 +-- .../simple_planning_simulator_core.cpp | 93 +++++++--------- .../sim_model_delay_steer_acc.cpp | 16 +-- .../sim_model_delay_steer_acc_geared.cpp | 22 ++-- .../sim_model_ideal_steer_acc.cpp | 24 ++-- .../sim_model_ideal_steer_acc_geared.cpp | 23 ++-- .../sim_model_ideal_steer_vel.cpp | 19 ++-- .../vehicle_model/sim_model_interface.cpp | 13 +-- .../test/test_simple_planning_simulator.cpp | 76 ++++++------- 20 files changed, 285 insertions(+), 373 deletions(-) diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md index a2e34cf6c7cc6..aa3c38f7136a1 100644 --- a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md +++ b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md @@ -1,101 +1,88 @@ -simple_planning_simulator {#simple_planning_simulator-package-design} -=========== - +# simple_planning_simulator {#simple_planning_simulator-package-design} # Purpose / Use cases This node simulates the vehicle motion for a vehicle command in 2D using a simple vehicle model. - - # Design - The purpose of this simulator is for the integration test of planning and control modules. This does not simulate sensing or perception, but is implemented in pure c++ only and works without GPU. ## Assumptions / Known limits - - It simulates only in 2D motion. - - It does not perform physical operations such as collision and sensing, but only calculates the integral results of vehicle dynamics. - +- It simulates only in 2D motion. +- It does not perform physical operations such as collision and sensing, but only calculates the integral results of vehicle dynamics. ## Inputs / Outputs / API - **input** - - input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle. - - input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle. - - input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear). - - /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose + +- input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle. +- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle. +- input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear). +- /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose **output** - - /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link) - - /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM) - - /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.) +- /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link) +- /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM) +- /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.) ## Inner-workings / Algorithms ### Common Parameters -|Name|Type|Description|Default value| -|:---|:---|:---|:---| -|simulated_frame_id | string | set to the child_frame_id in output tf |"base_link"| -|origin_frame_id | string | set to the frame_id in output tf |"odom"| -|initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | "INITIAL_POSE_TOPIC" | -|add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results.| true| -|pos_noise_stddev | double | Standard deviation for position noise | 0.01| -|rpy_noise_stddev | double | Standard deviation for Euler angle noise| 0.0001| -|vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0| -|angvel_noise_stddev | double | Standard deviation for angular velocity noise| 0.0| -|steer_noise_stddev | double | Standard deviation for steering angle noise| 0.0001| - - +| Name | Type | Description | Default value | +| :-------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------ | :------------------- | -------------------- | +| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" | +| origin_frame_id | string | set to the frame_id in output tf | "odom" | +| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | "INITIAL_POSE_TOPIC" | +| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true | +| pos_noise_stddev | double | Standard deviation for position noise | 0.01 | +| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 | +| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 | +| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 | +| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 | ### Vehicle Model Parameters - **vehicle_model_type options** - - `IDEAL_STEER_VEL` - - `IDEAL_STEER_ACC` - - `IDEAL_STEER_ACC_GEARED` - - `DELAY_STEER_ACC` - - `DELAY_STEER_ACC_GEARED` +- `IDEAL_STEER_VEL` +- `IDEAL_STEER_ACC` +- `IDEAL_STEER_ACC_GEARED` +- `DELAY_STEER_ACC` +- `DELAY_STEER_ACC_GEARED` The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). +| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_A | D_ST_A_G | Default value | unit | +| :------------------ | :------------------- | :--------------------------------------------------- | :---------------------------------- | :----- | :------- | :----- | :------- | :------------ | :------ | ----- | --- | +| acc_time_delay | double | dead time for the acceleration input | x | x | x | o | o | 0.1 | [s] | +| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | 0.24 | [s] | +| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | o | o | 0.1 | [s] | +| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | 0.27 | [s] | +| vel_lim | double | limit of velocity | x | x | x | o | o | 50.0 | [m/s] | +| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | 7.0 | [m/ss] | +| steer_lim | double | limit of steering angle | x | x | x | o | o | 1.0 | [rad] | +| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | 5.0 | [rad/s] | +| | -|Name|Type|Description|I_ST_V|I_ST_A|I_ST_A_G|D_ST_A|D_ST_A_G|Default value| unit | -|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---| -|acc_time_delay | double | dead time for the acceleration input | x | x | x | o | o | 0.1 | [s] | -|steer_time_delay | double | dead time for the steering input | x | x | x | o | o | 0.24| [s] | -|acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | o | o | 0.1 | [s] | -|steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | 0.27| [s] | -|vel_lim | double | limit of velocity | x | x | x | o | o | 50.0| [m/s] | -|vel_rate_lim | double | limit of acceleration | x | x | x | o | o | 7.0 | [m/ss] | -|steer_lim | double | limit of steering angle | x | x | x | o | o | 1.0 | [rad] | -|steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | 5.0 | [rad/s] | - - - -*Note*: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a *delay* model. The definition of the *time constant* is the time it takes for the step response to rise up to 63% of its final value. The *deadtime* is a delay in the response to a control input. - +_Note_: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a _delay_ model. The definition of the _time constant_ is the time it takes for the step response to rise up to 63% of its final value. The _deadtime_ is a delay in the response to a control input. ### Default TF configuration Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. - ## Error detection and handling The only validation on inputs being done is testing for a valid vehicle model type. - # Security considerations + - # References / External links + This is originally developed in the Autoware.AI. See the link below. -https://github.com/Autoware-AI/simulation/tree/master/wf_simulator + # Future extensions / Unimplemented parts - - Improving the accuracy of vehicle models (e.g., adding steering dead zones and slip behavior) - - Cooperation with modules that output pseudo pointcloud or pseudo perception results - +- Improving the accuracy of vehicle models (e.g., adding steering dead zones and slip behavior) +- Cooperation with modules that output pseudo pointcloud or pseudo perception results # Related issues - - #1142: Follow-up to #570 - Integrate simple_planning_simulator Into CI +- #1142: Follow-up to #570 - Integrate simple_planning_simulator Into CI diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 5cfa6371eee66..3df3c1d192238 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -15,53 +15,47 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ - -#include -#include - -#include -#include -#include - +#include "autoware_api_utils/autoware_api_utils.hpp" +#include "common/types.hpp" #include "rclcpp/rclcpp.hpp" - +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #include "simple_planning_simulator/visibility_control.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "nav_msgs/msg/odometry.hpp" - #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" +#include "autoware_auto_geometry_msgs/msg/complex32.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" #include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp" #include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" -#include "autoware_auto_geometry_msgs/msg/complex32.hpp" -#include "common/types.hpp" - -#include "autoware_api_utils/autoware_api_utils.hpp" #include "autoware_external_api_msgs/srv/initialize_pose.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "nav_msgs/msg/odometry.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include +#include +#include +#include +#include namespace simulation { namespace simple_planning_simulator { +using autoware::common::types::bool8_t; using autoware::common::types::float32_t; using autoware::common::types::float64_t; -using autoware::common::types::bool8_t; using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_geometry_msgs::msg::Complex32; @@ -69,11 +63,11 @@ using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::ControlModeReport; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::GearReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::HazardLightsReport; using autoware_auto_vehicle_msgs::msg::SteeringReport; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; using autoware_auto_vehicle_msgs::msg::VelocityReport; using autoware_external_api_msgs::srv::InitializePose; @@ -87,8 +81,7 @@ using nav_msgs::msg::Odometry; class DeltaTime { public: - DeltaTime() - : prev_updated_time_ptr_(nullptr) {} + DeltaTime() : prev_updated_time_ptr_(nullptr) {} float64_t get_dt(const rclcpp::Time & now) { if (prev_updated_time_ptr_ == nullptr) { @@ -144,7 +137,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_api_service_; autoware_api_utils::Service::SharedPtr srv_set_pose_; - uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time + uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -168,7 +161,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /* frame_id */ std::string simulated_frame_id_; //!< @brief simulated vehicle frame id - std::string origin_frame_id_; //!< @brief map frame_id + std::string origin_frame_id_; //!< @brief map frame_id /* flags */ bool8_t is_initialized_; //!< @brief flag to check the initial position is set @@ -182,8 +175,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node float64_t y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate /* vehicle model */ - enum class VehicleModelType - { + enum class VehicleModelType { IDEAL_STEER_ACC = 0, IDEAL_STEER_ACC_GEARED = 1, DELAY_STEER_ACC = 2, @@ -213,13 +205,13 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node void on_gear_cmd(const GearCommand::ConstSharedPtr msg); /** - * @brief set current_vehicle_state_ with received message - */ + * @brief set current_vehicle_state_ with received message + */ void on_turn_indicators_cmd(const TurnIndicatorsCommand::ConstSharedPtr msg); /** - * @brief set current_vehicle_state_ with received message - */ + * @brief set current_vehicle_state_ with received message + */ void on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg); /** @@ -321,8 +313,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node void publish_turn_indicators_report(); /** - * @brief publish hazard lights report - */ + * @brief publish hazard lights report + */ void publish_hazard_lights_report(); /** diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index ef72b9e1f2c2c..6facf5f363da7 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -15,12 +15,11 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" - +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index f0a610c54eb0f..1941189463859 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -15,15 +15,14 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + #include #include #include -#include "eigen3/Eigen/LU" -#include "eigen3/Eigen/Core" - -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - class SimModelDelaySteerAcc : public SimModelInterface { public: @@ -53,8 +52,7 @@ class SimModelDelaySteerAcc : public SimModelInterface private: const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX - { + enum IDX { X = 0, Y, YAW, @@ -62,8 +60,7 @@ class SimModelDelaySteerAcc : public SimModelInterface STEER, ACCX, }; - enum IDX_U - { + enum IDX_U { ACCX_DES = 0, STEER_DES, DRIVE_SHIFT, diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index c9bcf1bb32e6f..39dc4c7e3abdc 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -15,15 +15,14 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ +#include "eigen3/Eigen/Core" +#include "eigen3/Eigen/LU" +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + #include #include #include -#include "eigen3/Eigen/LU" -#include "eigen3/Eigen/Core" - -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - class SimModelDelaySteerAccGeared : public SimModelInterface { public: @@ -53,8 +52,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface private: const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX - { + enum IDX { X = 0, Y, YAW, @@ -62,8 +60,7 @@ class SimModelDelaySteerAccGeared : public SimModelInterface STEER, ACCX, }; - enum IDX_U - { + enum IDX_U { ACCX_DES = 0, STEER_DES, DRIVE_SHIFT, diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp index c4aca56e5bd51..7669d88c71f99 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp @@ -15,13 +15,12 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ -#include - #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" - #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include + /** * @class SimModelIdealSteerAcc * @brief calculate ideal steering dynamics @@ -41,15 +40,8 @@ class SimModelIdealSteerAcc : public SimModelInterface ~SimModelIdealSteerAcc() = default; private: - enum IDX - { - X = 0, - Y, - YAW, - VX - }; - enum IDX_U - { + enum IDX { X = 0, Y, YAW, VX }; + enum IDX_U { AX_DES = 0, STEER_DES, }; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index 8b74c5f1baf19..cd21785ce101f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -15,13 +15,12 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ -#include - #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" - #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include + /** * @class SimModelIdealSteerAccGeared * @brief calculate ideal steering dynamics @@ -41,21 +40,14 @@ class SimModelIdealSteerAccGeared : public SimModelInterface ~SimModelIdealSteerAccGeared() = default; private: - enum IDX - { - X = 0, - Y, - YAW, - VX - }; - enum IDX_U - { + enum IDX { X = 0, Y, YAW, VX }; + enum IDX_U { AX_DES = 0, STEER_DES, }; const float64_t wheelbase_; //!< @brief vehicle wheelbase length - float64_t current_acc_; //!< @brief current_acc with gear consideration + float64_t current_acc_; //!< @brief current_acc with gear consideration /** * @brief get vehicle position x diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp index dae5f0fab633c..06f62e45c1b16 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp @@ -15,12 +15,11 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ -#include - #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" - #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + +#include /** * @class SimModelIdealSteerVel * @brief calculate ideal steering dynamics @@ -40,14 +39,8 @@ class SimModelIdealSteerVel : public SimModelInterface ~SimModelIdealSteerVel() = default; private: - enum IDX - { - X = 0, - Y, - YAW - }; - enum IDX_U - { + enum IDX { X = 0, Y, YAW }; + enum IDX_U { VX_DES = 0, STEER_DES, }; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index bdcafdda14de5..d25decf265e06 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -15,13 +15,14 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ +#include "common/types.hpp" #include "eigen3/Eigen/Core" + #include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" -#include "common/types.hpp" +using autoware::common::types::bool8_t; using autoware::common::types::float32_t; using autoware::common::types::float64_t; -using autoware::common::types::bool8_t; /** * @class SimModelInterface @@ -144,12 +145,12 @@ class SimModelInterface /** * @brief get state vector dimension */ - inline int getDimX() {return dim_x_;} + inline int getDimX() { return dim_x_; } /** * @brief get input vector demension */ - inline int getDimU() {return dim_u_;} + inline int getDimU() { return dim_u_; } /** * @brief calculate derivative of states with vehicle model diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp index d1179bbcc833d..093893b3f9ae9 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp @@ -12,13 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. - #ifndef SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ #if defined(__WIN32) -#if defined(PLANNING_SIMULATOR_BUILDING_DLL) || \ - defined(PLANNING_SIMULATOR_EXPORTS) +#if defined(PLANNING_SIMULATOR_BUILDING_DLL) || defined(PLANNING_SIMULATOR_EXPORTS) #define PLANNING_SIMULATOR_PUBLIC __declspec(dllexport) #define PLANNING_SIMULATOR_LOCAL #else // defined(PLANNING_SIMULATOR_BUILDING_DLL) || diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 72ce051a55d83..4591845e71b28 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -12,70 +12,67 @@ # See the License for the specific language governing permissions and # limitations under the License. +import os + import ament_index_python +from ament_index_python import get_package_share_directory import launch -import launch_ros.actions - from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -from ament_index_python import get_package_share_directory - -import os +import launch_ros.actions def generate_launch_description(): default_vehicle_characteristics_param = os.path.join( - get_package_share_directory('simple_planning_simulator'), - 'param/vehicle_characteristics.param.yaml') + get_package_share_directory("simple_planning_simulator"), + "param/vehicle_characteristics.param.yaml", + ) vehicle_characteristics_param = DeclareLaunchArgument( - 'vehicle_characteristics_param_file', + "vehicle_characteristics_param_file", default_value=default_vehicle_characteristics_param, - description='Path to config file for vehicle characteristics' + description="Path to config file for vehicle characteristics", ) simple_planning_simulator_node = launch_ros.actions.Node( - package='simple_planning_simulator', - executable='simple_planning_simulator_exe', - name='simple_planning_simulator', - namespace='simulation', - output='screen', + package="simple_planning_simulator", + executable="simple_planning_simulator_exe", + name="simple_planning_simulator", + namespace="simulation", + output="screen", parameters=[ "{}/param/simple_planning_simulator_default.param.yaml".format( - ament_index_python.get_package_share_directory( - "simple_planning_simulator" - ) + ament_index_python.get_package_share_directory("simple_planning_simulator") ), - LaunchConfiguration('vehicle_characteristics_param_file'), + LaunchConfiguration("vehicle_characteristics_param_file"), ], remappings=[ - ('input/ackermann_control_command', '/control/command/control_cmd'), - ('input/gear_command', '/control/command/gear_cmd'), - ('input/turn_indicators_command', '/control/command/turn_indicators_cmd'), - ('input/hazard_lights_command', '/control/command/hazard_lights_cmd'), - ('input/trajectory', '/planning/scenario_planning/trajectory'), - ('output/twist', '/vehicle/status/velocity_status'), - ('output/odometry', '/localization/kinematic_state'), - ('output/steering', '/vehicle/status/steering_status'), - ('output/gear_report', '/vehicle/status/gear_status'), - ('output/turn_indicators_report', '/vehicle/status/turn_indicators_status'), - ('output/hazard_lights_report', '/vehicle/status/hazard_lights_status'), - ('output/control_mode_report', '/vehicle/status/control_mode'), - ('/initialpose', '/initialpose'), - ] + ("input/ackermann_control_command", "/control/command/control_cmd"), + ("input/gear_command", "/control/command/gear_cmd"), + ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"), + ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"), + ("input/trajectory", "/planning/scenario_planning/trajectory"), + ("output/twist", "/vehicle/status/velocity_status"), + ("output/odometry", "/localization/kinematic_state"), + ("output/steering", "/vehicle/status/steering_status"), + ("output/gear_report", "/vehicle/status/gear_status"), + ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"), + ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"), + ("output/control_mode_report", "/vehicle/status/control_mode"), + ("/initialpose", "/initialpose"), + ], ) map_to_odom_tf_publisher = launch_ros.actions.Node( - package='tf2_ros', - executable='static_transform_publisher', - name='static_map_to_odom_tf_publisher', - output='screen', - arguments=['0.0', '0.0', '0.0', '0', '0', '0', 'map', 'odom']) + package="tf2_ros", + executable="static_transform_publisher", + name="static_map_to_odom_tf_publisher", + output="screen", + arguments=["0.0", "0.0", "0.0", "0", "0", "0", "map", "odom"], + ) - ld = launch.LaunchDescription([ - vehicle_characteristics_param, - simple_planning_simulator_node, - map_to_odom_tf_publisher - ]) + ld = launch.LaunchDescription( + [vehicle_characteristics_param, simple_planning_simulator_node, map_to_odom_tf_publisher] + ) return ld diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index 032ad95f4bf0f..cf6aa024e34dd 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -11,22 +11,22 @@ autoware_auto_cmake autoware_api_utils - autoware_auto_planning_msgs + autoware_auto_common autoware_auto_control_msgs - autoware_auto_vehicle_msgs + autoware_auto_planning_msgs autoware_auto_tf2 - autoware_auto_common + autoware_auto_vehicle_msgs autoware_external_api_msgs - geometry_msgs - nav_msgs - vehicle_info_util autoware_utils - + geometry_msgs motion_common - + nav_msgs rclcpp tf2 tf2_ros + vehicle_info_util + + launch_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index fef5e432c452f..b3e9b8ca7c6ea 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -12,32 +12,29 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" + +#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "common/types.hpp" +#include "motion_common/motion_common.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include +#include +#include #include #include #include #include -#include -#include - -#include "simple_planning_simulator/simple_planning_simulator_core.hpp" - -#include "common/types.hpp" -#include "autoware_utils/ros/update_param.hpp" -#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model.hpp" -#include "motion_common/motion_common.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include "rclcpp_components/register_node_macro.hpp" using namespace std::chrono_literals; namespace { - autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( const std::shared_ptr vehicle_model_ptr) { @@ -74,7 +71,6 @@ namespace simulation { namespace simple_planning_simulator { - SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & options) : Node("simple_planning_simulator", options), tf_buffer_(get_clock()), @@ -89,8 +85,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt using std::placeholders::_2; sub_init_pose_ = create_subscription( - "/initialpose", QoS{1}, - std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); + "/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1)); @@ -119,8 +114,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt pub_tf_ = create_publisher("/tf", QoS{1}); /* set param callback */ - set_param_res_ = - this->add_on_set_parameters_callback(std::bind(&SimplePlanningSimulator::on_parameter, this, _1)); + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&SimplePlanningSimulator::on_parameter, this, _1)); timer_sampling_time_ms_ = static_cast(declare_parameter("timer_sampling_time_ms", 25)); on_timer_ = create_wall_timer( @@ -141,13 +136,12 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt RCLCPP_INFO(this->get_logger(), "initialize_source : %s", initialize_source.c_str()); if (initialize_source == "ORIGIN") { Pose p; - p.orientation.w = 1.0; // yaw = 0 - set_initial_state(p, Twist{}); // initialize with 0 for all variables + p.orientation.w = 1.0; // yaw = 0 + set_initial_state(p, Twist{}); // initialize with 0 for all variables } else if (initialize_source == "INITIAL_POSE_TOPIC") { // initialpose sub already exists. Do nothing. } - // measurement noise { std::random_device seed; @@ -196,17 +190,13 @@ void SimplePlanningSimulator::initialize_vehicle_model() } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, - steer_rate_lim, wheelbase, - timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, - steer_time_constant); + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, - steer_rate_lim, wheelbase, - timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, - steer_time_constant); + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } @@ -273,8 +263,7 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } -void SimplePlanningSimulator::on_initialpose( - const PoseWithCovarianceStamped::ConstSharedPtr msg) +void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg) { // save initial pose Twist initial_twist; @@ -302,8 +291,7 @@ void SimplePlanningSimulator::on_ackermann_cmd( { current_ackermann_cmd_ptr_ = msg; set_input( - msg->lateral.steering_tire_angle, msg->longitudinal.speed, - msg->longitudinal.acceleration); + msg->lateral.steering_tire_angle, msg->longitudinal.speed, msg->longitudinal.acceleration); } void SimplePlanningSimulator::set_input(const float steer, const float vel, const float accel) @@ -314,9 +302,11 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons // TODO (Watanabe): The definition of the sign of acceleration in REVERSE mode is different // between .auto and proposal.iv, and will be discussed later. float acc = accel; - if(!current_gear_cmd_ptr_) { + if (!current_gear_cmd_ptr_) { acc = 0.0; - } else if (current_gear_cmd_ptr_->command == GearCommand::REVERSE || current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) { + } else if ( + current_gear_cmd_ptr_->command == GearCommand::REVERSE || + current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) { acc = -accel; } @@ -324,26 +314,23 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) - { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { input << acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) - { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { input << acc, steer; } vehicle_model_ptr_->setInput(input); } -void SimplePlanningSimulator::on_gear_cmd( - const GearCommand::ConstSharedPtr msg) +void SimplePlanningSimulator::on_gear_cmd(const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ptr_ = msg; - if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) - { + if ( + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { vehicle_model_ptr_->setGear(current_gear_cmd_ptr_->command); } } @@ -354,8 +341,7 @@ void SimplePlanningSimulator::on_turn_indicators_cmd( current_turn_indicators_cmd_ptr_ = msg; } -void SimplePlanningSimulator::on_hazard_lights_cmd( - const HazardLightsCommand::ConstSharedPtr msg) +void SimplePlanningSimulator::on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg) { current_hazard_lights_cmd_ptr_ = msg; } @@ -382,7 +368,6 @@ void SimplePlanningSimulator::add_measurement_noise( steer.steering_tire_angle += static_cast((*n.steer_dist_)(*n.rand_engine_)); } - void SimplePlanningSimulator::set_initial_state_with_transform( const PoseStamped & pose_stamped, const Twist & twist) { @@ -395,8 +380,7 @@ void SimplePlanningSimulator::set_initial_state_with_transform( set_initial_state(pose, twist); } -void SimplePlanningSimulator::set_initial_state( - const Pose & pose, const Twist & twist) +void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & twist) { const float64_t x = pose.position.x; const float64_t y = pose.position.y; @@ -411,13 +395,11 @@ void SimplePlanningSimulator::set_initial_state( state << x, y, yaw; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || - vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) - { + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) { state << x, y, yaw, vx; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) - { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { state << x, y, yaw, vx, steer, accx; } vehicle_model_ptr_->setState(state); @@ -461,8 +443,7 @@ TransformStamped SimplePlanningSimulator::get_transform_msg( try { const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); transform = tf_buffer_.lookupTransform( - parent_frame, child_frame, time_point, tf2::durationFromSec( - 0.0)); + parent_frame, child_frame, time_point, tf2::durationFromSec(0.0)); break; } catch (tf2::TransformException & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index d2fff30b202ce..df3845bf14fa0 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -35,17 +35,17 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( initializeInputQueue(dt); } -float64_t SimModelDelaySteerAcc::getX() {return state_(IDX::X);} -float64_t SimModelDelaySteerAcc::getY() {return state_(IDX::Y);} -float64_t SimModelDelaySteerAcc::getYaw() {return state_(IDX::YAW);} -float64_t SimModelDelaySteerAcc::getVx() {return state_(IDX::VX);} -float64_t SimModelDelaySteerAcc::getVy() {return 0.0;} -float64_t SimModelDelaySteerAcc::getAx() {return state_(IDX::ACCX);} +float64_t SimModelDelaySteerAcc::getX() { return state_(IDX::X); } +float64_t SimModelDelaySteerAcc::getY() { return state_(IDX::Y); } +float64_t SimModelDelaySteerAcc::getYaw() { return state_(IDX::YAW); } +float64_t SimModelDelaySteerAcc::getVx() { return state_(IDX::VX); } +float64_t SimModelDelaySteerAcc::getVy() { return 0.0; } +float64_t SimModelDelaySteerAcc::getAx() { return state_(IDX::ACCX); } float64_t SimModelDelaySteerAcc::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -float64_t SimModelDelaySteerAcc::getSteer() {return state_(IDX::STEER);} +float64_t SimModelDelaySteerAcc::getSteer() { return state_(IDX::STEER); } void SimModelDelaySteerAcc::update(const float64_t & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -76,7 +76,7 @@ void SimModelDelaySteerAcc::initializeInputQueue(const float64_t & dt) Eigen::VectorXd SimModelDelaySteerAcc::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);}; + auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); }; const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index dda3048387e07..997be698969e4 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -12,11 +12,12 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" + #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include + SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( 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 acc_delay, float64_t acc_time_constant, @@ -36,17 +37,17 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( initializeInputQueue(dt); } -float64_t SimModelDelaySteerAccGeared::getX() {return state_(IDX::X);} -float64_t SimModelDelaySteerAccGeared::getY() {return state_(IDX::Y);} -float64_t SimModelDelaySteerAccGeared::getYaw() {return state_(IDX::YAW);} -float64_t SimModelDelaySteerAccGeared::getVx() {return state_(IDX::VX);} -float64_t SimModelDelaySteerAccGeared::getVy() {return 0.0;} -float64_t SimModelDelaySteerAccGeared::getAx() {return state_(IDX::ACCX);} +float64_t SimModelDelaySteerAccGeared::getX() { return state_(IDX::X); } +float64_t SimModelDelaySteerAccGeared::getY() { return state_(IDX::Y); } +float64_t SimModelDelaySteerAccGeared::getYaw() { return state_(IDX::YAW); } +float64_t SimModelDelaySteerAccGeared::getVx() { return state_(IDX::VX); } +float64_t SimModelDelaySteerAccGeared::getVy() { return 0.0; } +float64_t SimModelDelaySteerAccGeared::getAx() { return state_(IDX::ACCX); } float64_t SimModelDelaySteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -float64_t SimModelDelaySteerAccGeared::getSteer() {return state_(IDX::STEER);} +float64_t SimModelDelaySteerAccGeared::getSteer() { return state_(IDX::STEER); } void SimModelDelaySteerAccGeared::update(const float64_t & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -85,7 +86,7 @@ void SimModelDelaySteerAccGeared::initializeInputQueue(const float64_t & dt) Eigen::VectorXd SimModelDelaySteerAccGeared::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);}; + auto sat = [](float64_t val, float64_t u, float64_t l) { return std::max(std::min(val, u), l); }; const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); @@ -107,7 +108,6 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( return d_state; } - float64_t SimModelDelaySteerAccGeared::calcVelocityWithGear( const Eigen::VectorXd & state, const uint8_t gear) const { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp index ee74b645850cf..9933ba5be790c 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp @@ -14,25 +14,23 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" - SimModelIdealSteerAcc::SimModelIdealSteerAcc(float64_t wheelbase) -: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) +{ +} -float64_t SimModelIdealSteerAcc::getX() {return state_(IDX::X);} -float64_t SimModelIdealSteerAcc::getY() {return state_(IDX::Y);} -float64_t SimModelIdealSteerAcc::getYaw() {return state_(IDX::YAW);} -float64_t SimModelIdealSteerAcc::getVx() {return state_(IDX::VX);} -float64_t SimModelIdealSteerAcc::getVy() {return 0.0;} -float64_t SimModelIdealSteerAcc::getAx() {return input_(IDX_U::AX_DES);} +float64_t SimModelIdealSteerAcc::getX() { return state_(IDX::X); } +float64_t SimModelIdealSteerAcc::getY() { return state_(IDX::Y); } +float64_t SimModelIdealSteerAcc::getYaw() { return state_(IDX::YAW); } +float64_t SimModelIdealSteerAcc::getVx() { return state_(IDX::VX); } +float64_t SimModelIdealSteerAcc::getVy() { return 0.0; } +float64_t SimModelIdealSteerAcc::getAx() { return input_(IDX_U::AX_DES); } float64_t SimModelIdealSteerAcc::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerAcc::getSteer() {return input_(IDX_U::STEER_DES);} -void SimModelIdealSteerAcc::update(const float64_t & dt) -{ - updateRungeKutta(dt, input_); -} +float64_t SimModelIdealSteerAcc::getSteer() { return input_(IDX_U::STEER_DES); } +void SimModelIdealSteerAcc::update(const float64_t & dt) { updateRungeKutta(dt, input_); } Eigen::VectorXd SimModelIdealSteerAcc::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index a3c7c42428606..49e065cf651f7 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -12,25 +12,28 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include - #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" + #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" +#include + SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(float64_t wheelbase) -: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) {} +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) +{ +} -float64_t SimModelIdealSteerAccGeared::getX() {return state_(IDX::X);} -float64_t SimModelIdealSteerAccGeared::getY() {return state_(IDX::Y);} -float64_t SimModelIdealSteerAccGeared::getYaw() {return state_(IDX::YAW);} -float64_t SimModelIdealSteerAccGeared::getVx() {return state_(IDX::VX);} -float64_t SimModelIdealSteerAccGeared::getVy() {return 0.0;} -float64_t SimModelIdealSteerAccGeared::getAx() {return current_acc_;} +float64_t SimModelIdealSteerAccGeared::getX() { return state_(IDX::X); } +float64_t SimModelIdealSteerAccGeared::getY() { return state_(IDX::Y); } +float64_t SimModelIdealSteerAccGeared::getYaw() { return state_(IDX::YAW); } +float64_t SimModelIdealSteerAccGeared::getVx() { return state_(IDX::VX); } +float64_t SimModelIdealSteerAccGeared::getVy() { return 0.0; } +float64_t SimModelIdealSteerAccGeared::getAx() { return current_acc_; } float64_t SimModelIdealSteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerAccGeared::getSteer() {return input_(IDX_U::STEER_DES);} +float64_t SimModelIdealSteerAccGeared::getSteer() { return input_(IDX_U::STEER_DES); } void SimModelIdealSteerAccGeared::update(const float64_t & dt) { const auto prev_vx = state_(IDX::VX); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp index ea40f647e2379..7755f50054454 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp @@ -14,21 +14,22 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" - SimModelIdealSteerVel::SimModelIdealSteerVel(float64_t wheelbase) -: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} +: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) +{ +} -float64_t SimModelIdealSteerVel::getX() {return state_(IDX::X);} -float64_t SimModelIdealSteerVel::getY() {return state_(IDX::Y);} -float64_t SimModelIdealSteerVel::getYaw() {return state_(IDX::YAW);} -float64_t SimModelIdealSteerVel::getVx() {return input_(IDX_U::VX_DES);} -float64_t SimModelIdealSteerVel::getVy() {return 0.0;} -float64_t SimModelIdealSteerVel::getAx() {return current_ax_;} +float64_t SimModelIdealSteerVel::getX() { return state_(IDX::X); } +float64_t SimModelIdealSteerVel::getY() { return state_(IDX::Y); } +float64_t SimModelIdealSteerVel::getYaw() { return state_(IDX::YAW); } +float64_t SimModelIdealSteerVel::getVx() { return input_(IDX_U::VX_DES); } +float64_t SimModelIdealSteerVel::getVy() { return 0.0; } +float64_t SimModelIdealSteerVel::getAx() { return current_ax_; } float64_t SimModelIdealSteerVel::getWz() { return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerVel::getSteer() {return input_(IDX_U::STEER_DES);} +float64_t SimModelIdealSteerVel::getSteer() { return input_(IDX_U::STEER_DES); } void SimModelIdealSteerVel::update(const float64_t & dt) { updateRungeKutta(dt, input_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp index 13bf38fc11f72..c3c36fd7846fc 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp @@ -14,8 +14,7 @@ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -SimModelInterface::SimModelInterface(int dim_x, int dim_u) -: dim_x_(dim_x), dim_u_(dim_u) +SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) { state_ = Eigen::VectorXd::Zero(dim_x_); input_ = Eigen::VectorXd::Zero(dim_u_); @@ -34,8 +33,8 @@ void SimModelInterface::updateEuler(const float64_t & dt, const Eigen::VectorXd { state_ += calcModel(state_, input) * dt; } -void SimModelInterface::getState(Eigen::VectorXd & state) {state = state_;} -void SimModelInterface::getInput(Eigen::VectorXd & input) {input = input_;} -void SimModelInterface::setState(const Eigen::VectorXd & state) {state_ = state;} -void SimModelInterface::setInput(const Eigen::VectorXd & input) {input_ = input;} -void SimModelInterface::setGear(const uint8_t gear) {gear_ = gear;} +void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; } +void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; } +void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; } +void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; } +void SimModelInterface::setGear(const uint8_t gear) { gear_ = gear; } diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index b048b73553675..655cac2b6921a 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include #include "gtest/gtest.h" - -#include "simple_planning_simulator/simple_planning_simulator_core.hpp" #include "motion_common/motion_common.hpp" +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" + +#include using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; @@ -30,46 +30,35 @@ std::string toStrInfo(const VehicleKinematicState & state) { const auto & s = state.state; std::stringstream ss; - ss << "state x: " << s.pose.position.x << ", y: " << s.pose.position.y << ", yaw: " << - motion::motion_common::to_angle( - s.pose.orientation) << ", vx = " << s.longitudinal_velocity_mps << ", vy: " << - s.lateral_velocity_mps << - ", ax: " << s.acceleration_mps2 << ", steer: " << s.front_wheel_angle_rad; + ss << "state x: " << s.pose.position.x << ", y: " << s.pose.position.y + << ", yaw: " << motion::motion_common::to_angle(s.pose.orientation) + << ", vx = " << s.longitudinal_velocity_mps << ", vy: " << s.lateral_velocity_mps + << ", ax: " << s.acceleration_mps2 << ", steer: " << s.front_wheel_angle_rad; return ss.str(); } -const std::vector vehicle_model_type_vec = { // NOLINT - "IDEAL_STEER_VEL", - "IDEAL_STEER_ACC", - "IDEAL_STEER_ACC_GEARED", - "DELAY_STEER_ACC", - "DELAY_STEER_ACC_GEARED", +const std::vector vehicle_model_type_vec = { + // NOLINT + "IDEAL_STEER_VEL", "IDEAL_STEER_ACC", "IDEAL_STEER_ACC_GEARED", + "DELAY_STEER_ACC", "DELAY_STEER_ACC_GEARED", }; static constexpr float64_t COM_TO_BASELINK = 1.5; class PubSubNode : public rclcpp::Node { public: - PubSubNode() - : Node{"test_simple_planning_simulator_pubsub"} + PubSubNode() : Node{"test_simple_planning_simulator_pubsub"} { kinematic_state_sub_ = create_subscription( "output/kinematic_state", rclcpp::QoS{1}, - [this](const VehicleKinematicState::SharedPtr msg) { - current_state_ = msg; - }); - pub_control_command_ = create_publisher( - "input/vehicle_control_command", - rclcpp::QoS{1}); - pub_ackermann_command_ = create_publisher( - "input/ackermann_control_command", - rclcpp::QoS{1}); - pub_initialpose_ = create_publisher( - "/initialpose", - rclcpp::QoS{1}); - pub_state_cmd_ = create_publisher( - "/input/vehicle_state_command", - rclcpp::QoS{1}); + [this](const VehicleKinematicState::SharedPtr msg) { current_state_ = msg; }); + pub_control_command_ = + create_publisher("input/vehicle_control_command", rclcpp::QoS{1}); + pub_ackermann_command_ = + create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); + pub_initialpose_ = create_publisher("/initialpose", rclcpp::QoS{1}); + pub_state_cmd_ = + create_publisher("/input/vehicle_state_command", rclcpp::QoS{1}); } rclcpp::Publisher::SharedPtr pub_control_command_; @@ -134,8 +123,7 @@ void resetInitialpose(rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) + uint8_t gear, rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) { VehicleStateCommand cmd; cmd.stamp = sim_node->now(); @@ -193,7 +181,6 @@ VehicleKinematicState comToBaselink(const VehicleKinematicState & com) return baselink; } - // Check which direction the vehicle is heading on the baselink coordinates. // y // | @@ -266,12 +253,10 @@ TEST(TestSimplePlanningSimulatorIdealSteerVel, TestMoving) const float32_t target_acc = 5.0f; const float32_t target_steer = 0.2f; - auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; + auto _resetInitialpose = [&]() { resetInitialpose(sim_node, pub_sub_node); }; auto _sendFwdGear = [&]() { sendGear(GearCommand::DRIVE, sim_node, pub_sub_node); }; auto _sendBwdGear = [&]() { sendGear(GearCommand::REVERSE, sim_node, pub_sub_node); }; - auto _sendCommand = [&](const auto & _cmd) { - sendCommand(_cmd, sim_node, pub_sub_node); - }; + auto _sendCommand = [&](const auto & _cmd) { sendCommand(_cmd, sim_node, pub_sub_node); }; // check pub-sub connections { @@ -335,13 +320,14 @@ TEST(test_simple_planning_simulator, test_moving_ackermann) const float32_t target_acc = 5.0f; const float32_t target_steer = 0.2f; - auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; - auto _sendFwdGear = [&]() {sendGear(VehicleStateCommand::GEAR_DRIVE, sim_node, pub_sub_node);}; - auto _sendBwdGear = - [&]() {sendGear(VehicleStateCommand::GEAR_REVERSE, sim_node, pub_sub_node);}; - auto _sendCommand = [&](const auto & _cmd) { - sendCommand(_cmd, sim_node, pub_sub_node); - }; + auto _resetInitialpose = [&]() { resetInitialpose(sim_node, pub_sub_node); }; + auto _sendFwdGear = [&]() { + sendGear(VehicleStateCommand::GEAR_DRIVE, sim_node, pub_sub_node); + }; + auto _sendBwdGear = [&]() { + sendGear(VehicleStateCommand::GEAR_REVERSE, sim_node, pub_sub_node); + }; + auto _sendCommand = [&](const auto & _cmd) { sendCommand(_cmd, sim_node, pub_sub_node); }; // check pub-sub connections { From a6f18eb38280f2599b8ef3ca1ef8af2039026ccb Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Mon, 6 Dec 2021 12:01:37 +0900 Subject: [PATCH 49/52] remove tests --- simulator/simple_planning_simulator/CMakeLists.txt | 9 --------- 1 file changed, 9 deletions(-) diff --git a/simulator/simple_planning_simulator/CMakeLists.txt b/simulator/simple_planning_simulator/CMakeLists.txt index 4bfcac1ab41da..e217ca572e583 100644 --- a/simulator/simple_planning_simulator/CMakeLists.txt +++ b/simulator/simple_planning_simulator/CMakeLists.txt @@ -36,15 +36,6 @@ rclcpp_components_register_node(${PROJECT_NAME} if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() - - # Unit test - #ament_add_gtest( - # simple_planning_simulator_unit_tests - # test/test_simple_planning_simulator.cpp - # TIMEOUT 120) - #autoware_set_compile_options(simple_planning_simulator_unit_tests) - #target_link_libraries(simple_planning_simulator_unit_tests ${PROJECT_NAME}) - #target_include_directories(simple_planning_simulator_unit_tests PRIVATE "include") endif() From e1abab4b00d4922465f6681f4e6796bdffb1044e Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Mon, 6 Dec 2021 15:58:02 +0900 Subject: [PATCH 50/52] Revert "ci(pre-commit): autofix" This reverts commit 7731f6240e23376819705690df2d2b58f5c007b8. --- .../simple_planning_simulator-design.md | 104 ++++++++++-------- .../simple_planning_simulator_core.hpp | 70 ++++++------ .../vehicle_model/sim_model.hpp | 9 +- .../sim_model_delay_steer_acc.hpp | 15 ++- .../sim_model_delay_steer_acc_geared.hpp | 15 ++- .../sim_model_ideal_steer_acc.hpp | 16 ++- .../sim_model_ideal_steer_acc_geared.hpp | 18 ++- .../sim_model_ideal_steer_vel.hpp | 15 ++- .../vehicle_model/sim_model_interface.hpp | 9 +- .../visibility_control.hpp | 4 +- .../simple_planning_simulator.launch.py | 81 +++++++------- .../simple_planning_simulator/package.xml | 16 +-- .../simple_planning_simulator_core.cpp | 93 +++++++++------- .../sim_model_delay_steer_acc.cpp | 16 +-- .../sim_model_delay_steer_acc_geared.cpp | 22 ++-- .../sim_model_ideal_steer_acc.cpp | 24 ++-- .../sim_model_ideal_steer_acc_geared.cpp | 23 ++-- .../sim_model_ideal_steer_vel.cpp | 19 ++-- .../vehicle_model/sim_model_interface.cpp | 13 ++- .../test/test_simple_planning_simulator.cpp | 76 +++++++------ 20 files changed, 373 insertions(+), 285 deletions(-) diff --git a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md index aa3c38f7136a1..a2e34cf6c7cc6 100644 --- a/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md +++ b/simulator/simple_planning_simulator/design/simple_planning_simulator-design.md @@ -1,88 +1,101 @@ -# simple_planning_simulator {#simple_planning_simulator-package-design} +simple_planning_simulator {#simple_planning_simulator-package-design} +=========== + # Purpose / Use cases This node simulates the vehicle motion for a vehicle command in 2D using a simple vehicle model. + + # Design + The purpose of this simulator is for the integration test of planning and control modules. This does not simulate sensing or perception, but is implemented in pure c++ only and works without GPU. ## Assumptions / Known limits -- It simulates only in 2D motion. -- It does not perform physical operations such as collision and sensing, but only calculates the integral results of vehicle dynamics. + - It simulates only in 2D motion. + - It does not perform physical operations such as collision and sensing, but only calculates the integral results of vehicle dynamics. + ## Inputs / Outputs / API -**input** -- input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle. -- input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle. -- input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear). -- /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose +**input** + - input/vehicle_control_command [`autoware_auto_msgs/msg/VehicleControlCommand`] : target command to drive a vehicle. + - input/ackermann_control_command [`autoware_auto_msgs/msg/AckermannControlCommand`] : target command to drive a vehicle. + - input/vehicle_state_command [`autoware_auto_msgs/msg/VehicleStateCommand`] : target state command (e.g. gear). + - /initialpose [`geometry_msgs/msg/PoseWithCovarianceStamped`] : for initial pose **output** + - /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link) + - /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM) + - /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.) -- /tf [`tf2_msgs/msg/TFMessage`] : simulated vehicle pose (base_link) -- /vehicle/vehicle_kinematic_state [`autoware_auto_msgs/msg/VehicleKinematicState`] : simulated kinematic state (defined in CoM) -- /vehicle/state_report [`autoware_auto_msgs/msg/VehicleStateReport`] : current vehicle state (e.g. gear, mode, etc.) ## Inner-workings / Algorithms ### Common Parameters -| Name | Type | Description | Default value | -| :-------------------- | :----- | :------------------------------------------------------------------------------------------------------------------------------------ | :------------------- | -------------------- | -| simulated_frame_id | string | set to the child_frame_id in output tf | "base_link" | -| origin_frame_id | string | set to the frame_id in output tf | "odom" | -| initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | "INITIAL_POSE_TOPIC" | -| add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results. | true | -| pos_noise_stddev | double | Standard deviation for position noise | 0.01 | -| rpy_noise_stddev | double | Standard deviation for Euler angle noise | 0.0001 | -| vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0 | -| angvel_noise_stddev | double | Standard deviation for angular velocity noise | 0.0 | -| steer_noise_stddev | double | Standard deviation for steering angle noise | 0.0001 | +|Name|Type|Description|Default value| +|:---|:---|:---|:---| +|simulated_frame_id | string | set to the child_frame_id in output tf |"base_link"| +|origin_frame_id | string | set to the frame_id in output tf |"odom"| +|initialize_source | string | If "ORIGIN", the initial pose is set at (0,0,0). If "INITIAL_POSE_TOPIC", node will wait until the `/initialpose` topic is published. | "INITIAL_POSE_TOPIC" | "INITIAL_POSE_TOPIC" | +|add_measurement_noise | bool | If true, the Gaussian noise is added to the simulated results.| true| +|pos_noise_stddev | double | Standard deviation for position noise | 0.01| +|rpy_noise_stddev | double | Standard deviation for Euler angle noise| 0.0001| +|vel_noise_stddev | double | Standard deviation for longitudinal velocity noise | 0.0| +|angvel_noise_stddev | double | Standard deviation for angular velocity noise| 0.0| +|steer_noise_stddev | double | Standard deviation for steering angle noise| 0.0001| + + ### Vehicle Model Parameters + **vehicle_model_type options** -- `IDEAL_STEER_VEL` -- `IDEAL_STEER_ACC` -- `IDEAL_STEER_ACC_GEARED` -- `DELAY_STEER_ACC` -- `DELAY_STEER_ACC_GEARED` + - `IDEAL_STEER_VEL` + - `IDEAL_STEER_ACC` + - `IDEAL_STEER_ACC_GEARED` + - `DELAY_STEER_ACC` + - `DELAY_STEER_ACC_GEARED` The `IDEAL` model moves ideally as commanded, while the `DELAY` model moves based on a 1st-order with time delay model. The `STEER` means the model receives the steer command. The `VEL` means the model receives the target velocity command, while the `ACC` model receives the target acceleration command. The `GEARED` suffix means that the motion will consider the gear command: the vehicle moves only one direction following the gear. The table below shows which models correspond to what parameters. The model names are written in abbreviated form (e.g. IDEAL_STEER_VEL = I_ST_V). -| Name | Type | Description | I_ST_V | I_ST_A | I_ST_A_G | D_ST_A | D_ST_A_G | Default value | unit | -| :------------------ | :------------------- | :--------------------------------------------------- | :---------------------------------- | :----- | :------- | :----- | :------- | :------------ | :------ | ----- | --- | -| acc_time_delay | double | dead time for the acceleration input | x | x | x | o | o | 0.1 | [s] | -| steer_time_delay | double | dead time for the steering input | x | x | x | o | o | 0.24 | [s] | -| acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | o | o | 0.1 | [s] | -| steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | 0.27 | [s] | -| vel_lim | double | limit of velocity | x | x | x | o | o | 50.0 | [m/s] | -| vel_rate_lim | double | limit of acceleration | x | x | x | o | o | 7.0 | [m/ss] | -| steer_lim | double | limit of steering angle | x | x | x | o | o | 1.0 | [rad] | -| steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | 5.0 | [rad/s] | -| | -_Note_: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a _delay_ model. The definition of the _time constant_ is the time it takes for the step response to rise up to 63% of its final value. The _deadtime_ is a delay in the response to a control input. +|Name|Type|Description|I_ST_V|I_ST_A|I_ST_A_G|D_ST_A|D_ST_A_G|Default value| unit | +|:---|:---|:---|:---|:---|:---|:---|:---|:---|:---| +|acc_time_delay | double | dead time for the acceleration input | x | x | x | o | o | 0.1 | [s] | +|steer_time_delay | double | dead time for the steering input | x | x | x | o | o | 0.24| [s] | +|acc_time_constant | double | time constant of the 1st-order acceleration dynamics | x | x | x | o | o | 0.1 | [s] | +|steer_time_constant | double | time constant of the 1st-order steering dynamics | x | x | x | o | o | 0.27| [s] | +|vel_lim | double | limit of velocity | x | x | x | o | o | 50.0| [m/s] | +|vel_rate_lim | double | limit of acceleration | x | x | x | o | o | 7.0 | [m/ss] | +|steer_lim | double | limit of steering angle | x | x | x | o | o | 1.0 | [rad] | +|steer_rate_lim | double | limit of steering angle change rate | x | x | x | o | o | 5.0 | [rad/s] | + + + +*Note*: The steering/velocity/acceleration dynamics is modeled by a first order system with a deadtime in a *delay* model. The definition of the *time constant* is the time it takes for the step response to rise up to 63% of its final value. The *deadtime* is a delay in the response to a control input. + ### Default TF configuration Since the vehicle outputs `odom`->`base_link` tf, this simulator outputs the tf with the same frame_id configuration. In the simple_planning_simulator.launch.py, the node that outputs the `map`->`odom` tf, that usually estimated by the localization module (e.g. NDT), will be launched as well. Since the tf output by this simulator module is an ideal value, `odom`->`map` will always be 0. + ## Error detection and handling The only validation on inputs being done is testing for a valid vehicle model type. -# Security considerations +# Security considerations -# References / External links +# References / External links This is originally developed in the Autoware.AI. See the link below. - +https://github.com/Autoware-AI/simulation/tree/master/wf_simulator # Future extensions / Unimplemented parts -- Improving the accuracy of vehicle models (e.g., adding steering dead zones and slip behavior) -- Cooperation with modules that output pseudo pointcloud or pseudo perception results + - Improving the accuracy of vehicle models (e.g., adding steering dead zones and slip behavior) + - Cooperation with modules that output pseudo pointcloud or pseudo perception results + # Related issues -- #1142: Follow-up to #570 - Integrate simple_planning_simulator Into CI + - #1142: Follow-up to #570 - Integrate simple_planning_simulator Into CI diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp index 3df3c1d192238..5cfa6371eee66 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp @@ -15,47 +15,53 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__SIMPLE_PLANNING_SIMULATOR_CORE_HPP_ -#include "autoware_api_utils/autoware_api_utils.hpp" -#include "common/types.hpp" + +#include +#include + +#include +#include +#include + #include "rclcpp/rclcpp.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + #include "simple_planning_simulator/visibility_control.hpp" +#include "geometry_msgs/msg/pose.hpp" +#include "geometry_msgs/msg/pose_stamped.hpp" +#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" +#include "geometry_msgs/msg/twist.hpp" +#include "geometry_msgs/msg/transform_stamped.hpp" +#include "nav_msgs/msg/odometry.hpp" + #include "autoware_auto_control_msgs/msg/ackermann_control_command.hpp" -#include "autoware_auto_geometry_msgs/msg/complex32.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" +#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/control_mode_report.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp" -#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp" -#include "autoware_auto_vehicle_msgs/msg/steering_report.hpp" #include "autoware_auto_vehicle_msgs/msg/turn_indicators_command.hpp" #include "autoware_auto_vehicle_msgs/msg/turn_indicators_report.hpp" +#include "autoware_auto_vehicle_msgs/msg/hazard_lights_command.hpp" +#include "autoware_auto_vehicle_msgs/msg/hazard_lights_report.hpp" #include "autoware_auto_vehicle_msgs/msg/vehicle_control_command.hpp" #include "autoware_auto_vehicle_msgs/msg/velocity_report.hpp" +#include "autoware_auto_geometry_msgs/msg/complex32.hpp" +#include "common/types.hpp" + +#include "autoware_api_utils/autoware_api_utils.hpp" #include "autoware_external_api_msgs/srv/initialize_pose.hpp" -#include "geometry_msgs/msg/pose.hpp" -#include "geometry_msgs/msg/pose_stamped.hpp" -#include "geometry_msgs/msg/pose_with_covariance_stamped.hpp" -#include "geometry_msgs/msg/transform_stamped.hpp" -#include "geometry_msgs/msg/twist.hpp" -#include "nav_msgs/msg/odometry.hpp" -#include -#include +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include -#include -#include namespace simulation { namespace simple_planning_simulator { -using autoware::common::types::bool8_t; using autoware::common::types::float32_t; using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_geometry_msgs::msg::Complex32; @@ -63,11 +69,11 @@ using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_vehicle_msgs::msg::ControlModeReport; using autoware_auto_vehicle_msgs::msg::GearCommand; using autoware_auto_vehicle_msgs::msg::GearReport; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; +using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; using autoware_auto_vehicle_msgs::msg::HazardLightsCommand; using autoware_auto_vehicle_msgs::msg::HazardLightsReport; using autoware_auto_vehicle_msgs::msg::SteeringReport; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsCommand; -using autoware_auto_vehicle_msgs::msg::TurnIndicatorsReport; using autoware_auto_vehicle_msgs::msg::VehicleControlCommand; using autoware_auto_vehicle_msgs::msg::VelocityReport; using autoware_external_api_msgs::srv::InitializePose; @@ -81,7 +87,8 @@ using nav_msgs::msg::Odometry; class DeltaTime { public: - DeltaTime() : prev_updated_time_ptr_(nullptr) {} + DeltaTime() + : prev_updated_time_ptr_(nullptr) {} float64_t get_dt(const rclcpp::Time & now) { if (prev_updated_time_ptr_ == nullptr) { @@ -137,7 +144,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node rclcpp::CallbackGroup::SharedPtr group_api_service_; autoware_api_utils::Service::SharedPtr srv_set_pose_; - uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time + uint32_t timer_sampling_time_ms_; //!< @brief timer sampling time rclcpp::TimerBase::SharedPtr on_timer_; //!< @brief timer for simulation OnSetParametersCallbackHandle::SharedPtr set_param_res_; @@ -161,7 +168,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node /* frame_id */ std::string simulated_frame_id_; //!< @brief simulated vehicle frame id - std::string origin_frame_id_; //!< @brief map frame_id + std::string origin_frame_id_; //!< @brief map frame_id /* flags */ bool8_t is_initialized_; //!< @brief flag to check the initial position is set @@ -175,7 +182,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node float64_t y_stddev_; //!< @brief y standard deviation for dummy covariance in map coordinate /* vehicle model */ - enum class VehicleModelType { + enum class VehicleModelType + { IDEAL_STEER_ACC = 0, IDEAL_STEER_ACC_GEARED = 1, DELAY_STEER_ACC = 2, @@ -205,13 +213,13 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node void on_gear_cmd(const GearCommand::ConstSharedPtr msg); /** - * @brief set current_vehicle_state_ with received message - */ + * @brief set current_vehicle_state_ with received message + */ void on_turn_indicators_cmd(const TurnIndicatorsCommand::ConstSharedPtr msg); /** - * @brief set current_vehicle_state_ with received message - */ + * @brief set current_vehicle_state_ with received message + */ void on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg); /** @@ -313,8 +321,8 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node void publish_turn_indicators_report(); /** - * @brief publish hazard lights report - */ + * @brief publish hazard lights report + */ void publish_hazard_lights_report(); /** diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp index 6facf5f363da7..ef72b9e1f2c2c 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model.hpp @@ -15,11 +15,12 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" + #endif // SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_HPP_ diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp index 1941189463859..f0a610c54eb0f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.hpp @@ -15,14 +15,15 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - #include #include #include +#include "eigen3/Eigen/LU" +#include "eigen3/Eigen/Core" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + class SimModelDelaySteerAcc : public SimModelInterface { public: @@ -52,7 +53,8 @@ class SimModelDelaySteerAcc : public SimModelInterface private: const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX { + enum IDX + { X = 0, Y, YAW, @@ -60,7 +62,8 @@ class SimModelDelaySteerAcc : public SimModelInterface STEER, ACCX, }; - enum IDX_U { + enum IDX_U + { ACCX_DES = 0, STEER_DES, DRIVE_SHIFT, diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp index 39dc4c7e3abdc..c9bcf1bb32e6f 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp @@ -15,14 +15,15 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_DELAY_STEER_ACC_GEARED_HPP_ -#include "eigen3/Eigen/Core" -#include "eigen3/Eigen/LU" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" - #include #include #include +#include "eigen3/Eigen/LU" +#include "eigen3/Eigen/Core" + +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" + class SimModelDelaySteerAccGeared : public SimModelInterface { public: @@ -52,7 +53,8 @@ class SimModelDelaySteerAccGeared : public SimModelInterface private: const float64_t MIN_TIME_CONSTANT; //!< @brief minimum time constant - enum IDX { + enum IDX + { X = 0, Y, YAW, @@ -60,7 +62,8 @@ class SimModelDelaySteerAccGeared : public SimModelInterface STEER, ACCX, }; - enum IDX_U { + enum IDX_U + { ACCX_DES = 0, STEER_DES, DRIVE_SHIFT, diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp index 7669d88c71f99..c4aca56e5bd51 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp @@ -15,11 +15,12 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_HPP_ +#include + #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" /** * @class SimModelIdealSteerAcc @@ -40,8 +41,15 @@ class SimModelIdealSteerAcc : public SimModelInterface ~SimModelIdealSteerAcc() = default; private: - enum IDX { X = 0, Y, YAW, VX }; - enum IDX_U { + enum IDX + { + X = 0, + Y, + YAW, + VX + }; + enum IDX_U + { AX_DES = 0, STEER_DES, }; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp index cd21785ce101f..8b74c5f1baf19 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp @@ -15,11 +15,12 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_ACC_GEARED_HPP_ +#include + #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" /** * @class SimModelIdealSteerAccGeared @@ -40,14 +41,21 @@ class SimModelIdealSteerAccGeared : public SimModelInterface ~SimModelIdealSteerAccGeared() = default; private: - enum IDX { X = 0, Y, YAW, VX }; - enum IDX_U { + enum IDX + { + X = 0, + Y, + YAW, + VX + }; + enum IDX_U + { AX_DES = 0, STEER_DES, }; const float64_t wheelbase_; //!< @brief vehicle wheelbase length - float64_t current_acc_; //!< @brief current_acc with gear consideration + float64_t current_acc_; //!< @brief current_acc with gear consideration /** * @brief get vehicle position x diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp index 06f62e45c1b16..dae5f0fab633c 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp @@ -15,11 +15,12 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_IDEAL_STEER_VEL_HPP_ +#include + #include "eigen3/Eigen/Core" #include "eigen3/Eigen/LU" -#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -#include +#include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" /** * @class SimModelIdealSteerVel * @brief calculate ideal steering dynamics @@ -39,8 +40,14 @@ class SimModelIdealSteerVel : public SimModelInterface ~SimModelIdealSteerVel() = default; private: - enum IDX { X = 0, Y, YAW }; - enum IDX_U { + enum IDX + { + X = 0, + Y, + YAW + }; + enum IDX_U + { VX_DES = 0, STEER_DES, }; diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp index d25decf265e06..bdcafdda14de5 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/vehicle_model/sim_model_interface.hpp @@ -15,14 +15,13 @@ #ifndef SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VEHICLE_MODEL__SIM_MODEL_INTERFACE_HPP_ -#include "common/types.hpp" #include "eigen3/Eigen/Core" - #include "autoware_auto_vehicle_msgs/msg/vehicle_state_command.hpp" +#include "common/types.hpp" -using autoware::common::types::bool8_t; using autoware::common::types::float32_t; using autoware::common::types::float64_t; +using autoware::common::types::bool8_t; /** * @class SimModelInterface @@ -145,12 +144,12 @@ class SimModelInterface /** * @brief get state vector dimension */ - inline int getDimX() { return dim_x_; } + inline int getDimX() {return dim_x_;} /** * @brief get input vector demension */ - inline int getDimU() { return dim_u_; } + inline int getDimU() {return dim_u_;} /** * @brief calculate derivative of states with vehicle model diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp index 093893b3f9ae9..d1179bbcc833d 100644 --- a/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp +++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/visibility_control.hpp @@ -12,11 +12,13 @@ // See the License for the specific language governing permissions and // limitations under the License. + #ifndef SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ #define SIMPLE_PLANNING_SIMULATOR__VISIBILITY_CONTROL_HPP_ #if defined(__WIN32) -#if defined(PLANNING_SIMULATOR_BUILDING_DLL) || defined(PLANNING_SIMULATOR_EXPORTS) +#if defined(PLANNING_SIMULATOR_BUILDING_DLL) || \ + defined(PLANNING_SIMULATOR_EXPORTS) #define PLANNING_SIMULATOR_PUBLIC __declspec(dllexport) #define PLANNING_SIMULATOR_LOCAL #else // defined(PLANNING_SIMULATOR_BUILDING_DLL) || diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py index 4591845e71b28..72ce051a55d83 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -12,67 +12,70 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - import ament_index_python -from ament_index_python import get_package_share_directory import launch +import launch_ros.actions + from launch.actions import DeclareLaunchArgument from launch.substitutions import LaunchConfiguration -import launch_ros.actions +from ament_index_python import get_package_share_directory + +import os def generate_launch_description(): default_vehicle_characteristics_param = os.path.join( - get_package_share_directory("simple_planning_simulator"), - "param/vehicle_characteristics.param.yaml", - ) + get_package_share_directory('simple_planning_simulator'), + 'param/vehicle_characteristics.param.yaml') vehicle_characteristics_param = DeclareLaunchArgument( - "vehicle_characteristics_param_file", + 'vehicle_characteristics_param_file', default_value=default_vehicle_characteristics_param, - description="Path to config file for vehicle characteristics", + description='Path to config file for vehicle characteristics' ) simple_planning_simulator_node = launch_ros.actions.Node( - package="simple_planning_simulator", - executable="simple_planning_simulator_exe", - name="simple_planning_simulator", - namespace="simulation", - output="screen", + package='simple_planning_simulator', + executable='simple_planning_simulator_exe', + name='simple_planning_simulator', + namespace='simulation', + output='screen', parameters=[ "{}/param/simple_planning_simulator_default.param.yaml".format( - ament_index_python.get_package_share_directory("simple_planning_simulator") + ament_index_python.get_package_share_directory( + "simple_planning_simulator" + ) ), - LaunchConfiguration("vehicle_characteristics_param_file"), + LaunchConfiguration('vehicle_characteristics_param_file'), ], remappings=[ - ("input/ackermann_control_command", "/control/command/control_cmd"), - ("input/gear_command", "/control/command/gear_cmd"), - ("input/turn_indicators_command", "/control/command/turn_indicators_cmd"), - ("input/hazard_lights_command", "/control/command/hazard_lights_cmd"), - ("input/trajectory", "/planning/scenario_planning/trajectory"), - ("output/twist", "/vehicle/status/velocity_status"), - ("output/odometry", "/localization/kinematic_state"), - ("output/steering", "/vehicle/status/steering_status"), - ("output/gear_report", "/vehicle/status/gear_status"), - ("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"), - ("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"), - ("output/control_mode_report", "/vehicle/status/control_mode"), - ("/initialpose", "/initialpose"), - ], + ('input/ackermann_control_command', '/control/command/control_cmd'), + ('input/gear_command', '/control/command/gear_cmd'), + ('input/turn_indicators_command', '/control/command/turn_indicators_cmd'), + ('input/hazard_lights_command', '/control/command/hazard_lights_cmd'), + ('input/trajectory', '/planning/scenario_planning/trajectory'), + ('output/twist', '/vehicle/status/velocity_status'), + ('output/odometry', '/localization/kinematic_state'), + ('output/steering', '/vehicle/status/steering_status'), + ('output/gear_report', '/vehicle/status/gear_status'), + ('output/turn_indicators_report', '/vehicle/status/turn_indicators_status'), + ('output/hazard_lights_report', '/vehicle/status/hazard_lights_status'), + ('output/control_mode_report', '/vehicle/status/control_mode'), + ('/initialpose', '/initialpose'), + ] ) map_to_odom_tf_publisher = launch_ros.actions.Node( - package="tf2_ros", - executable="static_transform_publisher", - name="static_map_to_odom_tf_publisher", - output="screen", - arguments=["0.0", "0.0", "0.0", "0", "0", "0", "map", "odom"], - ) + package='tf2_ros', + executable='static_transform_publisher', + name='static_map_to_odom_tf_publisher', + output='screen', + arguments=['0.0', '0.0', '0.0', '0', '0', '0', 'map', 'odom']) - ld = launch.LaunchDescription( - [vehicle_characteristics_param, simple_planning_simulator_node, map_to_odom_tf_publisher] - ) + ld = launch.LaunchDescription([ + vehicle_characteristics_param, + simple_planning_simulator_node, + map_to_odom_tf_publisher + ]) return ld diff --git a/simulator/simple_planning_simulator/package.xml b/simulator/simple_planning_simulator/package.xml index cf6aa024e34dd..032ad95f4bf0f 100644 --- a/simulator/simple_planning_simulator/package.xml +++ b/simulator/simple_planning_simulator/package.xml @@ -11,22 +11,22 @@ autoware_auto_cmake autoware_api_utils - autoware_auto_common - autoware_auto_control_msgs autoware_auto_planning_msgs - autoware_auto_tf2 + autoware_auto_control_msgs autoware_auto_vehicle_msgs + autoware_auto_tf2 + autoware_auto_common autoware_external_api_msgs - autoware_utils geometry_msgs - motion_common nav_msgs - rclcpp - tf2 - tf2_ros vehicle_info_util + autoware_utils + motion_common + rclcpp + tf2 + tf2_ros launch_ros diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index b3e9b8ca7c6ea..fef5e432c452f 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -12,29 +12,32 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/simple_planning_simulator_core.hpp" - -#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" -#include "autoware_utils/ros/update_param.hpp" -#include "common/types.hpp" -#include "motion_common/motion_common.hpp" -#include "rclcpp_components/register_node_macro.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" #include -#include -#include #include #include #include #include +#include +#include + +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" + +#include "common/types.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "motion_common/motion_common.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" + +#include "rclcpp_components/register_node_macro.hpp" using namespace std::chrono_literals; namespace { + autoware_auto_vehicle_msgs::msg::VelocityReport to_velocity_report( const std::shared_ptr vehicle_model_ptr) { @@ -71,6 +74,7 @@ namespace simulation { namespace simple_planning_simulator { + SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & options) : Node("simple_planning_simulator", options), tf_buffer_(get_clock()), @@ -85,7 +89,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt using std::placeholders::_2; sub_init_pose_ = create_subscription( - "/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); + "/initialpose", QoS{1}, + std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1)); @@ -114,8 +119,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt pub_tf_ = create_publisher("/tf", QoS{1}); /* set param callback */ - set_param_res_ = this->add_on_set_parameters_callback( - std::bind(&SimplePlanningSimulator::on_parameter, this, _1)); + set_param_res_ = + this->add_on_set_parameters_callback(std::bind(&SimplePlanningSimulator::on_parameter, this, _1)); timer_sampling_time_ms_ = static_cast(declare_parameter("timer_sampling_time_ms", 25)); on_timer_ = create_wall_timer( @@ -136,12 +141,13 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt RCLCPP_INFO(this->get_logger(), "initialize_source : %s", initialize_source.c_str()); if (initialize_source == "ORIGIN") { Pose p; - p.orientation.w = 1.0; // yaw = 0 - set_initial_state(p, Twist{}); // initialize with 0 for all variables + p.orientation.w = 1.0; // yaw = 0 + set_initial_state(p, Twist{}); // initialize with 0 for all variables } else if (initialize_source == "INITIAL_POSE_TOPIC") { // initialpose sub already exists. Do nothing. } + // measurement noise { std::random_device seed; @@ -190,13 +196,17 @@ void SimplePlanningSimulator::initialize_vehicle_model() } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); + vel_lim, steer_lim, vel_rate_lim, + steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, + steer_time_constant); } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, - acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); + vel_lim, steer_lim, vel_rate_lim, + steer_rate_lim, wheelbase, + timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, + steer_time_constant); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } @@ -263,7 +273,8 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } -void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg) +void SimplePlanningSimulator::on_initialpose( + const PoseWithCovarianceStamped::ConstSharedPtr msg) { // save initial pose Twist initial_twist; @@ -291,7 +302,8 @@ void SimplePlanningSimulator::on_ackermann_cmd( { current_ackermann_cmd_ptr_ = msg; set_input( - msg->lateral.steering_tire_angle, msg->longitudinal.speed, msg->longitudinal.acceleration); + msg->lateral.steering_tire_angle, msg->longitudinal.speed, + msg->longitudinal.acceleration); } void SimplePlanningSimulator::set_input(const float steer, const float vel, const float accel) @@ -302,11 +314,9 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons // TODO (Watanabe): The definition of the sign of acceleration in REVERSE mode is different // between .auto and proposal.iv, and will be discussed later. float acc = accel; - if (!current_gear_cmd_ptr_) { + if(!current_gear_cmd_ptr_) { acc = 0.0; - } else if ( - current_gear_cmd_ptr_->command == GearCommand::REVERSE || - current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) { + } else if (current_gear_cmd_ptr_->command == GearCommand::REVERSE || current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) { acc = -accel; } @@ -314,23 +324,26 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons input << vel, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC) + { input << acc, steer; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { input << acc, steer; } vehicle_model_ptr_->setInput(input); } -void SimplePlanningSimulator::on_gear_cmd(const GearCommand::ConstSharedPtr msg) +void SimplePlanningSimulator::on_gear_cmd( + const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ptr_ = msg; - if ( - vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { + if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { vehicle_model_ptr_->setGear(current_gear_cmd_ptr_->command); } } @@ -341,7 +354,8 @@ void SimplePlanningSimulator::on_turn_indicators_cmd( current_turn_indicators_cmd_ptr_ = msg; } -void SimplePlanningSimulator::on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg) +void SimplePlanningSimulator::on_hazard_lights_cmd( + const HazardLightsCommand::ConstSharedPtr msg) { current_hazard_lights_cmd_ptr_ = msg; } @@ -368,6 +382,7 @@ void SimplePlanningSimulator::add_measurement_noise( steer.steering_tire_angle += static_cast((*n.steer_dist_)(*n.rand_engine_)); } + void SimplePlanningSimulator::set_initial_state_with_transform( const PoseStamped & pose_stamped, const Twist & twist) { @@ -380,7 +395,8 @@ void SimplePlanningSimulator::set_initial_state_with_transform( set_initial_state(pose, twist); } -void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & twist) +void SimplePlanningSimulator::set_initial_state( + const Pose & pose, const Twist & twist) { const float64_t x = pose.position.x; const float64_t y = pose.position.y; @@ -395,11 +411,13 @@ void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & state << x, y, yaw; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC || - vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) { + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED) + { state << x, y, yaw, vx; } else if ( // NOLINT vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC || - vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { + vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) + { state << x, y, yaw, vx, steer, accx; } vehicle_model_ptr_->setState(state); @@ -443,7 +461,8 @@ TransformStamped SimplePlanningSimulator::get_transform_msg( try { const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); transform = tf_buffer_.lookupTransform( - parent_frame, child_frame, time_point, tf2::durationFromSec(0.0)); + parent_frame, child_frame, time_point, tf2::durationFromSec( + 0.0)); break; } catch (tf2::TransformException & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp index df3845bf14fa0..d2fff30b202ce 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc.cpp @@ -35,17 +35,17 @@ SimModelDelaySteerAcc::SimModelDelaySteerAcc( initializeInputQueue(dt); } -float64_t SimModelDelaySteerAcc::getX() { return state_(IDX::X); } -float64_t SimModelDelaySteerAcc::getY() { return state_(IDX::Y); } -float64_t SimModelDelaySteerAcc::getYaw() { return state_(IDX::YAW); } -float64_t SimModelDelaySteerAcc::getVx() { return state_(IDX::VX); } -float64_t SimModelDelaySteerAcc::getVy() { return 0.0; } -float64_t SimModelDelaySteerAcc::getAx() { return state_(IDX::ACCX); } +float64_t SimModelDelaySteerAcc::getX() {return state_(IDX::X);} +float64_t SimModelDelaySteerAcc::getY() {return state_(IDX::Y);} +float64_t SimModelDelaySteerAcc::getYaw() {return state_(IDX::YAW);} +float64_t SimModelDelaySteerAcc::getVx() {return state_(IDX::VX);} +float64_t SimModelDelaySteerAcc::getVy() {return 0.0;} +float64_t SimModelDelaySteerAcc::getAx() {return state_(IDX::ACCX);} float64_t SimModelDelaySteerAcc::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -float64_t SimModelDelaySteerAcc::getSteer() { return state_(IDX::STEER); } +float64_t SimModelDelaySteerAcc::getSteer() {return state_(IDX::STEER);} void SimModelDelaySteerAcc::update(const float64_t & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -76,7 +76,7 @@ void SimModelDelaySteerAcc::initializeInputQueue(const float64_t & dt) Eigen::VectorXd SimModelDelaySteerAcc::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); }; + auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);}; const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index 997be698969e4..dda3048387e07 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -12,12 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" +#include +#include "simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" -#include - SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( 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 acc_delay, float64_t acc_time_constant, @@ -37,17 +36,17 @@ SimModelDelaySteerAccGeared::SimModelDelaySteerAccGeared( initializeInputQueue(dt); } -float64_t SimModelDelaySteerAccGeared::getX() { return state_(IDX::X); } -float64_t SimModelDelaySteerAccGeared::getY() { return state_(IDX::Y); } -float64_t SimModelDelaySteerAccGeared::getYaw() { return state_(IDX::YAW); } -float64_t SimModelDelaySteerAccGeared::getVx() { return state_(IDX::VX); } -float64_t SimModelDelaySteerAccGeared::getVy() { return 0.0; } -float64_t SimModelDelaySteerAccGeared::getAx() { return state_(IDX::ACCX); } +float64_t SimModelDelaySteerAccGeared::getX() {return state_(IDX::X);} +float64_t SimModelDelaySteerAccGeared::getY() {return state_(IDX::Y);} +float64_t SimModelDelaySteerAccGeared::getYaw() {return state_(IDX::YAW);} +float64_t SimModelDelaySteerAccGeared::getVx() {return state_(IDX::VX);} +float64_t SimModelDelaySteerAccGeared::getVy() {return 0.0;} +float64_t SimModelDelaySteerAccGeared::getAx() {return state_(IDX::ACCX);} float64_t SimModelDelaySteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(state_(IDX::STEER)) / wheelbase_; } -float64_t SimModelDelaySteerAccGeared::getSteer() { return state_(IDX::STEER); } +float64_t SimModelDelaySteerAccGeared::getSteer() {return state_(IDX::STEER);} void SimModelDelaySteerAccGeared::update(const float64_t & dt) { Eigen::VectorXd delayed_input = Eigen::VectorXd::Zero(dim_u_); @@ -86,7 +85,7 @@ void SimModelDelaySteerAccGeared::initializeInputQueue(const float64_t & dt) Eigen::VectorXd SimModelDelaySteerAccGeared::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); }; + auto sat = [](float64_t val, float64_t u, float64_t l) {return std::max(std::min(val, u), l);}; const float64_t vel = sat(state(IDX::VX), vx_lim_, -vx_lim_); const float64_t acc = sat(state(IDX::ACCX), vx_rate_lim_, -vx_rate_lim_); @@ -108,6 +107,7 @@ Eigen::VectorXd SimModelDelaySteerAccGeared::calcModel( return d_state; } + float64_t SimModelDelaySteerAccGeared::calcVelocityWithGear( const Eigen::VectorXd & state, const uint8_t gear) const { diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp index 9933ba5be790c..ee74b645850cf 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.cpp @@ -14,23 +14,25 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc.hpp" + SimModelIdealSteerAcc::SimModelIdealSteerAcc(float64_t wheelbase) -: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) -{ -} +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} -float64_t SimModelIdealSteerAcc::getX() { return state_(IDX::X); } -float64_t SimModelIdealSteerAcc::getY() { return state_(IDX::Y); } -float64_t SimModelIdealSteerAcc::getYaw() { return state_(IDX::YAW); } -float64_t SimModelIdealSteerAcc::getVx() { return state_(IDX::VX); } -float64_t SimModelIdealSteerAcc::getVy() { return 0.0; } -float64_t SimModelIdealSteerAcc::getAx() { return input_(IDX_U::AX_DES); } +float64_t SimModelIdealSteerAcc::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerAcc::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerAcc::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerAcc::getVx() {return state_(IDX::VX);} +float64_t SimModelIdealSteerAcc::getVy() {return 0.0;} +float64_t SimModelIdealSteerAcc::getAx() {return input_(IDX_U::AX_DES);} float64_t SimModelIdealSteerAcc::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerAcc::getSteer() { return input_(IDX_U::STEER_DES); } -void SimModelIdealSteerAcc::update(const float64_t & dt) { updateRungeKutta(dt, input_); } +float64_t SimModelIdealSteerAcc::getSteer() {return input_(IDX_U::STEER_DES);} +void SimModelIdealSteerAcc::update(const float64_t & dt) +{ + updateRungeKutta(dt, input_); +} Eigen::VectorXd SimModelIdealSteerAcc::calcModel( const Eigen::VectorXd & state, const Eigen::VectorXd & input) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index 49e065cf651f7..a3c7c42428606 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -12,28 +12,25 @@ // See the License for the specific language governing permissions and // limitations under the License. -#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" +#include +#include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.hpp" #include "autoware_auto_vehicle_msgs/msg/gear_command.hpp" -#include - SimModelIdealSteerAccGeared::SimModelIdealSteerAccGeared(float64_t wheelbase) -: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) -{ -} +: SimModelInterface(4 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase), current_acc_(0.0) {} -float64_t SimModelIdealSteerAccGeared::getX() { return state_(IDX::X); } -float64_t SimModelIdealSteerAccGeared::getY() { return state_(IDX::Y); } -float64_t SimModelIdealSteerAccGeared::getYaw() { return state_(IDX::YAW); } -float64_t SimModelIdealSteerAccGeared::getVx() { return state_(IDX::VX); } -float64_t SimModelIdealSteerAccGeared::getVy() { return 0.0; } -float64_t SimModelIdealSteerAccGeared::getAx() { return current_acc_; } +float64_t SimModelIdealSteerAccGeared::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerAccGeared::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerAccGeared::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerAccGeared::getVx() {return state_(IDX::VX);} +float64_t SimModelIdealSteerAccGeared::getVy() {return 0.0;} +float64_t SimModelIdealSteerAccGeared::getAx() {return current_acc_;} float64_t SimModelIdealSteerAccGeared::getWz() { return state_(IDX::VX) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerAccGeared::getSteer() { return input_(IDX_U::STEER_DES); } +float64_t SimModelIdealSteerAccGeared::getSteer() {return input_(IDX_U::STEER_DES);} void SimModelIdealSteerAccGeared::update(const float64_t & dt) { const auto prev_vx = state_(IDX::VX); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp index 7755f50054454..ea40f647e2379 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.cpp @@ -14,22 +14,21 @@ #include "simple_planning_simulator/vehicle_model/sim_model_ideal_steer_vel.hpp" + SimModelIdealSteerVel::SimModelIdealSteerVel(float64_t wheelbase) -: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) -{ -} +: SimModelInterface(3 /* dim x */, 2 /* dim u */), wheelbase_(wheelbase) {} -float64_t SimModelIdealSteerVel::getX() { return state_(IDX::X); } -float64_t SimModelIdealSteerVel::getY() { return state_(IDX::Y); } -float64_t SimModelIdealSteerVel::getYaw() { return state_(IDX::YAW); } -float64_t SimModelIdealSteerVel::getVx() { return input_(IDX_U::VX_DES); } -float64_t SimModelIdealSteerVel::getVy() { return 0.0; } -float64_t SimModelIdealSteerVel::getAx() { return current_ax_; } +float64_t SimModelIdealSteerVel::getX() {return state_(IDX::X);} +float64_t SimModelIdealSteerVel::getY() {return state_(IDX::Y);} +float64_t SimModelIdealSteerVel::getYaw() {return state_(IDX::YAW);} +float64_t SimModelIdealSteerVel::getVx() {return input_(IDX_U::VX_DES);} +float64_t SimModelIdealSteerVel::getVy() {return 0.0;} +float64_t SimModelIdealSteerVel::getAx() {return current_ax_;} float64_t SimModelIdealSteerVel::getWz() { return input_(IDX_U::VX_DES) * std::tan(input_(IDX_U::STEER_DES)) / wheelbase_; } -float64_t SimModelIdealSteerVel::getSteer() { return input_(IDX_U::STEER_DES); } +float64_t SimModelIdealSteerVel::getSteer() {return input_(IDX_U::STEER_DES);} void SimModelIdealSteerVel::update(const float64_t & dt) { updateRungeKutta(dt, input_); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp index c3c36fd7846fc..13bf38fc11f72 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_interface.cpp @@ -14,7 +14,8 @@ #include "simple_planning_simulator/vehicle_model/sim_model_interface.hpp" -SimModelInterface::SimModelInterface(int dim_x, int dim_u) : dim_x_(dim_x), dim_u_(dim_u) +SimModelInterface::SimModelInterface(int dim_x, int dim_u) +: dim_x_(dim_x), dim_u_(dim_u) { state_ = Eigen::VectorXd::Zero(dim_x_); input_ = Eigen::VectorXd::Zero(dim_u_); @@ -33,8 +34,8 @@ void SimModelInterface::updateEuler(const float64_t & dt, const Eigen::VectorXd { state_ += calcModel(state_, input) * dt; } -void SimModelInterface::getState(Eigen::VectorXd & state) { state = state_; } -void SimModelInterface::getInput(Eigen::VectorXd & input) { input = input_; } -void SimModelInterface::setState(const Eigen::VectorXd & state) { state_ = state; } -void SimModelInterface::setInput(const Eigen::VectorXd & input) { input_ = input; } -void SimModelInterface::setGear(const uint8_t gear) { gear_ = gear; } +void SimModelInterface::getState(Eigen::VectorXd & state) {state = state_;} +void SimModelInterface::getInput(Eigen::VectorXd & input) {input = input_;} +void SimModelInterface::setState(const Eigen::VectorXd & state) {state_ = state;} +void SimModelInterface::setInput(const Eigen::VectorXd & input) {input_ = input;} +void SimModelInterface::setGear(const uint8_t gear) {gear_ = gear;} diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index 655cac2b6921a..b048b73553675 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -12,11 +12,11 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include #include "gtest/gtest.h" -#include "motion_common/motion_common.hpp" -#include "simple_planning_simulator/simple_planning_simulator_core.hpp" -#include +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" +#include "motion_common/motion_common.hpp" using autoware_auto_control_msgs::msg::AckermannControlCommand; using autoware_auto_vehicle_msgs::msg::GearCommand; @@ -30,35 +30,46 @@ std::string toStrInfo(const VehicleKinematicState & state) { const auto & s = state.state; std::stringstream ss; - ss << "state x: " << s.pose.position.x << ", y: " << s.pose.position.y - << ", yaw: " << motion::motion_common::to_angle(s.pose.orientation) - << ", vx = " << s.longitudinal_velocity_mps << ", vy: " << s.lateral_velocity_mps - << ", ax: " << s.acceleration_mps2 << ", steer: " << s.front_wheel_angle_rad; + ss << "state x: " << s.pose.position.x << ", y: " << s.pose.position.y << ", yaw: " << + motion::motion_common::to_angle( + s.pose.orientation) << ", vx = " << s.longitudinal_velocity_mps << ", vy: " << + s.lateral_velocity_mps << + ", ax: " << s.acceleration_mps2 << ", steer: " << s.front_wheel_angle_rad; return ss.str(); } -const std::vector vehicle_model_type_vec = { - // NOLINT - "IDEAL_STEER_VEL", "IDEAL_STEER_ACC", "IDEAL_STEER_ACC_GEARED", - "DELAY_STEER_ACC", "DELAY_STEER_ACC_GEARED", +const std::vector vehicle_model_type_vec = { // NOLINT + "IDEAL_STEER_VEL", + "IDEAL_STEER_ACC", + "IDEAL_STEER_ACC_GEARED", + "DELAY_STEER_ACC", + "DELAY_STEER_ACC_GEARED", }; static constexpr float64_t COM_TO_BASELINK = 1.5; class PubSubNode : public rclcpp::Node { public: - PubSubNode() : Node{"test_simple_planning_simulator_pubsub"} + PubSubNode() + : Node{"test_simple_planning_simulator_pubsub"} { kinematic_state_sub_ = create_subscription( "output/kinematic_state", rclcpp::QoS{1}, - [this](const VehicleKinematicState::SharedPtr msg) { current_state_ = msg; }); - pub_control_command_ = - create_publisher("input/vehicle_control_command", rclcpp::QoS{1}); - pub_ackermann_command_ = - create_publisher("input/ackermann_control_command", rclcpp::QoS{1}); - pub_initialpose_ = create_publisher("/initialpose", rclcpp::QoS{1}); - pub_state_cmd_ = - create_publisher("/input/vehicle_state_command", rclcpp::QoS{1}); + [this](const VehicleKinematicState::SharedPtr msg) { + current_state_ = msg; + }); + pub_control_command_ = create_publisher( + "input/vehicle_control_command", + rclcpp::QoS{1}); + pub_ackermann_command_ = create_publisher( + "input/ackermann_control_command", + rclcpp::QoS{1}); + pub_initialpose_ = create_publisher( + "/initialpose", + rclcpp::QoS{1}); + pub_state_cmd_ = create_publisher( + "/input/vehicle_state_command", + rclcpp::QoS{1}); } rclcpp::Publisher::SharedPtr pub_control_command_; @@ -123,7 +134,8 @@ void resetInitialpose(rclcpp::Node::SharedPtr sim_node, std::shared_ptr pub_sub_node) + uint8_t gear, rclcpp::Node::SharedPtr sim_node, + std::shared_ptr pub_sub_node) { VehicleStateCommand cmd; cmd.stamp = sim_node->now(); @@ -181,6 +193,7 @@ VehicleKinematicState comToBaselink(const VehicleKinematicState & com) return baselink; } + // Check which direction the vehicle is heading on the baselink coordinates. // y // | @@ -253,10 +266,12 @@ TEST(TestSimplePlanningSimulatorIdealSteerVel, TestMoving) const float32_t target_acc = 5.0f; const float32_t target_steer = 0.2f; - auto _resetInitialpose = [&]() { resetInitialpose(sim_node, pub_sub_node); }; + auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; auto _sendFwdGear = [&]() { sendGear(GearCommand::DRIVE, sim_node, pub_sub_node); }; auto _sendBwdGear = [&]() { sendGear(GearCommand::REVERSE, sim_node, pub_sub_node); }; - auto _sendCommand = [&](const auto & _cmd) { sendCommand(_cmd, sim_node, pub_sub_node); }; + auto _sendCommand = [&](const auto & _cmd) { + sendCommand(_cmd, sim_node, pub_sub_node); + }; // check pub-sub connections { @@ -320,14 +335,13 @@ TEST(test_simple_planning_simulator, test_moving_ackermann) const float32_t target_acc = 5.0f; const float32_t target_steer = 0.2f; - auto _resetInitialpose = [&]() { resetInitialpose(sim_node, pub_sub_node); }; - auto _sendFwdGear = [&]() { - sendGear(VehicleStateCommand::GEAR_DRIVE, sim_node, pub_sub_node); - }; - auto _sendBwdGear = [&]() { - sendGear(VehicleStateCommand::GEAR_REVERSE, sim_node, pub_sub_node); - }; - auto _sendCommand = [&](const auto & _cmd) { sendCommand(_cmd, sim_node, pub_sub_node); }; + auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; + auto _sendFwdGear = [&]() {sendGear(VehicleStateCommand::GEAR_DRIVE, sim_node, pub_sub_node);}; + auto _sendBwdGear = + [&]() {sendGear(VehicleStateCommand::GEAR_REVERSE, sim_node, pub_sub_node);}; + auto _sendCommand = [&](const auto & _cmd) { + sendCommand(_cmd, sim_node, pub_sub_node); + }; // check pub-sub connections { From e277c4c109cc2817ad499137703a00011e66bca0 Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Mon, 6 Dec 2021 16:23:27 +0900 Subject: [PATCH 51/52] fix format --- .../simple_planning_simulator_core.cpp | 77 ++++++++----------- .../sim_model_delay_steer_acc_geared.cpp | 3 +- .../sim_model_ideal_steer_acc_geared.cpp | 3 +- .../test/test_simple_planning_simulator.cpp | 4 +- 4 files changed, 39 insertions(+), 48 deletions(-) diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp index fef5e432c452f..bd204a33fc2ad 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp @@ -12,26 +12,24 @@ // See the License for the specific language governing permissions and // limitations under the License. +#include "simple_planning_simulator/simple_planning_simulator_core.hpp" + +#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" +#include "autoware_utils/ros/update_param.hpp" +#include "common/types.hpp" +#include "motion_common/motion_common.hpp" +#include "rclcpp_components/register_node_macro.hpp" +#include "simple_planning_simulator/vehicle_model/sim_model.hpp" +#include "vehicle_info_util/vehicle_info_util.hpp" #include +#include +#include #include #include #include #include -#include -#include - -#include "simple_planning_simulator/simple_planning_simulator_core.hpp" - -#include "common/types.hpp" -#include "autoware_utils/ros/update_param.hpp" -#include "autoware_auto_tf2/tf2_autoware_auto_msgs.hpp" -#include "simple_planning_simulator/vehicle_model/sim_model.hpp" -#include "motion_common/motion_common.hpp" -#include "vehicle_info_util/vehicle_info_util.hpp" - -#include "rclcpp_components/register_node_macro.hpp" using namespace std::chrono_literals; @@ -89,8 +87,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt using std::placeholders::_2; sub_init_pose_ = create_subscription( - "/initialpose", QoS{1}, - std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); + "/initialpose", QoS{1}, std::bind(&SimplePlanningSimulator::on_initialpose, this, _1)); sub_ackermann_cmd_ = create_subscription( "input/ackermann_control_command", QoS{1}, std::bind(&SimplePlanningSimulator::on_ackermann_cmd, this, _1)); @@ -119,8 +116,8 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt pub_tf_ = create_publisher("/tf", QoS{1}); /* set param callback */ - set_param_res_ = - this->add_on_set_parameters_callback(std::bind(&SimplePlanningSimulator::on_parameter, this, _1)); + set_param_res_ = this->add_on_set_parameters_callback( + std::bind(&SimplePlanningSimulator::on_parameter, this, _1)); timer_sampling_time_ms_ = static_cast(declare_parameter("timer_sampling_time_ms", 25)); on_timer_ = create_wall_timer( @@ -141,13 +138,12 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt RCLCPP_INFO(this->get_logger(), "initialize_source : %s", initialize_source.c_str()); if (initialize_source == "ORIGIN") { Pose p; - p.orientation.w = 1.0; // yaw = 0 - set_initial_state(p, Twist{}); // initialize with 0 for all variables + p.orientation.w = 1.0; // yaw = 0 + set_initial_state(p, Twist{}); // initialize with 0 for all variables } else if (initialize_source == "INITIAL_POSE_TOPIC") { // initialpose sub already exists. Do nothing. } - // measurement noise { std::random_device seed; @@ -196,17 +192,13 @@ void SimplePlanningSimulator::initialize_vehicle_model() } else if (vehicle_model_type_str == "DELAY_STEER_ACC") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC; vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, - steer_rate_lim, wheelbase, - timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, - steer_time_constant); + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); } else if (vehicle_model_type_str == "DELAY_STEER_ACC_GEARED") { vehicle_model_type_ = VehicleModelType::DELAY_STEER_ACC_GEARED; vehicle_model_ptr_ = std::make_shared( - vel_lim, steer_lim, vel_rate_lim, - steer_rate_lim, wheelbase, - timer_sampling_time_ms_ / 1000.0, acc_time_delay, acc_time_constant, steer_time_delay, - steer_time_constant); + vel_lim, steer_lim, vel_rate_lim, steer_rate_lim, wheelbase, timer_sampling_time_ms_ / 1000.0, + acc_time_delay, acc_time_constant, steer_time_delay, steer_time_constant); } else { throw std::invalid_argument("Invalid vehicle_model_type: " + vehicle_model_type_str); } @@ -273,8 +265,7 @@ void SimplePlanningSimulator::on_timer() publish_tf(current_odometry_); } -void SimplePlanningSimulator::on_initialpose( - const PoseWithCovarianceStamped::ConstSharedPtr msg) +void SimplePlanningSimulator::on_initialpose(const PoseWithCovarianceStamped::ConstSharedPtr msg) { // save initial pose Twist initial_twist; @@ -302,8 +293,7 @@ void SimplePlanningSimulator::on_ackermann_cmd( { current_ackermann_cmd_ptr_ = msg; set_input( - msg->lateral.steering_tire_angle, msg->longitudinal.speed, - msg->longitudinal.acceleration); + msg->lateral.steering_tire_angle, msg->longitudinal.speed, msg->longitudinal.acceleration); } void SimplePlanningSimulator::set_input(const float steer, const float vel, const float accel) @@ -314,9 +304,12 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons // TODO (Watanabe): The definition of the sign of acceleration in REVERSE mode is different // between .auto and proposal.iv, and will be discussed later. float acc = accel; - if(!current_gear_cmd_ptr_) { + if (!current_gear_cmd_ptr_) { acc = 0.0; - } else if (current_gear_cmd_ptr_->command == GearCommand::REVERSE || current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) { + } else if ( + current_gear_cmd_ptr_->command == GearCommand::REVERSE || + current_gear_cmd_ptr_->command == GearCommand::REVERSE_2) + { acc = -accel; } @@ -336,12 +329,12 @@ void SimplePlanningSimulator::set_input(const float steer, const float vel, cons vehicle_model_ptr_->setInput(input); } -void SimplePlanningSimulator::on_gear_cmd( - const GearCommand::ConstSharedPtr msg) +void SimplePlanningSimulator::on_gear_cmd(const GearCommand::ConstSharedPtr msg) { current_gear_cmd_ptr_ = msg; - if (vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || + if ( + vehicle_model_type_ == VehicleModelType::IDEAL_STEER_ACC_GEARED || vehicle_model_type_ == VehicleModelType::DELAY_STEER_ACC_GEARED) { vehicle_model_ptr_->setGear(current_gear_cmd_ptr_->command); @@ -354,8 +347,7 @@ void SimplePlanningSimulator::on_turn_indicators_cmd( current_turn_indicators_cmd_ptr_ = msg; } -void SimplePlanningSimulator::on_hazard_lights_cmd( - const HazardLightsCommand::ConstSharedPtr msg) +void SimplePlanningSimulator::on_hazard_lights_cmd(const HazardLightsCommand::ConstSharedPtr msg) { current_hazard_lights_cmd_ptr_ = msg; } @@ -382,7 +374,6 @@ void SimplePlanningSimulator::add_measurement_noise( steer.steering_tire_angle += static_cast((*n.steer_dist_)(*n.rand_engine_)); } - void SimplePlanningSimulator::set_initial_state_with_transform( const PoseStamped & pose_stamped, const Twist & twist) { @@ -395,8 +386,7 @@ void SimplePlanningSimulator::set_initial_state_with_transform( set_initial_state(pose, twist); } -void SimplePlanningSimulator::set_initial_state( - const Pose & pose, const Twist & twist) +void SimplePlanningSimulator::set_initial_state(const Pose & pose, const Twist & twist) { const float64_t x = pose.position.x; const float64_t y = pose.position.y; @@ -461,8 +451,7 @@ TransformStamped SimplePlanningSimulator::get_transform_msg( try { const auto time_point = tf2::TimePoint(std::chrono::milliseconds(0)); transform = tf_buffer_.lookupTransform( - parent_frame, child_frame, time_point, tf2::durationFromSec( - 0.0)); + parent_frame, child_frame, time_point, tf2::durationFromSec(0.0)); break; } catch (tf2::TransformException & ex) { RCLCPP_ERROR(this->get_logger(), "%s", ex.what()); diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp index dda3048387e07..092c672e311fa 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_delay_steer_acc_geared.cpp @@ -120,7 +120,8 @@ float64_t SimModelDelaySteerAccGeared::calcVelocityWithGear( gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || - gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) + { if (state(IDX::VX) < 0.0) { return 0.0; } diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp index a3c7c42428606..9f4b6663d05f5 100644 --- a/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp +++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/vehicle_model/sim_model_ideal_steer_acc_geared.cpp @@ -71,7 +71,8 @@ float64_t SimModelIdealSteerAccGeared::calcVelocityWithGear( gear == GearCommand::DRIVE_12 || gear == GearCommand::DRIVE_13 || gear == GearCommand::DRIVE_14 || gear == GearCommand::DRIVE_15 || gear == GearCommand::DRIVE_16 || gear == GearCommand::DRIVE_17 || - gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) { + gear == GearCommand::DRIVE_18 || gear == GearCommand::LOW || gear == GearCommand::LOW_2) + { if (state(IDX::VX) < 0.0) { return 0.0; } diff --git a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp index b048b73553675..e0de817247e2f 100644 --- a/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp +++ b/simulator/simple_planning_simulator/test/test_simple_planning_simulator.cpp @@ -267,8 +267,8 @@ TEST(TestSimplePlanningSimulatorIdealSteerVel, TestMoving) const float32_t target_steer = 0.2f; auto _resetInitialpose = [&]() {resetInitialpose(sim_node, pub_sub_node);}; - auto _sendFwdGear = [&]() { sendGear(GearCommand::DRIVE, sim_node, pub_sub_node); }; - auto _sendBwdGear = [&]() { sendGear(GearCommand::REVERSE, sim_node, pub_sub_node); }; + auto _sendFwdGear = [&]() {sendGear(GearCommand::DRIVE, sim_node, pub_sub_node);}; + auto _sendBwdGear = [&]() {sendGear(GearCommand::REVERSE, sim_node, pub_sub_node);}; auto _sendCommand = [&](const auto & _cmd) { sendCommand(_cmd, sim_node, pub_sub_node); }; From d947eeec755e0bb2a4d92fb46f96ed9776b8bb7e Mon Sep 17 00:00:00 2001 From: "tomoya.kimura" Date: Mon, 6 Dec 2021 19:19:25 +0900 Subject: [PATCH 52/52] fix autoware_api_utils --- .../include/autoware_api_utils/rclcpp/client.hpp | 6 ++---- .../include/autoware_api_utils/rclcpp/service.hpp | 6 ++---- 2 files changed, 4 insertions(+), 8 deletions(-) diff --git a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp b/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp index ebe6204b38d6a..ed2716df5d31e 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp +++ b/common/autoware_api_utils/include/autoware_api_utils/rclcpp/client.hpp @@ -41,8 +41,7 @@ class Client const typename ServiceT::Request::SharedPtr & request, const std::chrono::nanoseconds & timeout = std::chrono::seconds(2)) { - RCLCPP_INFO( - logger_, "client request: \n%s", rosidl_generator_traits::to_yaml(*request).c_str()); + RCLCPP_INFO(logger_, "client request"); if (!client_->service_is_ready()) { RCLCPP_INFO(logger_, "client available"); @@ -55,8 +54,7 @@ class Client return {response_error("Internal service has timed out."), nullptr}; } - RCLCPP_INFO( - logger_, "client response: \n%s", rosidl_generator_traits::to_yaml(future.get()).c_str()); + RCLCPP_INFO(logger_, "client response"); return {response_success(), future.get()}; } diff --git a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp b/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp index 08095bc8751ea..39d11b19fda68 100644 --- a/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp +++ b/common/autoware_api_utils/include/autoware_api_utils/rclcpp/service.hpp @@ -33,11 +33,9 @@ class Service auto wrapped_callback = [logger, callback]( typename ServiceT::Request::SharedPtr request, typename ServiceT::Response::SharedPtr response) { - RCLCPP_INFO( - logger, "service request: \n%s", rosidl_generator_traits::to_yaml(*request).c_str()); + RCLCPP_INFO(logger, "service request"); callback(request, response); - RCLCPP_INFO( - logger, "service response: \n%s", rosidl_generator_traits::to_yaml(*response).c_str()); + RCLCPP_INFO(logger, "service response"); }; return wrapped_callback; }