forked from autowarefoundation/autoware.universe
-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
feat(default_ad_api): add planning api (autowarefoundation#2481)
* feat(default_ad_api): add planning api Signed-off-by: Takagi, Isamu <[email protected]> * feat: complement velocity factor Signed-off-by: Takagi, Isamu <[email protected]> * feat: add stop check Signed-off-by: Takagi, Isamu <[email protected]> * feat: make the same process into a function Signed-off-by: Takagi, Isamu <[email protected]> * feat: update visualizer Signed-off-by: Takagi, Isamu <[email protected]> * fix: remove flake8 test Signed-off-by: Takagi, Isamu <[email protected]> Signed-off-by: Takagi, Isamu <[email protected]> Signed-off-by: kminoda <[email protected]>
- Loading branch information
1 parent
a8e5a72
commit a55bfa5
Showing
16 changed files
with
559 additions
and
0 deletions.
There are no files selected for viewing
46 changes: 46 additions & 0 deletions
46
common/autoware_ad_api_specs/include/autoware_ad_api_specs/planning.hpp
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,46 @@ | ||
// Copyright 2022 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. | ||
|
||
#ifndef AUTOWARE_AD_API_SPECS__PLANNING_HPP_ | ||
#define AUTOWARE_AD_API_SPECS__PLANNING_HPP_ | ||
|
||
#include <rclcpp/qos.hpp> | ||
|
||
#include <autoware_adapi_v1_msgs/msg/steering_factor_array.hpp> | ||
#include <autoware_adapi_v1_msgs/msg/velocity_factor_array.hpp> | ||
|
||
namespace autoware_ad_api::planning | ||
{ | ||
|
||
struct VelocityFactors | ||
{ | ||
using Message = autoware_adapi_v1_msgs::msg::VelocityFactorArray; | ||
static constexpr char name[] = "/api/planning/velocity_factors"; | ||
static constexpr size_t depth = 1; | ||
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; | ||
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; | ||
}; | ||
|
||
struct SteeringFactors | ||
{ | ||
using Message = autoware_adapi_v1_msgs::msg::SteeringFactorArray; | ||
static constexpr char name[] = "/api/planning/steering_factors"; | ||
static constexpr size_t depth = 1; | ||
static constexpr auto reliability = RMW_QOS_POLICY_RELIABILITY_RELIABLE; | ||
static constexpr auto durability = RMW_QOS_POLICY_DURABILITY_VOLATILE; | ||
}; | ||
|
||
} // namespace autoware_ad_api::planning | ||
|
||
#endif // AUTOWARE_AD_API_SPECS__PLANNING_HPP_ |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
Original file line number | Diff line number | Diff line change |
---|---|---|
@@ -0,0 +1,151 @@ | ||
// Copyright 2022 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 "planning.hpp" | ||
|
||
#include <memory> | ||
#include <string> | ||
#include <vector> | ||
|
||
namespace default_ad_api | ||
{ | ||
|
||
template <class T> | ||
void concat(std::vector<T> & v1, const std::vector<T> & v2) | ||
{ | ||
v1.insert(v1.end(), v2.begin(), v2.end()); | ||
} | ||
|
||
template <class T> | ||
std::vector<typename rclcpp::Subscription<T>::SharedPtr> init_factors( | ||
rclcpp::Node * node, std::vector<typename T::ConstSharedPtr> & factors, | ||
const std::vector<std::string> & topics) | ||
{ | ||
const auto callback = [&factors](const int index) { | ||
return [&factors, index](const typename T::ConstSharedPtr msg) { factors[index] = msg; }; | ||
}; | ||
|
||
std::vector<typename rclcpp::Subscription<T>::SharedPtr> subs; | ||
for (size_t index = 0; index < topics.size(); ++index) { | ||
subs.push_back(node->create_subscription<T>(topics[index], rclcpp::QoS(1), callback(index))); | ||
} | ||
factors.resize(topics.size()); | ||
return subs; | ||
} | ||
|
||
template <class T> | ||
T merge_factors(const rclcpp::Time stamp, const std::vector<typename T::ConstSharedPtr> & factors) | ||
{ | ||
T message; | ||
message.header.stamp = stamp; | ||
message.header.frame_id = "map"; | ||
|
||
for (const auto & factor : factors) { | ||
if (factor) { | ||
concat(message.factors, factor->factors); | ||
} | ||
} | ||
return message; | ||
} | ||
|
||
PlanningNode::PlanningNode(const rclcpp::NodeOptions & options) : Node("planning", options) | ||
{ | ||
// TODO(Takagi, Isamu): remove default value | ||
stop_distance_ = declare_parameter<double>("stop_distance", 1.0); | ||
stop_duration_ = declare_parameter<double>("stop_duration", 1.0); | ||
stop_checker_ = std::make_unique<VehicleStopChecker>(this, stop_duration_ + 1.0); | ||
|
||
std::vector<std::string> velocity_factor_topics = { | ||
"/planning/velocity_factors/blind_spot", | ||
"/planning/velocity_factors/crosswalk", | ||
"/planning/velocity_factors/detection_area", | ||
"/planning/velocity_factors/intersection", | ||
"/planning/velocity_factors/merge_from_private", | ||
"/planning/velocity_factors/no_stopping_area", | ||
"/planning/velocity_factors/obstacle_stop", | ||
"/planning/velocity_factors/obstacle_cruise", | ||
"/planning/velocity_factors/occlusion_spot", | ||
"/planning/velocity_factors/stop_line", | ||
"/planning/velocity_factors/surround_obstacle", | ||
"/planning/velocity_factors/traffic_light", | ||
"/planning/velocity_factors/virtual_traffic_light", | ||
"/planning/velocity_factors/walkway"}; | ||
|
||
std::vector<std::string> steering_factor_topics = { | ||
"/planning/steering_factor/avoidance", "/planning/steering_factor/intersection", | ||
"/planning/steering_factor/lane_change", "/planning/steering_factor/pull_out", | ||
"/planning/steering_factor/pull_over"}; | ||
|
||
sub_velocity_factors_ = | ||
init_factors<VelocityFactorArray>(this, velocity_factors_, velocity_factor_topics); | ||
sub_steering_factors_ = | ||
init_factors<SteeringFactorArray>(this, steering_factors_, steering_factor_topics); | ||
|
||
const auto adaptor = component_interface_utils::NodeAdaptor(this); | ||
adaptor.init_pub(pub_velocity_factors_); | ||
adaptor.init_pub(pub_steering_factors_); | ||
adaptor.init_sub(sub_kinematic_state_, this, &PlanningNode::on_kinematic_state); | ||
adaptor.init_sub(sub_trajectory_, this, &PlanningNode::on_trajectory); | ||
|
||
const auto rate = rclcpp::Rate(5); | ||
timer_ = rclcpp::create_timer(this, get_clock(), rate.period(), [this]() { on_timer(); }); | ||
} | ||
|
||
void PlanningNode::on_trajectory(const Trajectory::ConstSharedPtr msg) { trajectory_ = msg; } | ||
|
||
void PlanningNode::on_kinematic_state(const KinematicState::ConstSharedPtr msg) | ||
{ | ||
kinematic_state_ = msg; | ||
|
||
geometry_msgs::msg::TwistStamped twist; | ||
twist.header = msg->header; | ||
twist.twist = msg->twist.twist; | ||
stop_checker_->addTwist(twist); | ||
} | ||
|
||
void PlanningNode::on_timer() | ||
{ | ||
using autoware_adapi_v1_msgs::msg::VelocityFactor; | ||
auto velocity = merge_factors<VelocityFactorArray>(now(), velocity_factors_); | ||
auto steering = merge_factors<SteeringFactorArray>(now(), steering_factors_); | ||
|
||
// Set the distance if it is nan. | ||
if (trajectory_ && kinematic_state_) { | ||
for (auto & factor : velocity.factors) { | ||
if (std::isnan(factor.distance)) { | ||
const auto & curr_point = kinematic_state_->pose.pose.position; | ||
const auto & stop_point = factor.pose.position; | ||
const auto & points = trajectory_->points; | ||
factor.distance = motion_utils::calcSignedArcLength(points, curr_point, stop_point); | ||
} | ||
} | ||
} | ||
|
||
// Set the status if it is unknown. | ||
const auto is_vehicle_stopped = stop_checker_->isVehicleStopped(stop_duration_); | ||
for (auto & factor : velocity.factors) { | ||
if ((factor.status == VelocityFactor::UNKNOWN) && (!std::isnan(factor.distance))) { | ||
const auto is_stopped = is_vehicle_stopped && (factor.distance < stop_distance_); | ||
factor.status = is_stopped ? VelocityFactor::STOPPED : VelocityFactor::APPROACHING; | ||
} | ||
} | ||
|
||
pub_velocity_factors_->publish(velocity); | ||
pub_steering_factors_->publish(steering); | ||
} | ||
|
||
} // namespace default_ad_api | ||
|
||
#include <rclcpp_components/register_node_macro.hpp> | ||
RCLCPP_COMPONENTS_REGISTER_NODE(default_ad_api::PlanningNode) |
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,67 @@ | ||
// Copyright 2022 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. | ||
|
||
#ifndef PLANNING_HPP_ | ||
#define PLANNING_HPP_ | ||
|
||
#include <autoware_ad_api_specs/planning.hpp> | ||
#include <component_interface_specs/localization.hpp> | ||
#include <component_interface_specs/planning.hpp> | ||
#include <motion_utils/motion_utils.hpp> | ||
#include <rclcpp/rclcpp.hpp> | ||
|
||
#include <memory> | ||
#include <vector> | ||
|
||
// This file should be included after messages. | ||
#include "utils/types.hpp" | ||
|
||
namespace default_ad_api | ||
{ | ||
|
||
class PlanningNode : public rclcpp::Node | ||
{ | ||
public: | ||
explicit PlanningNode(const rclcpp::NodeOptions & options); | ||
|
||
private: | ||
using VelocityFactorArray = autoware_adapi_v1_msgs::msg::VelocityFactorArray; | ||
using SteeringFactorArray = autoware_adapi_v1_msgs::msg::SteeringFactorArray; | ||
Pub<autoware_ad_api::planning::VelocityFactors> pub_velocity_factors_; | ||
Pub<autoware_ad_api::planning::SteeringFactors> pub_steering_factors_; | ||
Sub<planning_interface::Trajectory> sub_trajectory_; | ||
Sub<localization_interface::KinematicState> sub_kinematic_state_; | ||
std::vector<rclcpp::Subscription<VelocityFactorArray>::SharedPtr> sub_velocity_factors_; | ||
std::vector<rclcpp::Subscription<SteeringFactorArray>::SharedPtr> sub_steering_factors_; | ||
std::vector<VelocityFactorArray::ConstSharedPtr> velocity_factors_; | ||
std::vector<SteeringFactorArray::ConstSharedPtr> steering_factors_; | ||
rclcpp::TimerBase::SharedPtr timer_; | ||
|
||
using VehicleStopChecker = motion_utils::VehicleStopCheckerBase; | ||
using Trajectory = planning_interface::Trajectory::Message; | ||
using KinematicState = localization_interface::KinematicState::Message; | ||
void on_trajectory(const Trajectory::ConstSharedPtr msg); | ||
void on_kinematic_state(const KinematicState::ConstSharedPtr msg); | ||
void on_timer(); | ||
|
||
double stop_distance_; | ||
double stop_duration_; | ||
std::unique_ptr<VehicleStopChecker> stop_checker_; | ||
Trajectory::ConstSharedPtr trajectory_; | ||
KinematicState::ConstSharedPtr kinematic_state_; | ||
}; | ||
|
||
} // namespace default_ad_api | ||
|
||
#endif // PLANNING_HPP_ |
Empty file.
Oops, something went wrong.