Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

feat(behavior_velocity_planner): output missing inputs #1477

Merged
merged 2 commits into from
Aug 2, 2022
Merged

feat(behavior_velocity_planner): output missing inputs #1477

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
cb020a5
feat(behavior_velocity_planner): output missing inputs
takayuki5168 Aug 1, 2022
af6157f
fix
takayuki5168 Aug 2, 2022
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion planning/behavior_velocity_planner/include/behavior_velocity_planner/node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -113,7 +113,7 @@ class BehaviorVelocityPlannerNode : public rclcpp::Node

// function
geometry_msgs::msg::PoseStamped getCurrentPose();
bool isDataReady(const PlannerData planner_data) const;
bool isDataReady(const PlannerData planner_data, rclcpp::Clock clock) const;
autoware_auto_planning_msgs::msg::Path generatePath(
const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg,
const PlannerData & planner_data);
Expand Down
16 changes: 14 additions & 2 deletions planning/behavior_velocity_planner/src/node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -203,35 +203,46 @@ BehaviorVelocityPlannerNode::BehaviorVelocityPlannerNode(const rclcpp::NodeOptio
}

// NOTE: argument planner_data must not be referenced for multithreading
bool BehaviorVelocityPlannerNode::isDataReady(const PlannerData planner_data) const
bool BehaviorVelocityPlannerNode::isDataReady(
const PlannerData planner_data, rclcpp::Clock clock) const
{
const auto & d = planner_data;

// from tf
if (d.current_pose.header.frame_id == "") {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Frame id of current pose is missing");
return false;
}

// from callbacks
if (!d.current_velocity) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current velocity");
return false;
}
if (!d.current_accel) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for current acceleration");
return false;
}
if (!d.predicted_objects) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for predicted_objects");
return false;
}
if (!d.no_ground_pointcloud) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for pointcloud");
return false;
}
if (!d.route_handler_) {
RCLCPP_INFO_THROTTLE(
get_logger(), clock, 3000, "Waiting for the initialization of route_handler");
return false;
}
if (!d.route_handler_->isMapMsgReady()) {
RCLCPP_INFO_THROTTLE(get_logger(), clock, 3000, "Waiting for the initialization of map");
return false;
}
if (!d.velocity_smoother_) {
RCLCPP_INFO_THROTTLE(
get_logger(), clock, 3000, "Waiting for the initialization of velocity smoother");
return false;
}
return true;
Expand Down Expand Up @@ -381,6 +392,7 @@ void BehaviorVelocityPlannerNode::onTrigger(
const autoware_auto_planning_msgs::msg::PathWithLaneId::ConstSharedPtr input_path_msg)
{
mutex_.lock(); // for planner_data_

// Check ready
try {
planner_data_.current_pose =
Expand All @@ -391,7 +403,7 @@ void BehaviorVelocityPlannerNode::onTrigger(
return;
}

if (!isDataReady(planner_data_)) {
if (!isDataReady(planner_data_, *get_clock())) {
mutex_.unlock();
return;
}
Expand Down