-
Notifications
You must be signed in to change notification settings - Fork 61
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Merge pull request #1447 from tier4/fix/find_nearest_segment_index
fix pitch and slope acceleration calculation for ego vehicle
- Loading branch information
Showing
6 changed files
with
246 additions
and
60 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
2 changes: 2 additions & 0 deletions
2
simulation/simple_sensor_simulator/test/src/vehicle_simulation/CMakeLists.txt
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,2 @@ | ||
| ament_add_gtest(test_ego_entity_simulation test_ego_entity_simulation.cpp) | ||
| target_link_libraries(test_ego_entity_simulation simple_sensor_simulator_component ${traffic_simulator_libraries}) |
114 changes: 114 additions & 0 deletions
114
...lation/simple_sensor_simulator/test/src/vehicle_simulation/test_ego_entity_simulation.cpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,114 @@ | ||
| // Copyright 2015 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 <gtest/gtest.h> | ||
|
|
||
| #include <ament_index_cpp/get_package_share_directory.hpp> | ||
| #include <memory> | ||
| #include <rclcpp/rclcpp.hpp> | ||
| #include <simple_sensor_simulator/vehicle_simulation/ego_entity_simulation.hpp> | ||
| #include <traffic_simulator/hdmap_utils/hdmap_utils.hpp> | ||
| #include <vector> | ||
|
|
||
| using namespace vehicle_simulation; | ||
|
|
||
| TEST(EgoEntitySimulation, calculateAccelerationBySlope) | ||
| { | ||
| // initialize rclcpp for rosparam in EgoEntitySimulation class | ||
| rclcpp::init(0, nullptr); | ||
|
|
||
| auto hdmap_utils = std::make_shared<hdmap_utils::HdMapUtils>( | ||
| ament_index_cpp::get_package_share_directory("traffic_simulator") + | ||
| "/map/slope/lanelet2_map.osm", | ||
| geographic_msgs::build<geographic_msgs::msg::GeoPoint>() | ||
| .latitude(0.0) | ||
| .longitude(-1.20294718763) | ||
| .altitude(0.0)); | ||
|
|
||
| constexpr double gravity_acceleration = -9.81; | ||
| // expected value in the lanelet(id:7) | ||
| // first 25m: 1m up | ||
| constexpr double expected_slope_acceleration_first_25m = | ||
| std::sin(std::atan(1. / 25.)) * gravity_acceleration; | ||
| // last 25m: 4m up | ||
| constexpr double expected_slope_acceleration_last_25m = | ||
| std::sin(std::atan(4. / 25.)) * gravity_acceleration; | ||
|
|
||
| auto get_slope_acceleration_at = | ||
| [&](const traffic_simulator_msgs::msg::LaneletPose & lanelet_pose, bool consider_slope) { | ||
| traffic_simulator_msgs::msg::EntityStatus initial_status; | ||
| initial_status.name = "ego"; | ||
| initial_status.lanelet_pose_valid = true; | ||
| initial_status.lanelet_pose = lanelet_pose; | ||
| initial_status.pose = | ||
| traffic_simulator::pose::toMapPose(initial_status.lanelet_pose, hdmap_utils); | ||
|
|
||
| EgoEntitySimulation ego_entity_simulation( | ||
| initial_status, traffic_simulator_msgs::msg::VehicleParameters(), 1.f / 30.f, hdmap_utils, | ||
| rclcpp::Parameter("use_sim_time", false), consider_slope); | ||
| return ego_entity_simulation.calculateAccelerationBySlope(); | ||
| }; | ||
|
|
||
| traffic_simulator_msgs::msg::LaneletPose lanelet_pose; | ||
| lanelet_pose.lanelet_id = 7; | ||
|
|
||
| // This value is determined manually by @Hans_Robo. | ||
| // Since it is affected by the calculation algorithm inside lanelet2, etc., | ||
| // it will not exactly match the ideal value, so we manually selected the smallest possible value specifically for this test. | ||
| constexpr double compare_epsilon = 1e-7; | ||
|
|
||
| // first 25m, up, with considering slope | ||
| lanelet_pose.s = 12.5; | ||
| lanelet_pose.rpy.z = 0.0; | ||
| EXPECT_NEAR( | ||
| expected_slope_acceleration_first_25m, get_slope_acceleration_at(lanelet_pose, true), | ||
| compare_epsilon); | ||
|
|
||
| // first 25m, up, without considering slope | ||
| EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false)); | ||
|
|
||
| // last 25m, up | ||
| lanelet_pose.s = 37.5; | ||
| lanelet_pose.rpy.z = 0.0; | ||
| EXPECT_NEAR( | ||
| expected_slope_acceleration_last_25m, get_slope_acceleration_at(lanelet_pose, true), | ||
| compare_epsilon); | ||
|
|
||
| // last 25m, up, without considering slope | ||
| EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false)); | ||
|
|
||
| // first 25m, down | ||
| lanelet_pose.s = 12.5; | ||
| lanelet_pose.rpy.z = M_PI; | ||
| EXPECT_NEAR( | ||
| -expected_slope_acceleration_first_25m, get_slope_acceleration_at(lanelet_pose, true), | ||
| compare_epsilon); | ||
|
|
||
| // first 25m, down, without considering slope | ||
| EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false)); | ||
|
|
||
| // last 25m, down | ||
| lanelet_pose.s = 37.5; | ||
| lanelet_pose.rpy.z = M_PI; | ||
| EXPECT_NEAR( | ||
| -expected_slope_acceleration_last_25m, get_slope_acceleration_at(lanelet_pose, true), | ||
| compare_epsilon); | ||
|
|
||
| // last 25m, down, without considering slope | ||
| EXPECT_DOUBLE_EQ(0.0, get_slope_acceleration_at(lanelet_pose, false)); | ||
|
|
||
| rclcpp::shutdown(); | ||
|
|
||
| // EXPECT_NEAR(ego_entity_simulation.); | ||
| } |
119 changes: 119 additions & 0 deletions
119
simulation/traffic_simulator/test/map/slope/lanelet2_map.osm
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,119 @@ | ||
| <?xml version="1.0" encoding="UTF-8"?> | ||
| <osm generator="VMB"> | ||
| <MetaInfo format_version="1" map_version="4"/> | ||
| <node id="1" lat="0" lon="-1.20294718763"> | ||
| <tag k="local_x" v="0"/> | ||
| <tag k="local_y" v="0"/> | ||
| <tag k="ele" v="0"/> | ||
| </node> | ||
| <node id="2" lat="0" lon="-1.20231842689"> | ||
| <tag k="local_x" v="70"/> | ||
| <tag k="local_y" v="0"/> | ||
| <tag k="ele" v="0"/> | ||
| </node> | ||
| <node id="3" lat="0.00003617134" lon="-1.20294718763"> | ||
| <tag k="local_x" v="0"/> | ||
| <tag k="local_y" v="4"/> | ||
| <tag k="ele" v="0"/> | ||
| </node> | ||
| <node id="4" lat="0.00003617132" lon="-1.20231842689"> | ||
| <tag k="local_x" v="70"/> | ||
| <tag k="local_y" v="4"/> | ||
| <tag k="ele" v="0"/> | ||
| </node> | ||
| <node id="5" lat="0" lon="-1.2027226302"> | ||
| <tag k="local_x" v="25"/> | ||
| <tag k="local_y" v="0"/> | ||
| <tag k="ele" v="1"/> | ||
| </node> | ||
| <node id="6" lat="0.00003617133" lon="-1.2027226302"> | ||
| <tag k="local_x" v="25"/> | ||
| <tag k="local_y" v="4"/> | ||
| <tag k="ele" v="1"/> | ||
| </node> | ||
| <node id="141" lat="0.00003617133" lon="-1.20249807279"> | ||
| <tag k="local_x" v="50"/> | ||
| <tag k="local_y" v="4"/> | ||
| <tag k="ele" v="5"/> | ||
| </node> | ||
| <node id="142" lat="0" lon="-1.20249807279"> | ||
| <tag k="local_x" v="50"/> | ||
| <tag k="local_y" v="0"/> | ||
| <tag k="ele" v="5"/> | ||
| </node> | ||
| <node id="148" lat="0.00004521416" lon="-1.20240824984"> | ||
| <tag k="local_x" v="60"/> | ||
| <tag k="local_y" v="5"/> | ||
| <tag k="ele" v="5"/> | ||
| </node> | ||
| <node id="149" lat="0" lon="-1.20240824984"> | ||
| <tag k="local_x" v="60"/> | ||
| <tag k="local_y" v="0"/> | ||
| <tag k="ele" v="5"/> | ||
| </node> | ||
| <way id="143"> | ||
| <nd ref="3"/> | ||
| <nd ref="6"/> | ||
| <nd ref="141"/> | ||
| <tag k="type" v="line_thin"/> | ||
| <tag k="subtype" v="solid"/> | ||
| </way> | ||
| <way id="145"> | ||
| <nd ref="1"/> | ||
| <nd ref="5"/> | ||
| <nd ref="142"/> | ||
| <tag k="type" v="line_thin"/> | ||
| <tag k="subtype" v="solid"/> | ||
| </way> | ||
| <way id="150"> | ||
| <nd ref="141"/> | ||
| <nd ref="148"/> | ||
| <tag k="type" v="line_thin"/> | ||
| <tag k="subtype" v="solid"/> | ||
| </way> | ||
| <way id="151"> | ||
| <nd ref="148"/> | ||
| <nd ref="4"/> | ||
| <tag k="type" v="line_thin"/> | ||
| <tag k="subtype" v="solid"/> | ||
| </way> | ||
| <way id="152"> | ||
| <nd ref="142"/> | ||
| <nd ref="149"/> | ||
| <tag k="type" v="line_thin"/> | ||
| <tag k="subtype" v="solid"/> | ||
| </way> | ||
| <way id="153"> | ||
| <nd ref="149"/> | ||
| <nd ref="2"/> | ||
| <tag k="type" v="line_thin"/> | ||
| <tag k="subtype" v="solid"/> | ||
| </way> | ||
| <relation id="7"> | ||
| <member type="way" role="left" ref="143"/> | ||
| <member type="way" role="right" ref="145"/> | ||
| <tag k="type" v="lanelet"/> | ||
| <tag k="subtype" v="road"/> | ||
| <tag k="speed_limit" v="10"/> | ||
| <tag k="location" v="urban"/> | ||
| <tag k="one_way" v="yes"/> | ||
| </relation> | ||
| <relation id="147"> | ||
| <member type="way" role="left" ref="150"/> | ||
| <member type="way" role="right" ref="152"/> | ||
| <tag k="type" v="lanelet"/> | ||
| <tag k="subtype" v="road"/> | ||
| <tag k="speed_limit" v="10"/> | ||
| <tag k="location" v="urban"/> | ||
| <tag k="one_way" v="yes"/> | ||
| </relation> | ||
| <relation id="154"> | ||
| <member type="way" role="left" ref="151"/> | ||
| <member type="way" role="right" ref="153"/> | ||
| <tag k="type" v="lanelet"/> | ||
| <tag k="subtype" v="road"/> | ||
| <tag k="speed_limit" v="10"/> | ||
| <tag k="location" v="urban"/> | ||
| <tag k="one_way" v="yes"/> | ||
| </relation> | ||
| </osm> |