Skip to content

Commit

Permalink
Merge pull request #3 from autowarefoundation/tier4/proposal
Browse files Browse the repository at this point in the history
chore: sync upstream
  • Loading branch information
kenji-miyake authored Jan 26, 2022
2 parents 23f96c0 + 45446dd commit a0fe549
Show file tree
Hide file tree
Showing 14 changed files with 53 additions and 132 deletions.
Original file line number Diff line number Diff line change
Expand Up @@ -9,7 +9,6 @@

<arg name="output_pose_topic" value="/localization/pose_estimator/pose"/>
<arg name="output_pose_with_covariance_topic" value="/localization/pose_estimator/pose_with_covariance"/>
<arg name="output_diagnostics_topic" value="/localization/diagnostics"/>

<arg name="param_file" value="$(find-pkg-share tier4_localization_launch)/config/ndt_scan_matcher.param.yaml" />
</include>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -8,7 +8,6 @@

<arg name="output_pose_topic" default="ndt_pose" description="Estimated self position" />
<arg name="output_pose_with_covariance_topic" default="ndt_pose_with_covariance" description="Estimated self position with covariance" />
<arg name="output_diagnostics_topic" default="/diagnostics" description="Diagnostic topic" />


<arg name="node_name" default="ndt_scan_matcher" description="Use a different name for this node" />
Expand All @@ -21,7 +20,6 @@

<remap from="ndt_pose" to="$(var output_pose_topic)" />
<remap from="ndt_pose_with_covariance" to="$(var output_pose_with_covariance_topic)" />
<remap from="diagnostics" to="$(var output_diagnostics_topic)" />

<param from="$(var param_file)" />
</node>
Expand Down
Original file line number Diff line number Diff line change
@@ -0,0 +1,12 @@
/**:
ros__parameters:
frame_id: base_link
twist_covariance:
[
0.04, 0.0, 0.0, 0.0, 0.0, 0.0,
0.0, 10000.0, 0.0, 0.0, 0.0, 0.0,
0.0, 0.0, 10000.0, 0.0, 0.0, 0.0,
0.0, 0.0, 0.0, 10000.0, 0.0, 0.0,
0.0, 0.0, 0.0, 0.0, 10000.0, 0.0,
0.0, 0.0, 0.0, 0.0, 0.0, 0.01,
]

This file was deleted.

Original file line number Diff line number Diff line change
@@ -1,7 +1,7 @@
<launch>
<arg name="input_vehicle_velocity_topic" default="velocity_status"/>
<arg name="output_twist_with_covariance" default="twist_with_covariance"/>
<arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.yaml"/>
<arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.param.yaml"/>

<node pkg="vehicle_velocity_converter" exec="vehicle_velocity_converter" name="vehicle_velocity_converter_node" output="screen">
<param from="$(var config_file)"/>
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
// limitations under the License.

#include <scene_module/crosswalk/scene_walkway.hpp>
#include <tier4_autoware_utils/trajectory/trajectory.hpp>
#include <utilization/util.hpp>

#include <cmath>
Expand Down Expand Up @@ -80,18 +81,21 @@ bool WalkwayModule::modifyPathVelocity(
stop_factor.stop_factor_points.emplace_back(debug_data_.nearest_collision_point);
planning_utils::appendStopReason(stop_factor, stop_reason);

// update state
const Point self_pose = {
planner_data_->current_pose.pose.position.x, planner_data_->current_pose.pose.position.y};
const Point stop_pose = {
debug_data_.first_stop_pose.position.x, debug_data_.first_stop_pose.position.y};
const double distance = bg::distance(stop_pose, self_pose);
// use arc length to identify if ego vehicle is in front of walkway stop or not.
const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
path->points, planner_data_->current_pose.pose.position,
debug_data_.first_stop_pose.position);
const double distance_threshold = 1.0;
debug_data_.stop_judge_range = distance_threshold;
if (
distance < distance_threshold &&
signed_arc_dist_to_stop_point < distance_threshold &&
planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
// If ego vehicle is after walkway stop and stopped then move to stop state
state_ = State::STOP;
if (signed_arc_dist_to_stop_point < -distance_threshold) {
RCLCPP_ERROR(
logger_, "Failed to stop near walkway but ego stopped. Change state to STOPPED");
}
}
return true;
} else if (state_ == State::STOP) {
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -120,13 +120,17 @@ bool MergeFromPrivateRoadModule::modifyPathVelocity(
planning_utils::appendStopReason(stop_factor, stop_reason);
}

const double distance =
planning_utils::calcDist2d(current_pose.pose, path->points.at(stop_line_idx).point.pose);
const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
path->points, current_pose.pose.position, path->points.at(stop_line_idx).point.pose.position);

constexpr double distance_threshold = 2.0;
if (
distance < distance_threshold &&
signed_arc_dist_to_stop_point < distance_threshold &&
planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
state_machine_.setState(State::GO);
if (signed_arc_dist_to_stop_point < -distance_threshold) {
RCLCPP_ERROR(logger_, "Failed to stop near stop line but ego stopped. Change state to GO");
}
}

return true;
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -40,7 +40,7 @@ bool splineInterpolate(
*output = input;

if (input.points.size() <= 1) {
RCLCPP_WARN(logger, "Do not interpolate because path size is 1.");
RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1.");
return false;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -53,7 +53,7 @@ bool splineInterpolate(
*output = input;

if (input.points.size() <= 1) {
RCLCPP_WARN(logger, "Do not interpolate because path size is 1.");
RCLCPP_DEBUG(logger, "Do not interpolate because path size is 1.");
return false;
}

Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -13,6 +13,7 @@
// limitations under the License.

#include <scene_module/stop_line/scene.hpp>
#include <tier4_autoware_utils/trajectory/trajectory.hpp>
#include <utilization/util.hpp>

#include <algorithm>
Expand Down Expand Up @@ -87,72 +88,6 @@ boost::optional<StopLineModule::SegmentIndexWithOffset> findBackwardOffsetSegmen
return {};
}

StopLineModule::SegmentIndexWithOffset findNearestSegment(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Point & point)
{
std::vector<double> distances;
distances.reserve(path.points.size());

std::transform(
path.points.cbegin(), path.points.cend(), std::back_inserter(distances),
[&point](const autoware_auto_planning_msgs::msg::PathPointWithLaneId & p) {
return planning_utils::calcDist2d(p.point.pose.position, point);
});

const auto min_itr = std::min_element(distances.cbegin(), distances.cend());
const auto min_idx = static_cast<size_t>(std::distance(distances.cbegin(), min_itr));

// Process boundary conditions
if (min_idx == 0 || min_idx == path.points.size() - 1) {
const size_t segment_idx = min_idx == 0 ? min_idx : min_idx - 1;

return StopLineModule::SegmentIndexWithOffset{
segment_idx, planning_utils::calcDist2d(path.points.at(segment_idx), point)};
}

// Process normal conditions(having previous and next points)
const auto & p_prev = path.points.at(min_idx - 1);
const auto d_prev = planning_utils::calcDist2d(p_prev, point);

const auto & p_next = path.points.at(min_idx + 1);
const auto d_next = planning_utils::calcDist2d(p_next, point);

const size_t segment_idx = (d_prev <= d_next) ? min_idx - 1 : min_idx;

return StopLineModule::SegmentIndexWithOffset{
segment_idx, planning_utils::calcDist2d(path.points.at(segment_idx), point)};
}

double calcSignedArcLength(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path, const size_t & idx_front,
const size_t & idx_back)
{
// If flipped, reverse index and take negative
if (idx_front > idx_back) {
return -calcSignedArcLength(path, idx_back, idx_front);
}

double sum_length = 0.0;
for (size_t i = idx_front; i < idx_back; ++i) {
const auto & p1 = path.points.at(i).point.pose.position;
const auto & p2 = path.points.at(i + 1).point.pose.position;
sum_length += planning_utils::calcDist2d(p1, p2);
}

return sum_length;
}

double calcSignedArcLength(
const autoware_auto_planning_msgs::msg::PathWithLaneId & path,
const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2)
{
const auto seg_1 = findNearestSegment(path, p1);
const auto seg_2 = findNearestSegment(path, p2);

return -seg_1.offset + calcSignedArcLength(path, seg_1.index, seg_2.index) + seg_2.offset;
}

} // namespace

StopLineModule::StopLineModule(
Expand Down Expand Up @@ -282,19 +217,23 @@ bool StopLineModule::modifyPathVelocity(
const auto stop_pose_with_index = calcStopPose(*path, offset_segment);

// Calc dist to stop pose
const double signed_arc_dist_to_stop_point = calcSignedArcLength(
*path, planner_data_->current_pose.pose.position, stop_pose_with_index->pose.position);
const double signed_arc_dist_to_stop_point = tier4_autoware_utils::calcSignedArcLength(
path->points, planner_data_->current_pose.pose.position, stop_pose_with_index->pose.position);

if (state_ == State::APPROACH) {
// Insert stop pose
*path = insertStopPose(*path, *stop_pose_with_index, stop_reason);

// Move to stopped state if stopped
if (
std::abs(signed_arc_dist_to_stop_point) < planner_param_.stop_check_dist &&
signed_arc_dist_to_stop_point < planner_param_.stop_check_dist &&
planner_data_->isVehicleStopped(planner_param_.stop_duration_sec)) {
RCLCPP_INFO(logger_, "APPROACH -> STOPPED");
state_ = State::STOPPED;
if (signed_arc_dist_to_stop_point < -planner_param_.stop_check_dist) {
RCLCPP_ERROR(
logger_, "Failed to stop near stop line but ego stopped. Change state to STOPPED");
}
}
} else if (state_ == State::STOPPED) {
// Change state after vehicle departure
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -55,6 +55,9 @@ def launch_setup(context, *args, **kwargs):
vehicle_info_param,
vehicle_characteristics_param,
simulator_model_param,
{
"initial_engage_state": LaunchConfiguration("initial_engage_state"),
},
],
remappings=[
('input/ackermann_control_command', '/control/command/control_cmd'),
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -76,12 +76,12 @@ 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<rclcpp::Node>(this, [](auto) {}), false),
current_engage_(false)
tf_listener_(tf_buffer_, std::shared_ptr<rclcpp::Node>(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);
current_engage_ = declare_parameter<bool>("initial_engage_state");

using rclcpp::QoS;
using std::placeholders::_1;
Expand Down Expand Up @@ -242,7 +242,10 @@ void SimplePlanningSimulator::on_timer()
// update vehicle dynamics
{
const float64_t dt = delta_time_.get_dt(get_clock()->now());
vehicle_model_ptr_->update(dt);

if (current_engage_) {
vehicle_model_ptr_->update(dt);
}
}

// set current state
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,7 @@

<param name="update_rate" value="$(var update_rate)" />
<param name="th_arrived_distance_m" value="$(var th_arrived_distance_m)" />
<param name="th_arrived_angle_deg" value="$(var th_arrived_angle_deg)" />
<param name="th_stopped_time_sec" value="$(var th_stopped_time_sec)" />
<param name="th_stopped_velocity_mps" value="$(var th_stopped_velocity_mps)" />
<param name="disengage_on_route" value="$(var disengage_on_route)" />
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -398,8 +398,8 @@ AutowareStateMonitorNode::AutowareStateMonitorNode()

// Parameter for StateMachine
state_param_.th_arrived_distance_m = this->declare_parameter("th_arrived_distance_m", 1.0);
state_param_.th_arrived_angle =
this->declare_parameter("th_arrived_angle_deg", tier4_autoware_utils::deg2rad(45.0));
const auto th_arrived_angle_deg = this->declare_parameter("th_arrived_angle_deg", 45.0);
state_param_.th_arrived_angle = tier4_autoware_utils::deg2rad(th_arrived_angle_deg);
state_param_.th_stopped_time_sec = this->declare_parameter("th_stopped_time_sec", 1.0);
state_param_.th_stopped_velocity_mps = this->declare_parameter("th_stopped_velocity_mps", 0.01);

Expand Down

0 comments on commit a0fe549

Please sign in to comment.