Skip to content

Commit ac48560

Browse files
authored
Merge branch 'main' into fix/preprocessor_error_directive
2 parents afff74a + 4214697 commit ac48560

File tree

12 files changed

+38
-28
lines changed

12 files changed

+38
-28
lines changed

.github/CODEOWNERS

+1-1
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,7 @@ launch/tier4_vehicle_launch/** yukihiro.saito@tier4.jp
8181
localization/autoware_landmark_based_localizer/autoware_ar_tag_based_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
8282
localization/autoware_landmark_based_localizer/autoware_landmark_manager/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
8383
localization/autoware_pose_covariance_modifier/** melike@leodrive.ai
84+
localization/autoware_stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
8485
localization/ekf_localizer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp takamasa.horibe@tier4.jp takeshi.ishita@tier4.jp yamato.ando@tier4.jp
8586
localization/geo_pose_projector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
8687
localization/gyro_odometer/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
@@ -91,7 +92,6 @@ localization/pose2twist/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masah
9192
localization/pose_estimator_arbiter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
9293
localization/pose_initializer/** anh.nguyen.2@tier4.jp isamu.takagi@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
9394
localization/pose_instability_detector/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
94-
localization/stop_filter/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
9595
localization/twist2accel/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp koji.minoda@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
9696
localization/yabloc/yabloc_common/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp
9797
localization/yabloc/yabloc_image_processing/** anh.nguyen.2@tier4.jp kento.yabuuchi.2@tier4.jp masahiro.sakamoto@tier4.jp ryu.yamamoto@tier4.jp shintaro.sakoda@tier4.jp taiki.yamada@tier4.jp yamato.ando@tier4.jp

launch/tier4_localization_launch/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -17,7 +17,7 @@
1717
</group>
1818

1919
<group>
20-
<include file="$(find-pkg-share stop_filter)/launch/stop_filter.launch.xml">
20+
<include file="$(find-pkg-share autoware_stop_filter)/launch/stop_filter.launch.xml">
2121
<arg name="use_twist_with_covariance" value="True"/>
2222
<arg name="input_odom_name" value="/localization/pose_twist_fusion_filter/kinematic_state"/>
2323
<arg name="input_twist_with_covariance_name" value="/localization/pose_twist_fusion_filter/twist_with_covariance"/>

localization/stop_filter/CMakeLists.txt localization/autoware_stop_filter/CMakeLists.txt

+2-1
Original file line numberDiff line numberDiff line change
@@ -1,11 +1,12 @@
11
cmake_minimum_required(VERSION 3.14)
2-
project(stop_filter)
2+
project(autoware_stop_filter)
33

44
find_package(autoware_cmake REQUIRED)
55
autoware_package()
66

77
ament_auto_add_library(${PROJECT_NAME} SHARED
88
src/stop_filter.cpp
9+
src/stop_filter.hpp
910
)
1011

1112
rclcpp_components_register_node(${PROJECT_NAME}
File renamed without changes.

localization/stop_filter/launch/stop_filter.launch.xml localization/autoware_stop_filter/launch/stop_filter.launch.xml

+2-2
Original file line numberDiff line numberDiff line change
@@ -1,9 +1,9 @@
11
<launch>
2-
<arg name="param_path" default="$(find-pkg-share stop_filter)/config/stop_filter.param.yaml"/>
2+
<arg name="param_path" default="$(find-pkg-share autoware_stop_filter)/config/stop_filter.param.yaml"/>
33
<arg name="input_odom_name" default="ekf_odom"/>
44
<arg name="output_odom_name" default="stop_filter_odom"/>
55
<arg name="debug_stop_flag" default="debug/stop_flag"/>
6-
<node pkg="stop_filter" exec="stop_filter_node" output="both">
6+
<node pkg="autoware_stop_filter" exec="autoware_stop_filter_node" output="both">
77
<remap from="input/odom" to="$(var input_odom_name)"/>
88

99
<remap from="output/odom" to="$(var output_odom_name)"/>

localization/stop_filter/package.xml localization/autoware_stop_filter/package.xml

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,7 @@
11
<?xml version="1.0"?>
22
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
33
<package format="3">
4-
<name>stop_filter</name>
4+
<name>autoware_stop_filter</name>
55
<version>0.0.0</version>
66
<description>The stop filter package</description>
77
<maintainer email="koji.minoda@tier4.jp">Koji Minoda</maintainer>

localization/stop_filter/src/stop_filter.cpp localization/autoware_stop_filter/src/stop_filter.cpp

+5-2
Original file line numberDiff line numberDiff line change
@@ -12,7 +12,7 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#include "stop_filter/stop_filter.hpp"
15+
#include "stop_filter.hpp"
1616

1717
#include <rclcpp/logging.hpp>
1818

@@ -24,6 +24,8 @@
2424

2525
using std::placeholders::_1;
2626

27+
namespace autoware::stop_filter
28+
{
2729
StopFilter::StopFilter(const rclcpp::NodeOptions & node_options)
2830
: rclcpp::Node("stop_filter", node_options)
2931
{
@@ -57,6 +59,7 @@ void StopFilter::callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg)
5759
pub_stop_flag_->publish(stop_flag_msg);
5860
pub_odom_->publish(odom_msg);
5961
}
62+
} // namespace autoware::stop_filter
6063

6164
#include <rclcpp_components/register_node_macro.hpp>
62-
RCLCPP_COMPONENTS_REGISTER_NODE(StopFilter)
65+
RCLCPP_COMPONENTS_REGISTER_NODE(autoware::stop_filter::StopFilter)

localization/stop_filter/include/stop_filter/stop_filter.hpp localization/autoware_stop_filter/src/stop_filter.hpp

+6-3
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,8 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef STOP_FILTER__STOP_FILTER_HPP_
16-
#define STOP_FILTER__STOP_FILTER_HPP_
15+
#ifndef STOP_FILTER_HPP_
16+
#define STOP_FILTER_HPP_
1717

1818
#include <rclcpp/rclcpp.hpp>
1919

@@ -34,6 +34,8 @@
3434
#include <string>
3535
#include <vector>
3636

37+
namespace autoware::stop_filter
38+
{
3739
class StopFilter : public rclcpp::Node
3840
{
3941
public:
@@ -54,4 +56,5 @@ class StopFilter : public rclcpp::Node
5456
*/
5557
void callback_odometry(const nav_msgs::msg::Odometry::SharedPtr msg);
5658
};
57-
#endif // STOP_FILTER__STOP_FILTER_HPP_
59+
} // namespace autoware::stop_filter
60+
#endif // STOP_FILTER_HPP_

perception/autoware_map_based_prediction/src/map_based_prediction_node.cpp

+7-6
Original file line numberDiff line numberDiff line change
@@ -1771,11 +1771,12 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
17711771
object.kinematics.acceleration_with_covariance.accel.linear.y)
17721772
: 0.0;
17731773
const double t_h = time_horizon;
1774-
const double λ = std::log(2) / acceleration_exponential_half_life_;
1774+
const double lambda = std::log(2) / acceleration_exponential_half_life_;
17751775

17761776
auto get_search_distance_with_decaying_acc = [&]() -> double {
17771777
const double acceleration_distance =
1778-
obj_acc * (1.0 / λ) * t_h + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
1778+
obj_acc * (1.0 / lambda) * t_h +
1779+
obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);
17791780
double search_dist = acceleration_distance + obj_vel * t_h;
17801781
return search_dist;
17811782
};
@@ -1787,13 +1788,13 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
17871788
return obj_vel * t_h;
17881789
}
17891790
// Time to reach final speed
1790-
const double t_f = (-1.0 / λ) * std::log(1 - ((final_speed - obj_vel) * λ) / obj_acc);
1791+
const double t_f = (-1.0 / lambda) * std::log(1 - ((final_speed - obj_vel) * lambda) / obj_acc);
17911792
// It is assumed the vehicle accelerates until final_speed is reached and
17921793
// then continues at constant speed for the rest of the time horizon
17931794
const double search_dist =
17941795
// Distance covered while accelerating
1795-
obj_acc * (1.0 / λ) * t_f + obj_acc * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) +
1796-
obj_vel * t_f +
1796+
obj_acc * (1.0 / lambda) * t_f +
1797+
obj_acc * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) + obj_vel * t_f +
17971798
// Distance covered at constant speed
17981799
final_speed * (t_h - t_f);
17991800
return search_dist;
@@ -1807,7 +1808,7 @@ std::vector<PredictedRefPath> MapBasedPredictionNode::getPredictedReferencePath(
18071808
const double legal_speed_limit = static_cast<double>(limit.speedLimit.value());
18081809

18091810
double final_speed_after_acceleration =
1810-
obj_vel + obj_acc * (1.0 / λ) * (1.0 - std::exp(-λ * t_h));
1811+
obj_vel + obj_acc * (1.0 / lambda) * (1.0 - std::exp(-lambda * t_h));
18111812

18121813
const double final_speed_limit = legal_speed_limit * speed_limit_multiplier_;
18131814
const bool final_speed_surpasses_limit = final_speed_after_acceleration > final_speed_limit;

perception/autoware_map_based_prediction/src/path_generator.cpp

+13-11
Original file line numberDiff line numberDiff line change
@@ -421,7 +421,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
421421

422422
// Using a decaying acceleration model. Consult the README for more information about the model.
423423
const double t_h = duration;
424-
const float λ = std::log(2) / acceleration_exponential_half_life_;
424+
const float lambda = std::log(2) / acceleration_exponential_half_life_;
425425

426426
auto have_same_sign = [](double a, double b) -> bool {
427427
return (a >= 0.0 && b >= 0.0) || (a < 0.0 && b < 0.0);
@@ -434,7 +434,7 @@ FrenetPoint PathGenerator::getFrenetPoint(
434434
return v;
435435
}
436436
// Get velocity after time horizon
437-
const auto terminal_velocity = v + a * (1.0 / λ) * (1 - std::exp(-λ * t_h));
437+
const auto terminal_velocity = v + a * (1.0 / lambda) * (1 - std::exp(-lambda * t_h));
438438

439439
// If vehicle is decelerating, make sure its speed does not change signs (we assume it will, at
440440
// most stop, not reverse its direction)
@@ -443,15 +443,16 @@ FrenetPoint PathGenerator::getFrenetPoint(
443443
// if the velocities don't have the same sign, calculate when the vehicle reaches 0 speed ->
444444
// time t_stop
445445

446-
// 0 = Vo + acc(1/λ)(1-e^(-λt_stop))
447-
// e^(-λt_stop) = 1 - (-Vo* λ)/acc
448-
// t_stop = (-1/λ)*ln(1 - (-Vo* λ)/acc)
449-
// t_stop = (-1/λ)*ln(1 + (Vo* λ)/acc)
450-
auto t_stop = (-1.0 / λ) * std::log(1 + (v * λ / a));
446+
// 0 = Vo + acc(1/lambda)(1-e^(-lambda t_stop))
447+
// e^(-lambda t_stop) = 1 - (-Vo* lambda)/acc
448+
// t_stop = (-1/lambda)*ln(1 - (-Vo* lambda)/acc)
449+
// t_stop = (-1/lambda)*ln(1 + (Vo* lambda)/acc)
450+
auto t_stop = (-1.0 / lambda) * std::log1p(v * lambda / a);
451451

452452
// Calculate the distance traveled until stopping
453453
auto distance_to_reach_zero_speed =
454-
v * t_stop + a * t_stop * (1.0 / λ) + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_h) - 1);
454+
v * t_stop + a * t_stop * (1.0 / lambda) +
455+
a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_h);
455456
// Output an equivalent constant speed
456457
return distance_to_reach_zero_speed / t_h;
457458
}
@@ -461,17 +462,18 @@ FrenetPoint PathGenerator::getFrenetPoint(
461462
// assume it will continue accelerating (reckless driving)
462463
const bool object_has_surpassed_limit_already = v > speed_limit;
463464
if (terminal_velocity < speed_limit || object_has_surpassed_limit_already)
464-
return v + a * (1.0 / λ) + (a / (t_h * std::pow(λ, 2))) * (std::exp(-λ * t_h) - 1);
465+
return v + a * (1.0 / lambda) + (a / (t_h * std::pow(lambda, 2))) * std::expm1(-lambda * t_h);
465466

466467
// It is assumed the vehicle accelerates until final_speed is reached and
467468
// then continues at constant speed for the rest of the time horizon
468469
// So, we calculate the time it takes to reach the speed limit and compute how far the vehicle
469470
// would go if it accelerated until reaching the speed limit, and then continued at a constant
470471
// speed.
471-
const double t_f = (-1.0 / λ) * std::log(1 - ((speed_limit - v) * λ) / a);
472+
const double t_f = (-1.0 / lambda) * std::log(1 - ((speed_limit - v) * lambda) / a);
472473
const double distance_covered =
473474
// Distance covered while accelerating
474-
a * (1.0 / λ) * t_f + a * (1.0 / std::pow(λ, 2)) * (std::exp(-λ * t_f) - 1) + v * t_f +
475+
a * (1.0 / lambda) * t_f + a * (1.0 / std::pow(lambda, 2)) * std::expm1(-lambda * t_f) +
476+
v * t_f +
475477
// Distance covered at constant speed for the rest of the horizon time
476478
speed_limit * (t_h - t_f);
477479
return distance_covered / t_h;

0 commit comments

Comments
 (0)