diff --git a/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt new file mode 100644 index 0000000000000..c6d4e30061626 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/CMakeLists.txt @@ -0,0 +1,35 @@ +cmake_minimum_required(VERSION 3.14) +project(tier4_automatic_goal_rviz_plugin) + +find_package(autoware_cmake REQUIRED) +autoware_package() + +find_package(Qt5 REQUIRED Core Widgets) +set(QT_LIBRARIES Qt5::Widgets) +set(CMAKE_AUTOMOC ON) +set(CMAKE_INCLUDE_CURRENT_DIR ON) +add_definitions(-DQT_NO_KEYWORDS) + +ament_auto_add_library(${PROJECT_NAME} SHARED + src/automatic_goal_sender.cpp + src/automatic_goal_append_tool.cpp + src/automatic_goal_panel.cpp +) + +ament_auto_add_executable(automatic_goal_sender + src/automatic_goal_sender.cpp +) + +target_link_libraries(${PROJECT_NAME} + ${QT_LIBRARIES} +) + +# Export the plugin to be imported by rviz2 +pluginlib_export_plugin_description_file(rviz_common plugins/plugin_description.xml) + +ament_auto_package( + INSTALL_TO_SHARE + launch + icons + plugins +) diff --git a/common/tier4_automatic_goal_rviz_plugin/README.md b/common/tier4_automatic_goal_rviz_plugin/README.md new file mode 100644 index 0000000000000..48fb577375714 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/README.md @@ -0,0 +1,80 @@ +# tier4_automatic_goal_rviz_plugin + +## Purpose + +1. Defining a `GoalsList` by adding goals using `RvizTool` (Pose on the map). + +2. Automatic execution of the created `GoalsList` from the selected goal - it can be stopped and restarted. + +3. Looping the current `GoalsList`. + +4. Saving achieved goals to a file. + +5. Plan the route to one (single) selected goal and starting that route - it can be stopped and restarted. + +6. Remove any goal from the list or clear the current route. + +7. Save the current `GoalsList` to a file and load the list from the file. + +8. The application enables/disables access to options depending on the current state. + +9. The saved `GoalsList` can be executed without using a plugin - using a node `automatic_goal_sender`. + +## Inputs / Outputs + +### Input + +| Name | Type | Description | +| ---------------------------- | ------------------------------------------------- | ------------------------------------------------ | +| `/api/operation_mode/state` | `autoware_adapi_v1_msgs::msg::OperationModeState` | The topic represents the state of operation mode | +| `/api/routing/state` | `autoware_adapi_v1_msgs::msg::RouteState` | The topic represents the state of route | +| `/rviz2/automatic_goal/goal` | `geometry_msgs::msgs::PoseStamped` | The topic for adding goals to GoalsList | + +### Output + +| Name | Type | Description | +| ------------------------------------------ | -------------------------------------------------- | -------------------------------------------------- | +| `/api/operation_mode/change_to_autonomous` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to autonomous | +| `/api/operation_mode/change_to_stop` | `autoware_adapi_v1_msgs::srv::ChangeOperationMode` | The service to change operation mode to stop | +| `/api/routing/set_route_points` | `autoware_adapi_v1_msgs::srv::SetRoutePoints` | The service to set route | +| `/api/routing/clear_route` | `autoware_adapi_v1_msgs::srv::ClearRoute` | The service to clear route state | +| `/rviz2/automatic_goal/markers` | `visualization_msgs::msg::MarkerArray` | The topic to visualize goals as rviz markers | + +## HowToUse + +1. Start rviz and select panels/Add new panel. + + ![select_panel](./images/select_panels.png) + +2. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalPanel` and press OK. + +3. Select Add a new tool. + + ![select_tool](./images/select_tool.png) + +4. Select `tier4_automatic_goal_rviz_plugin/AutowareAutomaticGoalTool` and press OK. + +5. Add goals visualization as markers to `Displays`. + + ![markers](./images/markers.png) + +6. Append goals to the `GoalsList` to be achieved using `2D Append Goal` - in such a way that routes can be planned. + +7. Start sequential planning and goal achievement by clicking `Send goals automatically` + + ![panel](./images/panel.png) + +8. You can save `GoalsList` by clicking `Save to file`. + +9. After saving, you can run the `GoalsList` without using a plugin also: + - example: `ros2 launch tier4_automatic_goal_rviz_plugin automatic_goal_sender.launch.xml goals_list_file_path:="/tmp/goals_list.yaml" goals_achieved_dir_path:="/tmp/"` + - `goals_list_file_path` - is the path to the saved `GoalsList` file to be loaded + - `goals_achieved_dir_path` - is the path to the directory where the file `goals_achieved.log` will be created and the achieved goals will be written to it + +### Hints + +If the application (Engagement) goes into `ERROR` mode (usually returns to `EDITING` later), it means that one of the services returned a calling error (`code!=0`). +In this situation, check the terminal output for more information. + +- Often it is enough to try again. +- Sometimes a clearing of the current route is required before retrying. diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalPanel.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png new file mode 100644 index 0000000000000..6a67573717ae1 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/icons/classes/AutowareAutomaticGoalTool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/markers.png b/common/tier4_automatic_goal_rviz_plugin/images/markers.png new file mode 100644 index 0000000000000..8dd4d9d02bef4 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/markers.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/panel.png b/common/tier4_automatic_goal_rviz_plugin/images/panel.png new file mode 100644 index 0000000000000..1800202ea9f57 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/panel.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png b/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png new file mode 100644 index 0000000000000..61dd9e1d7a1b3 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/select_panels.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png b/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png new file mode 100644 index 0000000000000..a6ddc6d24c575 Binary files /dev/null and b/common/tier4_automatic_goal_rviz_plugin/images/select_tool.png differ diff --git a/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml b/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml new file mode 100644 index 0000000000000..a9af89c078348 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/launch/automatic_goal_sender.launch.xml @@ -0,0 +1,10 @@ + + + + + + + + + + diff --git a/common/tier4_automatic_goal_rviz_plugin/package.xml b/common/tier4_automatic_goal_rviz_plugin/package.xml new file mode 100644 index 0000000000000..11d263e09c789 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/package.xml @@ -0,0 +1,35 @@ + + + + tier4_automatic_goal_rviz_plugin + 0.0.0 + The autoware automatic goal rviz plugin package + Shumpei Wakabayashi + Dawid Moszyński + Apache License 2.0 + Dawid Moszyński + + ament_cmake_auto + autoware_cmake + autoware_adapi_v1_msgs + geometry_msgs + libqt5-core + libqt5-gui + libqt5-widgets + qtbase5-dev + rclcpp + rviz_common + rviz_default_plugins + tf2 + tf2_geometry_msgs + visualization_msgs + yaml-cpp + + ament_lint_auto + autoware_lint_common + + + ament_cmake + + + diff --git a/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml new file mode 100644 index 0000000000000..e7d5224550868 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/plugins/plugin_description.xml @@ -0,0 +1,12 @@ + + + AutowareAutomaticGoalPanel + + + AutowareAutomaticGoalTool + + diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp new file mode 100644 index 0000000000000..43fc0edcccf84 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.cpp @@ -0,0 +1,121 @@ +// Copyright 2023 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include "automatic_goal_append_tool.hpp" + +#ifdef ROS_DISTRO_GALACTIC +#include +#else +#include +#endif +#include + +#include + +namespace rviz_plugins +{ +AutowareAutomaticGoalTool::AutowareAutomaticGoalTool() +{ + shortcut_key_ = 'c'; + + pose_topic_property_ = new rviz_common::properties::StringProperty( + "Pose Topic", "~/automatic_goal/goal", "The topic on which to publish automatic_goal.", + getPropertyContainer(), SLOT(updateTopic()), this); + std_dev_x_ = new rviz_common::properties::FloatProperty( + "X std deviation", 0.5, "X standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_y_ = new rviz_common::properties::FloatProperty( + "Y std deviation", 0.5, "Y standard deviation for checkpoint pose [m]", getPropertyContainer()); + std_dev_theta_ = new rviz_common::properties::FloatProperty( + "Theta std deviation", M_PI / 12.0, "Theta standard deviation for checkpoint pose [rad]", + getPropertyContainer()); + position_z_ = new rviz_common::properties::FloatProperty( + "Z position", 0.0, "Z position for checkpoint pose [m]", getPropertyContainer()); + std_dev_x_->setMin(0); + std_dev_y_->setMin(0); + std_dev_theta_->setMin(0); + position_z_->setMin(0); +} + +void AutowareAutomaticGoalTool::onInitialize() +{ + PoseTool::onInitialize(); + setName("2D AppendGoal"); + updateTopic(); +} + +void AutowareAutomaticGoalTool::updateTopic() +{ + rclcpp::Node::SharedPtr raw_node = context_->getRosNodeAbstraction().lock()->get_raw_node(); + pose_pub_ = raw_node->create_publisher( + pose_topic_property_->getStdString(), 1); + clock_ = raw_node->get_clock(); +} + +void AutowareAutomaticGoalTool::onPoseSet(double x, double y, double theta) +{ + // pose + std::string fixed_frame = context_->getFixedFrame().toStdString(); + geometry_msgs::msg::PoseStamped pose; + pose.header.frame_id = fixed_frame; + pose.header.stamp = clock_->now(); + pose.pose.position.x = x; + pose.pose.position.y = y; + pose.pose.position.z = position_z_->getFloat(); + + tf2::Quaternion quat; + quat.setRPY(0.0, 0.0, theta); + pose.pose.orientation = tf2::toMsg(quat); + RCLCPP_INFO( + rclcpp::get_logger("AutowareAutomaticGoalTool"), "Setting pose: %.3f %.3f %.3f %.3f [frame=%s]", + x, y, position_z_->getFloat(), theta, fixed_frame.c_str()); + pose_pub_->publish(pose); +} + +} // end namespace rviz_plugins + +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalTool, rviz_common::Tool) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp new file mode 100644 index 0000000000000..6fc98cee9afa1 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_append_tool.hpp @@ -0,0 +1,91 @@ +// Copyright 2023 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. +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2012, Willow Garage, Inc. + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * * Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * * Redistributions in binary form must reproduce the above + * copyright notice, this list of conditions and the following + * disclaimer in the documentation and/or other materials provided + * with the distribution. + * * Neither the name of Willow Garage, Inc. nor the names of its + * contributors may be used to endorse or promote products derived + * from this software without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; + * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef AUTOMATIC_GOAL_APPEND_TOOL_HPP_ +#define AUTOMATIC_GOAL_APPEND_TOOL_HPP_ + +#ifndef Q_MOC_RUN // See: https://bugreports.qt-project.org/browse/QTBUG-22829 +#include +#include +#include +#include +#include +#include +#endif + +#include + +namespace rviz_plugins +{ +class AutowareAutomaticGoalTool : public rviz_default_plugins::tools::PoseTool +{ + Q_OBJECT + +public: + AutowareAutomaticGoalTool(); + void onInitialize() override; + +protected: + void onPoseSet(double x, double y, double theta) override; + +private Q_SLOTS: + void updateTopic(); + +private: // NOLINT for Qt + rclcpp::Clock::SharedPtr clock_{nullptr}; + rclcpp::Publisher::SharedPtr pose_pub_{nullptr}; + + rviz_common::properties::StringProperty * pose_topic_property_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_x_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_y_{nullptr}; + rviz_common::properties::FloatProperty * std_dev_theta_{nullptr}; + rviz_common::properties::FloatProperty * position_z_{nullptr}; +}; + +} // namespace rviz_plugins + +#endif // AUTOMATIC_GOAL_APPEND_TOOL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp new file mode 100644 index 0000000000000..e04228a8f7b88 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.cpp @@ -0,0 +1,484 @@ +// +// Copyright 2023 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 "automatic_goal_panel.hpp" + +namespace rviz_plugins +{ +AutowareAutomaticGoalPanel::AutowareAutomaticGoalPanel(QWidget * parent) +: rviz_common::Panel(parent) +{ + qt_timer_ = new QTimer(this); + connect( + qt_timer_, &QTimer::timeout, this, &AutowareAutomaticGoalPanel::updateAutoExecutionTimerTick); + + auto * h_layout = new QHBoxLayout(this); + auto * v_layout = new QVBoxLayout(this); + h_layout->addWidget(makeGoalsListGroup()); + v_layout->addWidget(makeEngagementGroup()); + v_layout->addWidget(makeRoutingGroup()); + h_layout->addLayout(v_layout); + setLayout(h_layout); +} + +// Layouts +QGroupBox * AutowareAutomaticGoalPanel::makeGoalsListGroup() +{ + auto * group = new QGroupBox("GoalsList", this); + auto * grid = new QGridLayout(group); + + load_file_btn_ptr_ = new QPushButton("Load from file", group); + connect(load_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickLoadListFromFile())); + grid->addWidget(load_file_btn_ptr_, 0, 0); + + save_file_btn_ptr_ = new QPushButton("Save to file", group); + connect(save_file_btn_ptr_, SIGNAL(clicked()), SLOT(onClickSaveListToFile())); + grid->addWidget(save_file_btn_ptr_, 1, 0); + + goals_list_widget_ptr_ = new QListWidget(group); + goals_list_widget_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(goals_list_widget_ptr_, 2, 0); + + remove_selected_goal_btn_ptr_ = new QPushButton("Remove selected", group); + connect(remove_selected_goal_btn_ptr_, SIGNAL(clicked()), SLOT(onClickRemove())); + grid->addWidget(remove_selected_goal_btn_ptr_, 3, 0); + + loop_list_btn_ptr_ = new QPushButton("Loop list", group); + loop_list_btn_ptr_->setCheckable(true); + connect(loop_list_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleLoopList(bool))); + grid->addWidget(loop_list_btn_ptr_, 4, 0); + + goals_achieved_btn_ptr_ = new QPushButton("Saving achieved goals to file", group); + goals_achieved_btn_ptr_->setCheckable(true); + connect(goals_achieved_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleSaveGoalsAchievement(bool))); + grid->addWidget(goals_achieved_btn_ptr_, 5, 0); + + group->setLayout(grid); + return group; +} + +QGroupBox * AutowareAutomaticGoalPanel::makeRoutingGroup() +{ + auto * group = new QGroupBox("Routing", this); + auto * grid = new QGridLayout(group); + + routing_label_ptr_ = new QLabel("INIT", group); + routing_label_ptr_->setMinimumSize(100, 25); + routing_label_ptr_->setAlignment(Qt::AlignCenter); + routing_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(routing_label_ptr_, 0, 0); + + clear_route_btn_ptr_ = new QPushButton("Clear planned route", group); + connect(clear_route_btn_ptr_, &QPushButton::clicked, [this]() { onClickClearRoute(); }); + grid->addWidget(clear_route_btn_ptr_, 1, 0); + group->setLayout(grid); + + group->setLayout(grid); + return group; +} + +QGroupBox * AutowareAutomaticGoalPanel::makeEngagementGroup() +{ + auto * group = new QGroupBox("Engagement", this); + auto * grid = new QGridLayout(group); + + engagement_label_ptr_ = new QLabel("INITIALIZING", group); + engagement_label_ptr_->setMinimumSize(100, 25); + engagement_label_ptr_->setAlignment(Qt::AlignCenter); + engagement_label_ptr_->setStyleSheet("border:1px solid black;"); + grid->addWidget(engagement_label_ptr_, 0, 0); + + automatic_mode_btn_ptr_ = new QPushButton("Send goals automatically", group); + automatic_mode_btn_ptr_->setCheckable(true); + + connect(automatic_mode_btn_ptr_, SIGNAL(toggled(bool)), SLOT(onToggleAutoMode(bool))); + grid->addWidget(automatic_mode_btn_ptr_, 1, 0); + + plan_btn_ptr_ = new QPushButton("Plan to selected goal", group); + connect(plan_btn_ptr_, &QPushButton::clicked, [this] { onClickPlan(); }); + grid->addWidget(plan_btn_ptr_, 2, 0); + + start_btn_ptr_ = new QPushButton("Start current plan", group); + connect(start_btn_ptr_, &QPushButton::clicked, [this] { onClickStart(); }); + grid->addWidget(start_btn_ptr_, 3, 0); + + stop_btn_ptr_ = new QPushButton("Stop current plan", group); + connect(stop_btn_ptr_, SIGNAL(clicked()), SLOT(onClickStop())); + grid->addWidget(stop_btn_ptr_, 4, 0); + group->setLayout(grid); + + group->setLayout(grid); + return group; +} + +void AutowareAutomaticGoalPanel::showMessageBox(const QString & string) +{ + QMessageBox msg_box(this); + msg_box.setText(string); + msg_box.exec(); +} + +// Slots +void AutowareAutomaticGoalPanel::onInitialize() +{ + raw_node_ = this->getDisplayContext()->getRosNodeAbstraction().lock()->get_raw_node(); + pub_marker_ = raw_node_->create_publisher("~/automatic_goal/markers", 0); + sub_append_goal_ = raw_node_->create_subscription( + "~/automatic_goal/goal", 5, + std::bind(&AutowareAutomaticGoalPanel::onAppendGoal, this, std::placeholders::_1)); + initCommunication(raw_node_.get()); +} + +void AutowareAutomaticGoalPanel::onToggleLoopList(bool checked) +{ + is_loop_list_on_ = checked; + updateGUI(); +} + +void AutowareAutomaticGoalPanel::onToggleSaveGoalsAchievement(bool checked) +{ + if (checked) { + QString file_name = QFileDialog::getSaveFileName( + this, tr("Save File with GoalsList"), "/tmp/goals_achieved.log", + tr("Achieved goals (*.log)")); + goals_achieved_file_path_ = file_name.toStdString(); + } else { + goals_achieved_file_path_ = ""; + } + updateGUI(); +} + +void AutowareAutomaticGoalPanel::onToggleAutoMode(bool checked) +{ + if (checked && goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select the first goal in GoalsList"); + automatic_mode_btn_ptr_->setChecked(false); + } else { + if (checked) current_goal_ = goals_list_widget_ptr_->currentRow(); + is_automatic_mode_on_ = checked; + is_automatic_mode_on_ ? qt_timer_->start(1000) : qt_timer_->stop(); + onClickClearRoute(); // here will be set State::AUTO_NEXT or State::EDITING; + } +} + +void AutowareAutomaticGoalPanel::onClickPlan() +{ + if (goals_list_widget_ptr_->selectedItems().count() != 1) { + showMessageBox("Select a goal in GoalsList"); + return; + } + + if (callPlanToGoalIndex(cli_set_route_, goals_list_widget_ptr_->currentRow())) { + state_ = State::PLANNING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickStart() +{ + if (callService(cli_change_to_autonomous_)) { + state_ = State::STARTING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickStop() +{ + // if ERROR is set then the ego is already stopped + if (state_ == State::ERROR) { + state_ = State::STOPPED; + updateGUI(); + } else if (callService(cli_change_to_stop_)) { + state_ = State::STOPPING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickClearRoute() +{ + if (callService(cli_clear_route_)) { + state_ = State::CLEARING; + updateGUI(); + } +} + +void AutowareAutomaticGoalPanel::onClickRemove() +{ + if (static_cast(goals_list_widget_ptr_->currentRow()) < goals_list_.size()) + goals_list_.erase(goals_list_.begin() + goals_list_widget_ptr_->currentRow()); + resetAchievedGoals(); + updateGUI(); + updateGoalsList(); +} + +void AutowareAutomaticGoalPanel::onClickLoadListFromFile() +{ + QString file_name = QFileDialog::getOpenFileName( + this, tr("Open File with GoalsList"), "/tmp", tr("Goal lists (*.yaml)")); + if (file_name.count() > 0) loadGoalsList(file_name.toStdString()); +} + +void AutowareAutomaticGoalPanel::onClickSaveListToFile() +{ + if (!goals_list_.empty()) { + QString file_name = QFileDialog::getSaveFileName( + this, tr("Save File with GoalsList"), "/tmp/goals_list.yaml", tr("Goal lists (*.yaml)")); + if (file_name.count() > 0) saveGoalsList(file_name.toStdString()); + } +} + +// Inputs +void AutowareAutomaticGoalPanel::onAppendGoal(const PoseStamped::ConstSharedPtr pose) +{ + if (state_ == State::EDITING) { + goals_list_.push_back(pose); + updateGoalsList(); + updateGUI(); + } +} + +// Override +void AutowareAutomaticGoalPanel::onCallResult() { updateGUI(); } +void AutowareAutomaticGoalPanel::onGoalListUpdated() +{ + goals_list_widget_ptr_->clear(); + for (auto const & goal : goals_achieved_) { + auto * item = + new QListWidgetItem(QString::fromStdString(goal.second.first), goals_list_widget_ptr_); + goals_list_widget_ptr_->addItem(item); + updateGoalIcon(goals_list_widget_ptr_->count() - 1, QColor("lightGray")); + } + publishMarkers(); +} +void AutowareAutomaticGoalPanel::onOperationModeUpdated(const OperationModeState::ConstSharedPtr) +{ + updateGUI(); +} +void AutowareAutomaticGoalPanel::onRouteUpdated(const RouteState::ConstSharedPtr msg) +{ + std::pair style; + if (msg->state == RouteState::UNSET) + style = std::make_pair("UNSET", "background-color: #FFFF00;"); // yellow + else if (msg->state == RouteState::SET) + style = std::make_pair("SET", "background-color: #00FF00;"); // green + else if (msg->state == RouteState::ARRIVED) + style = std::make_pair("ARRIVED", "background-color: #FFA500;"); // orange + else if (msg->state == RouteState::CHANGING) + style = std::make_pair("CHANGING", "background-color: #FFFF00;"); // yellow + else + style = std::make_pair("UNKNOWN", "background-color: #FF0000;"); // red + + updateLabel(routing_label_ptr_, style.first, style.second); + updateGUI(); +} + +void AutowareAutomaticGoalPanel::updateAutoExecutionTimerTick() +{ + if (is_automatic_mode_on_) { + if (state_ == State::AUTO_NEXT) { + // end if loop is off + if (current_goal_ >= goals_list_.size() && !is_loop_list_on_) { + disableAutomaticMode(); + return; + } + // plan to next goal + current_goal_ = current_goal_ % goals_list_.size(); + if (callPlanToGoalIndex(cli_set_route_, current_goal_)) { + state_ = State::PLANNING; + updateGUI(); + } + } else if (state_ == State::PLANNED) { + updateGoalIcon(current_goal_, QColor("yellow")); + onClickStart(); + } else if (state_ == State::ARRIVED) { + goals_achieved_[current_goal_].second++; + updateAchievedGoalsFile(current_goal_); + updateGoalIcon(current_goal_++, QColor("green")); + onClickClearRoute(); // will be set AUTO_NEXT as next state_ + } else if (state_ == State::STOPPED || state_ == State::ERROR) { + disableAutomaticMode(); + } + } +} + +// Visual updates +void AutowareAutomaticGoalPanel::updateGUI() +{ + deactivateButton(automatic_mode_btn_ptr_); + deactivateButton(remove_selected_goal_btn_ptr_); + deactivateButton(clear_route_btn_ptr_); + deactivateButton(plan_btn_ptr_); + deactivateButton(start_btn_ptr_); + deactivateButton(stop_btn_ptr_); + deactivateButton(load_file_btn_ptr_); + deactivateButton(save_file_btn_ptr_); + deactivateButton(loop_list_btn_ptr_); + deactivateButton(goals_achieved_btn_ptr_); + + std::pair style; + switch (state_) { + case State::EDITING: + activateButton(load_file_btn_ptr_); + if (!goals_list_.empty()) { + activateButton(goals_achieved_btn_ptr_); + activateButton(plan_btn_ptr_); + activateButton(remove_selected_goal_btn_ptr_); + activateButton(automatic_mode_btn_ptr_); + activateButton(save_file_btn_ptr_); + activateButton(loop_list_btn_ptr_); + } + style = std::make_pair("EDITING", "background-color: #FFFF00;"); + break; + case State::PLANNED: + activateButton(start_btn_ptr_); + activateButton(clear_route_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("PLANNED", "background-color: #00FF00;"); + break; + case State::STARTED: + activateButton(stop_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("STARTED", "background-color: #00FF00;"); + break; + case State::STOPPED: + activateButton(start_btn_ptr_); + activateButton(automatic_mode_btn_ptr_); + activateButton(clear_route_btn_ptr_); + activateButton(save_file_btn_ptr_); + style = std::make_pair("STOPPED", "background-color: #00FF00;"); + break; + case State::ARRIVED: + if (!is_automatic_mode_on_) onClickClearRoute(); // will be set EDITING as next state_ + break; + case State::CLEARED: + is_automatic_mode_on_ ? state_ = State::AUTO_NEXT : state_ = State::EDITING; + updateGUI(); + break; + case State::ERROR: + activateButton(stop_btn_ptr_); + if (!goals_list_.empty()) activateButton(save_file_btn_ptr_); + style = std::make_pair("ERROR", "background-color: #FF0000;"); + break; + case State::PLANNING: + activateButton(clear_route_btn_ptr_); + style = std::make_pair("PLANNING", "background-color: #FFA500;"); + break; + case State::STARTING: + style = std::make_pair("STARTING", "background-color: #FFA500;"); + break; + case State::STOPPING: + style = std::make_pair("STOPPING", "background-color: #FFA500;"); + break; + case State::CLEARING: + style = std::make_pair("CLEARING", "background-color: #FFA500;"); + break; + default: + break; + } + + automatic_mode_btn_ptr_->setStyleSheet(""); + loop_list_btn_ptr_->setStyleSheet(""); + goals_achieved_btn_ptr_->setStyleSheet(""); + if (is_automatic_mode_on_) automatic_mode_btn_ptr_->setStyleSheet("background-color: green"); + if (is_loop_list_on_) loop_list_btn_ptr_->setStyleSheet("background-color: green"); + if (!goals_achieved_file_path_.empty()) + goals_achieved_btn_ptr_->setStyleSheet("background-color: green"); + + updateLabel(engagement_label_ptr_, style.first, style.second); +} + +void AutowareAutomaticGoalPanel::updateGoalIcon(const unsigned goal_index, const QColor & color) +{ + QPixmap pixmap(24, 24); + pixmap.fill(color); + QPainter painter(&pixmap); + painter.setPen(QColor("black")); + painter.setFont(QFont("fixed-width", 10)); + QString text = QString::number(goals_achieved_[goal_index].second); + painter.drawText(QRect(0, 0, 24, 24), Qt::AlignCenter, text); + QIcon icon(pixmap); + goals_list_widget_ptr_->item(static_cast(goal_index))->setIcon(icon); +} + +void AutowareAutomaticGoalPanel::publishMarkers() +{ + MarkerArray text_array; + MarkerArray arrow_array; + // Clear existing + visualization_msgs::msg::Marker marker; + marker.header.stamp = rclcpp::Time(); + marker.ns = "names"; + marker.id = 0; + marker.action = visualization_msgs::msg::Marker::DELETEALL; + text_array.markers.push_back(marker); + marker.ns = "poses"; + arrow_array.markers.push_back(marker); + pub_marker_->publish(text_array); + pub_marker_->publish(arrow_array); + text_array.markers.clear(); + arrow_array.markers.clear(); + // Publish current + for (unsigned i = 0; i < goals_list_.size(); i++) { + visualization_msgs::msg::Marker marker; + marker.header.frame_id = "map"; + marker.header.stamp = rclcpp::Time(); + marker.ns = "names"; + marker.id = static_cast(i); + marker.type = visualization_msgs::msg::Marker::TEXT_VIEW_FACING; + marker.action = visualization_msgs::msg::Marker::ADD; + marker.pose = goals_list_[i]->pose; + marker.lifetime = rclcpp::Duration(0, 0); + marker.scale.x = 1.6; + marker.scale.y = 1.6; + marker.scale.z = 1.6; + marker.color.r = 1.0; + marker.color.g = 1.0; + marker.color.b = 1.0; + marker.color.a = 1.0; + marker.frame_locked = false; + marker.text = "G" + std::to_string(i); + text_array.markers.push_back(marker); + marker.ns = "poses"; + marker.scale.y = 0.2; + marker.scale.z = 0.2; + marker.type = visualization_msgs::msg::Marker::ARROW; + arrow_array.markers.push_back(marker); + } + pub_marker_->publish(text_array); + pub_marker_->publish(arrow_array); +} + +// File +void AutowareAutomaticGoalPanel::saveGoalsList(const std::string & file_path) +{ + YAML::Node node; + for (unsigned i = 0; i < goals_list_.size(); ++i) { + node[i]["position_x"] = goals_list_[i]->pose.position.x; + node[i]["position_y"] = goals_list_[i]->pose.position.y; + node[i]["position_z"] = goals_list_[i]->pose.position.z; + node[i]["orientation_x"] = goals_list_[i]->pose.orientation.x; + node[i]["orientation_y"] = goals_list_[i]->pose.orientation.y; + node[i]["orientation_z"] = goals_list_[i]->pose.orientation.z; + node[i]["orientation_w"] = goals_list_[i]->pose.orientation.w; + } + std::ofstream file_out(file_path); + file_out << node; + file_out.close(); +} + +} // namespace rviz_plugins +#include +PLUGINLIB_EXPORT_CLASS(rviz_plugins::AutowareAutomaticGoalPanel, rviz_common::Panel) diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp new file mode 100644 index 0000000000000..0ec9ca530f074 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_panel.hpp @@ -0,0 +1,147 @@ +// +// Copyright 2023 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. +// + +#ifndef AUTOMATIC_GOAL_PANEL_HPP_ +#define AUTOMATIC_GOAL_PANEL_HPP_ + +#include "automatic_goal_sender.hpp" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include + +namespace rviz_plugins +{ +class AutowareAutomaticGoalPanel : public rviz_common::Panel, + public automatic_goal::AutowareAutomaticGoalSender +{ + using State = automatic_goal::AutomaticGoalState; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using MarkerArray = visualization_msgs::msg::MarkerArray; + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + Q_OBJECT + +public: + explicit AutowareAutomaticGoalPanel(QWidget * parent = nullptr); + void onInitialize() override; + +public Q_SLOTS: // NOLINT for Qt + void onToggleLoopList(bool checked); + void onToggleAutoMode(bool checked); + void onToggleSaveGoalsAchievement(bool checked); + void onClickPlan(); + void onClickStart(); + void onClickStop(); + void onClickClearRoute(); + void onClickRemove(); + void onClickLoadListFromFile(); + void onClickSaveListToFile(); + +private: + // Override + void updateAutoExecutionTimerTick() override; + void onRouteUpdated(const RouteState::ConstSharedPtr msg) override; + void onOperationModeUpdated(const OperationModeState::ConstSharedPtr msg) override; + void onCallResult() override; + void onGoalListUpdated() override; + + // Inputs + void onAppendGoal(const PoseStamped::ConstSharedPtr pose); + + // Visual updates + void updateGUI(); + void updateGoalIcon(const unsigned goal_index, const QColor & color); + void publishMarkers(); + void showMessageBox(const QString & string); + void disableAutomaticMode() { automatic_mode_btn_ptr_->setChecked(false); } + static void activateButton(QAbstractButton * btn) { btn->setEnabled(true); } + static void deactivateButton(QAbstractButton * btn) { btn->setEnabled(false); } + static void updateLabel(QLabel * label, const QString & text, const QString & style_sheet) + { + label->setText(text); + label->setStyleSheet(style_sheet); + } + // File + void saveGoalsList(const std::string & file); + + // Pub/Sub + rclcpp::Publisher::SharedPtr pub_marker_{nullptr}; + rclcpp::Subscription::SharedPtr sub_append_goal_{nullptr}; + + // Containers + rclcpp::Node::SharedPtr raw_node_{nullptr}; + bool is_automatic_mode_on_{false}; + bool is_loop_list_on_{false}; + + // QT Containers + QGroupBox * makeGoalsListGroup(); + QGroupBox * makeRoutingGroup(); + QGroupBox * makeEngagementGroup(); + QTimer * qt_timer_{nullptr}; + QListWidget * goals_list_widget_ptr_{nullptr}; + QLabel * routing_label_ptr_{nullptr}; + QLabel * operation_mode_label_ptr_{nullptr}; + QLabel * engagement_label_ptr_{nullptr}; + QPushButton * loop_list_btn_ptr_{nullptr}; + QPushButton * goals_achieved_btn_ptr_{nullptr}; + QPushButton * load_file_btn_ptr_{nullptr}; + QPushButton * save_file_btn_ptr_{nullptr}; + QPushButton * automatic_mode_btn_ptr_{nullptr}; + QPushButton * remove_selected_goal_btn_ptr_{nullptr}; + QPushButton * clear_route_btn_ptr_{nullptr}; + QPushButton * plan_btn_ptr_{nullptr}; + QPushButton * start_btn_ptr_{nullptr}; + QPushButton * stop_btn_ptr_{nullptr}; +}; +} // namespace rviz_plugins + +#endif // AUTOMATIC_GOAL_PANEL_HPP_ diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp new file mode 100644 index 0000000000000..51f7716863465 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.cpp @@ -0,0 +1,227 @@ +// Copyright 2016 Open Source Robotics Foundation, 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 "automatic_goal_sender.hpp" + +namespace automatic_goal +{ +AutowareAutomaticGoalSender::AutowareAutomaticGoalSender() : Node("automatic_goal_sender") {} + +void AutowareAutomaticGoalSender::init() +{ + loadParams(this); + initCommunication(this); + loadGoalsList(goals_list_file_path_); + timer_ = this->create_wall_timer( + std::chrono::milliseconds(500), + std::bind(&AutowareAutomaticGoalSender::updateAutoExecutionTimerTick, this)); + + // Print info + RCLCPP_INFO_STREAM(get_logger(), "GoalsList has been loaded from: " << goals_list_file_path_); + for (auto const & goal : goals_achieved_) + RCLCPP_INFO_STREAM(get_logger(), "Loaded: " << goal.second.first); + RCLCPP_INFO_STREAM( + get_logger(), "Achieved goals will be saved in: " << goals_achieved_file_path_); +} + +void AutowareAutomaticGoalSender::loadParams(rclcpp::Node * node) +{ + namespace fs = std::filesystem; + node->declare_parameter("goals_list_file_path", ""); + node->declare_parameter("goals_achieved_dir_path", ""); + // goals_list + goals_list_file_path_ = node->get_parameter("goals_list_file_path").as_string(); + if (!fs::exists(goals_list_file_path_) || !fs::is_regular_file(goals_list_file_path_)) + throw std::invalid_argument( + "goals_list_file_path parameter - file path is invalid: " + goals_list_file_path_); + // goals_achieved + goals_achieved_file_path_ = node->get_parameter("goals_achieved_dir_path").as_string(); + if (!fs::exists(goals_achieved_file_path_) || fs::is_regular_file(goals_achieved_file_path_)) + throw std::invalid_argument( + "goals_achieved_dir_path - directory path is invalid: " + goals_achieved_file_path_); + goals_achieved_file_path_ += "goals_achieved.log"; +} + +void AutowareAutomaticGoalSender::initCommunication(rclcpp::Node * node) +{ + // Executing + sub_operation_mode_ = node->create_subscription( + "/api/operation_mode/state", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareAutomaticGoalSender::onOperationMode, this, std::placeholders::_1)); + + cli_change_to_autonomous_ = node->create_client( + "/api/operation_mode/change_to_autonomous", rmw_qos_profile_services_default); + + cli_change_to_stop_ = node->create_client( + "/api/operation_mode/change_to_stop", rmw_qos_profile_services_default); + + // Planning + sub_route_ = node->create_subscription( + "/api/routing/state", rclcpp::QoS{1}.transient_local(), + std::bind(&AutowareAutomaticGoalSender::onRoute, this, std::placeholders::_1)); + + cli_clear_route_ = + node->create_client("/api/routing/clear_route", rmw_qos_profile_services_default); + + cli_set_route_ = node->create_client( + "/api/routing/set_route_points", rmw_qos_profile_services_default); +} + +// Sub +void AutowareAutomaticGoalSender::onRoute(const RouteState::ConstSharedPtr msg) +{ + if (msg->state == RouteState::UNSET && state_ == State::CLEARING) + state_ = State::CLEARED; + else if (msg->state == RouteState::SET && state_ == State::PLANNING) + state_ = State::PLANNED; + else if (msg->state == RouteState::ARRIVED && state_ == State::STARTED) + state_ = State::ARRIVED; + onRouteUpdated(msg); +} + +void AutowareAutomaticGoalSender::onOperationMode(const OperationModeState::ConstSharedPtr msg) +{ + if (msg->mode == OperationModeState::STOP && state_ == State::INITIALIZING) + state_ = State::EDITING; + else if (msg->mode == OperationModeState::STOP && state_ == State::STOPPING) + state_ = State::STOPPED; + else if (msg->mode == OperationModeState::AUTONOMOUS && state_ == State::STARTING) + state_ = State::STARTED; + onOperationModeUpdated(msg); +} + +// Update +void AutowareAutomaticGoalSender::updateGoalsList() +{ + unsigned i = 0; + for (const auto & goal : goals_list_) { + std::stringstream ss; + ss << std::fixed << std::setprecision(2); + tf2::Quaternion tf2_quat; + tf2::convert(goal->pose.orientation, tf2_quat); + ss << "G" << i << " (" << goal->pose.position.x << ", "; + ss << goal->pose.position.y << ", " << tf2::getYaw(tf2_quat) << ")"; + goals_achieved_.insert({i++, std::make_pair(ss.str(), 0)}); + } + onGoalListUpdated(); +} + +void AutowareAutomaticGoalSender::updateAutoExecutionTimerTick() +{ + auto goal = goals_achieved_[current_goal_].first; + + if (state_ == State::INITIALIZING) { + RCLCPP_INFO_THROTTLE( + get_logger(), *get_clock(), 3000, "Initializing... waiting for OperationModeState::STOP"); + + } else if (state_ == State::EDITING) { // skip the editing step by default + state_ = State::AUTO_NEXT; + + } else if (state_ == State::AUTO_NEXT) { // plan to next goal + RCLCPP_INFO_STREAM(get_logger(), goal << ": Goal set as the next. Planning in progress..."); + if (callPlanToGoalIndex(cli_set_route_, current_goal_)) state_ = State::PLANNING; + + } else if (state_ == State::PLANNED) { // start plan to next goal + RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been planned. Route starting..."); + if (callService(cli_change_to_autonomous_)) state_ = State::STARTING; + + } else if (state_ == State::STARTED) { + RCLCPP_INFO_STREAM_THROTTLE(get_logger(), *get_clock(), 5000, goal << ": Driving to the goal."); + + } else if (state_ == State::ARRIVED) { // clear plan after achieving next goal, goal++ + RCLCPP_INFO_STREAM( + get_logger(), goal << ": Goal has been ACHIEVED " << ++goals_achieved_[current_goal_].second + << " times. Route clearing..."); + updateAchievedGoalsFile(current_goal_); + if (callService(cli_clear_route_)) state_ = State::CLEARING; + + } else if (state_ == State::CLEARED) { + RCLCPP_INFO_STREAM(get_logger(), goal << ": Route has been cleared."); + current_goal_++; + current_goal_ = current_goal_ % goals_list_.size(); + state_ = State::AUTO_NEXT; + + } else if (state_ == State::STOPPED) { + RCLCPP_WARN_STREAM( + get_logger(), goal << ": The execution has been stopped unexpectedly! Restarting..."); + if (callService(cli_change_to_autonomous_)) state_ = State::STARTING; + + } else if (state_ == State::ERROR) { + timer_->cancel(); + throw std::runtime_error(goal + ": Error encountered during execution!"); + } +} + +// File +void AutowareAutomaticGoalSender::loadGoalsList(const std::string & file_path) +{ + YAML::Node node = YAML::LoadFile(file_path); + goals_list_.clear(); + for (auto && goal : node) { + std::shared_ptr pose = std::make_shared(); + pose->header.frame_id = "map"; + pose->header.stamp = rclcpp::Time(); + pose->pose.position.x = goal["position_x"].as(); + pose->pose.position.y = goal["position_y"].as(); + pose->pose.position.z = goal["position_z"].as(); + pose->pose.orientation.x = goal["orientation_x"].as(); + pose->pose.orientation.y = goal["orientation_y"].as(); + pose->pose.orientation.z = goal["orientation_z"].as(); + pose->pose.orientation.w = goal["orientation_w"].as(); + goals_list_.push_back(pose); + } + resetAchievedGoals(); + updateGoalsList(); +} + +void AutowareAutomaticGoalSender::updateAchievedGoalsFile(const unsigned goal_index) +{ + if (!goals_achieved_file_path_.empty()) { + std::ofstream out(goals_achieved_file_path_, std::fstream::app); + std::stringstream ss; + ss << "[" << getTimestamp() << "] Achieved: " << goals_achieved_[goal_index].first; + ss << ", Current number of achievements: " << goals_achieved_[goal_index].second << "\n"; + out << ss.str(); + out.close(); + } +} + +void AutowareAutomaticGoalSender::resetAchievedGoals() +{ + goals_achieved_.clear(); + if (!goals_achieved_file_path_.empty()) { + std::ofstream out(goals_achieved_file_path_, std::fstream::app); + out << "[" << getTimestamp() + << "] GoalsList was loaded from a file or a goal was removed - counters have been reset\n"; + out.close(); + } +} +} // namespace automatic_goal + +int main(int argc, char * argv[]) +{ + rclcpp::init(argc, argv); + + std::shared_ptr node{nullptr}; + try { + node = std::make_shared(); + node->init(); + } catch (const std::exception & e) { + fprintf(stderr, "%s Exiting...\n", e.what()); + return 1; + } + + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp new file mode 100644 index 0000000000000..aacdada352811 --- /dev/null +++ b/common/tier4_automatic_goal_rviz_plugin/src/automatic_goal_sender.hpp @@ -0,0 +1,174 @@ +// Copyright 2016 Open Source Robotics Foundation, 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 AUTOMATIC_GOAL_SENDER_HPP_ +#define AUTOMATIC_GOAL_SENDER_HPP_ +#include + +#include +#include +#include +#include +#include +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +namespace automatic_goal +{ +enum class AutomaticGoalState { + INITIALIZING, + EDITING, + PLANNING, + PLANNED, + STARTING, + STARTED, + ARRIVED, + AUTO_NEXT, + STOPPING, + STOPPED, + CLEARING, + CLEARED, + ERROR, +}; + +class AutowareAutomaticGoalSender : public rclcpp::Node +{ + using State = AutomaticGoalState; + using PoseStamped = geometry_msgs::msg::PoseStamped; + using OperationModeState = autoware_adapi_v1_msgs::msg::OperationModeState; + using ChangeOperationMode = autoware_adapi_v1_msgs::srv::ChangeOperationMode; + using RouteState = autoware_adapi_v1_msgs::msg::RouteState; + using SetRoutePoints = autoware_adapi_v1_msgs::srv::SetRoutePoints; + using ClearRoute = autoware_adapi_v1_msgs::srv::ClearRoute; + +public: + AutowareAutomaticGoalSender(); + void init(); + +protected: + void initCommunication(rclcpp::Node * node); + // Calls + bool callPlanToGoalIndex( + const rclcpp::Client::SharedPtr client, const unsigned goal_index) + { + if (!client->service_is_ready()) { + RCLCPP_WARN(get_logger(), "SetRoutePoints client is unavailable"); + return false; + } + + auto req = std::make_shared(); + req->header = goals_list_.at(goal_index)->header; + req->goal = goals_list_.at(goal_index)->pose; + client->async_send_request( + req, [this](typename rclcpp::Client::SharedFuture result) { + if (result.get()->status.code != 0) state_ = State::ERROR; + printCallResult(result); + onCallResult(); + }); + return true; + } + template + bool callService(const typename rclcpp::Client::SharedPtr client) + { + if (!client->service_is_ready()) { + RCLCPP_WARN(get_logger(), "Client is unavailable"); + return false; + } + + auto req = std::make_shared(); + client->async_send_request(req, [this](typename rclcpp::Client::SharedFuture result) { + if (result.get()->status.code != 0) state_ = State::ERROR; + printCallResult(result); + onCallResult(); + }); + return true; + } + template + void printCallResult(typename rclcpp::Client::SharedFuture result) + { + if (result.get()->status.code != 0) { + RCLCPP_ERROR( + get_logger(), "Service type \"%s\" status: %d, %s", typeid(T).name(), + result.get()->status.code, result.get()->status.message.c_str()); + } else { + RCLCPP_DEBUG( + get_logger(), "Service type \"%s\" status: %d, %s", typeid(T).name(), + result.get()->status.code, result.get()->status.message.c_str()); + } + } + + // Update + void updateGoalsList(); + virtual void updateAutoExecutionTimerTick(); + + // File + void loadGoalsList(const std::string & file_path); + void updateAchievedGoalsFile(const unsigned goal_index); + void resetAchievedGoals(); + static std::string getTimestamp() + { + char buffer[128]; + std::time_t now = std::chrono::system_clock::to_time_t(std::chrono::system_clock::now()); + std::strftime(&buffer[0], sizeof(buffer), "%Y-%m-%d %H:%M:%S", std::localtime(&now)); + return std::string{buffer}; + } + + // Sub + void onRoute(const RouteState::ConstSharedPtr msg); + void onOperationMode(const OperationModeState::ConstSharedPtr msg); + + // Interface + virtual void onRouteUpdated(const RouteState::ConstSharedPtr) {} + virtual void onOperationModeUpdated(const OperationModeState::ConstSharedPtr) {} + virtual void onCallResult() {} + virtual void onGoalListUpdated() {} + + // Cli + rclcpp::Client::SharedPtr cli_change_to_autonomous_{nullptr}; + rclcpp::Client::SharedPtr cli_change_to_stop_{nullptr}; + rclcpp::Client::SharedPtr cli_clear_route_{nullptr}; + rclcpp::Client::SharedPtr cli_set_route_{nullptr}; + + // Containers + unsigned current_goal_{0}; + State state_{State::INITIALIZING}; + std::vector goals_list_{}; + std::map> goals_achieved_{}; + std::string goals_achieved_file_path_{}; + +private: + void loadParams(rclcpp::Node * node); + + // Sub + rclcpp::Subscription::SharedPtr sub_route_{nullptr}; + rclcpp::Subscription::SharedPtr sub_operation_mode_{nullptr}; + + // Containers + std::string goals_list_file_path_{}; + rclcpp::TimerBase::SharedPtr timer_{nullptr}; +}; +} // namespace automatic_goal +#endif // AUTOMATIC_GOAL_SENDER_HPP_ diff --git a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp index 14026d47abdbb..99e3018962eb2 100644 --- a/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp +++ b/common/tier4_autoware_utils/include/tier4_autoware_utils/geometry/boost_polygon_utils.hpp @@ -44,7 +44,7 @@ Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::DetectedObject & Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::TrackedObject & object); Polygon2d toPolygon2d(const autoware_auto_perception_msgs::msg::PredictedObject & object); double getArea(const autoware_auto_perception_msgs::msg::Shape & shape); - +Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset); } // namespace tier4_autoware_utils #endif // TIER4_AUTOWARE_UTILS__GEOMETRY__BOOST_POLYGON_UTILS_HPP_ diff --git a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp index c08fd6c22c357..2cdb385d19ad6 100644 --- a/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/src/geometry/boost_polygon_utils.cpp @@ -210,4 +210,37 @@ double getArea(const autoware_auto_perception_msgs::msg::Shape & shape) throw std::logic_error("The shape type is not supported in tier4_autoware_utils."); } + +// NOTE: The number of vertices on the expanded polygon by boost::geometry::buffer +// is larger than the original one. +// This function fixes the issue. +Polygon2d expandPolygon(const Polygon2d & input_polygon, const double offset) +{ + // NOTE: input_polygon is supposed to have a duplicated point. + const size_t num_points = input_polygon.outer().size() - 1; + + Polygon2d expanded_polygon; + for (size_t i = 0; i < num_points; ++i) { + const auto & curr_p = input_polygon.outer().at(i); + const auto & next_p = input_polygon.outer().at(i + 1); + const auto & prev_p = + i == 0 ? input_polygon.outer().at(num_points - 1) : input_polygon.outer().at(i - 1); + + Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); + Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); + current_to_next.normalize(); + current_to_prev.normalize(); + + const Eigen::Vector2d offset_vector = (-current_to_next - current_to_prev).normalized(); + const double theta = std::acos(offset_vector.dot(current_to_next)); + const double scaled_offset = offset / std::sin(theta); + const Eigen::Vector2d offset_point = + Eigen::Vector2d(curr_p.x(), curr_p.y()) + offset_vector * scaled_offset; + + expanded_polygon.outer().push_back(Point2d(offset_point.x(), offset_point.y())); + } + + boost::geometry::correct(expanded_polygon); + return expanded_polygon; +} } // namespace tier4_autoware_utils diff --git a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp index 421e434c60e1e..3e028a481e380 100644 --- a/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp +++ b/common/tier4_autoware_utils/test/src/geometry/test_boost_polygon_utils.cpp @@ -259,3 +259,45 @@ TEST(boost_geometry, boost_getArea) EXPECT_DOUBLE_EQ(anti_clock_wise_area, x * y); } } + +TEST(boost_geometry, boost_expandPolygon) +{ + using tier4_autoware_utils::expandPolygon; + + { // box with a certain offset + Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}}; + const auto expanded_poly = expandPolygon(box_poly, 1.0); + + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).x(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).y(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).x(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).y(), 2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).x(), 2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).y(), 2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).x(), 2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).y(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).x(), -2.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).y(), -2.0); + } + + { // box with no offset + Polygon2d box_poly{{{-1.0, -1.0}, {-1.0, 1.0}, {1.0, 1.0}, {1.0, -1.0}, {-1.0, -1.0}}}; + const auto expanded_poly = expandPolygon(box_poly, 0.0); + + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).x(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(0).y(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).x(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(1).y(), 1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).x(), 1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(2).y(), 1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).x(), 1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(3).y(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).x(), -1.0); + EXPECT_DOUBLE_EQ(expanded_poly.outer().at(4).y(), -1.0); + } + + { // empty polygon + Polygon2d empty_poly; + EXPECT_THROW(expandPolygon(empty_poly, 1.0), std::out_of_range); + } +} diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp index 9605f53cde1d3..244e75210cec3 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker.hpp @@ -66,7 +66,6 @@ struct Param struct Input { - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose{}; nav_msgs::msg::Odometry::ConstSharedPtr current_odom{}; lanelet::LaneletMapPtr lanelet_map{}; LaneletRoute::ConstSharedPtr route{}; diff --git a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp index 6d8f267384075..85fd73c63f544 100644 --- a/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp +++ b/control/lane_departure_checker/include/lane_departure_checker/lane_departure_checker_node.hpp @@ -23,7 +23,6 @@ #include #include #include -#include #include #include @@ -61,7 +60,6 @@ class LaneDepartureCheckerNode : public rclcpp::Node private: // Subscriber - tier4_autoware_utils::SelfPoseListener self_pose_listener_{this}; rclcpp::Subscription::SharedPtr sub_odom_; rclcpp::Subscription::SharedPtr sub_lanelet_map_bin_; rclcpp::Subscription::SharedPtr sub_route_; @@ -69,7 +67,6 @@ class LaneDepartureCheckerNode : public rclcpp::Node rclcpp::Subscription::SharedPtr sub_predicted_trajectory_; // Data Buffer - geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_; nav_msgs::msg::Odometry::ConstSharedPtr current_odom_; lanelet::LaneletMapPtr lanelet_map_; lanelet::ConstLanelets shoulder_lanelets_; diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp index 1d594180a638b..1a63fb3eb395b 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker.cpp @@ -100,7 +100,7 @@ Output LaneDepartureChecker::update(const Input & input) tier4_autoware_utils::StopWatch stop_watch; output.trajectory_deviation = calcTrajectoryDeviation( - *input.reference_trajectory, input.current_pose->pose, param_.ego_nearest_dist_threshold, + *input.reference_trajectory, input.current_odom->pose.pose, param_.ego_nearest_dist_threshold, param_.ego_nearest_yaw_threshold); output.processing_time_map["calcTrajectoryDeviation"] = stop_watch.toc(true); diff --git a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp index c60cf59de2407..a3508018e0869 100644 --- a/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp +++ b/control/lane_departure_checker/src/lane_departure_checker_node/lane_departure_checker_node.cpp @@ -183,9 +183,6 @@ LaneDepartureCheckerNode::LaneDepartureCheckerNode(const rclcpp::NodeOptions & o updater_.add("trajectory_deviation", this, &LaneDepartureCheckerNode::checkTrajectoryDeviation); - // Wait for first self pose - self_pose_listener_.waitForFirstPose(); - // Timer const auto period_ns = rclcpp::Rate(node_param_.update_rate).period(); timer_ = rclcpp::create_timer( @@ -221,11 +218,6 @@ void LaneDepartureCheckerNode::onPredictedTrajectory(const Trajectory::ConstShar bool LaneDepartureCheckerNode::isDataReady() { - if (!current_pose_) { - RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_pose..."); - return false; - } - if (!current_odom_) { RCLCPP_INFO_THROTTLE(get_logger(), *get_clock(), 5000, "waiting for current_twist msg..."); return false; @@ -261,7 +253,7 @@ bool LaneDepartureCheckerNode::isDataTimeout() const auto now = this->now(); constexpr double th_pose_timeout = 1.0; - const auto pose_time_diff = rclcpp::Time(current_pose_->header.stamp) - now; + const auto pose_time_diff = rclcpp::Time(current_odom_->header.stamp) - now; if (pose_time_diff.seconds() > th_pose_timeout) { RCLCPP_WARN_THROTTLE(get_logger(), *get_clock(), 5000, "pose is timeout..."); return true; @@ -293,8 +285,6 @@ void LaneDepartureCheckerNode::onTimer() tier4_autoware_utils::StopWatch stop_watch; stop_watch.tic("Total"); - current_pose_ = self_pose_listener_.getCurrentPose(); - if (!isDataReady()) { return; } @@ -337,7 +327,6 @@ void LaneDepartureCheckerNode::onTimer() processing_time_map["Node: getRouteLanelets"] = stop_watch.toc(true); input_.current_odom = current_odom_; - input_.current_pose = current_pose_; input_.lanelet_map = lanelet_map_; input_.route = route_; input_.route_lanelets = route_lanelets_; @@ -471,7 +460,7 @@ visualization_msgs::msg::MarkerArray LaneDepartureCheckerNode::createMarkerArray visualization_msgs::msg::MarkerArray marker_array; - const auto base_link_z = current_pose_->pose.position.z; + const auto base_link_z = current_odom_->pose.pose.position.z; if (node_param_.visualize_lanelet) { // Route Lanelets diff --git a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp index 226dd803e3945..c6a920e347cab 100644 --- a/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp +++ b/control/mpc_lateral_controller/include/mpc_lateral_controller/mpc.hpp @@ -199,10 +199,7 @@ class MPC * @brief set the reference trajectory to follow */ void storeSteerCmd(const double steer); - /** - * @brief reset previous result of MPC - */ - void resetPrevResult(const SteeringReport & current_steer); + /** * @brief set initial condition for mpc * @param [in] data mpc data @@ -393,6 +390,12 @@ class MPC const bool enable_path_smoothing, const int path_filter_moving_ave_num, const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer, const bool extend_trajectory_for_end_yaw_control); + + /** + * @brief reset previous result of MPC + */ + void resetPrevResult(const SteeringReport & current_steer); + /** * @brief set the vehicle model of this MPC */ diff --git a/control/mpc_lateral_controller/src/mpc.cpp b/control/mpc_lateral_controller/src/mpc.cpp index ceb5ac5b9c6bc..fb4353d142ee0 100644 --- a/control/mpc_lateral_controller/src/mpc.cpp +++ b/control/mpc_lateral_controller/src/mpc.cpp @@ -248,8 +248,8 @@ void MPC::setReferenceTrajectory( void MPC::resetPrevResult(const SteeringReport & current_steer) { - // Consider limit. The prev value larger than limiation brakes the optimization constraint and - // resluts in optimization failure. + // Consider limit. The prev value larger than limitaion brakes the optimization constraint and + // results in optimization failure. const float steer_lim_f = static_cast(m_steer_lim); m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f); m_raw_steer_cmd_pprev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f); @@ -264,12 +264,6 @@ bool MPC::getData( if (!MPCUtils::calcNearestPoseInterp( traj, current_pose, &(data->nearest_pose), &(nearest_idx), &(data->nearest_time), ego_nearest_dist_threshold, ego_nearest_yaw_threshold, m_logger, *m_clock)) { - // reset previous MPC result - // Note: When a large deviation from the trajectory occurs, the optimization stops and - // the vehicle will return to the path by re-planning the trajectory or external operation. - // After the recovery, the previous value of the optimization may deviate greatly from - // the actual steer angle, and it may make the optimization result unstable. - resetPrevResult(current_steer); RCLCPP_WARN_SKIPFIRST_THROTTLE( m_logger, *m_clock, duration, "calculateMPC: error in calculating nearest pose. stop mpc."); return false; diff --git a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp index 192694d48af72..2a129d544587b 100644 --- a/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp +++ b/control/mpc_lateral_controller/src/mpc_lateral_controller.cpp @@ -204,6 +204,15 @@ trajectory_follower::LateralOutput MpcLateralController::run( m_current_steering, m_current_kinematic_state.twist.twist.linear.x, m_current_kinematic_state.pose.pose, ctrl_cmd, predicted_traj, debug_values); + // reset previous MPC result + // Note: When a large deviation from the trajectory occurs, the optimization stops and + // the vehicle will return to the path by re-planning the trajectory or external operation. + // After the recovery, the previous value of the optimization may deviate greatly from + // the actual steer angle, and it may make the optimization result unstable. + if (!is_mpc_solved) { + m_mpc.resetPrevResult(m_current_steering); + } + if (enable_auto_steering_offset_removal_) { steering_offset_->updateOffset( m_current_kinematic_state.twist.twist, diff --git a/control/trajectory_follower_node/CMakeLists.txt b/control/trajectory_follower_node/CMakeLists.txt index a2316a6db30d2..4ba5bee16eb2d 100644 --- a/control/trajectory_follower_node/CMakeLists.txt +++ b/control/trajectory_follower_node/CMakeLists.txt @@ -10,7 +10,6 @@ ament_auto_add_library(${CONTROLLER_NODE} SHARED src/controller_node.cpp ) -target_compile_options(${CONTROLLER_NODE} PRIVATE -Wno-error=old-style-cast) rclcpp_components_register_node(${CONTROLLER_NODE} PLUGIN "autoware::motion::control::trajectory_follower_node::Controller" EXECUTABLE ${CONTROLLER_NODE}_exe @@ -23,9 +22,6 @@ ament_auto_add_library(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} SHARED src/simple_trajectory_follower.cpp ) -# TODO(simple_trajectory_follower) : RCLCPP_ERROR_THROTTLE() has built-in old-style casts. -# TODO(simple_trajectory_follower) : Temporary workaround until this is fixed. -target_compile_options(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} PRIVATE -Wno-error=old-style-cast) rclcpp_components_register_node(${SIMPLE_TRAJECTORY_FOLLOWER_NODE} PLUGIN "simple_trajectory_follower::SimpleTrajectoryFollower" EXECUTABLE ${SIMPLE_TRAJECTORY_FOLLOWER_NODE}_exe diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp index e71b887ff3a10..04a5b758d62e1 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/deviation_metrics.hpp @@ -26,6 +26,8 @@ namespace metrics { using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; /** * @brief calculate lateral deviation of the given trajectory from the reference trajectory @@ -51,6 +53,30 @@ Stat calcYawDeviation(const Trajectory & ref, const Trajectory & traj); */ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & traj); +/** + * @brief calculate longitudinal deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_point target point + * @return calculated statistics + */ +Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point); + +/** + * @brief calculate lateral deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_point target point + * @return calculated statistics + */ +Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point); + +/** + * @brief calculate yaw deviation of the given ego pose from the modified goal pose + * @param [in] base_pose base pose + * @param [in] target_pose target pose + * @return calculated statistics + */ +Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose); + } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp index 16811f91b32b1..e1ec69dbaef5c 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics/metric.hpp @@ -41,6 +41,9 @@ enum class Metric { stability_frechet, obstacle_distance, obstacle_ttc, + modified_goal_longitudinal_deviation, + modified_goal_lateral_deviation, + modified_goal_yaw_deviation, SIZE, }; @@ -63,7 +66,10 @@ static const std::unordered_map str_to_metric = { {"stability_frechet", Metric::stability_frechet}, {"obstacle_distance", Metric::obstacle_distance}, {"obstacle_ttc", Metric::obstacle_ttc}, -}; + {"modified_goal_longitudinal_deviation", Metric::modified_goal_longitudinal_deviation}, + {"modified_goal_lateral_deviation", Metric::modified_goal_lateral_deviation}, + {"modified_goal_yaw_deviation", Metric::modified_goal_yaw_deviation}}; + static const std::unordered_map metric_to_str = { {Metric::curvature, "curvature"}, {Metric::point_interval, "point_interval"}, @@ -79,7 +85,10 @@ static const std::unordered_map metric_to_str = { {Metric::stability, "stability"}, {Metric::stability_frechet, "stability_frechet"}, {Metric::obstacle_distance, "obstacle_distance"}, - {Metric::obstacle_ttc, "obstacle_ttc"}}; + {Metric::obstacle_ttc, "obstacle_ttc"}, + {Metric::modified_goal_longitudinal_deviation, "modified_goal_longitudinal_deviation"}, + {Metric::modified_goal_lateral_deviation, "modified_goal_lateral_deviation"}, + {Metric::modified_goal_yaw_deviation, "modified_goal_yaw_deviation"}}; // Metrics descriptions static const std::unordered_map metric_descriptions = { @@ -97,7 +106,10 @@ static const std::unordered_map metric_descriptions = { {Metric::stability, "Stability[m]"}, {Metric::stability_frechet, "StabilityFrechet[m]"}, {Metric::obstacle_distance, "Obstacle_distance[m]"}, - {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}}; + {Metric::obstacle_ttc, "Obstacle_time_to_collision[s]"}, + {Metric::modified_goal_longitudinal_deviation, "Modified_goal_longitudinal_deviation[m]"}, + {Metric::modified_goal_lateral_deviation, "Modified_goal_lateral_deviation[m]"}, + {Metric::modified_goal_yaw_deviation, "Modified_goal_yaw_deviation[rad]"}}; namespace details { diff --git a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp index 0ed2af7b3862e..851678e55d755 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/metrics_calculator.hpp @@ -14,7 +14,6 @@ #ifndef PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ #define PLANNING_EVALUATOR__METRICS_CALCULATOR_HPP_ - #include "planning_evaluator/metrics/metric.hpp" #include "planning_evaluator/parameters.hpp" #include "planning_evaluator/stat.hpp" @@ -22,13 +21,19 @@ #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "geometry_msgs/msg/pose.hpp" +#include + namespace planning_diagnostics { using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::PoseWithUuidStamped; +using geometry_msgs::msg::Point; +using geometry_msgs::msg::Pose; class MetricsCalculator { @@ -42,7 +47,9 @@ class MetricsCalculator * @param [in] metric Metric enum value * @return string describing the requested metric */ - Stat calculate(const Metric metric, const Trajectory & traj) const; + std::optional> calculate(const Metric metric, const Trajectory & traj) const; + std::optional> calculate( + const Metric metric, const Pose & base_pose, const Pose & target_pose) const; /** * @brief set the reference trajectory used to calculate the deviation metrics @@ -68,6 +75,12 @@ class MetricsCalculator */ void setEgoPose(const geometry_msgs::msg::Pose & pose); + /** + * @brief get the ego pose + * @return ego pose + */ + Pose getEgoPose(); + private: /** * @brief trim a trajectory from the current ego pose to some fixed time or distance @@ -86,6 +99,7 @@ class MetricsCalculator Trajectory previous_trajectory_lookahead_; PredictedObjects dynamic_objects_; geometry_msgs::msg::Pose ego_pose_; + PoseWithUuidStamped modified_goal_; }; // class MetricsCalculator } // namespace planning_diagnostics diff --git a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp index e9fd82c85cf7b..888eea855295c 100644 --- a/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp +++ b/evaluator/planning_evaluator/include/planning_evaluator/planning_evaluator_node.hpp @@ -24,7 +24,9 @@ #include "autoware_auto_perception_msgs/msg/predicted_objects.hpp" #include "autoware_auto_planning_msgs/msg/trajectory.hpp" #include "autoware_auto_planning_msgs/msg/trajectory_point.hpp" +#include "autoware_planning_msgs/msg/pose_with_uuid_stamped.hpp" #include "diagnostic_msgs/msg/diagnostic_array.hpp" +#include "nav_msgs/msg/odometry.hpp" #include #include @@ -37,8 +39,10 @@ namespace planning_diagnostics using autoware_auto_perception_msgs::msg::PredictedObjects; using autoware_auto_planning_msgs::msg::Trajectory; using autoware_auto_planning_msgs::msg::TrajectoryPoint; +using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; using diagnostic_msgs::msg::DiagnosticStatus; +using nav_msgs::msg::Odometry; /** * @brief Node for planning evaluation @@ -49,6 +53,12 @@ class PlanningEvaluatorNode : public rclcpp::Node explicit PlanningEvaluatorNode(const rclcpp::NodeOptions & node_options); ~PlanningEvaluatorNode(); + /** + * @brief callback on receiving an odometry + * @param [in] odometry_msg received odometry message + */ + void onOdometry(const Odometry::ConstSharedPtr odometry_msg); + /** * @brief callback on receiving a trajectory * @param [in] traj_msg received trajectory message @@ -68,9 +78,10 @@ class PlanningEvaluatorNode : public rclcpp::Node void onObjects(const PredictedObjects::ConstSharedPtr objects_msg); /** - * @brief update the ego pose stored in the MetricsCalculator + * @brief callback on receiving a modified goal + * @param [in] modified_goal_msg received modified goal message */ - void updateCalculatorEgoPose(const std::string & target_frame); + void onModifiedGoal(const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg); /** * @brief publish the given metric statistic @@ -80,11 +91,15 @@ class PlanningEvaluatorNode : public rclcpp::Node private: static bool isFinite(const TrajectoryPoint & p); + void publishModifiedGoalDeviationMetrics(); // ROS rclcpp::Subscription::SharedPtr traj_sub_; rclcpp::Subscription::SharedPtr ref_sub_; rclcpp::Subscription::SharedPtr objects_sub_; + rclcpp::Subscription::SharedPtr modified_goal_sub_; + rclcpp::Subscription::SharedPtr odom_sub_; + rclcpp::Publisher::SharedPtr metrics_pub_; std::shared_ptr transform_listener_{nullptr}; std::unique_ptr tf_buffer_; @@ -99,6 +114,9 @@ class PlanningEvaluatorNode : public rclcpp::Node std::vector metrics_; std::deque stamps_; std::array>, static_cast(Metric::SIZE)> metric_stats_; + + Odometry::ConstSharedPtr ego_state_ptr_; + PoseWithUuidStamped::ConstSharedPtr modified_goal_ptr_; }; } // namespace planning_diagnostics diff --git a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml index af7c5369f7422..1b7510c2ced69 100644 --- a/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml +++ b/evaluator/planning_evaluator/launch/planning_evaluator.launch.xml @@ -1,15 +1,19 @@ + + + + diff --git a/evaluator/planning_evaluator/package.xml b/evaluator/planning_evaluator/package.xml index 73186f6b20e88..ef2bca288c8b0 100644 --- a/evaluator/planning_evaluator/package.xml +++ b/evaluator/planning_evaluator/package.xml @@ -16,6 +16,7 @@ autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_planning_msgs diagnostic_msgs eigen geometry_msgs diff --git a/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml b/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml index bd47deedb282f..2e92a174ca79f 100644 --- a/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml +++ b/evaluator/planning_evaluator/param/planning_evaluator.defaults.yaml @@ -19,6 +19,9 @@ - stability_frechet - obstacle_distance - obstacle_ttc + - modified_goal_longitudinal_deviation + - modified_goal_lateral_deviation + - modified_goal_yaw_deviation trajectory: min_point_dist_m: 0.1 # [m] minimum distance between two successive points to use for angle calculation diff --git a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp index d6d8aa661e5ea..5146fcd3ec0f2 100644 --- a/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp +++ b/evaluator/planning_evaluator/src/metrics/deviation_metrics.cpp @@ -77,5 +77,25 @@ Stat calcVelocityDeviation(const Trajectory & ref, const Trajectory & tr return stat; } +Stat calcLongitudinalDeviation(const Pose & base_pose, const Point & target_point) +{ + Stat stat; + stat.add(std::abs(tier4_autoware_utils::calcLongitudinalDeviation(base_pose, target_point))); + return stat; +} + +Stat calcLateralDeviation(const Pose & base_pose, const Point & target_point) +{ + Stat stat; + stat.add(std::abs(tier4_autoware_utils::calcLateralDeviation(base_pose, target_point))); + return stat; +} + +Stat calcYawDeviation(const Pose & base_pose, const Pose & target_pose) +{ + Stat stat; + stat.add(std::abs(tier4_autoware_utils::calcYawDeviation(base_pose, target_pose))); + return stat; +} } // namespace metrics } // namespace planning_diagnostics diff --git a/evaluator/planning_evaluator/src/metrics_calculator.cpp b/evaluator/planning_evaluator/src/metrics_calculator.cpp index 802e1dde94fd1..adfdddd7d2c5e 100644 --- a/evaluator/planning_evaluator/src/metrics_calculator.cpp +++ b/evaluator/planning_evaluator/src/metrics_calculator.cpp @@ -23,9 +23,10 @@ namespace planning_diagnostics { -Stat MetricsCalculator::calculate(const Metric metric, const Trajectory & traj) const +std::optional> MetricsCalculator::calculate( + const Metric metric, const Trajectory & traj) const { - // Functions to calculate metrics + // Functions to calculate trajectory metrics switch (metric) { case Metric::curvature: return metrics::calcTrajectoryCurvature(traj); @@ -70,9 +71,23 @@ Stat MetricsCalculator::calculate(const Metric metric, const Trajectory case Metric::obstacle_ttc: return metrics::calcTimeToCollision(dynamic_objects_, traj, parameters.obstacle.dist_thr_m); default: - throw std::runtime_error( - "[MetricsCalculator][calculate()] unknown Metric " + - std::to_string(static_cast(metric))); + return {}; + } +} + +std::optional> MetricsCalculator::calculate( + const Metric metric, const Pose & base_pose, const Pose & target_pose) const +{ + // Functions to calculate pose metrics + switch (metric) { + case Metric::modified_goal_longitudinal_deviation: + return metrics::calcLongitudinalDeviation(base_pose, target_pose.position); + case Metric::modified_goal_lateral_deviation: + return metrics::calcLateralDeviation(base_pose, target_pose.position); + case Metric::modified_goal_yaw_deviation: + return metrics::calcYawDeviation(base_pose, target_pose); + default: + return {}; } } @@ -93,6 +108,8 @@ void MetricsCalculator::setPredictedObjects(const PredictedObjects & objects) void MetricsCalculator::setEgoPose(const geometry_msgs::msg::Pose & pose) { ego_pose_ = pose; } +Pose MetricsCalculator::getEgoPose() { return ego_pose_; } + Trajectory MetricsCalculator::getLookaheadTrajectory( const Trajectory & traj, const double max_dist_m, const double max_time_s) const { diff --git a/evaluator/planning_evaluator/src/motion_evaluator_node.cpp b/evaluator/planning_evaluator/src/motion_evaluator_node.cpp index 56b08fca81f9f..9a103890d53ac 100644 --- a/evaluator/planning_evaluator/src/motion_evaluator_node.cpp +++ b/evaluator/planning_evaluator/src/motion_evaluator_node.cpp @@ -55,7 +55,9 @@ MotionEvaluatorNode::~MotionEvaluatorNode() f << std::endl; for (Metric metric : metrics_) { const auto & stat = metrics_calculator_.calculate(metric, accumulated_trajectory_); - f /* << std::setw(3 * column_width) */ << stat << " "; + if (stat) { + f /* << std::setw(3 * column_width) */ << *stat << " "; + } } f.close(); } diff --git a/evaluator/planning_evaluator/src/planning_evaluator_node.cpp b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp index 663f0482cf77b..346b029922e3e 100644 --- a/evaluator/planning_evaluator/src/planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/src/planning_evaluator_node.cpp @@ -40,6 +40,13 @@ PlanningEvaluatorNode::PlanningEvaluatorNode(const rclcpp::NodeOptions & node_op objects_sub_ = create_subscription( "~/input/objects", 1, std::bind(&PlanningEvaluatorNode::onObjects, this, _1)); + + modified_goal_sub_ = create_subscription( + "~/input/modified_goal", 1, std::bind(&PlanningEvaluatorNode::onModifiedGoal, this, _1)); + + odom_sub_ = create_subscription( + "~/input/odometry", 1, std::bind(&PlanningEvaluatorNode::onOdometry, this, _1)); + tf_buffer_ = std::make_unique(this->get_clock()); transform_listener_ = std::make_shared(*tf_buffer_); @@ -98,24 +105,6 @@ PlanningEvaluatorNode::~PlanningEvaluatorNode() } } -void PlanningEvaluatorNode::updateCalculatorEgoPose(const std::string & target_frame) -{ - try { - const geometry_msgs::msg::TransformStamped transform = - tf_buffer_->lookupTransform(target_frame, ego_frame_str_, tf2::TimePointZero); - geometry_msgs::msg::Pose ego_pose; - ego_pose.position.x = transform.transform.translation.x; - ego_pose.position.y = transform.transform.translation.y; - ego_pose.position.z = transform.transform.translation.z; - ego_pose.orientation = transform.transform.rotation; - metrics_calculator_.setEgoPose(ego_pose); - } catch (tf2::TransformException & ex) { - RCLCPP_INFO( - this->get_logger(), "Cannot set ego pose: could not transform %s to %s: %s", - target_frame.c_str(), ego_frame_str_.c_str(), ex.what()); - } -} - DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( const Metric & metric, const Stat & metric_stat) const { @@ -137,18 +126,24 @@ DiagnosticStatus PlanningEvaluatorNode::generateDiagnosticStatus( void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_msg) { + if (!ego_state_ptr_) { + return; + } + auto start = now(); stamps_.push_back(traj_msg->header.stamp); - updateCalculatorEgoPose(traj_msg->header.frame_id); - DiagnosticArray metrics_msg; metrics_msg.header.stamp = now(); for (Metric metric : metrics_) { - const Stat metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); - metric_stats_[static_cast(metric)].push_back(metric_stat); - if (metric_stat.count() > 0) { - metrics_msg.status.push_back(generateDiagnosticStatus(metric, metric_stat)); + const auto metric_stat = metrics_calculator_.calculate(Metric(metric), *traj_msg); + if (!metric_stat) { + continue; + } + + metric_stats_[static_cast(metric)].push_back(*metric_stat); + if (metric_stat->count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); } } if (!metrics_msg.status.empty()) { @@ -159,6 +154,51 @@ void PlanningEvaluatorNode::onTrajectory(const Trajectory::ConstSharedPtr traj_m RCLCPP_DEBUG(get_logger(), "Planning evaluation calculation time: %2.2f ms", runtime * 1e3); } +void PlanningEvaluatorNode::onModifiedGoal( + const PoseWithUuidStamped::ConstSharedPtr modified_goal_msg) +{ + modified_goal_ptr_ = modified_goal_msg; + if (ego_state_ptr_) { + publishModifiedGoalDeviationMetrics(); + } +} + +void PlanningEvaluatorNode::onOdometry(const Odometry::ConstSharedPtr odometry_msg) +{ + ego_state_ptr_ = odometry_msg; + metrics_calculator_.setEgoPose(odometry_msg->pose.pose); + + if (modified_goal_ptr_) { + publishModifiedGoalDeviationMetrics(); + } +} + +void PlanningEvaluatorNode::publishModifiedGoalDeviationMetrics() +{ + auto start = now(); + + DiagnosticArray metrics_msg; + metrics_msg.header.stamp = now(); + for (Metric metric : metrics_) { + const auto metric_stat = metrics_calculator_.calculate( + Metric(metric), modified_goal_ptr_->pose, ego_state_ptr_->pose.pose); + if (!metric_stat) { + continue; + } + metric_stats_[static_cast(metric)].push_back(*metric_stat); + if (metric_stat->count() > 0) { + metrics_msg.status.push_back(generateDiagnosticStatus(metric, *metric_stat)); + } + } + if (!metrics_msg.status.empty()) { + metrics_pub_->publish(metrics_msg); + } + auto runtime = (now() - start).seconds(); + RCLCPP_DEBUG( + get_logger(), "Planning evaluation modified goal deviation calculation time: %2.2f ms", + runtime * 1e3); +} + void PlanningEvaluatorNode::onReferenceTrajectory(const Trajectory::ConstSharedPtr traj_msg) { metrics_calculator_.setReferenceTrajectory(*traj_msg); diff --git a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp index 8e9bcdd734222..4f2ab014a79d6 100644 --- a/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp +++ b/evaluator/planning_evaluator/test/test_planning_evaluator_node.cpp @@ -36,7 +36,11 @@ using EvalNode = planning_diagnostics::PlanningEvaluatorNode; using Trajectory = autoware_auto_planning_msgs::msg::Trajectory; using TrajectoryPoint = autoware_auto_planning_msgs::msg::TrajectoryPoint; using Objects = autoware_auto_perception_msgs::msg::PredictedObjects; +using autoware_planning_msgs::msg::PoseWithUuidStamped; using diagnostic_msgs::msg::DiagnosticArray; +using nav_msgs::msg::Odometry; + +constexpr double epsilon = 1e-6; class EvalTest : public ::testing::Test { @@ -70,6 +74,10 @@ class EvalTest : public ::testing::Test dummy_node, "/planning_evaluator/input/reference_trajectory", 1); objects_pub_ = rclcpp::create_publisher(dummy_node, "/planning_evaluator/input/objects", 1); + odom_pub_ = + rclcpp::create_publisher(dummy_node, "/planning_evaluator/input/odometry", 1); + modified_goal_pub_ = rclcpp::create_publisher( + dummy_node, "/planning_evaluator/input/modified_goal", 1); tf_broadcaster_ = std::make_unique(dummy_node); publishEgoPose(0.0, 0.0, 0.0); @@ -139,28 +147,51 @@ class EvalTest : public ::testing::Test return metric_value_; } - void publishEgoPose(const double x, const double y, const double yaw) + double publishModifiedGoalAndGetMetric(const double x, const double y, const double yaw) { - geometry_msgs::msg::TransformStamped t; + metric_updated_ = false; - // Read message content and assign it to - // corresponding tf variables - t.header.stamp = dummy_node->now(); - t.header.frame_id = "map"; - t.child_frame_id = "base_link"; + PoseWithUuidStamped goal; + goal.header.frame_id = "map"; + goal.header.stamp = dummy_node->now(); + goal.pose.position.x = x; + goal.pose.position.y = y; + goal.pose.position.z = 0.0; + tf2::Quaternion q; + q.setRPY(0.0, 0.0, yaw); + goal.pose.orientation.x = q.x(); + goal.pose.orientation.y = q.y(); + goal.pose.orientation.z = q.z(); + goal.pose.orientation.w = q.w(); + modified_goal_pub_->publish(goal); - t.transform.translation.x = x; - t.transform.translation.y = y; - t.transform.translation.z = 0.0; + while (!metric_updated_) { + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); + } + return metric_value_; + } + void publishEgoPose(const double x, const double y, const double yaw) + { + Odometry odom; + odom.header.frame_id = "map"; + odom.header.stamp = dummy_node->now(); + odom.pose.pose.position.x = x; + odom.pose.pose.position.y = y; + odom.pose.pose.position.z = 0.0; tf2::Quaternion q; q.setRPY(0.0, 0.0, yaw); - t.transform.rotation.x = q.x(); - t.transform.rotation.y = q.y(); - t.transform.rotation.z = q.z(); - t.transform.rotation.w = q.w(); + odom.pose.pose.orientation.x = q.x(); + odom.pose.pose.orientation.y = q.y(); + odom.pose.pose.orientation.z = q.z(); + odom.pose.pose.orientation.w = q.w(); - tf_broadcaster_->sendTransform(t); + odom_pub_->publish(odom); + rclcpp::spin_some(eval_node); + rclcpp::spin_some(dummy_node); + rclcpp::sleep_for(std::chrono::milliseconds(100)); } // Latest metric value @@ -173,6 +204,8 @@ class EvalTest : public ::testing::Test rclcpp::Publisher::SharedPtr traj_pub_; rclcpp::Publisher::SharedPtr ref_traj_pub_; rclcpp::Publisher::SharedPtr objects_pub_; + rclcpp::Publisher::SharedPtr modified_goal_pub_; + rclcpp::Publisher::SharedPtr odom_pub_; rclcpp::Subscription::SharedPtr metric_sub_; // TF broadcaster std::unique_ptr tf_broadcaster_; @@ -407,3 +440,29 @@ TEST_F(EvalTest, TestObstacleTTC) t.points[1].pose.position.x = 1.0; EXPECT_DOUBLE_EQ(publishTrajectoryAndGetMetric(t), 2.0); } + +TEST_F(EvalTest, TestModifiedGoalLongitudinalDeviation) +{ + setTargetMetric(planning_diagnostics::Metric::modified_goal_longitudinal_deviation); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, 0.0), 1.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, M_PI_2), 0.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, 0.0), 0.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, M_PI_2), 1.0, epsilon); +} + +TEST_F(EvalTest, TestModifiedGoalLateralDeviation) +{ + setTargetMetric(planning_diagnostics::Metric::modified_goal_lateral_deviation); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, 0.0), 0.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 0.0, M_PI_2), 1.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, 0.0), 1.0, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 1.0, M_PI_2), 0.0, epsilon); +} + +TEST_F(EvalTest, TestModifiedGoalYawDeviation) +{ + setTargetMetric(planning_diagnostics::Metric::modified_goal_yaw_deviation); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(0.0, 0.0, M_PI_2), M_PI_2, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 1.0, -M_PI_2), M_PI_2, epsilon); + EXPECT_NEAR(publishModifiedGoalAndGetMetric(1.0, 1.0, -M_PI_4), M_PI_4, epsilon); +} diff --git a/launch/tier4_localization_launch/launch/localization.launch.xml b/launch/tier4_localization_launch/launch/localization.launch.xml index 14fe9c55f7e5d..12ad512557cb1 100644 --- a/launch/tier4_localization_launch/launch/localization.launch.xml +++ b/launch/tier4_localization_launch/launch/localization.launch.xml @@ -6,6 +6,8 @@ + + diff --git a/launch/tier4_localization_launch/launch/util/util.launch.xml b/launch/tier4_localization_launch/launch/util/util.launch.xml index d820e06deb614..31a453c082522 100644 --- a/launch/tier4_localization_launch/launch/util/util.launch.xml +++ b/launch/tier4_localization_launch/launch/util/util.launch.xml @@ -14,10 +14,8 @@ - - - - + + diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py index d371fa43986ab..0a6b459d9f5f3 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/laserscan_based_occupancy_grid_map.launch.py @@ -12,8 +12,10 @@ # See the License for the specific language governing permissions and # limitations under the License. +from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition @@ -21,23 +23,14 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) +def launch_setup(context, *args, **kwargs): + # load parameter files + param_file = LaunchConfiguration("param_file").perform(context) + with open(param_file, "r") as f: + laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"] composable_nodes = [ ComposableNode( @@ -53,7 +46,9 @@ def add_launch_arg(name: str, default_value=None): ], parameters=[ { - "target_frame": "base_link", # Leave disabled to output scan in pointcloud frame + "target_frame": laserscan_based_occupancy_grid_map_node_params[ + "scan_origin_frame" + ], # Leave disabled to output scan in pointcloud frame "transform_tolerance": 0.01, "min_height": 0.0, "max_height": 2.0, @@ -86,14 +81,13 @@ def add_launch_arg(name: str, default_value=None): ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], parameters=[ + laserscan_based_occupancy_grid_map_node_params, { - "map_resolution": 0.5, - "use_height_filter": True, "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), "input_obstacle_and_raw_pointcloud": LaunchConfiguration( "input_obstacle_and_raw_pointcloud" ), - } + }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), @@ -115,6 +109,25 @@ def add_launch_arg(name: str, default_value=None): condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return LaunchDescription( [ add_launch_arg("use_multithread", "false"), @@ -128,11 +141,15 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output/stixel", "virtual_scan/stixel"), add_launch_arg("input_obstacle_pointcloud", "false"), add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), - add_launch_arg("use_pointcloud_container", "False"), + add_launch_arg( + "param_file", + get_package_share_directory("probabilistic_occupancy_grid_map") + + "/config/laserscan_based_occupancy_grid_map.param.yaml", + ), + add_launch_arg("use_pointcloud_container", "false"), add_launch_arg("container_name", "occupancy_grid_map_container"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py b/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py index 2feefdfb1053d..e8a9e173cfe0a 100644 --- a/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py +++ b/launch/tier4_perception_launch/launch/occupancy_grid_map/pointcloud_based_occupancy_grid_map.launch.py @@ -14,6 +14,7 @@ from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition from launch.conditions import UnlessCondition @@ -21,23 +22,16 @@ from launch_ros.actions import ComposableNodeContainer from launch_ros.actions import LoadComposableNodes from launch_ros.descriptions import ComposableNode +import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) +def launch_setup(context, *args, **kwargs): + # load parameter files + param_file = LaunchConfiguration("param_file").perform(context) + with open(param_file, "r") as f: + pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][ + "ros__parameters" + ] composable_nodes = [ ComposableNode( @@ -49,12 +43,7 @@ def add_launch_arg(name: str, default_value=None): ("~/input/raw_pointcloud", LaunchConfiguration("input/raw_pointcloud")), ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], - parameters=[ - { - "map_resolution": 0.5, - "use_height_filter": True, - } - ], + parameters=[pointcloud_based_occupancy_grid_map_node_params], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ] @@ -75,18 +64,37 @@ def add_launch_arg(name: str, default_value=None): condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), ) + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), + ) + return LaunchDescription( [ - add_launch_arg("use_multithread", "False"), - add_launch_arg("use_intra_process", "True"), - add_launch_arg("use_pointcloud_container", "False"), + add_launch_arg("use_multithread", "false"), + add_launch_arg("use_intra_process", "true"), + add_launch_arg("use_pointcloud_container", "false"), add_launch_arg("container_name", "occupancy_grid_map_container"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), add_launch_arg("output", "occupancy_grid"), + add_launch_arg("param_file", "config/pointcloud_based_occupancy_grid_map.param.yaml"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/launch/tier4_perception_launch/launch/perception.launch.xml b/launch/tier4_perception_launch/launch/perception.launch.xml index ffc6f908983bb..b2a800c5b278d 100644 --- a/launch/tier4_perception_launch/launch/perception.launch.xml +++ b/launch/tier4_perception_launch/launch/perception.launch.xml @@ -76,6 +76,7 @@ + diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 9769a6a36332f..70b87921ecca5 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -2,6 +2,8 @@ + + @@ -42,6 +44,7 @@ + @@ -98,10 +101,8 @@ - - - - + + diff --git a/launch/tier4_simulator_launch/package.xml b/launch/tier4_simulator_launch/package.xml index 7964e7c499a11..8094f95d78b15 100644 --- a/launch/tier4_simulator_launch/package.xml +++ b/launch/tier4_simulator_launch/package.xml @@ -5,6 +5,10 @@ 0.1.0 The tier4_simulator_launch package Keisuke Shima + Takayuki Murooka + Takamasa Horibe + Tomoya Kimura + Taiki Tanaka Apache License 2.0 ament_cmake_auto diff --git a/launch/tier4_system_launch/launch/system.launch.xml b/launch/tier4_system_launch/launch/system.launch.xml index 8e674b0435528..016d613cfa72d 100644 --- a/launch/tier4_system_launch/launch/system.launch.xml +++ b/launch/tier4_system_launch/launch/system.launch.xml @@ -21,7 +21,7 @@ - + @@ -66,6 +66,7 @@ + diff --git a/localization/pose_initializer/config/pose_initializer.param.yaml b/localization/pose_initializer/config/pose_initializer.param.yaml index e94f86c8d6c7e..c1486b71aae9c 100644 --- a/localization/pose_initializer/config/pose_initializer.param.yaml +++ b/localization/pose_initializer/config/pose_initializer.param.yaml @@ -1,24 +1,6 @@ /**: ros__parameters: - - # from gnss - gnss_particle_covariance: - [ - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, - ] - - # output - output_pose_covariance: - [ - 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, - 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, - ] + gnss_enabled: true + ndt_enabled: true + ekf_enabled: true + stop_check_enabled: true diff --git a/localization/pose_initializer/config/pose_initializer_common.param.yaml b/localization/pose_initializer/config/pose_initializer_common.param.yaml new file mode 100644 index 0000000000000..a05cc7c35cb1a --- /dev/null +++ b/localization/pose_initializer/config/pose_initializer_common.param.yaml @@ -0,0 +1,26 @@ +/**: + ros__parameters: + gnss_pose_timeout: 3.0 # [sec] + stop_check_duration: 3.0 # [sec] + + # from gnss + gnss_particle_covariance: + [ + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 10.0, + ] + + # output + output_pose_covariance: + [ + 1.0, 0.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 1.0, 0.0, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.01, 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.01, 0.0, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.01, 0.0, + 0.0, 0.0, 0.0, 0.0, 0.0, 0.2, + ] diff --git a/localization/pose_initializer/launch/pose_initializer.launch.xml b/localization/pose_initializer/launch/pose_initializer.launch.xml index 9483ff47bc093..6558b578e6386 100644 --- a/localization/pose_initializer/launch/pose_initializer.launch.xml +++ b/localization/pose_initializer/launch/pose_initializer.launch.xml @@ -1,20 +1,12 @@ + - - - - + - - - - - - diff --git a/perception/crosswalk_traffic_light_estimator/package.xml b/perception/crosswalk_traffic_light_estimator/package.xml index 3f1bdb6880bd1..d4147643eb7bd 100644 --- a/perception/crosswalk_traffic_light_estimator/package.xml +++ b/perception/crosswalk_traffic_light_estimator/package.xml @@ -14,6 +14,7 @@ autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_planning_msgs lanelet2_extension rclcpp rclcpp_components diff --git a/perception/lidar_centerpoint/README.md b/perception/lidar_centerpoint/README.md index baaea06e59346..f03e5bb777299 100644 --- a/perception/lidar_centerpoint/README.md +++ b/perception/lidar_centerpoint/README.md @@ -43,6 +43,7 @@ We trained the models using . | `nms_iou_target_class_names` | list[string] | - | target classes for IoU-based Non Maximum Suppression | | `nms_iou_search_distance_2d` | double | - | If two objects are farther than the value, NMS isn't applied. | | `nms_iou_threshold` | double | - | IoU threshold for the IoU-based Non Maximum Suppression | +| `build_only` | bool | `false` | shutdown the node after TensorRT engine file is built | ## Assumptions / Known limits diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp index 25891958f9194..fee17f1c0156a 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/centerpoint_trt.hpp @@ -81,7 +81,6 @@ class CenterPointTRT std::unique_ptr post_proc_ptr_{nullptr}; cudaStream_t stream_{nullptr}; - bool verbose_{false}; std::size_t class_size_{0}; CenterPointConfig config_; std::size_t num_voxels_{0}; diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp index c2c2dbc179361..639aa0ba8bad3 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/network_trt.hpp @@ -38,9 +38,7 @@ class HeadTRT : public TensorRTWrapper public: using TensorRTWrapper::TensorRTWrapper; - HeadTRT( - const std::vector & out_channel_sizes, const CenterPointConfig & config, - const bool verbose); + HeadTRT(const std::vector & out_channel_sizes, const CenterPointConfig & config); protected: bool setProfile( diff --git a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp b/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp index 2cd05400bc9af..7f55ab6f5e414 100644 --- a/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp +++ b/perception/lidar_centerpoint/include/lidar_centerpoint/network/tensorrt_wrapper.hpp @@ -16,6 +16,7 @@ #define LIDAR_CENTERPOINT__NETWORK__TENSORRT_WRAPPER_HPP_ #include +#include #include @@ -25,45 +26,18 @@ namespace centerpoint { -struct Deleter -{ - template - void operator()(T * obj) const - { - if (obj) { - delete obj; - } - } -}; - -template -using unique_ptr = std::unique_ptr; - -class Logger : public nvinfer1::ILogger -{ -public: - explicit Logger(bool verbose) : verbose_(verbose) {} - - void log(Severity severity, const char * msg) noexcept override - { - if (verbose_ || ((severity != Severity::kINFO) && (severity != Severity::kVERBOSE))) { - std::cout << msg << std::endl; - } - } - -private: - bool verbose_{false}; -}; class TensorRTWrapper { public: - explicit TensorRTWrapper(const CenterPointConfig & config, const bool verbose); + explicit TensorRTWrapper(const CenterPointConfig & config); + + ~TensorRTWrapper(); bool init( const std::string & onnx_path, const std::string & engine_path, const std::string & precision); - unique_ptr context_ = nullptr; + tensorrt_common::TrtUniquePtr context_{nullptr}; protected: virtual bool setProfile( @@ -71,7 +45,7 @@ class TensorRTWrapper nvinfer1::IBuilderConfig & config) = 0; CenterPointConfig config_; - Logger logger_; + tensorrt_common::Logger logger_; private: bool parseONNX( @@ -84,9 +58,9 @@ class TensorRTWrapper bool createContext(); - unique_ptr runtime_ = nullptr; - unique_ptr plan_ = nullptr; - unique_ptr engine_ = nullptr; + tensorrt_common::TrtUniquePtr runtime_{nullptr}; + tensorrt_common::TrtUniquePtr plan_{nullptr}; + tensorrt_common::TrtUniquePtr engine_{nullptr}; }; } // namespace centerpoint diff --git a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml index 522b488429c62..d552cb702b980 100644 --- a/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml +++ b/perception/lidar_centerpoint/launch/lidar_centerpoint.launch.xml @@ -8,6 +8,7 @@ + @@ -21,6 +22,7 @@ + diff --git a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp index a489be9e86538..9b01ea8f53675 100644 --- a/perception/lidar_centerpoint/lib/centerpoint_trt.cpp +++ b/perception/lidar_centerpoint/lib/centerpoint_trt.cpp @@ -35,7 +35,7 @@ CenterPointTRT::CenterPointTRT( post_proc_ptr_ = std::make_unique(config_); // encoder - encoder_trt_ptr_ = std::make_unique(config_, verbose_); + encoder_trt_ptr_ = std::make_unique(config_); encoder_trt_ptr_->init( encoder_param.onnx_path(), encoder_param.engine_path(), encoder_param.trt_precision()); encoder_trt_ptr_->context_->setBindingDimensions( @@ -47,7 +47,7 @@ CenterPointTRT::CenterPointTRT( std::vector out_channel_sizes = { config_.class_size_, config_.head_out_offset_size_, config_.head_out_z_size_, config_.head_out_dim_size_, config_.head_out_rot_size_, config_.head_out_vel_size_}; - head_trt_ptr_ = std::make_unique(out_channel_sizes, config_, verbose_); + head_trt_ptr_ = std::make_unique(out_channel_sizes, config_); head_trt_ptr_->init(head_param.onnx_path(), head_param.engine_path(), head_param.trt_precision()); head_trt_ptr_->context_->setBindingDimensions( 0, nvinfer1::Dims4( diff --git a/perception/lidar_centerpoint/lib/network/network_trt.cpp b/perception/lidar_centerpoint/lib/network/network_trt.cpp index ffc4ac4a2eef0..88319ff51fe35 100644 --- a/perception/lidar_centerpoint/lib/network/network_trt.cpp +++ b/perception/lidar_centerpoint/lib/network/network_trt.cpp @@ -39,9 +39,8 @@ bool VoxelEncoderTRT::setProfile( } HeadTRT::HeadTRT( - const std::vector & out_channel_sizes, const CenterPointConfig & config, - const bool verbose) -: TensorRTWrapper(config, verbose), out_channel_sizes_(out_channel_sizes) + const std::vector & out_channel_sizes, const CenterPointConfig & config) +: TensorRTWrapper(config), out_channel_sizes_(out_channel_sizes) { } diff --git a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp index a175079ab0348..079c41d06c6e0 100644 --- a/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp +++ b/perception/lidar_centerpoint/lib/network/tensorrt_wrapper.cpp @@ -22,15 +22,21 @@ namespace centerpoint { -TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config, const bool verbose) -: config_(config), logger_(Logger(verbose)) +TensorRTWrapper::TensorRTWrapper(const CenterPointConfig & config) : config_(config) {} + +TensorRTWrapper::~TensorRTWrapper() { + context_.reset(); + runtime_.reset(); + plan_.reset(); + engine_.reset(); } bool TensorRTWrapper::init( const std::string & onnx_path, const std::string & engine_path, const std::string & precision) { - runtime_ = unique_ptr(nvinfer1::createInferRuntime(logger_)); + runtime_ = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferRuntime(logger_)); if (!runtime_) { std::cout << "Fail to create runtime" << std::endl; return false; @@ -55,7 +61,8 @@ bool TensorRTWrapper::createContext() return false; } - context_ = unique_ptr(engine_->createExecutionContext()); + context_ = + tensorrt_common::TrtUniquePtr(engine_->createExecutionContext()); if (!context_) { std::cout << "Fail to create context" << std::endl; return false; @@ -68,13 +75,15 @@ bool TensorRTWrapper::parseONNX( const std::string & onnx_path, const std::string & engine_path, const std::string & precision, const size_t workspace_size) { - auto builder = unique_ptr(nvinfer1::createInferBuilder(logger_)); + auto builder = + tensorrt_common::TrtUniquePtr(nvinfer1::createInferBuilder(logger_)); if (!builder) { std::cout << "Fail to create builder" << std::endl; return false; } - auto config = unique_ptr(builder->createBuilderConfig()); + auto config = + tensorrt_common::TrtUniquePtr(builder->createBuilderConfig()); if (!config) { std::cout << "Fail to create config" << std::endl; return false; @@ -95,13 +104,15 @@ bool TensorRTWrapper::parseONNX( const auto flag = 1U << static_cast(nvinfer1::NetworkDefinitionCreationFlag::kEXPLICIT_BATCH); - auto network = unique_ptr(builder->createNetworkV2(flag)); + auto network = + tensorrt_common::TrtUniquePtr(builder->createNetworkV2(flag)); if (!network) { std::cout << "Fail to create network" << std::endl; return false; } - auto parser = unique_ptr(nvonnxparser::createParser(*network, logger_)); + auto parser = tensorrt_common::TrtUniquePtr( + nvonnxparser::createParser(*network, logger_)); parser->parseFromFile(onnx_path.c_str(), static_cast(nvinfer1::ILogger::Severity::kERROR)); if (!setProfile(*builder, *network, *config)) { @@ -111,12 +122,13 @@ bool TensorRTWrapper::parseONNX( std::cout << "Applying optimizations and building TRT CUDA engine (" << onnx_path << ") ..." << std::endl; - plan_ = unique_ptr(builder->buildSerializedNetwork(*network, *config)); + plan_ = tensorrt_common::TrtUniquePtr( + builder->buildSerializedNetwork(*network, *config)); if (!plan_) { std::cout << "Fail to create serialized network" << std::endl; return false; } - engine_ = unique_ptr( + engine_ = tensorrt_common::TrtUniquePtr( runtime_->deserializeCudaEngine(plan_->data(), plan_->size())); if (!engine_) { std::cout << "Fail to create engine" << std::endl; @@ -136,22 +148,13 @@ bool TensorRTWrapper::saveEngine(const std::string & engine_path) bool TensorRTWrapper::loadEngine(const std::string & engine_path) { - std::ifstream file(engine_path, std::ios::in | std::ios::binary); - file.seekg(0, std::ifstream::end); - const size_t size = file.tellg(); - file.seekg(0, std::ifstream::beg); - - std::unique_ptr buffer{new char[size]}; - file.read(buffer.get(), size); - file.close(); - - if (!runtime_) { - std::cout << "Fail to load engine: Runtime isn't created" << std::endl; - return false; - } - - std::cout << "Loading from " << engine_path << std::endl; - engine_ = unique_ptr(runtime_->deserializeCudaEngine(buffer.get(), size)); + std::ifstream engine_file(engine_path); + std::stringstream engine_buffer; + engine_buffer << engine_file.rdbuf(); + std::string engine_str = engine_buffer.str(); + engine_ = tensorrt_common::TrtUniquePtr(runtime_->deserializeCudaEngine( + reinterpret_cast(engine_str.data()), engine_str.size())); + std::cout << "Loaded engine from " << engine_path << std::endl; return true; } diff --git a/perception/lidar_centerpoint/package.xml b/perception/lidar_centerpoint/package.xml index 970f69921c6e2..879013189b246 100644 --- a/perception/lidar_centerpoint/package.xml +++ b/perception/lidar_centerpoint/package.xml @@ -19,6 +19,7 @@ python3-open3d rclcpp rclcpp_components + tensorrt_common tf2_eigen tf2_geometry_msgs tier4_autoware_utils diff --git a/perception/lidar_centerpoint/src/node.cpp b/perception/lidar_centerpoint/src/node.cpp index da4000b450858..39683755bcaf0 100644 --- a/perception/lidar_centerpoint/src/node.cpp +++ b/perception/lidar_centerpoint/src/node.cpp @@ -121,6 +121,11 @@ LidarCenterPointNode::LidarCenterPointNode(const rclcpp::NodeOptions & node_opti stop_watch_ptr_->tic("cyclic_time"); stop_watch_ptr_->tic("processing_time"); } + + if (this->declare_parameter("build_only", false)) { + RCLCPP_INFO(this->get_logger(), "TensorRT engine is built and shutdown node."); + rclcpp::shutdown(); + } } void LidarCenterPointNode::pointCloudCallback( diff --git a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt index 89906f86c5ce5..df0090d1b6cee 100644 --- a/perception/probabilistic_occupancy_grid_map/CMakeLists.txt +++ b/perception/probabilistic_occupancy_grid_map/CMakeLists.txt @@ -48,4 +48,5 @@ rclcpp_components_register_node(laserscan_based_occupancy_grid_map ament_auto_package( INSTALL_TO_SHARE launch + config ) diff --git a/perception/probabilistic_occupancy_grid_map/README.md b/perception/probabilistic_occupancy_grid_map/README.md index 655846f5cade1..d0fa909d58b40 100644 --- a/perception/probabilistic_occupancy_grid_map/README.md +++ b/perception/probabilistic_occupancy_grid_map/README.md @@ -9,7 +9,9 @@ This package outputs the probability of having an obstacle as occupancy grid map Occupancy grid map is generated on `map_frame`, and grid orientation is fixed. -You may need to choose `output_frame` which means grid map origin. Default is `base_link`, but your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) would be the better choice. +You may need to choose `scan_origin_frame` and `gridmap_origin_frame` which means sensor origin and gridmap origin respectively. Especially, set your main LiDAR sensor frame (e.g. `velodyne_top` in sample_vehicle) as a `scan_origin_frame` would result in better performance. + +![image_for_frame_parameter_visualization](./image/gridmap_frame_settings.drawio.svg) ### Each config paramters @@ -21,7 +23,8 @@ Config parameters are managed in `config/*.yaml` and here shows its outline. | --------------------------------- | ------------- | | map_frame | "map" | | base_link_frame | "base_link" | -| output_frame | "base_link" | +| scan_origin_frame | "base_link" | +| gridmap_origin_frame | "base_link" | | use_height_filter | true | | enable_single_frame_mode | false | | map_length | 100.0 [m] | @@ -40,7 +43,8 @@ Config parameters are managed in `config/*.yaml` and here shows its outline. | enable_single_frame_mode | false | | map_frame | "map" | | base_link_frame | "base_link" | -| output_frame | "base_link" | +| scan_origin_frame | "base_link" | +| gridmap_origin_frame | "base_link" | ## References/External links diff --git a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml index c3e498b3e3320..3568ac7ec47a0 100644 --- a/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/laserscan_based_occupancy_grid_map.param.yaml @@ -2,13 +2,13 @@ ros__parameters: map_frame: "map" base_link_frame: "base_link" - output_frame: "base_link" - # using top of the main lidar frame is appropriate. ex) velodyne_top + # center of the grid map + gridmap_origin_frame: "base_link" + # ray-tracing center: main sensor frame is preferable like: "velodyne_top" + scan_origin_frame: "base_link" use_height_filter: true enable_single_frame_mode: false map_length: 100.0 map_width: 100.0 map_resolution: 0.5 - input_obstacle_pointcloud: true - input_obstacle_and_raw_pointcloud: true diff --git a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml index 3b84200d118af..49a1a7097d308 100644 --- a/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml +++ b/perception/probabilistic_occupancy_grid_map/config/pointcloud_based_occupancy_grid_map.param.yaml @@ -1,6 +1,6 @@ /**: ros__parameters: - map_length: 100 # [m] + map_length: 100.0 # [m] map_resolution: 0.5 # [m] use_height_filter: true @@ -9,5 +9,7 @@ # grid map coordinate map_frame: "map" base_link_frame: "base_link" - output_frame: "base_link" - # center of grid_map. Main LiDAR frame is preferable like: "velodyne_top" + # center of the grid map + gridmap_origin_frame: "base_link" + # ray-tracing center: main sensor frame is preferable like: "velodyne_top" + scan_origin_frame: "base_link" diff --git a/perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg b/perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg new file mode 100644 index 0000000000000..d1aaaa2c51176 --- /dev/null +++ b/perception/probabilistic_occupancy_grid_map/image/gridmap_frame_settings.drawio.svg @@ -0,0 +1,80 @@ + + + + + + + + + + + + + + + + + + + + + + + +
+
+
+ scan_origin_frame +
+
+
+ + scan_origin_frame + + + + + +
+
+
+ gridmap_origin_frame +
+
+
+ + gridmap_origin_frame + + + + + + +
+
+
+ gridmap_origin = center of the gridmap +
+
+
+ + gridmap_origin = center of the gridmap + + + + + + + Text is not SVG - cannot display + + + diff --git a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp index 857d657618d20..fbef72e29e0f2 100644 --- a/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.hpp @@ -92,7 +92,8 @@ class LaserscanBasedOccupancyGridMapNode : public rclcpp::Node // ROS Parameters std::string map_frame_; std::string base_link_frame_; - std::string output_frame_; + std::string gridmap_origin_frame_; + std::string scan_origin_frame_; bool use_height_filter_; bool enable_single_frame_mode_; }; diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp index a8def3560f86f..88f2cfbc146f3 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/occupancy_grid_map.hpp @@ -72,7 +72,7 @@ class OccupancyGridMap : public nav2_costmap_2d::Costmap2D void updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, - const Pose & robot_pose, const Pose & gridmap_origin); + const Pose & robot_pose, const Pose & scan_origin); void updateOrigin(double new_origin_x, double new_origin_y) override; diff --git a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp index 68aed57677909..5b7571fa8abcc 100644 --- a/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp +++ b/perception/probabilistic_occupancy_grid_map/include/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.hpp @@ -82,7 +82,8 @@ class PointcloudBasedOccupancyGridMapNode : public rclcpp::Node // ROS Parameters std::string map_frame_; std::string base_link_frame_; - std::string output_frame_; + std::string gridmap_origin_frame_; + std::string scan_origin_frame_; bool use_height_filter_; bool enable_single_frame_mode_; }; diff --git a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py index 1e7bbef48338c..0a6b459d9f5f3 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/laserscan_based_occupancy_grid_map.launch.py @@ -12,15 +12,12 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition -from launch.conditions import LaunchConfigurationEquals -from launch.conditions import LaunchConfigurationNotEquals from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer @@ -29,27 +26,9 @@ import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - +def launch_setup(context, *args, **kwargs): # load parameter files - param_file = os.path.join( - get_package_share_directory("probablistic_occupancy_grid_map"), - "config/laserscan_based_occupancy_grid_map.param.yaml", - ) + param_file = LaunchConfiguration("param_file").perform(context) with open(param_file, "r") as f: laserscan_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"]["ros__parameters"] @@ -68,7 +47,7 @@ def add_launch_arg(name: str, default_value=None): parameters=[ { "target_frame": laserscan_based_occupancy_grid_map_node_params[ - "output_frame" + "scan_origin_frame" ], # Leave disabled to output scan in pointcloud frame "transform_tolerance": 0.01, "min_height": 0.0, @@ -102,36 +81,55 @@ def add_launch_arg(name: str, default_value=None): ("~/output/occupancy_grid_map", LaunchConfiguration("output")), ], parameters=[ + laserscan_based_occupancy_grid_map_node_params, { "input_obstacle_pointcloud": LaunchConfiguration("input_obstacle_pointcloud"), "input_obstacle_and_raw_pointcloud": LaunchConfiguration( "input_obstacle_and_raw_pointcloud" ), - }.update(laserscan_based_occupancy_grid_map_node_params) + }, ], extra_arguments=[{"use_intra_process_comms": LaunchConfiguration("use_intra_process")}], ), ] occupancy_grid_map_container = ComposableNodeContainer( - condition=LaunchConfigurationEquals("container", ""), - name="occupancy_grid_map_container", + name=LaunchConfiguration("container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=composable_nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), output="screen", ) load_composable_nodes = LoadComposableNodes( - condition=LaunchConfigurationNotEquals("container", ""), composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container"), + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), ) return LaunchDescription( [ - add_launch_arg("container", ""), add_launch_arg("use_multithread", "false"), add_launch_arg("use_intra_process", "false"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), @@ -143,9 +141,15 @@ def add_launch_arg(name: str, default_value=None): add_launch_arg("output/stixel", "virtual_scan/stixel"), add_launch_arg("input_obstacle_pointcloud", "false"), add_launch_arg("input_obstacle_and_raw_pointcloud", "true"), + add_launch_arg( + "param_file", + get_package_share_directory("probabilistic_occupancy_grid_map") + + "/config/laserscan_based_occupancy_grid_map.param.yaml", + ), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("container_name", "occupancy_grid_map_container"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py index af5ecdfa02873..e8a9e173cfe0a 100644 --- a/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py +++ b/perception/probabilistic_occupancy_grid_map/launch/pointcloud_based_occupancy_grid_map.launch.py @@ -12,15 +12,11 @@ # See the License for the specific language governing permissions and # limitations under the License. -import os - -from ament_index_python.packages import get_package_share_directory from launch import LaunchDescription from launch.actions import DeclareLaunchArgument +from launch.actions import OpaqueFunction from launch.actions import SetLaunchConfiguration from launch.conditions import IfCondition -from launch.conditions import LaunchConfigurationEquals -from launch.conditions import LaunchConfigurationNotEquals from launch.conditions import UnlessCondition from launch.substitutions import LaunchConfiguration from launch_ros.actions import ComposableNodeContainer @@ -29,27 +25,9 @@ import yaml -def generate_launch_description(): - def add_launch_arg(name: str, default_value=None): - return DeclareLaunchArgument(name, default_value=default_value) - - set_container_executable = SetLaunchConfiguration( - "container_executable", - "component_container", - condition=UnlessCondition(LaunchConfiguration("use_multithread")), - ) - - set_container_mt_executable = SetLaunchConfiguration( - "container_executable", - "component_container_mt", - condition=IfCondition(LaunchConfiguration("use_multithread")), - ) - +def launch_setup(context, *args, **kwargs): # load parameter files - param_file = os.path.join( - get_package_share_directory("probablistic_occupancy_grid_map"), - "config/pointcloud_based_occupancy_grid_map.param.yaml", - ) + param_file = LaunchConfiguration("param_file").perform(context) with open(param_file, "r") as f: pointcloud_based_occupancy_grid_map_node_params = yaml.safe_load(f)["/**"][ "ros__parameters" @@ -71,32 +49,52 @@ def add_launch_arg(name: str, default_value=None): ] occupancy_grid_map_container = ComposableNodeContainer( - condition=LaunchConfigurationEquals("container", ""), - name="occupancy_grid_map_container", + name=LaunchConfiguration("container_name"), namespace="", package="rclcpp_components", executable=LaunchConfiguration("container_executable"), composable_node_descriptions=composable_nodes, + condition=UnlessCondition(LaunchConfiguration("use_pointcloud_container")), output="screen", ) load_composable_nodes = LoadComposableNodes( - condition=LaunchConfigurationNotEquals("container", ""), composable_node_descriptions=composable_nodes, - target_container=LaunchConfiguration("container"), + target_container=LaunchConfiguration("container_name"), + condition=IfCondition(LaunchConfiguration("use_pointcloud_container")), + ) + + return [occupancy_grid_map_container, load_composable_nodes] + + +def generate_launch_description(): + def add_launch_arg(name: str, default_value=None): + return DeclareLaunchArgument(name, default_value=default_value) + + set_container_executable = SetLaunchConfiguration( + "container_executable", + "component_container", + condition=UnlessCondition(LaunchConfiguration("use_multithread")), + ) + + set_container_mt_executable = SetLaunchConfiguration( + "container_executable", + "component_container_mt", + condition=IfCondition(LaunchConfiguration("use_multithread")), ) return LaunchDescription( [ - add_launch_arg("container", ""), add_launch_arg("use_multithread", "false"), - add_launch_arg("use_intra_process", "false"), + add_launch_arg("use_intra_process", "true"), + add_launch_arg("use_pointcloud_container", "false"), + add_launch_arg("container_name", "occupancy_grid_map_container"), add_launch_arg("input/obstacle_pointcloud", "no_ground/oneshot/pointcloud"), add_launch_arg("input/raw_pointcloud", "concatenated/pointcloud"), add_launch_arg("output", "occupancy_grid"), + add_launch_arg("param_file", "config/pointcloud_based_occupancy_grid_map.param.yaml"), set_container_executable, set_container_mt_executable, - occupancy_grid_map_container, - load_composable_nodes, ] + + [OpaqueFunction(function=launch_setup)] ) diff --git a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp index b65dfb97e2f84..2ded71a0ec0b4 100644 --- a/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/laserscan_based_occupancy_grid_map/laserscan_based_occupancy_grid_map_node.cpp @@ -93,6 +93,18 @@ bool cropPointcloudByHeight( pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); return pose; } + +geometry_msgs::msg::Pose getPose( + const builtin_interfaces::msg::Time & stamp, const tf2_ros::Buffer & tf2, + const std::string & source_frame, const std::string & target_frame) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = + tf2.lookupTransform(target_frame, source_frame, stamp, rclcpp::Duration::from_seconds(0.5)); + pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + return pose; +} } // namespace namespace occupancy_grid_map @@ -112,7 +124,8 @@ LaserscanBasedOccupancyGridMapNode::LaserscanBasedOccupancyGridMapNode( /* params */ map_frame_ = declare_parameter("map_frame", "map"); base_link_frame_ = declare_parameter("base_link_frame", "base_link"); - output_frame_ = declare_parameter("output_frame", "base_link"); + gridmap_origin_frame_ = declare_parameter("gridmap_origin_frame", "base_link"); + scan_origin_frame_ = declare_parameter("scan_origin_frame", "base_link"); use_height_filter_ = declare_parameter("use_height_filter", true); enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false); const double map_length{declare_parameter("map_length", 100.0)}; @@ -209,17 +222,15 @@ void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRa PointCloud2 trans_laserscan_pc{}; PointCloud2 trans_obstacle_pc{}; PointCloud2 trans_raw_pc{}; - Pose pose{}; + Pose gridmap_origin{}; + Pose scan_origin{}; try { transformPointcloud(*laserscan_pc_ptr, *tf2_, map_frame_, trans_laserscan_pc); transformPointcloud(filtered_obstacle_pc, *tf2_, map_frame_, trans_obstacle_pc); transformPointcloud(filtered_raw_pc, *tf2_, map_frame_, trans_raw_pc); - // pose = getPose(laserscan_pc_ptr->header, *tf2_, map_frame_); - geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2_->lookupTransform( - map_frame_, output_frame_, laserscan_pc_ptr->header.stamp, - rclcpp::Duration::from_seconds(0.5)); - pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + gridmap_origin = + getPose(laserscan_pc_ptr->header.stamp, *tf2_, gridmap_origin_frame_, map_frame_); + scan_origin = getPose(laserscan_pc_ptr->header.stamp, *tf2_, scan_origin_frame_, map_frame_); } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM(get_logger(), ex.what()); return; @@ -231,16 +242,16 @@ void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRa occupancy_grid_map_updater_ptr_->getSizeInCellsY(), occupancy_grid_map_updater_ptr_->getResolution()); single_frame_occupancy_grid_map.updateOrigin( - pose.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2, - pose.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2); + gridmap_origin.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2, + gridmap_origin.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2); single_frame_occupancy_grid_map.updateFreespaceCells(trans_raw_pc); - single_frame_occupancy_grid_map.raytrace2D(trans_laserscan_pc, pose); + single_frame_occupancy_grid_map.raytrace2D(trans_laserscan_pc, scan_origin); single_frame_occupancy_grid_map.updateOccupiedCells(trans_obstacle_pc); if (enable_single_frame_mode_) { // publish occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( - map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z, + map_frame_, laserscan_pc_ptr->header.stamp, gridmap_origin.position.z, single_frame_occupancy_grid_map)); } else { // Update with bayes filter @@ -248,7 +259,7 @@ void LaserscanBasedOccupancyGridMapNode::onLaserscanPointCloud2WithObstacleAndRa // publish occupancy_grid_map_pub_->publish(OccupancyGridMapToMsgPtr( - map_frame_, laserscan_pc_ptr->header.stamp, pose.position.z, + map_frame_, laserscan_pc_ptr->header.stamp, gridmap_origin.position.z, *occupancy_grid_map_updater_ptr_)); } } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp index 213db4330f850..b6b69ebf44391 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/occupancy_grid_map.cpp @@ -81,6 +81,30 @@ void transformPointcloud( output.header.stamp = input.header.stamp; output.header.frame_id = ""; } + +/** + * @brief Get the Inverse Pose object + * + * @param input + * @return geometry_msgs::msg::Pose inverted pose + */ +geometry_msgs::msg::Pose getInversePose(const geometry_msgs::msg::Pose & pose) +{ + tf2::Vector3 position(pose.position.x, pose.position.y, pose.position.z); + tf2::Quaternion orientation( + pose.orientation.x, pose.orientation.y, pose.orientation.z, pose.orientation.w); + tf2::Transform tf(orientation, position); + const auto inv_tf = tf.inverse(); + geometry_msgs::msg::Pose inv_pose; + inv_pose.position.x = inv_tf.getOrigin().x(); + inv_pose.position.y = inv_tf.getOrigin().y(); + inv_pose.position.z = inv_tf.getOrigin().z(); + inv_pose.orientation.x = inv_tf.getRotation().x(); + inv_pose.orientation.y = inv_tf.getRotation().y(); + inv_pose.orientation.z = inv_tf.getRotation().z(); + inv_pose.orientation.w = inv_tf.getRotation().w(); + return inv_pose; +} } // namespace namespace costmap_2d @@ -165,22 +189,28 @@ void OccupancyGridMap::updateOrigin(double new_origin_x, double new_origin_y) * * @param raw_pointcloud raw point cloud on a certain frame (usually base_link) * @param obstacle_pointcloud raw point cloud on a certain frame (usually base_link) - * @param robot_pose frame of point cloud (usually base_link) - * @param gridmap_origin manually chosen grid map origin frame + * @param robot_pose frame of the input point cloud (usually base_link) + * @param scan_origin manually chosen grid map origin frame */ void OccupancyGridMap::updateWithPointCloud( const PointCloud2 & raw_pointcloud, const PointCloud2 & obstacle_pointcloud, - const Pose & robot_pose, const Pose & gridmap_origin) + const Pose & robot_pose, const Pose & scan_origin) { constexpr double min_angle = tier4_autoware_utils::deg2rad(-180.0); constexpr double max_angle = tier4_autoware_utils::deg2rad(180.0); constexpr double angle_increment = tier4_autoware_utils::deg2rad(0.1); const size_t angle_bin_size = ((max_angle - min_angle) / angle_increment) + size_t(1 /*margin*/); - // Transform to map frame - PointCloud2 trans_raw_pointcloud, trans_obstacle_pointcloud; - transformPointcloud(raw_pointcloud, robot_pose, trans_raw_pointcloud); - transformPointcloud(obstacle_pointcloud, robot_pose, trans_obstacle_pointcloud); + // Transform from base_link to map frame + PointCloud2 map_raw_pointcloud, map_obstacle_pointcloud; // point cloud in map frame + transformPointcloud(raw_pointcloud, robot_pose, map_raw_pointcloud); + transformPointcloud(obstacle_pointcloud, robot_pose, map_obstacle_pointcloud); + + // Transform from map frame to scan frame + PointCloud2 scan_raw_pointcloud, scan_obstacle_pointcloud; // point cloud in scan frame + const auto scan2map_pose = getInversePose(scan_origin); // scan -> map transform pose + transformPointcloud(map_raw_pointcloud, scan2map_pose, scan_raw_pointcloud); + transformPointcloud(map_obstacle_pointcloud, scan2map_pose, scan_obstacle_pointcloud); // Create angle bins struct BinInfo @@ -198,17 +228,18 @@ void OccupancyGridMap::updateWithPointCloud( std::vector> raw_pointcloud_angle_bins; obstacle_pointcloud_angle_bins.resize(angle_bin_size); raw_pointcloud_angle_bins.resize(angle_bin_size); - for (PointCloud2ConstIterator iter_x(raw_pointcloud, "x"), iter_y(raw_pointcloud, "y"), - iter_wx(trans_raw_pointcloud, "x"), iter_wy(trans_raw_pointcloud, "y"); + for (PointCloud2ConstIterator iter_x(scan_raw_pointcloud, "x"), + iter_y(scan_raw_pointcloud, "y"), iter_wx(map_raw_pointcloud, "x"), + iter_wy(map_raw_pointcloud, "y"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) { const double angle = atan2(*iter_y, *iter_x); const int angle_bin_index = (angle - min_angle) / angle_increment; raw_pointcloud_angle_bins.at(angle_bin_index) .push_back(BinInfo(std::hypot(*iter_y, *iter_x), *iter_wx, *iter_wy)); } - for (PointCloud2ConstIterator iter_x(obstacle_pointcloud, "x"), - iter_y(obstacle_pointcloud, "y"), iter_wx(trans_obstacle_pointcloud, "x"), - iter_wy(trans_obstacle_pointcloud, "y"); + for (PointCloud2ConstIterator iter_x(scan_obstacle_pointcloud, "x"), + iter_y(scan_obstacle_pointcloud, "y"), iter_wx(map_obstacle_pointcloud, "x"), + iter_wy(map_obstacle_pointcloud, "y"); iter_x != iter_x.end(); ++iter_x, ++iter_y, ++iter_wx, ++iter_wy) { const double angle = atan2(*iter_y, *iter_x); int angle_bin_index = (angle - min_angle) / angle_increment; @@ -248,7 +279,7 @@ void OccupancyGridMap::updateWithPointCloud( : obstacle_pointcloud_angle_bin.back(); } raytrace( - gridmap_origin.position.x, gridmap_origin.position.y, end_distance.wx, end_distance.wy, + scan_origin.position.x, scan_origin.position.y, end_distance.wx, end_distance.wy, occupancy_cost_value::FREE_SPACE); } diff --git a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp index cbfcce22d1b61..c594a0be0703f 100644 --- a/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp +++ b/perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp @@ -93,6 +93,18 @@ geometry_msgs::msg::Pose getPose( pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); return pose; } + +geometry_msgs::msg::Pose getPose( + const builtin_interfaces::msg::Time & stamp, const tf2_ros::Buffer & tf2, + const std::string & source_frame, const std::string & target_frame) +{ + geometry_msgs::msg::Pose pose; + geometry_msgs::msg::TransformStamped tf_stamped; + tf_stamped = + tf2.lookupTransform(target_frame, source_frame, stamp, rclcpp::Duration::from_seconds(0.5)); + pose = tier4_autoware_utils::transform2pose(tf_stamped.transform); + return pose; +} } // namespace namespace occupancy_grid_map @@ -111,7 +123,8 @@ PointcloudBasedOccupancyGridMapNode::PointcloudBasedOccupancyGridMapNode( /* params */ map_frame_ = declare_parameter("map_frame", "map"); base_link_frame_ = declare_parameter("base_link_frame", "base_link"); - output_frame_ = declare_parameter("output_frame", "base_link"); + gridmap_origin_frame_ = declare_parameter("gridmap_origin_frame", "base_link"); + scan_origin_frame_ = declare_parameter("scan_origin_frame", "base_link"); use_height_filter_ = declare_parameter("use_height_filter", true); enable_single_frame_mode_ = declare_parameter("enable_single_frame_mode", false); const double map_length{declare_parameter("map_length", 100.0)}; @@ -174,12 +187,11 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( // Get from map to sensor frame pose Pose robot_pose{}; Pose gridmap_origin{}; + Pose scan_origin{}; try { robot_pose = getPose(input_raw_msg->header, *tf2_, map_frame_); - geometry_msgs::msg::TransformStamped tf_stamped; - tf_stamped = tf2_->lookupTransform( - map_frame_, output_frame_, input_raw_msg->header.stamp, rclcpp::Duration::from_seconds(0.5)); - gridmap_origin = tier4_autoware_utils::transform2pose(tf_stamped.transform); + gridmap_origin = getPose(input_raw_msg->header.stamp, *tf2_, gridmap_origin_frame_, map_frame_); + scan_origin = getPose(input_raw_msg->header.stamp, *tf2_, scan_origin_frame_, map_frame_); } catch (tf2::TransformException & ex) { RCLCPP_WARN_STREAM(get_logger(), ex.what()); return; @@ -194,7 +206,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw( gridmap_origin.position.x - single_frame_occupancy_grid_map.getSizeInMetersX() / 2, gridmap_origin.position.y - single_frame_occupancy_grid_map.getSizeInMetersY() / 2); single_frame_occupancy_grid_map.updateWithPointCloud( - filtered_raw_pc, filtered_obstacle_pc, robot_pose, gridmap_origin); + filtered_raw_pc, filtered_obstacle_pc, robot_pose, scan_origin); if (enable_single_frame_mode_) { // publish diff --git a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp index a10e8e6332488..8d03bfaee1877 100644 --- a/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp +++ b/perception/tensorrt_yolox/src/tensorrt_yolox_node.cpp @@ -148,8 +148,8 @@ void TrtYoloXNode::replaceLabelMap() if (label == "PERSON") { label = "PEDESTRIAN"; } else if ( - label != "CAR" || label != "PEDESTRIAN" || label != "BUS" || label != "TRUCK" || - label != "BICYCLE" || label != "MOTORCYCLE") { + label != "CAR" && label != "PEDESTRIAN" && label != "BUS" && label != "TRUCK" && + label != "BICYCLE" && label != "MOTORCYCLE") { label = "UNKNOWN"; } } diff --git a/perception/traffic_light_map_based_detector/package.xml b/perception/traffic_light_map_based_detector/package.xml index 4da85f41816e6..46220572ae9a3 100644 --- a/perception/traffic_light_map_based_detector/package.xml +++ b/perception/traffic_light_map_based_detector/package.xml @@ -14,6 +14,7 @@ autoware_auto_mapping_msgs autoware_auto_perception_msgs autoware_auto_planning_msgs + autoware_planning_msgs geometry_msgs image_geometry lanelet2_extension diff --git a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp index 164fb72887cd8..8277ae102b312 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/parameters.hpp @@ -33,7 +33,10 @@ struct BehaviorPathPlannerParameters ModuleConfigParameters config_pull_out; ModuleConfigParameters config_pull_over; ModuleConfigParameters config_side_shift; - ModuleConfigParameters config_lane_change; + ModuleConfigParameters config_lane_change_left; + ModuleConfigParameters config_lane_change_right; + ModuleConfigParameters config_external_lane_change_left; + ModuleConfigParameters config_external_lane_change_right; double backward_path_length; double forward_path_length; @@ -56,7 +59,9 @@ struct BehaviorPathPlannerParameters double turn_signal_shift_length_threshold; bool turn_signal_on_swerving; - double path_interval; + double enable_akima_spline_first; + double input_path_interval; + double output_path_interval; double ego_nearest_dist_threshold; double ego_nearest_yaw_threshold; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp index 489dbfd3b5764..bbdc7c14435d7 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/lane_change_module.hpp @@ -45,6 +45,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using marker_utils::CollisionCheckDebug; +using route_handler::Direction; using tier4_planning_msgs::msg::LaneChangeDebugMsg; using tier4_planning_msgs::msg::LaneChangeDebugMsgArray; @@ -59,8 +60,7 @@ class LaneChangeModule : public SceneModuleInterface LaneChangeModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface_left, - const std::shared_ptr & rtc_interface_right); + const std::shared_ptr & rtc_interface, Direction direction); #endif bool isExecutionRequested() const override; bool isExecutionReady() const override; @@ -75,13 +75,7 @@ class LaneChangeModule : public SceneModuleInterface void acceptVisitor( [[maybe_unused]] const std::shared_ptr & visitor) const override; -#ifndef USE_OLD_ARCHITECTURE - void updateModuleParams(const std::shared_ptr & parameters) - { - parameters_ = parameters; - } -#endif - +#ifdef USE_OLD_ARCHITECTURE void publishRTCStatus() override { rtc_interface_left_->publishCooperateStatus(clock_->now()); @@ -110,6 +104,12 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_left_->unlockCommandUpdate(); rtc_interface_right_->unlockCommandUpdate(); } +#else + void updateModuleParams(const std::shared_ptr & parameters) + { + parameters_ = parameters; + } +#endif private: std::shared_ptr parameters_; @@ -125,16 +125,21 @@ class LaneChangeModule : public SceneModuleInterface bool is_abort_path_approved_{false}; bool is_abort_approval_requested_{false}; bool is_abort_condition_satisfied_{false}; - bool is_activated_ = false; + bool is_activated_{false}; +#ifdef USE_OLD_ARCHITECTURE std::shared_ptr rtc_interface_left_; std::shared_ptr rtc_interface_right_; UUID uuid_left_; UUID uuid_right_; UUID candidate_uuid_; +#else + Direction direction_{Direction::NONE}; +#endif void resetParameters(); +#ifdef USE_OLD_ARCHITECTURE void waitApprovalLeft(const double start_distance, const double finish_distance) { rtc_interface_left_->updateCooperateStatus( @@ -190,8 +195,8 @@ class LaneChangeModule : public SceneModuleInterface rtc_interface_right_->removeCooperateStatus(uuid_right_); } } +#endif - lanelet::ConstLanelets get_original_lanes() const; PathWithLaneId getReferencePath() const; lanelet::ConstLanelets getLaneChangeLanes( const lanelet::ConstLanelets & current_lanes, const double lane_change_lane_length) const; diff --git a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp index 87e5d42688d06..ecb2164855bb0 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/scene_module/lane_change/manager.hpp @@ -33,23 +33,24 @@ class LaneChangeModuleManager : public SceneModuleManagerInterface public: LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - std::shared_ptr parameters); + std::shared_ptr parameters, Direction direction); std::shared_ptr createNewSceneModuleInstance() override { return std::make_shared( - name_, *node_, parameters_, rtc_interface_left_, rtc_interface_right_); + name_, *node_, parameters_, rtc_interface_, direction_); } void updateModuleParams(const std::vector & parameters) override; private: - std::shared_ptr rtc_interface_left_; - std::shared_ptr rtc_interface_right_; + std::shared_ptr rtc_interface_; std::shared_ptr parameters_; std::vector> registered_modules_; + + Direction direction_; }; } // namespace behavior_path_planner diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp index 1d16f1bd84046..7ef20d546327b 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/lane_change_module_data.hpp @@ -79,6 +79,11 @@ struct LaneChangeTargetObjectIndices std::vector target_lane{}; std::vector other_lane{}; }; + +enum class LaneChangeType { + Normal = 0, + ExternalRequest = 1, +}; } // namespace behavior_path_planner #endif // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__LANE_CHANGE_MODULE_DATA_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp index 443ce4d3aaa66..679f1ae230f1c 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/util/lane_change/util.hpp @@ -45,6 +45,7 @@ using autoware_auto_planning_msgs::msg::PathWithLaneId; using geometry_msgs::msg::Pose; using geometry_msgs::msg::Twist; using marker_utils::CollisionCheckDebug; +using route_handler::Direction; using tier4_autoware_utils::Polygon2d; PathWithLaneId combineReferencePath(const PathWithLaneId & path1, const PathWithLaneId & path2); @@ -53,11 +54,7 @@ bool isPathInLanelets( const PathWithLaneId & path, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets); -double getExpectedVelocityWhenDecelerate( - const double & current_velocity, const double & expected_acceleration, - const double & lane_change_prepare_duration); - -std::pair calcLaneChangingSpeedAndDistanceWhenDecelerate( +std::pair calcLaneChangingSpeedAndDistance( const double velocity, const double shift_length, const double deceleration, const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param); @@ -78,6 +75,14 @@ std::pair getLaneChangePaths( const double check_distance, LaneChangePaths * candidate_paths, std::unordered_map * debug_data); +std::pair getLaneChangePaths( + const PathWithLaneId & original_path, const RouteHandler & route_handler, + const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, + const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects, + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, + const double check_distance, LaneChangePaths * candidate_paths, + std::unordered_map * debug_data); + bool isLaneChangePathSafe( const LaneChangePath & lane_change_path, const PredictedObjects::ConstSharedPtr dynamic_objects, const LaneChangeTargetObjectIndices & dynamic_object_indices, const Pose & current_pose, @@ -93,22 +98,27 @@ bool hasEnoughDistance( const lanelet::ConstLanelets & target_lanes, const Pose & current_pose, const Pose & goal_pose, const RouteHandler & route_handler, const double minimum_lane_change_length); -ShiftLine getLaneChangeShiftLine( - const PathWithLaneId & path1, const PathWithLaneId & path2, +ShiftLine getLaneChangingShiftLine( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path); PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, - const LaneChangePhaseInfo dist_prepare_to_lc_end, const double min_total_lane_changing_distance, + const double lane_changing_distance, const double min_total_lane_changing_distance, const double forward_path_length, const double resample_interval, const bool is_goal_in_route); -PathWithLaneId getLaneChangePathPrepareSegment( +PathWithLaneId getPrepareSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const double arc_length_from_current, const double backward_path_length, const double prepare_distance, const double prepare_speed); -PathWithLaneId getLaneChangePathLaneChangingSegment( +PathWithLaneId getPrepareSegment( + const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets, + const Pose & current_pose, const double backward_path_length, const double prepare_distance, + const double prepare_speed); + +PathWithLaneId getLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const double arc_length_from_target, const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, @@ -149,5 +159,7 @@ LaneChangeTargetObjectIndices filterObjectIndices( double calcLateralBufferForFiltering(const double vehicle_width, const double lateral_buffer = 0.0); +std::string getStrDirection(const std::string name, const Direction direction); + } // namespace behavior_path_planner::lane_change_utils #endif // BEHAVIOR_PATH_PLANNER__UTIL__LANE_CHANGE__UTIL_HPP_ diff --git a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp index c0598ed7f04d8..f374246c44b7d 100644 --- a/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp +++ b/planning/behavior_path_planner/include/behavior_path_planner/utilities.hpp @@ -534,6 +534,9 @@ double calcLaneChangeBuffer( lanelet::ConstLanelets getLaneletsFromPath( const PathWithLaneId & path, const std::shared_ptr & route_handler); + +double calcLateralDistanceToLanelet( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose); } // namespace behavior_path_planner::util #endif // BEHAVIOR_PATH_PLANNER__UTILITIES_HPP_ diff --git a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp index 7fc12eb616da9..be99d578e1856 100644 --- a/planning/behavior_path_planner/src/behavior_path_planner_node.cpp +++ b/planning/behavior_path_planner/src/behavior_path_planner_node.cpp @@ -234,10 +234,23 @@ BehaviorPathPlannerNode::BehaviorPathPlannerNode(const rclcpp::NodeOptions & nod "side_shift", create_publisher(path_reference_name_space + "side_shift", 1)); } - if (p.config_lane_change.enable_module) { - const std::string module_topic = "lane_change"; + if (p.config_lane_change_left.enable_module) { + const std::string module_topic = "lane_change_left"; auto manager = std::make_shared( - this, module_topic, p.config_lane_change, lane_change_param_ptr_); + this, module_topic, p.config_lane_change_left, lane_change_param_ptr_, + route_handler::Direction::LEFT); + planner_manager_->registerSceneModuleManager(manager); + path_candidate_publishers_.emplace( + module_topic, create_publisher(path_candidate_name_space + module_topic, 1)); + path_reference_publishers_.emplace( + module_topic, create_publisher(path_reference_name_space + module_topic, 1)); + } + + if (p.config_lane_change_right.enable_module) { + const std::string module_topic = "lane_change_right"; + auto manager = std::make_shared( + this, module_topic, p.config_lane_change_right, lane_change_param_ptr_, + route_handler::Direction::RIGHT); planner_manager_->registerSceneModuleManager(manager); path_candidate_publishers_.emplace( module_topic, create_publisher(path_candidate_name_space + module_topic, 1)); @@ -316,12 +329,21 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() } { - const std::string ns = "lane_change."; - p.config_lane_change.enable_module = declare_parameter(ns + "enable_module"); - p.config_lane_change.enable_simultaneous_execution = + const std::string ns = "lane_change_left."; + p.config_lane_change_left.enable_module = declare_parameter(ns + "enable_module"); + p.config_lane_change_left.enable_simultaneous_execution = + declare_parameter(ns + "enable_simultaneous_execution"); + p.config_lane_change_left.priority = declare_parameter(ns + "priority"); + p.config_lane_change_left.max_module_size = declare_parameter(ns + "max_module_size"); + } + + { + const std::string ns = "lane_change_right."; + p.config_lane_change_right.enable_module = declare_parameter(ns + "enable_module"); + p.config_lane_change_right.enable_simultaneous_execution = declare_parameter(ns + "enable_simultaneous_execution"); - p.config_lane_change.priority = declare_parameter(ns + "priority"); - p.config_lane_change.max_module_size = declare_parameter(ns + "max_module_size"); + p.config_lane_change_right.priority = declare_parameter(ns + "priority"); + p.config_lane_change_right.max_module_size = declare_parameter(ns + "max_module_size"); } { @@ -383,7 +405,9 @@ BehaviorPathPlannerParameters BehaviorPathPlannerNode::getCommonParam() declare_parameter("turn_signal_shift_length_threshold", 0.3); p.turn_signal_on_swerving = declare_parameter("turn_signal_on_swerving", true); - p.path_interval = declare_parameter("path_interval"); + p.enable_akima_spline_first = declare_parameter("enable_akima_spline_first"); + p.input_path_interval = declare_parameter("input_path_interval"); + p.output_path_interval = declare_parameter("output_path_interval"); p.visualize_maximum_drivable_area = declare_parameter("visualize_maximum_drivable_area", true); p.ego_nearest_dist_threshold = declare_parameter("ego_nearest_dist_threshold"); p.ego_nearest_yaw_threshold = declare_parameter("ego_nearest_yaw_threshold"); @@ -1270,7 +1294,7 @@ PathWithLaneId::SharedPtr BehaviorPathPlannerNode::getPath( } const auto resampled_path = util::resamplePathWithSpline( - connected_path, planner_data_->parameters.path_interval, + connected_path, planner_data_->parameters.output_path_interval, keepInputPoints(module_status_ptr_vec)); return std::make_shared(resampled_path); } diff --git a/planning/behavior_path_planner/src/planner_manager.cpp b/planning/behavior_path_planner/src/planner_manager.cpp index a90e1cef71cf8..767369904a466 100644 --- a/planning/behavior_path_planner/src/planner_manager.cpp +++ b/planning/behavior_path_planner/src/planner_manager.cpp @@ -281,7 +281,7 @@ BehaviorModuleOutput PlannerManager::update(const std::shared_ptr & if ((*itr)->getCurrentStatus() != ModuleStatus::RUNNING) { if (itr == approved_module_ptrs_.begin()) { // update root lanelet when the lane change is done. - if (name == "lane_change") { + if (name.find("lane_change") != std::string::npos) { root_lanelet_ = updateRootLanelet(data); } diff --git a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp index 3928f7f9f2b15..60811e5c4469f 100644 --- a/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/avoidance/avoidance_module.cpp @@ -114,6 +114,7 @@ bool AvoidanceModule::isExecutionRequested() const } // Check ego is in preferred lane +#ifdef USE_OLD_ARCHITECTURE const auto current_lanes = util::getCurrentLanes(planner_data_); lanelet::ConstLanelet current_lane; lanelet::utils::query::getClosestLanelet( @@ -123,6 +124,7 @@ bool AvoidanceModule::isExecutionRequested() const if (num != 0) { return false; } +#endif // Check avoidance targets exist const auto avoid_data = calcAvoidancePlanningData(debug_data_); @@ -235,9 +237,14 @@ AvoidancePlanningData AvoidanceModule::calcAvoidancePlanningData(DebugData & deb calcSignedArcLength(data.reference_path.points, getEgoPosition(), 0)); // lanelet info +#ifdef USE_OLD_ARCHITECTURE data.current_lanelets = util::calcLaneAroundPose( planner_data_->route_handler, reference_pose, planner_data_->parameters.forward_path_length, planner_data_->parameters.backward_path_length); +#else + data.current_lanelets = + util::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); +#endif // keep avoidance state data.state = avoidance_data_.state; @@ -1369,7 +1376,7 @@ void AvoidanceModule::generateTotalShiftLine( for (size_t i = 1; i < N; ++i) { bool has_object = false; for (const auto & al : avoid_lines) { - if (al.start_idx < i && i < al.end_idx) { + if (al.start_idx <= i && i <= al.end_idx) { has_object = true; break; } @@ -2955,8 +2962,13 @@ PathWithLaneId AvoidanceModule::calcCenterLinePath( "p.backward_path_length = %f, longest_dist_to_shift_line = %f, backward_length = %f", p.backward_path_length, longest_dist_to_shift_line, backward_length); +#ifdef USE_OLD_ARCHITECTURE const lanelet::ConstLanelets current_lanes = util::calcLaneAroundPose(route_handler, pose, p.forward_path_length, backward_length); +#else + const lanelet::ConstLanelets current_lanes = + util::getCurrentLanesFromPath(*getPreviousModuleOutput().reference_path, planner_data_); +#endif centerline_path = util::getCenterLinePath( *route_handler, current_lanes, pose, backward_length, p.forward_path_length, p); diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp index 0554c5d298aa6..99c0ff5ddd467 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/lane_change_module.cpp @@ -31,15 +31,6 @@ #include #include -std::string to_string(const unique_identifier_msgs::msg::UUID & uuid) -{ - std::stringstream ss; - for (auto i = 0; i < 16; ++i) { - ss << std::hex << std::setfill('0') << std::setw(2) << +uuid.uuid[i]; - } - return ss.str(); -} - namespace behavior_path_planner { using autoware_auto_perception_msgs::msg::ObjectClassification; @@ -60,15 +51,10 @@ LaneChangeModule::LaneChangeModule( LaneChangeModule::LaneChangeModule( const std::string & name, rclcpp::Node & node, const std::shared_ptr & parameters, - const std::shared_ptr & rtc_interface_left, - const std::shared_ptr & rtc_interface_right) -: SceneModuleInterface{name, node}, - parameters_{parameters}, - rtc_interface_left_{rtc_interface_left}, - rtc_interface_right_{rtc_interface_right}, - uuid_left_{generateUUID()}, - uuid_right_{generateUUID()} + const std::shared_ptr & rtc_interface, Direction direction) +: SceneModuleInterface{name, node}, parameters_{parameters}, direction_{direction} { + rtc_interface_ptr_ = rtc_interface; steering_factor_interface_ptr_ = std::make_unique(&node, "lane_change"); } #endif @@ -80,6 +66,7 @@ void LaneChangeModule::onEntry() current_state_ = ModuleStatus::SUCCESS; #else current_state_ = ModuleStatus::IDLE; + waitApproval(); #endif current_lane_change_state_ = LaneChangeStates::Normal; updateLaneChangeStatus(); @@ -106,6 +93,10 @@ bool LaneChangeModule::isExecutionRequested() const #endif const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + if (lane_change_lanes.empty()) { + return false; + } + LaneChangePath selected_path; const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); @@ -127,6 +118,10 @@ bool LaneChangeModule::isExecutionReady() const #endif const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + if (lane_change_lanes.empty()) { + return false; + } + LaneChangePath selected_path; const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); @@ -217,6 +212,7 @@ BehaviorModuleOutput LaneChangeModule::plan() void LaneChangeModule::resetPathIfAbort() { if (!is_abort_approval_requested_) { +#ifdef USE_OLD_ARCHITECTURE const auto lateral_shift = lane_change_utils::getLateralShift(*abort_path_); if (lateral_shift > 0.0) { removePreviousRTCStatusRight(); @@ -225,6 +221,9 @@ void LaneChangeModule::resetPathIfAbort() removePreviousRTCStatusLeft(); uuid_left_ = generateUUID(); } +#else + removeRTCStatus(); +#endif RCLCPP_DEBUG(getLogger(), "[abort] uuid is reset to request abort approval."); is_abort_approval_requested_ = true; is_abort_path_approved_ = false; @@ -256,8 +255,17 @@ CandidateOutput LaneChangeModule::planCandidate() const #endif const auto lane_change_lanes = getLaneChangeLanes(current_lanes, lane_change_lane_length_); + if (lane_change_lanes.empty()) { + return output; + } + +#ifdef USE_OLD_ARCHITECTURE [[maybe_unused]] const auto [found_valid_path, found_safe_path] = getSafePath(lane_change_lanes, check_distance_, selected_path); +#else + selected_path = status_.lane_change_path; +#endif + selected_path.path.header = planner_data_->route_handler->getRouteHeader(); if (isAbortState()) { @@ -293,13 +301,24 @@ BehaviorModuleOutput LaneChangeModule::planWaitingApproval() BehaviorModuleOutput out; out.path = std::make_shared(prev_approved_path_); out.reference_path = getPreviousModuleOutput().reference_path; + out.turn_signal_info = getPreviousModuleOutput().turn_signal_info; + +#ifndef USE_OLD_ARCHITECTURE + updateLaneChangeStatus(); +#endif const auto candidate = planCandidate(); path_candidate_ = std::make_shared(candidate.path_candidate); path_reference_ = getPreviousModuleOutput().reference_path; +#ifdef USE_OLD_ARCHITECTURE updateRTCStatus(candidate); waitApproval(); +#else + updateRTCStatus( + candidate.start_distance_to_path_change, candidate.finish_distance_to_path_change); +#endif is_abort_path_approved_ = false; + return out; } @@ -405,7 +424,11 @@ lanelet::ConstLanelets LaneChangeModule::getLaneChangeLanes( lanelet::ConstLanelets current_check_lanes = route_handler->getLaneletSequence(current_lane, current_pose, 0.0, lane_change_prepare_length); lanelet::ConstLanelet lane_change_lane; +#ifdef USE_OLD_ARCHITECTURE if (route_handler->getLaneChangeTarget(current_check_lanes, &lane_change_lane)) { +#else + if (route_handler->getLaneChangeTarget(current_check_lanes, &lane_change_lane, direction_)) { +#endif lane_change_lanes = route_handler->getLaneletSequence( lane_change_lane, current_pose, lane_change_lane_length, lane_change_lane_length); } else { @@ -437,10 +460,17 @@ std::pair LaneChangeModule::getSafePath( // find candidate paths LaneChangePaths valid_paths; +#ifdef USE_OLD_ARCHITECTURE const auto [found_valid_path, found_safe_path] = lane_change_utils::getLaneChangePaths( *route_handler, current_lanes, lane_change_lanes, current_pose, current_twist, planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, &valid_paths, &object_debug_); +#else + const auto [found_valid_path, found_safe_path] = lane_change_utils::getLaneChangePaths( + *getPreviousModuleOutput().path, *route_handler, current_lanes, lane_change_lanes, current_pose, + current_twist, planner_data_->dynamic_object, common_parameters, *parameters_, check_distance, + &valid_paths, &object_debug_); +#endif debug_valid_path_ = valid_paths; if (parameters_->publish_debug_marker) { @@ -649,13 +679,14 @@ std::shared_ptr LaneChangeModule::get_debug_msg_array() void LaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & output) { - const auto turn_signal_info = output.turn_signal_info; const auto current_pose = getEgoPose(); const double start_distance = motion_utils::calcSignedArcLength( output.path->points, current_pose.position, status_.lane_change_path.shift_line.start.position); const double finish_distance = motion_utils::calcSignedArcLength( output.path->points, current_pose.position, status_.lane_change_path.shift_line.end.position); +#ifdef USE_OLD_ARCHITECTURE + const auto turn_signal_info = output.turn_signal_info; const uint16_t steering_factor_direction = std::invoke([this, &start_distance, &finish_distance, &turn_signal_info]() { if (turn_signal_info.turn_signal.command == TurnIndicatorsCommand::ENABLE_LEFT) { @@ -668,6 +699,17 @@ void LaneChangeModule::updateSteeringFactorPtr(const BehaviorModuleOutput & outp } return SteeringFactor::UNKNOWN; }); +#else + const auto steering_factor_direction = std::invoke([&]() { + if (direction_ == Direction::LEFT) { + return SteeringFactor::LEFT; + } + if (direction_ == Direction::RIGHT) { + return SteeringFactor::RIGHT; + } + return SteeringFactor::UNKNOWN; + }); +#endif // TODO(tkhmy) add handle status TRYING steering_factor_interface_ptr_->updateSteeringFactor( diff --git a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp index 79bc075de2ffe..13bb4c7db5b4c 100644 --- a/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp +++ b/planning/behavior_path_planner/src/scene_module/lane_change/manager.cpp @@ -23,14 +23,15 @@ namespace behavior_path_planner { - +using route_handler::Direction; LaneChangeModuleManager::LaneChangeModuleManager( rclcpp::Node * node, const std::string & name, const ModuleConfigParameters & config, - std::shared_ptr parameters) -: SceneModuleManagerInterface(node, name, config), parameters_{std::move(parameters)} + std::shared_ptr parameters, Direction direction) +: SceneModuleManagerInterface(node, name, config), + parameters_{std::move(parameters)}, + direction_{direction} { - rtc_interface_left_ = std::make_shared(node, name + "_left"); - rtc_interface_right_ = std::make_shared(node, name + "_right"); + rtc_interface_ = std::make_shared(node, name); } void LaneChangeModuleManager::updateModuleParams( @@ -40,7 +41,7 @@ void LaneChangeModuleManager::updateModuleParams( [[maybe_unused]] auto p = parameters_; - [[maybe_unused]] std::string ns = "lane_change."; + [[maybe_unused]] const std::string ns = name_ + "."; // updateParam(parameters, ns + ..., ...); std::for_each(registered_modules_.begin(), registered_modules_.end(), [&p](const auto & m) { diff --git a/planning/behavior_path_planner/src/util/avoidance/util.cpp b/planning/behavior_path_planner/src/util/avoidance/util.cpp index b1d1c5d2017fe..6681fd19e7d00 100644 --- a/planning/behavior_path_planner/src/util/avoidance/util.cpp +++ b/planning/behavior_path_planner/src/util/avoidance/util.cpp @@ -230,38 +230,6 @@ double calcDecelDistPlanType3(const double v0, const double a0, const double ja) return x1; } -tier4_autoware_utils::Polygon2d expandPolygon( - const tier4_autoware_utils::Polygon2d & input_polygon, const double offset) -{ - // NOTE: There is a duplicated point. - const size_t num_points = input_polygon.outer().size() - 1; - - tier4_autoware_utils::Polygon2d expanded_polygon; - for (size_t i = 0; i < num_points; ++i) { - const auto & curr_p = input_polygon.outer().at(i); - const auto & next_p = input_polygon.outer().at(i + 1); - const auto & prev_p = - i == 0 ? input_polygon.outer().at(num_points - 1) : input_polygon.outer().at(i - 1); - - Eigen::Vector2d current_to_next(next_p.x() - curr_p.x(), next_p.y() - curr_p.y()); - Eigen::Vector2d current_to_prev(prev_p.x() - curr_p.x(), prev_p.y() - curr_p.y()); - current_to_next.normalize(); - current_to_prev.normalize(); - - const Eigen::Vector2d offset_vector = (-current_to_next - current_to_prev).normalized(); - const double theta = std::acos(offset_vector.dot(current_to_next)); - const double scaled_offset = offset / std::sin(theta); - const Eigen::Vector2d offset_point = - Eigen::Vector2d(curr_p.x(), curr_p.y()) + offset_vector * scaled_offset; - - expanded_polygon.outer().push_back( - tier4_autoware_utils::Point2d(offset_point.x(), offset_point.y())); - } - - boost::geometry::correct(expanded_polygon); - return expanded_polygon; -} - boost::optional intersect( const geometry_msgs::msg::Point & p1, const geometry_msgs::msg::Point & p2, const geometry_msgs::msg::Point & p3, const geometry_msgs::msg::Point & p4) @@ -834,7 +802,8 @@ Polygon2d createEnvelopePolygon( tf2::doTransform( toMsg(envelope_poly, closest_pose.position.z), envelope_ros_polygon, geometry_tf); - const auto expanded_polygon = expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); + const auto expanded_polygon = + tier4_autoware_utils::expandPolygon(toPolygon2d(envelope_ros_polygon), envelope_buffer); return expanded_polygon; } @@ -869,7 +838,8 @@ void generateDrivableArea( const auto & obj_pose = object.object.kinematics.initial_pose_with_covariance.pose; const double diff_poly_buffer = object.avoid_margin.get() - original_object_buffer - planner_data->parameters.vehicle_width / 2.0; - const auto obj_poly = expandPolygon(object.envelope_poly, diff_poly_buffer); + const auto obj_poly = + tier4_autoware_utils::expandPolygon(object.envelope_poly, diff_poly_buffer); // get edge points of the object const size_t nearest_path_idx = motion_utils::findNearestIndex( diff --git a/planning/behavior_path_planner/src/util/lane_change/util.cpp b/planning/behavior_path_planner/src/util/lane_change/util.cpp index 4c48c16179586..9dcf4b11dcfdb 100644 --- a/planning/behavior_path_planner/src/util/lane_change/util.cpp +++ b/planning/behavior_path_planner/src/util/lane_change/util.cpp @@ -115,7 +115,7 @@ using autoware_auto_planning_msgs::msg::PathPointWithLaneId; using lanelet::ArcCoordinates; using util::getHighestProbLabel; -inline double calcLaneChangeResamplingInterval( +inline double calcLaneChangeResampleInterval( const double lane_changing_distance, const double lane_changing_speed) { constexpr auto min_resampling_points{30.0}; @@ -159,20 +159,6 @@ bool isPathInLanelets( return true; } -double getExpectedVelocityWhenDecelerate( - const double & velocity, const double & expected_acceleration, const double & duration) -{ - return velocity + expected_acceleration * duration; -} - -double getDistanceWhenDecelerate( - const double & velocity, const double & expected_acceleration, const double & duration, - const double & minimum_distance) -{ - const auto distance = velocity * duration + 0.5 * expected_acceleration * std::pow(duration, 2); - return std::max(distance, minimum_distance); -} - std::optional constructCandidatePath( const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const PathWithLaneId & target_lane_reference_path, const ShiftLine & shift_line, @@ -231,10 +217,10 @@ std::optional constructCandidatePath( const PathPointWithLaneId & lane_changing_start_point = prepare_segment.points.back(); const PathPointWithLaneId & lane_changing_end_point = lane_changing_segment.points.front(); const Pose & lane_changing_end_pose = lane_changing_end_point.point.pose; - const auto lanechange_end_idx = + const auto lane_change_end_idx = motion_utils::findNearestIndex(shifted_path.path.points, lane_changing_end_pose); - if (!lanechange_end_idx) { + if (!lane_change_end_idx) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "lane change end idx not found on target path."); @@ -243,7 +229,7 @@ std::optional constructCandidatePath( for (size_t i = 0; i < shifted_path.path.points.size(); ++i) { auto & point = shifted_path.path.points.at(i); - if (i < *lanechange_end_idx) { + if (i < *lane_change_end_idx) { point.lane_ids = replaceWithSortedIds(point.lane_ids, sorted_lane_ids); point.point.longitudinal_velocity_mps = std::min( point.point.longitudinal_velocity_mps, @@ -257,17 +243,18 @@ std::optional constructCandidatePath( point.lane_ids = lane_changing_segment.points.at(*nearest_idx).lane_ids; } + // check candidate path is in lanelet if (!isPathInLanelets(shifted_path.path, original_lanelets, target_lanelets)) { return std::nullopt; } - // check candidate path is in lanelet candidate_path.path = combineReferencePath(prepare_segment, shifted_path.path); candidate_path.shifted_path = shifted_path; return std::optional{candidate_path}; } +#ifdef USE_OLD_ARCHITECTURE std::pair getLaneChangePaths( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, const Pose & pose, const Twist & twist, @@ -275,6 +262,15 @@ std::pair getLaneChangePaths( const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, const double check_distance, LaneChangePaths * candidate_paths, std::unordered_map * debug_data) +#else +std::pair getLaneChangePaths( + const PathWithLaneId & original_path, const RouteHandler & route_handler, + const lanelet::ConstLanelets & original_lanelets, const lanelet::ConstLanelets & target_lanelets, + const Pose & pose, const Twist & twist, const PredictedObjects::ConstSharedPtr dynamic_objects, + const BehaviorPathPlannerParameters & common_parameter, const LaneChangeParameters & parameter, + const double check_distance, LaneChangePaths * candidate_paths, + std::unordered_map * debug_data) +#endif { debug_data->clear(); if (original_lanelets.empty() || target_lanelets.empty()) { @@ -286,9 +282,8 @@ std::pair getLaneChangePaths( // rename parameter const auto backward_path_length = common_parameter.backward_path_length; const auto forward_path_length = common_parameter.forward_path_length; - const auto lane_change_prepare_duration = parameter.lane_change_prepare_duration; - const auto minimum_lane_change_prepare_distance = - common_parameter.minimum_lane_change_prepare_distance; + const auto prepare_duration = parameter.lane_change_prepare_duration; + const auto minimum_prepare_distance = common_parameter.minimum_lane_change_prepare_distance; const auto minimum_lane_change_velocity = parameter.minimum_lane_change_velocity; const auto maximum_deceleration = parameter.maximum_deceleration; const auto lane_change_sampling_num = parameter.lane_change_sampling_num; @@ -335,8 +330,7 @@ std::pair getLaneChangePaths( candidate_paths->reserve(lane_change_sampling_num); for (double acceleration = 0.0; acceleration >= -maximum_deceleration; acceleration -= acceleration_resolution) { - const auto prepare_speed = getExpectedVelocityWhenDecelerate( - current_velocity, acceleration, lane_change_prepare_duration); + const double prepare_speed = current_velocity + acceleration * prepare_duration; // skip if velocity becomes less than zero before starting lane change if (prepare_speed < 0.0) { @@ -344,63 +338,67 @@ std::pair getLaneChangePaths( } // get path on original lanes - const auto prepare_distance = getDistanceWhenDecelerate( - current_velocity, acceleration, lane_change_prepare_duration, - minimum_lane_change_prepare_distance); + const double prepare_distance = std::max( + current_velocity * prepare_duration + 0.5 * acceleration * std::pow(prepare_duration, 2), + minimum_prepare_distance); if (prepare_distance < target_distance) { - continue; + break; } - const auto prepare_segment_reference = getLaneChangePathPrepareSegment( +#ifdef USE_OLD_ARCHITECTURE + const auto prepare_segment = getPrepareSegment( route_handler, original_lanelets, arc_position_from_current.length, backward_path_length, prepare_distance, std::max(prepare_speed, minimum_lane_change_velocity)); +#else + const auto prepare_segment = getPrepareSegment( + original_path, original_lanelets, pose, backward_path_length, prepare_distance, + std::max(prepare_speed, minimum_lane_change_velocity)); +#endif - const auto estimated_shift_length = lanelet::utils::getArcCoordinates( - target_lanelets, prepare_segment_reference.points.front().point.pose); + const auto estimated_shift_length = util::calcLateralDistanceToLanelet( + target_lanelets, prepare_segment.points.front().point.pose); - const auto [lane_changing_speed, lane_changing_distance] = - calcLaneChangingSpeedAndDistanceWhenDecelerate( - prepare_speed, estimated_shift_length.distance, acceleration, end_of_lane_dist, - common_parameter, parameter); + const auto [lane_changing_speed, lane_changing_distance] = calcLaneChangingSpeedAndDistance( + prepare_speed, estimated_shift_length, acceleration, end_of_lane_dist, common_parameter, + parameter); const auto lc_dist = LaneChangePhaseInfo{prepare_distance, lane_changing_distance}; - const auto lane_changing_segment_reference = getLaneChangePathLaneChangingSegment( + const auto lane_changing_segment = getLaneChangingSegment( route_handler, target_lanelets, forward_path_length, arc_position_from_target.length, target_lane_length, lc_dist, lane_changing_speed, required_total_min_distance); - if ( - prepare_segment_reference.points.empty() || lane_changing_segment_reference.points.empty()) { + if (prepare_segment.points.empty() || lane_changing_segment.points.empty()) { RCLCPP_ERROR_STREAM( rclcpp::get_logger("behavior_path_planner").get_child("lane_change").get_child("util"), "reference path is empty!! something wrong..."); continue; } - const auto & lane_changing_start_pose = prepare_segment_reference.points.back().point.pose; + const auto & lane_changing_start_pose = prepare_segment.points.back().point.pose; const auto resample_interval = - calcLaneChangeResamplingInterval(lane_changing_distance, lane_changing_speed); + calcLaneChangeResampleInterval(lane_changing_distance, lane_changing_speed); const auto target_lane_reference_path = getReferencePathFromTargetLane( - route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, lc_dist, - required_total_min_distance, forward_path_length, resample_interval, is_goal_in_route); + route_handler, target_lanelets, lane_changing_start_pose, target_lane_length, + lc_dist.lane_changing, required_total_min_distance, forward_path_length, resample_interval, + is_goal_in_route); if (target_lane_reference_path.points.empty()) { continue; } - const auto shift_line = getLaneChangeShiftLine( - prepare_segment_reference, lane_changing_segment_reference, target_lanelets, - target_lane_reference_path); + const auto shift_line = getLaneChangingShiftLine( + prepare_segment, lane_changing_segment, target_lanelets, target_lane_reference_path); const auto lc_speed = LaneChangePhaseInfo{prepare_speed, lane_changing_speed}; const auto candidate_path = constructCandidatePath( - prepare_segment_reference, lane_changing_segment_reference, target_lane_reference_path, - shift_line, original_lanelets, target_lanelets, sorted_lane_ids, acceleration, lc_dist, - lc_speed, parameter); + prepare_segment, lane_changing_segment, target_lane_reference_path, shift_line, + original_lanelets, target_lanelets, sorted_lane_ids, acceleration, lc_dist, lc_speed, + parameter); if (!candidate_path) { continue; @@ -610,35 +608,34 @@ bool isLaneChangePathSafe( return true; } -ShiftLine getLaneChangeShiftLine( - const PathWithLaneId & path1, const PathWithLaneId & path2, +ShiftLine getLaneChangingShiftLine( + const PathWithLaneId & prepare_segment, const PathWithLaneId & lane_changing_segment, const lanelet::ConstLanelets & target_lanes, const PathWithLaneId & reference_path) { - const Pose & lane_change_start_on_self_lane = path1.points.back().point.pose; - const Pose & lane_change_end_on_target_lane = path2.points.front().point.pose; - const ArcCoordinates lane_change_start_on_self_lane_arc = - lanelet::utils::getArcCoordinates(target_lanes, lane_change_start_on_self_lane); + const Pose & lane_changing_start_pose = prepare_segment.points.back().point.pose; + const Pose & lane_changing_end_pose = lane_changing_segment.points.front().point.pose; ShiftLine shift_line; - shift_line.end_shift_length = lane_change_start_on_self_lane_arc.distance; - shift_line.start = lane_change_start_on_self_lane; - shift_line.end = lane_change_end_on_target_lane; + shift_line.end_shift_length = + util::calcLateralDistanceToLanelet(target_lanes, lane_changing_start_pose); + shift_line.start = lane_changing_start_pose; + shift_line.end = lane_changing_end_pose; shift_line.start_idx = - motion_utils::findNearestIndex(reference_path.points, lane_change_start_on_self_lane.position); + motion_utils::findNearestIndex(reference_path.points, lane_changing_start_pose.position); shift_line.end_idx = - motion_utils::findNearestIndex(reference_path.points, lane_change_end_on_target_lane.position); + motion_utils::findNearestIndex(reference_path.points, lane_changing_end_pose.position); RCLCPP_DEBUG( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") .get_child("util") - .get_child("getLaneChangeShiftLine"), + .get_child("getLaneChangingShiftLine"), "shift_line distance: %f", util::getSignedDistance(shift_line.start, shift_line.end, target_lanes)); return shift_line; } -PathWithLaneId getLaneChangePathPrepareSegment( +PathWithLaneId getPrepareSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & original_lanelets, const double arc_length_from_current, const double backward_path_length, const double prepare_distance, const double prepare_speed) @@ -654,7 +651,7 @@ PathWithLaneId getLaneChangePathPrepareSegment( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") .get_child("util") - .get_child("getLaneChangePathPrepareSegment"), + .get_child("getPrepareSegment"), "start: %f, end: %f", s_start, s_end); PathWithLaneId prepare_segment = @@ -667,7 +664,28 @@ PathWithLaneId getLaneChangePathPrepareSegment( return prepare_segment; } -std::pair calcLaneChangingSpeedAndDistanceWhenDecelerate( +PathWithLaneId getPrepareSegment( + const PathWithLaneId & original_path, const lanelet::ConstLanelets & original_lanelets, + const Pose & current_pose, const double backward_path_length, const double prepare_distance, + const double prepare_speed) +{ + if (original_lanelets.empty()) { + return PathWithLaneId(); + } + + auto prepare_segment = original_path; + const size_t current_seg_idx = motion_utils::findFirstNearestSegmentIndexWithSoftConstraints( + prepare_segment.points, current_pose, 3.0, 1.0); + util::clipPathLength(prepare_segment, current_seg_idx, prepare_distance, backward_path_length); + + prepare_segment.points.back().point.longitudinal_velocity_mps = std::min( + prepare_segment.points.back().point.longitudinal_velocity_mps, + static_cast(prepare_speed)); + + return prepare_segment; +} + +std::pair calcLaneChangingSpeedAndDistance( const double velocity, const double shift_length, const double deceleration, const double min_total_lc_len, const BehaviorPathPlannerParameters & com_param, const LaneChangeParameters & lc_param) @@ -685,14 +703,14 @@ std::pair calcLaneChangingSpeedAndDistanceWhenDecelerate( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") .get_child("util") - .get_child("calcLaneChangingSpeedAndDistanceWhenDecelerate"), + .get_child("calcLaneChangingSpeedAndDistance"), "required_time: %f [s] average_speed: %f [m/s], lane_changing_distance : %f [m]", required_time, lane_changing_average_speed, lane_changing_distance); return {lane_changing_average_speed, lane_changing_distance}; } -PathWithLaneId getLaneChangePathLaneChangingSegment( +PathWithLaneId getLaneChangingSegment( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanelets, const double forward_path_length, const double arc_length_from_target, const double target_lane_length, const LaneChangePhaseInfo dist_prepare_to_lc_end, @@ -717,7 +735,7 @@ PathWithLaneId getLaneChangePathLaneChangingSegment( rclcpp::get_logger("behavior_path_planner") .get_child("lane_change") .get_child("util") - .get_child("getLaneChangePathLaneChangingSegment"), + .get_child("getLaneChangingSegment"), "start: %f, end: %f", s_start, s_end); PathWithLaneId lane_changing_segment = @@ -733,7 +751,7 @@ PathWithLaneId getLaneChangePathLaneChangingSegment( PathWithLaneId getReferencePathFromTargetLane( const RouteHandler & route_handler, const lanelet::ConstLanelets & target_lanes, const Pose & lane_changing_start_pose, const double target_lane_length, - const LaneChangePhaseInfo dist_prepare_to_lc_end, const double min_total_lane_changing_distance, + const double lane_changing_distance, const double min_total_lane_changing_distance, const double forward_path_length, const double resample_interval, const bool is_goal_in_route) { const ArcCoordinates lane_change_start_arc_position = @@ -741,7 +759,7 @@ PathWithLaneId getReferencePathFromTargetLane( const double s_start = lane_change_start_arc_position.length; const double s_end = std::invoke([&]() { - const auto dist_from_lc_start = s_start + dist_prepare_to_lc_end.sum() + forward_path_length; + const auto dist_from_lc_start = s_start + lane_changing_distance + forward_path_length; if (is_goal_in_route) { const auto goal_arc_coordinates = lanelet::utils::getArcCoordinates(target_lanes, route_handler.getGoalPose()); @@ -763,8 +781,7 @@ PathWithLaneId getReferencePathFromTargetLane( route_handler.getCenterLinePath(target_lanes, s_start, s_end); return util::resamplePathWithSpline( - lane_changing_reference_path, resample_interval, true, - {0.0, dist_prepare_to_lc_end.lane_changing}); + lane_changing_reference_path, resample_interval, true, {0.0, lane_changing_distance}); } bool isEgoWithinOriginalLane( @@ -1125,4 +1142,14 @@ double calcLateralBufferForFiltering(const double vehicle_width, const double la return lateral_buffer + 0.5 * vehicle_width; } +std::string getStrDirection(const std::string & name, const Direction direction) +{ + if (direction == Direction::LEFT) { + return name + "_left"; + } + if (direction == Direction::RIGHT) { + return name + "_right"; + } + return ""; +} } // namespace behavior_path_planner::lane_change_utils diff --git a/planning/behavior_path_planner/src/utilities.cpp b/planning/behavior_path_planner/src/utilities.cpp index 9160e4aef11bc..de43052b79662 100644 --- a/planning/behavior_path_planner/src/utilities.cpp +++ b/planning/behavior_path_planner/src/utilities.cpp @@ -1975,7 +1975,11 @@ PathWithLaneId getCenterLinePath( s_forward = std::min(s_forward, goal_arc_coordinates.length - lane_change_buffer); } - return route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true); + const auto raw_path_with_lane_id = + route_handler.getCenterLinePath(lanelet_sequence, s_backward, s_forward, true); + const auto resampled_path_with_lane_id = motion_utils::resamplePath( + raw_path_with_lane_id, parameter.input_path_interval, parameter.enable_akima_spline_first); + return raw_path_with_lane_id; } // for lane following @@ -2687,4 +2691,16 @@ lanelet::ConstLanelets getLaneletsFromPath( return lanelets; } + +double calcLateralDistanceToLanelet( + const lanelet::ConstLanelets & lanelet_sequence, const geometry_msgs::msg::Pose & pose) +{ + lanelet::ConstLanelet closest_lanelet; + lanelet::utils::query::getClosestLanelet(lanelet_sequence, pose, &closest_lanelet); + const auto & centerline_2d = lanelet::utils::to2D(closest_lanelet.centerline()); + + const auto lanelet_point = lanelet::utils::conversion::toLaneletPoint(pose.position); + return lanelet::geometry::signedDistance( + centerline_2d, lanelet::utils::to2D(lanelet_point).basicPoint()); +} } // namespace behavior_path_planner::util diff --git a/planning/freespace_planning_algorithms/src/astar_search.cpp b/planning/freespace_planning_algorithms/src/astar_search.cpp index 5686d7eee26e0..b2b2321adacd0 100644 --- a/planning/freespace_planning_algorithms/src/astar_search.cpp +++ b/planning/freespace_planning_algorithms/src/astar_search.cpp @@ -76,13 +76,13 @@ AstarSearch::TransitionTable createTransitionTable( const auto & R_min = minimum_turning_radius; const auto & R_max = maximum_turning_radius; const double step_min = R_min * dtheta; - const double dR = (R_max - R_min) / turning_radius_size; + const double dR = (R_max - R_min) / std::max(turning_radius_size - 1, 1); // NodeUpdate actions std::vector forward_node_candidates; const NodeUpdate forward_straight{step_min, 0.0, 0.0, step_min, false, false}; forward_node_candidates.push_back(forward_straight); - for (int i = 0; i < turning_radius_size + 1; ++i) { + for (int i = 0; i < turning_radius_size; ++i) { double R = R_min + i * dR; double step = R * dtheta; const NodeUpdate forward_left{ diff --git a/planning/obstacle_avoidance_planner/CMakeLists.txt b/planning/obstacle_avoidance_planner/CMakeLists.txt index 89fa09e23dce4..c851de40aa9ec 100644 --- a/planning/obstacle_avoidance_planner/CMakeLists.txt +++ b/planning/obstacle_avoidance_planner/CMakeLists.txt @@ -49,6 +49,11 @@ rclcpp_components_register_node(obstacle_avoidance_planner ament_auto_package( INSTALL_TO_SHARE launch - scripts config + rviz +) + +install(PROGRAMS + scripts/calclation_time_plotter.py + DESTINATION lib/${PROJECT_NAME} ) diff --git a/planning/obstacle_avoidance_planner/README.md b/planning/obstacle_avoidance_planner/README.md index 1a03b695aa699..200e1f9d26eb0 100644 --- a/planning/obstacle_avoidance_planner/README.md +++ b/planning/obstacle_avoidance_planner/README.md @@ -132,6 +132,11 @@ Some examples are shown in the following figure, and it is shown that the trajec More details can be seen [here](docs/mpt.md). +### applyInputVelocity + +Velocity is assigned in the optimized trajectory from the velocity in the behavior path. +The shapes of the optimized trajectory and the path are different, therefore the each nearest trajectory point to the path is searched and the velocity is interpolated with zero-order hold. + ### insertZeroVelocityOutsideDrivableArea Optimized trajectory is too short for velocity planning, therefore extend the trajectory by concatenating the optimized trajectory and the behavior path considering drivability. @@ -155,11 +160,6 @@ _Rationale_ In the current design, since there are some modelling errors, the constraints are considered to be soft constraints. Therefore, we have to make sure that the optimized trajectory is inside the drivable area or not after optimization. -### alignVelocity - -Velocity is assigned in the result trajectory from the velocity in the behavior path. -The shapes of the trajectory and the path are different, therefore the each nearest trajectory point to the path is searched and interpolated linearly. - ## Limitation - Computation cost is sometimes high. @@ -192,41 +192,21 @@ Although it has a cons to converge to the local minima, it can get a good soluti ### Drivability in narrow roads -- set `option.drivability_check.use_vehicle_circles` true - - use a set of circles as a shape of the vehicle when checking if the generated trajectory will be outside the drivable area. -- make `mpt.clearance.soft_clearance_from_road` smaller -- make `mpt.kinematics.optimization_center_offset` different +- modify `mpt.clearance.soft_clearance_from_road` + - This parameter describes how much margin to make between the trajectory and road boundaries. + - Due to the model error for optimization, the constraint such as collision-free is not fully met. + - By making this parameter larger, the is for narrow-road driving may be resolved. 12180 +- modify `mpt.kinematics.optimization_center_offset` - - The point on the vehicle, offset forward from the base link` tries to follow the reference path. + - The point on the vehicle, offset forward with this parameter from the base link` tries to follow the reference path. - - This may cause the a part of generated trajectory will be outside the drivable area. +- change or tune the method to approximate footprints with a set of circles. + - See [here](https://autowarefoundation.github.io/autoware.universe/main/planning/obstacle_avoidance_planner/docs/mpt/#collision-free) + - Tuning means changing the ratio of circle's radius. ### Computation time -- Loose EB optimization - - - 1. make `eb.common.delta_arc_length` large and `eb.common.num_points` small - - This makes the number of design variables smaller - - Be careful about the trajectory length between MPT and EB as shown in Assumptions. - - However, empirically this causes large turn at the corner (e.g. The vehicle turns a steering wheel to the opposite side (=left) a bit just before the corner turning to right) - - 2. make `eb.qp.eps_abs` and `eb.qp.eps_rel` small - - This causes very unstable reference path generation for MPT, or turning a steering wheel a little bit larger - -- Enable computation reduction flag - - - 1. set l_inf_norm true (by default) - - use L-inf norm optimization for MPT w.r.t. slack variables, resulting in lower number of design variables - - 2. set enable_warm_start true - - 3. set enable_manual_warm_start true (by default) - - 4. set steer_limit_constraint false - - This causes no assumption for trajectory generation where steering angle will not exceeds its hardware limitation - - 5. make the number of collision-free constraints small - - How to change parameters depend on the type of collision-free constraints - - If - - This may cause the trajectory generation where a part of ego vehicle is out of drivable area - -- Disable publishing debug visualization markers - - set `option.is_publishing_*` false +- under construction ### Robustness @@ -243,10 +223,6 @@ Although it has a cons to converge to the local minima, it can get a good soluti - EB is not required if the reference path for MPT is smooth enough and does not change its shape suddenly - `option.enable_calculation_time_info` enables showing each calculation time for functions and total calculation time on the terminal. - `option.enable_outside_drivable_area_stop` enables stopping just before the generated trajectory point will be outside the drivable area. -- `mpt.option.plan_from_ego` enables planning from the ego pose when the ego's velocity is zero. -- `mpt.option.max_plan_from_ego_length` maximum length threshold to plan from ego. it is enabled when the length of trajectory is shorter than this value. -- `mpt.option.two_step_soft_constraint` enables two step of soft constraints for collision free - - `mpt.option.soft_clearance_from_road` and `mpt.option.soft_second_clearance_from_road` are the weight. ## How To Debug diff --git a/planning/obstacle_avoidance_planner/docs/debug.md b/planning/obstacle_avoidance_planner/docs/debug.md index 4c8b6eb910bcc..9ee18b732d375 100644 --- a/planning/obstacle_avoidance_planner/docs/debug.md +++ b/planning/obstacle_avoidance_planner/docs/debug.md @@ -2,116 +2,189 @@ ## Debug visualization -Topics for debugging will be explained in this section. +The visualization markers of the planning flow (Input, Elastic Band, Model Predictive Trajectory, and Output) are explained here. -- **Drivable area** - - Drivable area to cover the road. Whether this area is continuous and covers the road can be checked. - - `/planning/scenario_planning/lane_driving/behavior_planning/behavior_path_planner/debug/drivable_area`, whose type is `nav_msgs/msg/OccupancyGrid` +All the following markers can be visualized by -![drivable_area](../media/drivable_area.png) +```bash +ros2 launch obstacle_avoidance_planner launch_visualiation.launch.xml vehilce_model:=sample_vehicle +``` + +The `vehicle_model` must be specified to make footprints with vehicle's size. + +### Input + +- **Path** + - The path generated in the `behavior` planner. + - The semitransparent and thick, green and red band, that is visualized by default. + +![path](../media/debug/path_visualization.png) + +- **Path Footprint** + - The path generated in the `behavior` planner is converted to footprints. + - NOTE: + - Check if there is no high curvature. + - The path may be outside the drivable area in some cases, but it is okay to ignore it since the `behavior` planner does not support it. + +![path_footprint](../media/debug/path_footprint_visualization.png) + +- **Drivalbe Area** + - The Drivable area generated in the `behavior` planner. + - The skyblue left and right line strings, that is visualized by default. + - NOTE: + - Check if the path is almost inside the drivable area. + - Then, the `obstacle_avoidance_planner` will try to make the trajectory fully inside the drivable area. + - During avoidance or lane change by the `behavior` planner, please make sure that the drivable area is expanded correctly. + +![drivable_area](../media/debug/drivable_area_visualization.png) + +### Elastic Band (EB) + +- **EB Fixed Trajectory** + - The fixed trajectory points as a constraint of elastic band. + +![eb_fixed_traj](../media/debug/eb_fixed_traj_visualization.png) + +- **EB Trajectory** + - The optimized trajectory points by elastic band. + +![eb_traj](../media/debug/eb_traj_visualization.png) + +### Model Predictive Trajectory (MPT) + +- **MPT Reference Trajectory** + - The reference trajectory points of model predictive trajectory. + +![mpt_ref_traj](../media/debug/mpt_ref_traj_visualization.png) + +- **MPT Fixed Trajectory** + - The fixed trajectory points as a constraint of model predictive trajectory. -- **Path from behavior** - - The input path of obstacle_avoidance_planner. Whether this path is continuous and the curvature is not so high can be checked. - - `Path` or `PathFootprint` rviz plugin. +![mpt_fixed_traj](../media/debug/mpt_fixed_traj_visualization.png) -![behavior_path](../media/behavior_path.png) +- **Boundaries' Width** + - The boundaries' width is calculated from the drivable area line strings. -- **EB trajectory** - - The output trajectory of elastic band. Whether this trajectory is very smooth and a sampling width is constant can be checked. - - `Trajectory` or `TrajectoryFootprint` rviz plugin. +![bounds](../media/debug/bounds_visualization.png) -![eb_traj](../media/eb_traj.png) +- **Vehicle Circles** + - The vehicle's shape is represented by a set of circles. + - The `obstcle_avoidance_planner` will try to make the these circles inside the above boundaries' width. -- **MPT reference trajectory** - - The reference trajectory of model predictive trajectory. Whether this trajectory is very smooth and a sampling width is constant can be checked. - - `Trajectory` or `TrajectoryFootprint` rviz plugin. +![vehicle_circles](../media/debug/vehicle_circles_visualization.png) -![mpt_ref_traj](../media/mpt_ref_traj.png) +- **Vehicle Circles on Trajectory** + - The vehicle's circles on the MPT trajectory. + - Check if the circles are not so big compared to the road's width. -- **MPT fixed trajectory** - - The fixed trajectory around the ego of model predictive trajectory. - - `Trajectory` or `TrajectoryFootprint` rviz plugin. +![vehicle_traj_circles](../media/debug/vehicle_traj_circles_visualization.png) -![mpt_fixed_traj](../media/mpt_fixed_traj.png) +- **MPT Trajectory** + - The optimized trajectory points by model predictive trajectory. + - The footprints are supposed to be fully inside the drivable area. -- **bounds** - - Lateral Distance to the road or object boundaries to check collision in model predictive trajectory. - - Whether these lines' ends align the road or obstacle boundaries can be checked. - - `bounds*` of `/planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker` whose type is `visualization_msgs/msg/MarkerArray` +![mpt_traj](../media/debug/mpt_traj_visualization.png) -![bounds](../media/bounds.png) +### Output -- **MPT trajectory** - - The output of model predictive trajectory. Whether this trajectory is smooth enough and inside the drivable area can be checked. - - `Trajectory` or `TrajectoryFootprint` rviz plugin. +- **Trajectory** + - The output trajectory. + - The dark and thin, green and red band, that is visualized by default. -![mpt_traj](../media/mpt_traj.png) +![traj](../media/debug/traj_visualization.png) -- **Output trajectory** - - The output of obstacle_avoidance_planner. Whether this trajectory is smooth enough can be checked. - - `Trajectory` or `TrajectoryFootprint` rviz plugin. +- **Trajectory Footprint** + - The output trajectory is converted to footprints. -![output_traj](../media/output_traj.png) +![traj_footprint](../media/debug/traj_footprint_visualization.png) -## Calculation cost +## Calculation time -Obstacle avoidance planner consists of many functions such as clearance map generation, boundary search, reference path smoothing, trajectory optimization, ... +The `obstacle_avoidance_planner` consists of many functions such as boundaries' width calculation, reference path smoothing, collision-free planning, etc. We can see the calculation time for each function as follows. ### Raw data +Enable `option.enable_calculation_time_info` or echo the topic as follows. + ```sh $ ros2 topic echo /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/calculation_time --field data - - getDrivableAreaInCV:= 0.21 [ms] - getClearanceMap:= 1.327 [ms] - drawObstaclesOnImage:= 0.436 [ms] - getAreaWithObjects:= 0.029 [ms] - getClearanceMap:= 2.186 [ms] - getMaps:= 4.213 [ms] - calculateTrajectory:= 2.417 [ms] - getOptimizedTrajectory:= 5.203 [ms] - getEBTrajectory:= 5.231 [ms] - calcBounds:= 0.821 [ms] - calcVehicleBounds:= 0.27 [ms] - getReferencePoints:= 2.866 [ms] - generateMPTMatrix:= 0.195 [ms] - generateValueMatrix:= 0.019 [ms] - getObjectiveMatrix:= 0.559 [ms] - getConstraintMatrix:= 1.776 [ms] - initOsqp:= 9.074 [ms] - solveOsqp:= 3.809 [ms] - executeOptimization:= 15.46 [ms] - getMPTPoints:= 0.049 [ms] - getModelPredictiveTrajectory:= 18.928 [ms] - optimizeTrajectory:= 24.234 [ms] - insertZeroVelocityOutsideDrivableArea:= 0.023 [ms] - getDebugVisualizationMarker:= 0.446 [ms] - publishDebugVisualizationMarker:= 2.146 [ms] - getDebugVisualizationWallMarker:= 0.001 [ms] - publishDebugVisualizationWallMarker:= 0.013 [ms] - publishDebugDataInOptimization:= 2.696 [ms] - getExtendedTrajectory:= 0.016 [ms] - generateFineTrajectoryPoints:= 0.118 [ms] - alignVelocity:= 1.227 [ms] - generatePostProcessedTrajectory:= 1.375 [ms] - makePrevTrajectories:= 1.411 [ms] - generateOptimizedTrajectory:= 33.284 [ms] - getExtendedTrajectory:= 0.018 [ms] - generateFineTrajectoryPoints:= 0.084 [ms] - alignVelocity:= 1.109 [ms] - generatePostProcessedTrajectory:= 1.217 [ms] - getDebugCostMap * 3:= 0 [ms] - publishDebugDataInMain:= 0.023 [ms] -pathCallback:= 34.614 [ms] +--- + insertFixedPoint:= 0.008 [ms] + getPaddedTrajectoryPoints:= 0.002 [ms] + updateConstraint:= 0.741 [ms] + optimizeTrajectory:= 0.101 [ms] + convertOptimizedPointsToTrajectory:= 0.014 [ms] + getEBTrajectory:= 0.991 [ms] + resampleReferencePoints:= 0.058 [ms] + updateFixedPoint:= 0.237 [ms] + updateBounds:= 0.22 [ms] + updateVehicleBounds:= 0.509 [ms] + calcReferencePoints:= 1.649 [ms] + calcMatrix:= 0.209 [ms] + calcValueMatrix:= 0.015 [ms] + calcObjectiveMatrix:= 0.305 [ms] + calcConstraintMatrix:= 0.641 [ms] + initOsqp:= 6.896 [ms] + solveOsqp:= 2.796 [ms] + calcOptimizedSteerAngles:= 9.856 [ms] + calcMPTPoints:= 0.04 [ms] + getModelPredictiveTrajectory:= 12.782 [ms] + optimizeTrajectory:= 12.981 [ms] + applyInputVelocity:= 0.577 [ms] + insertZeroVelocityOutsideDrivableArea:= 0.81 [ms] + getDebugMarker:= 0.684 [ms] + publishDebugMarker:= 4.354 [ms] + publishDebugMarkerOfOptimization:= 5.047 [ms] + generateOptimizedTrajectory:= 20.374 [ms] + extendTrajectory:= 0.326 [ms] + publishDebugData:= 0.008 [ms] +onPath:= 20.737 [ms] ``` ### Plot -With a script, we can plot some of above time as follows. +With the following script, any calculation time of the above functions can be plot. ```sh -python3 scripts/calclation_time_analyzer.py -f "solveOsqp,initOsqp,pathCallback" +ros2 run obstacle_avoidance_planner calclation_time_plotter.py ``` -![calculation_cost_plot](../media/calculation_cost_plot.svg) +![calculation_time_plot](../media/debug/calculation_time_plot.png) + +You can specify functions to plot with the `-f` option. + +```sh +ros2 run obstacle_avoidance_planner calclation_time_plotter.py -f "onPath, generateOptimizedTrajectory, calcReferencePoints" +``` + +## Q&A for Debug + +### The output frequency is low + +Check the function which is comparatively heavy according to [this information](.#calculation-time). + +For your information, the following functions for optimization and its initialization may be heavy in some complicated cases. + +- MPT + - `initOsqp` + - `solveOsqp` +- EB + - `optimizeTrajectory` + +### When a part of the trajectory has high curvature + +Some of the following may have an issue. +Please check if there is something weird by the visualization. + +- Input Path +- Drivable Area +- Boundaries' Width + +### When the trajectory's shape is zigzag + +Some of the following may have an issue. +Please check if there is something weird by the visualization. + +- EB Trajectory +- Vehicle Circles on Trajectory diff --git a/planning/obstacle_avoidance_planner/docs/eb.md b/planning/obstacle_avoidance_planner/docs/eb.md index 6becc8b03cc24..7efb1edaa62db 100644 --- a/planning/obstacle_avoidance_planner/docs/eb.md +++ b/planning/obstacle_avoidance_planner/docs/eb.md @@ -16,25 +16,64 @@ title getEBTrajectory start :crop trajectory; +note right +Forward length is eb.common.num_points * eb.common.delta_arc_length. +Backward length is common.output_backward_traj_length. +end note :insertFixedPoint; :resample trajectory; +note right +Resampling interval is eb.common.delta_arc_length. +end note -:getPaddedTrajectoryPoints +:getPaddedTrajectoryPoints; +note right +Pad the last optimization point so that the number will be always eb.common.num_points. +This is for enabling warm start. +end note :updateConstraint; :optimizeTrajectory; :convertOptimizedPointsToTrajectory; +note right +The validation function is contained. +When eb.option.enable_optimization_validation is true, the optimization result will be ignored if the validation fails. +end note stop @enduml ``` +### General parameters + +| Parameter | Type | Description | +| ---------------------------- | ------ | ---------------------------------------------- | +| `eb.common.num_points` | int | points for elastic band optimization | +| `eb.common.delta_arc_length` | double | delta arc length for elastic band optimization | + +### Parameters for optimization + +| Parameter | Type | Description | +| ----------------------------- | ------ | --------------------------------------- | +| `eb.option.enable_warm_start` | bool | flag to use warm start | +| `eb.weight.smooth_weight` | double | weight for smoothing | +| `eb.weight.lat_error_weight` | double | weight for minimizing the lateral error | + +### Parameters for validation + +| Parameter | Type | Description | +| ------------------------------------------ | ------ | --------------------------------- | +| `eb.option.enable_optimization_validation` | bool | flag to validate optimization | +| `eb.validation.max_error` | double | max lateral error by optimization | + ## Formulation +### Objective function + We formulate a quadratic problem minimizing the diagonal length of the rhombus on each point generated by the current point and its previous and next points, shown as the red vector's length. ![eb](../media/eb.svg){: style="width:600px"} @@ -102,7 +141,9 @@ $$ \end{align} $$ -Regarding the constraint, the distance that each point can move is limited so that the path will not changed a lot but will be smoother. +### Constraint + +The distance that each point can move is limited so that the path will not changed a lot but will be smoother. In detail, the longitudinal distance that each point can move is zero, and the lateral distance is parameterized as `eb.clearance.clearance_for_fix`, `eb.clearance.clearance_for_joint` and `eb.clearance.clearance_for_smooth`. The following figure describes how to constrain the lateral distance to move. diff --git a/planning/obstacle_avoidance_planner/docs/mpt.md b/planning/obstacle_avoidance_planner/docs/mpt.md index 6c07244bdb45e..f76115b8da1cb 100644 --- a/planning/obstacle_avoidance_planner/docs/mpt.md +++ b/planning/obstacle_avoidance_planner/docs/mpt.md @@ -443,42 +443,3 @@ $$ \in \boldsymbol{R}^{3N_{ref} \times D_v + N_{ref}} \end{align} $$ - -#### Two-step soft constraints - -$$ -\begin{align} -\boldsymbol{v}' = - \begin{pmatrix} - \boldsymbol{v} \\ - \boldsymbol{\lambda}^{soft_1} \\ - \boldsymbol{\lambda}^{soft_2} \\ - \end{pmatrix} - \in \boldsymbol{R}^{D_v + 2N_{slack}} -\end{align} -$$ - -$*$ depends on whether to use L2 norm or L-infinity optimization. - -$$ -\begin{align} - A_{blk} & = - \begin{pmatrix} - A^{soft_1}_{blk} \\ - A^{soft_2}_{blk} \\ - \end{pmatrix}\\ - & = - \begin{pmatrix} - C_1^{soft_1} B & & \\ - -C_1^{soft_1} B & \Huge{*} & \Huge{O} \\ - O & & \\ - C_1^{soft_2} B & & \\ - -C_1^{soft_2} B & \Huge{O} & \Huge{*} \\ - O & & - \end{pmatrix} - \in \boldsymbol{R}^{6 N_{ref} \times D_v + 2 N_{slack}} -\end{align} -$$ - -$N_{slack}$ is $N_{circle}$ when L2 optimization, or $1$ when L-infinity optimization. -$N_{circle}$ is the number of circles to check collision. diff --git a/planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml b/planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml new file mode 100644 index 0000000000000..d8320e09cc001 --- /dev/null +++ b/planning/obstacle_avoidance_planner/launch/launch_visualization.launch.xml @@ -0,0 +1,18 @@ + + + + + + + + + + + + + + + + + + diff --git a/planning/obstacle_avoidance_planner/media/area_with_objects.png b/planning/obstacle_avoidance_planner/media/area_with_objects.png deleted file mode 100644 index 41a950452c783..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/area_with_objects.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/behavior_path.png b/planning/obstacle_avoidance_planner/media/behavior_path.png deleted file mode 100644 index cc99ffe44e1ec..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/behavior_path.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/bounds.png b/planning/obstacle_avoidance_planner/media/bounds.png deleted file mode 100644 index 54627b34f10ca..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/bounds.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/bounds_line.png b/planning/obstacle_avoidance_planner/media/bounds_line.png deleted file mode 100644 index 532d572c3d845..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/bounds_line.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/calculation_cost_plot.svg b/planning/obstacle_avoidance_planner/media/calculation_cost_plot.svg deleted file mode 100644 index 54f119daffbd2..0000000000000 --- a/planning/obstacle_avoidance_planner/media/calculation_cost_plot.svg +++ /dev/null @@ -1,1257 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - diff --git a/planning/obstacle_avoidance_planner/media/clearance_map.png b/planning/obstacle_avoidance_planner/media/clearance_map.png deleted file mode 100644 index c9bfc70ec01b1..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/clearance_map.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png b/planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png new file mode 100644 index 0000000000000..af26553951ee9 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/bounds_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png b/planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png new file mode 100644 index 0000000000000..dafb2efabf546 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/calculation_time_plot.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png b/planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png new file mode 100644 index 0000000000000..7c9ff28b68318 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/drivable_area_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/eb_fixed_traj_visualization.png b/planning/obstacle_avoidance_planner/media/debug/eb_fixed_traj_visualization.png new file mode 100644 index 0000000000000..0577b8984fb9f Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/eb_fixed_traj_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/eb_traj_visualization.png b/planning/obstacle_avoidance_planner/media/debug/eb_traj_visualization.png new file mode 100644 index 0000000000000..9672423125bd6 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/eb_traj_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png b/planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png new file mode 100644 index 0000000000000..ed1dc2cf8a71d Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/mpt_fixed_traj_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png b/planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png new file mode 100644 index 0000000000000..2afd01f9cd4ad Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/mpt_ref_traj_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png b/planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png new file mode 100644 index 0000000000000..8cdbe8a3f46de Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/mpt_traj_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png b/planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png new file mode 100644 index 0000000000000..b7e2d64a96757 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/path_footprint_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/path_visualization.png b/planning/obstacle_avoidance_planner/media/debug/path_visualization.png new file mode 100644 index 0000000000000..7c9ff28b68318 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/path_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png b/planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png new file mode 100644 index 0000000000000..d4a04706ec286 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/traj_footprint_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/traj_visualization.png b/planning/obstacle_avoidance_planner/media/debug/traj_visualization.png new file mode 100644 index 0000000000000..7c9ff28b68318 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/traj_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png b/planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png new file mode 100644 index 0000000000000..9c15fca3548e6 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/vehicle_circles_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png b/planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png new file mode 100644 index 0000000000000..d9a748acc9c95 Binary files /dev/null and b/planning/obstacle_avoidance_planner/media/debug/vehicle_traj_circles_visualization.png differ diff --git a/planning/obstacle_avoidance_planner/media/drivable_area.png b/planning/obstacle_avoidance_planner/media/drivable_area.png deleted file mode 100644 index 6606299705143..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/drivable_area.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/eb_traj.png b/planning/obstacle_avoidance_planner/media/eb_traj.png deleted file mode 100644 index 9910810920bff..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/eb_traj.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/interpolated_points_marker.png b/planning/obstacle_avoidance_planner/media/interpolated_points_marker.png deleted file mode 100644 index ce69fa0d0fc2c..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/interpolated_points_marker.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png b/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png deleted file mode 100644 index 2344467811a83..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/mpt_fixed_traj.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png b/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png deleted file mode 100644 index 6129ac23dc008..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/mpt_ref_traj.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/mpt_traj.png b/planning/obstacle_avoidance_planner/media/mpt_traj.png deleted file mode 100644 index 44a06bc1ef9f6..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/mpt_traj.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/object_clearance_map.png b/planning/obstacle_avoidance_planner/media/object_clearance_map.png deleted file mode 100644 index 199c91c0e49a8..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/object_clearance_map.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/object_clearance_map_updates.png b/planning/obstacle_avoidance_planner/media/object_clearance_map_updates.png deleted file mode 100644 index 188ce0ef206a7..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/object_clearance_map_updates.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/obstacle_avoidance_planner_flowchart.drawio.svg b/planning/obstacle_avoidance_planner/media/obstacle_avoidance_planner_flowchart.drawio.svg deleted file mode 100644 index ee067e07b2f1a..0000000000000 --- a/planning/obstacle_avoidance_planner/media/obstacle_avoidance_planner_flowchart.drawio.svg +++ /dev/null @@ -1,445 +0,0 @@ - - - - - - - - - - -
-
-
- use previously -
- generated trajectory -
- when optimization failed -
-
-
- - use previously... - - - - - - - -
-
-
- trajectory -
-
-
- - trajectory - - - - - - -
-
-
- Callback of trajectory generation -
-
-
- - Callback of trajectory generation - - - - - - -
-
-
- Generate trajectory -
-
-
- - Generate trajectory - - - - - - - -
-
-
- trajectory -
-
-
- - trajectory - - - - - - -
-
-
- Smooth path -
- (Elastic Band) -
-
-
- - Smooth path... - - - - - - - -
-
-
- trajectory -
-
-
- - trajectory - - - - - - -
-
-
- Optimize trajectory -
- (Model predictive trajectory) -
-
-
- - Optimize trajectory... - - - - - - - -
-
-
- trajectory -
-
-
- - trajectory - - - - - - -
-
-
- Extend trajectory -
-
-
- - Extend trajectory - - - - - - - - -
-
-
- Select object to avoid -
-
-
- - Select object to avoid - - - - - - - - -
-
-
- Generate object costmap -
-
-
- - Generate object costmap - - - - - - - -
-
-
- costmap -
-
-
- - costmap - - - - - - -
-
-
- Generate road boundary costmap -
-
-
- - Generate road boundary... - - - - - - - -
-
-
- dynamic objects -
-
-
- - dynamic ob... - - - - - - - -
-
-
- path -
-
-
- - path - - - - - - -
-
-
- Check drivable area -
-
-
- - Check drivable area - - - - - - - -
-
-
- path -
-
-
- - path - - - - - - - -
-
-
- trajectory -
-
-
-
- - trajectory - - - - - - -
-
-
- Apply path velocity -
-
-
- - Apply path velocity - - - - - - - - -
-
-
- Manage trajectory generation -
-
-
- - Manage trajectory generation - - - - - - - -
-
-
- path -
-
-
- - path - - - - - - - -
-
-
- path -
-
-
- - path - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/planning/obstacle_avoidance_planner/media/obstacle_to_avoid.drawio.svg b/planning/obstacle_avoidance_planner/media/obstacle_to_avoid.drawio.svg deleted file mode 100644 index ac98f159b50cb..0000000000000 --- a/planning/obstacle_avoidance_planner/media/obstacle_to_avoid.drawio.svg +++ /dev/null @@ -1,205 +0,0 @@ - - - - - - - - - -
-
-
- id: 5 -
- v: 0 m/s -
-
-
- - id: 5... - - - - - - -
-
-
- id: 4 -
- v: 0 m/s -
-
-
- - id: 4... - - - - - - -
-
-
- id: 3 -
- v: 0 m/s -
-
-
- - id: 3... - - - - - - -
-
-
- - id: 0 -
- v: 5 m/s - -
-
-
- - id: 0... - - - - - - -
-
-
- id: 1 -
- v: 5 m/s -
-
-
- - id: 1... - - - - - - -
-
-
- - id: 2 -
- v: 5 m/s - -
-
-
- - id: 2... - - - - - - - - -
-
-
- center line -
- x[m] -
-
-
- - center... - - - - - - -
-
-
- ego -
- vehicle -
-
-
- - ego... - - - - - - -
-
-
- drivable area -
-
-
- - drivable area - - - - - - - - Viewer does not support full SVG 1.1 - - - diff --git a/planning/obstacle_avoidance_planner/media/optimized_points_marker.png b/planning/obstacle_avoidance_planner/media/optimized_points_marker.png deleted file mode 100644 index 456ff552654ab..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/optimized_points_marker.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/output_traj.png b/planning/obstacle_avoidance_planner/media/output_traj.png deleted file mode 100644 index e4cfae92d0a8f..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/output_traj.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/smoothed_points_text.png b/planning/obstacle_avoidance_planner/media/smoothed_points_text.png deleted file mode 100644 index 2e87a616fd021..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/smoothed_points_text.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/media/trajectory_with_footprint.png b/planning/obstacle_avoidance_planner/media/trajectory_with_footprint.png deleted file mode 100644 index 1dce802e1c27a..0000000000000 Binary files a/planning/obstacle_avoidance_planner/media/trajectory_with_footprint.png and /dev/null differ diff --git a/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz b/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz new file mode 100644 index 0000000000000..8e62886d255ea --- /dev/null +++ b/planning/obstacle_avoidance_planner/rviz/obstacle_avoidance_planner.rviz @@ -0,0 +1,1467 @@ +Panels: + - Class: rviz_common/Displays + Help Height: 0 + Name: Displays + Property Tree Widget: + Expanded: + - /Sensing1/LiDAR1/ConcatenatePointCloud1/Autocompute Value Bounds1 + - /Obstacle Avoidance Planner1 + - /Obstacle Avoidance Planner1/Input1 + - /Obstacle Avoidance Planner1/Input1/Path1/View Path1 + - /Obstacle Avoidance Planner1/Elastic Band1 + - /Obstacle Avoidance Planner1/Model Predictive Trajectory1 + - /Obstacle Avoidance Planner1/Output1 + Splitter Ratio: 0.557669460773468 + Tree Height: 385 + - Class: rviz_common/Selection + Name: Selection + - Class: rviz_common/Tool Properties + Expanded: ~ + Name: Tool Properties + Splitter Ratio: 0.5886790156364441 + - Class: rviz_common/Views + Expanded: + - /Current View1 + Name: Views + Splitter Ratio: 0.5 + - Class: tier4_localization_rviz_plugin/InitialPoseButtonPanel + Name: InitialPoseButtonPanel + - Class: AutowareDateTimePanel + Name: AutowareDateTimePanel + - Class: rviz_plugins::AutowareStatePanel + Name: AutowareStatePanel +Visualization Manager: + Class: "" + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/TF + Enabled: false + Frame Timeout: 15 + Frames: + All Enabled: true + Marker Scale: 1 + Name: TF + Show Arrows: true + Show Axes: true + Show Names: true + Tree: + {} + Update Interval: 0 + Value: false + - Alpha: 0.5 + Cell Size: 1 + Class: rviz_default_plugins/Grid + Color: 160; 160; 164 + Enabled: false + Line Style: + Line Width: 0.029999999329447746 + Value: Lines + Name: Grid + Normal Cell Count: 0 + Offset: + X: 0 + Y: 0 + Z: 0 + Plane: XY + Plane Cell Count: 10 + Reference Frame: + Value: false + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/SteeringAngle + Enabled: true + Left: 64 + Length: 128 + Name: SteeringAngle + Scale: 17 + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/steering_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Class: rviz_plugins/ConsoleMeter + Enabled: true + Left: 256 + Length: 128 + Name: ConsoleMeter + Text Color: 25; 255; 240 + Top: 64 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + Value Scale: 0.14999249577522278 + Value height offset: 0 + - Alpha: 0.9990000128746033 + Class: rviz_plugins/VelocityHistory + Color Border Vel Max: 3 + Constant Color: + Color: 255; 255; 255 + Value: true + Enabled: true + Name: VelocityHistory + Scale: 0.30000001192092896 + Timeout: 10 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/velocity_status + Value: true + - Alpha: 0.30000001192092896 + Class: rviz_default_plugins/RobotModel + Collision Enabled: false + Description File: "" + Description Source: Topic + Description Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /robot_description + Enabled: true + Links: + All Links Enabled: true + Expand Joint Details: false + Expand Link Details: false + Expand Tree: false + Link Tree Style: Links in Alphabetic Order + base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera0/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera1/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera1/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera2/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera2/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera3/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera3/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera4/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera4/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera5/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera5/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera6/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera6/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + camera7/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + camera7/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + gnss_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_front_left: + Alpha: 1 + Show Axes: false + Show Trail: false + livox_front_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + livox_front_right: + Alpha: 1 + Show Axes: false + Show Trail: false + livox_front_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + sensor_kit_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + tamagawa/imu_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_left_camera/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_left_camera/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + traffic_light_right_camera/camera_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + traffic_light_right_camera/camera_optical_link: + Alpha: 1 + Show Axes: false + Show Trail: false + velodyne_left: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_left_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_rear_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_right_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + velodyne_top_base_link: + Alpha: 1 + Show Axes: false + Show Trail: false + Value: true + Mass Properties: + Inertia: false + Mass: false + Name: VehicleModel + TF Prefix: "" + Update Interval: 0 + Value: true + Visual Enabled: true + - Class: rviz_plugins/PolarGridDisplay + Color: 255; 255; 255 + Delta Range: 10 + Enabled: true + Max Alpha: 0.5 + Max Range: 100 + Max Wave Alpha: 0.5 + Min Alpha: 0.009999999776482582 + Min Wave Alpha: 0.009999999776482582 + Name: PolarGridDisplay + Reference Frame: base_link + Value: true + Wave Color: 255; 255; 255 + Wave Velocity: 40 + - Class: rviz_plugins/MaxVelocity + Enabled: true + Left: 298 + Length: 48 + Name: MaxVelocity + Text Color: 255; 255; 255 + Top: 140 + Topic: /planning/scenario_planning/current_max_velocity + Value: true + Value Scale: 0.25 + - Class: rviz_plugins/TurnSignal + Enabled: true + Height: 128 + Left: 98 + Name: TurnSignal + Top: 175 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /vehicle/status/turn_indicators_status + Value: true + Width: 256 + Enabled: true + Name: Vehicle + Enabled: true + Name: System + - Class: rviz_common/Group + Displays: + - Alpha: 0.20000000298023224 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 28.71826171875 + Min Value: -7.4224700927734375 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 237 + Min Color: 211; 215; 207 + Min Intensity: 0 + Name: PointCloudMap + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Transient Local + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/pointcloud_map + Use Fixed Frame: true + Use rainbow: false + Value: true + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: Lanelet2VectorMap + Namespaces: + center_lane_line: false + center_line_arrows: false + crosswalk_lanelets: true + lane_start_bound: false + lanelet direction: true + lanelet_id: false + left_lane_bound: true + parking_lots: true + parking_space: true + pedestrian_marking: true + right_lane_bound: true + road_lanelets: false + stop_lines: true + traffic_light: true + traffic_light_id: false + traffic_light_triangle: true + walkway_lanelets: true + Topic: + Depth: 5 + Durability Policy: Transient Local + History Policy: Keep Last + Reliability Policy: Reliable + Value: /map/vector_map_marker + Value: true + Enabled: true + Name: Map + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.4000000059604645 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 5 + Min Value: -1 + Value: false + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 255; 255; 255 + Color Transformer: AxisColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: ConcatenatePointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 1 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /sensing/lidar/concatenated/pointcloud + Use Fixed Frame: false + Use rainbow: true + Value: true + - Alpha: 0.9990000128746033 + Class: rviz_default_plugins/Polygon + Color: 25; 255; 0 + Enabled: false + Name: MeasurementRange + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/lidar/crop_box_filter/crop_box_polygon + Value: false + Enabled: true + Name: LiDAR + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 233; 185; 110 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: false + Position: + Alpha: 0.20000000298023224 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: true + Head Length: 0.699999988079071 + Head Radius: 1.2000000476837158 + Name: PoseWithCovariance + Shaft Length: 1 + Shaft Radius: 0.5 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /sensing/gnss/pose_with_covariance + Value: true + Enabled: false + Name: GNSS + Enabled: true + Name: Sensing + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 170; 255 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovInitial + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/initial_pose_with_covariance + Value: false + - Alpha: 0.9990000128746033 + Axes Length: 1 + Axes Radius: 0.10000000149011612 + Class: rviz_default_plugins/PoseWithCovariance + Color: 0; 255; 0 + Covariance: + Orientation: + Alpha: 0.5 + Color: 255; 255; 127 + Color Style: Unique + Frame: Local + Offset: 1 + Scale: 1 + Value: true + Position: + Alpha: 0.30000001192092896 + Color: 204; 51; 204 + Scale: 1 + Value: true + Value: true + Enabled: false + Head Length: 0.4000000059604645 + Head Radius: 0.6000000238418579 + Name: PoseWithCovAligned + Shaft Length: 0.6000000238418579 + Shaft Radius: 0.30000001192092896 + Shape: Arrow + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose_with_covariance + Value: false + - Buffer Size: 200 + Class: rviz_plugins::PoseHistory + Enabled: false + Line: + Alpha: 0.9990000128746033 + Color: 170; 255; 127 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/pose + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 0; 255; 255 + Color Transformer: "" + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 4096 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Initial + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /localization/util/downsample/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: false + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 10 + Min Value: -10 + Value: true + Axis: Z + Channel Name: intensity + Class: rviz_default_plugins/PointCloud2 + Color: 85; 255; 0 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: false + Invert Rainbow: false + Max Color: 85; 255; 127 + Max Intensity: 0 + Min Color: 0; 0; 0 + Min Intensity: 0 + Name: Aligned + Position Transformer: XYZ + Selectable: false + Size (Pixels): 10 + Size (m): 0.5 + Style: Squares + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/points_aligned + Use Fixed Frame: true + Use rainbow: true + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MonteCarloInitialPose + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_estimator/monte_carlo_initial_pose_marker + Value: true + Enabled: true + Name: NDT + - Class: rviz_common/Group + Displays: + - Buffer Size: 1000 + Class: rviz_plugins::PoseHistory + Enabled: true + Line: + Alpha: 0.9990000128746033 + Color: 0; 255; 255 + Value: true + Width: 0.10000000149011612 + Name: PoseHistory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /localization/pose_twist_fusion_filter/pose + Value: true + Enabled: true + Name: EKF + Enabled: true + Name: Localization + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Alpha: 0.9990000128746033 + Autocompute Intensity Bounds: true + Autocompute Value Bounds: + Max Value: 15 + Min Value: -2 + Value: false + Axis: Z + Channel Name: z + Class: rviz_default_plugins/PointCloud2 + Color: 200; 200; 200 + Color Transformer: FlatColor + Decay Time: 0 + Enabled: true + Invert Rainbow: false + Max Color: 255; 255; 255 + Max Intensity: 15 + Min Color: 0; 0; 0 + Min Intensity: -5 + Name: NoGroundPointCloud + Position Transformer: XYZ + Selectable: false + Size (Pixels): 3 + Size (m): 0.019999999552965164 + Style: Points + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/obstacle_segmentation/pointcloud + Use Fixed Frame: true + Use rainbow: true + Value: true + Enabled: true + Name: Segmentation + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/DetectedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: DetectedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/detection/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Detection + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/TrackedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: true + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: TrackedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/tracking/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + Enabled: false + Name: Tracking + - Class: rviz_common/Group + Displays: + - BUS: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CAR: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + CYCLIST: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Class: autoware_auto_perception_rviz_plugin/PredictedObjects + Display Acceleration: true + Display Label: true + Display PoseWithCovariance: false + Display Predicted Path Confidence: true + Display Predicted Paths: true + Display Twist: true + Display UUID: true + Display Velocity: true + Enabled: true + Line Width: 0.029999999329447746 + MOTORCYCLE: + Alpha: 0.9990000128746033 + Color: 119; 11; 32 + Name: PredictedObjects + Namespaces: + {} + PEDESTRIAN: + Alpha: 0.9990000128746033 + Color: 255; 192; 203 + Polygon Type: 3d + TRAILER: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + TRUCK: + Alpha: 0.9990000128746033 + Color: 30; 144; 255 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/objects + UNKNOWN: + Alpha: 0.9990000128746033 + Color: 255; 255; 255 + Value: true + Visualization Type: Normal + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: Maneuver + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/object_recognition/prediction/maneuver + Value: false + Enabled: true + Name: Prediction + Enabled: true + Name: ObjectRecognition + - Class: rviz_common/Group + Displays: + - Class: rviz_default_plugins/Image + Enabled: false + Max Value: 1 + Median window: 5 + Min Value: 0 + Name: RecognitionResultOnImage + Normalize Range: true + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/debug/rois + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: true + Name: MapBasedDetectionResult + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/traffic_light_recognition/traffic_light_map_based_detector/debug/markers + Value: true + Enabled: true + Name: TrafficLight + - Class: rviz_common/Group + Displays: + - Alpha: 0.5 + Class: rviz_default_plugins/Map + Color Scheme: map + Draw Behind: false + Enabled: true + Name: Map + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map + Update Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Best Effort + Value: /perception/occupancy_grid_map/map_updates + Use Timestamp: false + Value: true + Enabled: false + Name: OccupancyGrid + Enabled: true + Name: Perception + - Class: rviz_common/Group + Displays: + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Path + Color Border Vel Max: 3 + Enabled: true + Name: Path + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/behavior_planning/path + Value: true + View Drivable Area: + Alpha: 0.9990000128746033 + Color: 0; 148; 205 + Value: true + Width: 0.30000001192092896 + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 0.5 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 1.5 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + Enabled: true + Name: Input + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: eb fixed traj + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_fixed_traj + Value: false + View Footprint: + Alpha: 1 + Color: 255; 170; 255 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: true + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: eb traj + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/eb_traj + Value: false + View Footprint: + Alpha: 1 + Color: 0; 255; 255 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: true + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + Enabled: true + Name: Elastic Band + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: mpt ref traj + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_ref_traj + Value: false + View Footprint: + Alpha: 1 + Color: 0; 255; 0 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: true + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: mpt fixed traj + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_fixed_traj + Value: false + View Footprint: + Alpha: 1 + Color: 255; 0; 0 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: true + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + - Class: rviz_default_plugins/MarkerArray + Enabled: false + Name: MarkerArray + Namespaces: + {} + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/marker + Value: false + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: false + Name: mpt traj + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/debug/mpt_traj + Value: false + View Footprint: + Alpha: 1 + Color: 255; 255; 0 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: true + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: false + Width: 2 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + Enabled: true + Name: Model Predictive Trajectory + - Class: rviz_common/Group + Displays: + - Class: rviz_plugins/Trajectory + Color Border Vel Max: 3 + Enabled: true + Name: Trajectory + Topic: + Depth: 5 + Durability Policy: Volatile + Filter size: 10 + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/scenario_planning/lane_driving/motion_planning/obstacle_avoidance_planner/trajectory + Value: true + View Footprint: + Alpha: 1 + Color: 230; 230; 50 + Offset from BaseLink: 0 + Rear Overhang: 1.0299999713897705 + Value: false + Vehicle Length: 4.769999980926514 + Vehicle Width: 1.8300000429153442 + View Path: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Value: true + Width: 0.4000000059604645 + View Point: + Alpha: 1 + Color: 0; 60; 255 + Offset: 0 + Radius: 0.10000000149011612 + Value: false + View Text Velocity: + Scale: 0.30000001192092896 + Value: false + View Velocity: + Alpha: 1 + Color: 0; 0; 0 + Constant Color: false + Scale: 0.30000001192092896 + Value: false + Enabled: true + Name: Output + Enabled: true + Name: Obstacle Avoidance Planner + Enabled: true + Global Options: + Background Color: 10; 10; 10 + Fixed Frame: map + Frame Rate: 30 + Name: root + Tools: + - Class: rviz_default_plugins/Interact + Hide Inactive Objects: true + - Class: rviz_default_plugins/MoveCamera + - Class: rviz_default_plugins/Select + - Class: rviz_default_plugins/FocusCamera + - Class: rviz_default_plugins/Measure + Line color: 128; 128; 0 + - Class: rviz_default_plugins/SetInitialPose + Covariance x: 0.25 + Covariance y: 0.25 + Covariance yaw: 0.06853891909122467 + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /initialpose + - Class: rviz_default_plugins/SetGoal + Topic: + Depth: 5 + Durability Policy: Volatile + History Policy: Keep Last + Reliability Policy: Reliable + Value: /planning/mission_planning/goal + - Acceleration: 0 + Class: rviz_plugins/PedestrianInitialPoseTool + Interactive: false + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 1 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/CarInitialPoseTool + H vehicle height: 2 + Interactive: false + L vehicle length: 4 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 3 + W vehicle width: 1.7999999523162842 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Acceleration: 0 + Class: rviz_plugins/BusInitialPoseTool + H vehicle height: 3.5 + Interactive: false + L vehicle length: 10.5 + Max velocity: 33.29999923706055 + Min velocity: -33.29999923706055 + Pose Topic: /simulation/dummy_perception_publisher/object_info + Target Frame: + Theta std deviation: 0.0872664600610733 + Velocity: 0 + W vehicle width: 2.5 + X std deviation: 0.029999999329447746 + Y std deviation: 0.029999999329447746 + Z position: 0 + Z std deviation: 0.029999999329447746 + - Class: rviz_plugins/MissionCheckpointTool + Pose Topic: /planning/mission_planning/checkpoint + Theta std deviation: 0.2617993950843811 + X std deviation: 0.5 + Y std deviation: 0.5 + Z position: 0 + - Class: rviz_plugins/DeleteAllObjectsTool + Pose Topic: /simulation/dummy_perception_publisher/object_info + Transformation: + Current: + Class: rviz_default_plugins/TF + Value: true + Views: + Current: + Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: Current View + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz_default_plugins) + X: 0 + Y: 0 + Saved: + - Class: rviz_default_plugins/ThirdPersonFollower + Distance: 18 + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Focal Point: + X: 0 + Y: 0 + Z: 0 + Focal Shape Fixed Size: true + Focal Shape Size: 0.05000000074505806 + Invert Z Axis: false + Name: ThirdPersonFollower + Near Clip Distance: 0.009999999776482582 + Pitch: 0.20000000298023224 + Target Frame: base_link + Value: ThirdPersonFollower (rviz) + Yaw: 3.141592025756836 + - Angle: 0 + Class: rviz_default_plugins/TopDownOrtho + Enable Stereo Rendering: + Stereo Eye Separation: 0.05999999865889549 + Stereo Focal Distance: 1 + Swap Stereo Eyes: false + Value: false + Invert Z Axis: false + Name: TopDownOrtho + Near Clip Distance: 0.009999999776482582 + Scale: 10 + Target Frame: viewer + Value: TopDownOrtho (rviz) + X: 0 + Y: 0 +Window Geometry: + AutowareDateTimePanel: + collapsed: false + AutowareStatePanel: + collapsed: false + Displays: + collapsed: false + Height: 2032 + Hide Left Dock: false + Hide Right Dock: false + InitialPoseButtonPanel: + collapsed: false + QMainWindow State: 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 + RecognitionResultOnImage: + collapsed: false + Selection: + collapsed: false + Tool Properties: + collapsed: false + Views: + collapsed: false + Width: 3692 + X: 148 + Y: 54 diff --git a/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py b/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py old mode 100644 new mode 100755 index 275ae69ef177c..f0d32848adefe --- a/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py +++ b/planning/obstacle_avoidance_planner/scripts/calclation_time_plotter.py @@ -99,7 +99,12 @@ def CallbackCalculationCost(self, msg): if __name__ == "__main__": parser = argparse.ArgumentParser() - parser.add_argument("-f", "--functions", type=str, default="solveOsqp") + parser.add_argument( + "-f", + "--functions", + type=str, + default="onPath, getModelPredictiveTrajectory, getEBTrajectory", + ) args = parser.parse_args() rclpy.init() diff --git a/planning/obstacle_avoidance_planner/src/debug_marker.cpp b/planning/obstacle_avoidance_planner/src/debug_marker.cpp index c517ade57c6c4..a8af96e7b49de 100644 --- a/planning/obstacle_avoidance_planner/src/debug_marker.cpp +++ b/planning/obstacle_avoidance_planner/src/debug_marker.cpp @@ -84,18 +84,18 @@ MarkerArray getBoundsLineMarkerArray( // create lower bound marker auto lb_marker = createDefaultMarker( "map", rclcpp::Clock().now(), "", 0, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), - createMarkerColor(0.99 + 0.5, 0.99, 0.2, 0.3)); + createMarkerColor(0.5, 0.99, 0.2, 0.8)); lb_marker.lifetime = rclcpp::Duration::from_seconds(1.5); // create upper bound marker auto ub_marker = createDefaultMarker( "map", rclcpp::Clock().now(), "", 1, Marker::LINE_LIST, createMarkerScale(0.05, 0.0, 0.0), - createMarkerColor(0.99, 0.99 + 0.5, 0.2, 0.3)); + createMarkerColor(0.99, 0.5, 0.2, 0.8)); ub_marker.lifetime = rclcpp::Duration::from_seconds(1.5); for (size_t bound_idx = 0; bound_idx < ref_points.at(0).bounds_on_constraints.size(); ++bound_idx) { - const std::string ns = "base_bounds_" + std::to_string(bound_idx); + const std::string ns = "bounds" + std::to_string(bound_idx); { // lower bound lb_marker.points.clear(); diff --git a/planning/route_handler/include/route_handler/route_handler.hpp b/planning/route_handler/include/route_handler/route_handler.hpp index 52f8082f125e4..89e052b439f78 100644 --- a/planning/route_handler/include/route_handler/route_handler.hpp +++ b/planning/route_handler/include/route_handler/route_handler.hpp @@ -54,7 +54,7 @@ using std_msgs::msg::Header; using unique_identifier_msgs::msg::UUID; using RouteSections = std::vector; -enum class LaneChangeDirection { NONE, LEFT, RIGHT }; +enum class Direction { NONE, LEFT, RIGHT }; enum class PullOverDirection { NONE, LEFT, RIGHT }; enum class PullOutDirection { NONE, LEFT, RIGHT }; @@ -259,9 +259,19 @@ class RouteHandler const lanelet::ConstLanelet & lanelet, const double length, const lanelet::ConstLanelets & exclude_lanelets = {}) const; - int getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const; + /** + * Query input lanelet to see whether it exist in the preferred lane. If it doesn't exist, return + * the number of lane-changeable lane to the preferred lane. + * @param Desired lanelet to query + * @param lane change direction + * @return number of lanes from input to the preferred lane + */ + int getNumLaneToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction = Direction::NONE) const; + bool getClosestLaneletWithinRoute( const Pose & search_pose, lanelet::ConstLanelet * closest_lanelet) const; + lanelet::ConstLanelet getLaneletsFromId(const lanelet::Id id) const; lanelet::ConstLanelets getLaneletsFromIds(const lanelet::Ids & ids) const; lanelet::ConstLanelets getLaneletSequence( @@ -287,7 +297,8 @@ class RouteHandler const lanelet::ConstLanelets & lanelet_sequence, const double s_start, const double s_end, bool use_exact = true) const; bool getLaneChangeTarget( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; + const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet, + const Direction direction = Direction::NONE) const; bool getRightLaneChangeTargetExceptPreferredLane( const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const; bool getLeftLaneChangeTargetExceptPreferredLane( @@ -298,8 +309,7 @@ class RouteHandler static bool getPullOutStartLane( const lanelet::ConstLanelets & lanelets, const Pose & pose, const double vehicle_width, lanelet::ConstLanelet * target_lanelet); - double getLaneChangeableDistance( - const Pose & current_pose, const LaneChangeDirection & direction) const; + double getLaneChangeableDistance(const Pose & current_pose, const Direction & direction) const; lanelet::ConstPolygon3d getIntersectionAreaById(const lanelet::Id id) const; private: diff --git a/planning/route_handler/src/route_handler.cpp b/planning/route_handler/src/route_handler.cpp index d2ea4c65adaa9..2fe8e3549b661 100644 --- a/planning/route_handler/src/route_handler.cpp +++ b/planning/route_handler/src/route_handler.cpp @@ -1106,30 +1106,33 @@ std::vector RouteHandler::getPrecedingLaneletSequence( } bool RouteHandler::getLaneChangeTarget( - const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet) const + const lanelet::ConstLanelets & lanelets, lanelet::ConstLanelet * target_lanelet, + const Direction direction) const { for (const auto & lanelet : lanelets) { - const int num = getNumLaneToPreferredLane(lanelet); + const int num = getNumLaneToPreferredLane(lanelet, direction); if (num == 0) { continue; } - if (num < 0) { - if (!!routing_graph_ptr_->right(lanelet)) { - const auto right_lanelet = routing_graph_ptr_->right(lanelet); - *target_lanelet = right_lanelet.get(); - return true; + if (direction == Direction::NONE || direction == Direction::RIGHT) { + if (num < 0) { + if (!!routing_graph_ptr_->right(lanelet)) { + const auto right_lanelet = routing_graph_ptr_->right(lanelet); + *target_lanelet = right_lanelet.get(); + return true; + } } - continue; } - if (num > 0) { - if (!!routing_graph_ptr_->left(lanelet)) { - const auto left_lanelet = routing_graph_ptr_->left(lanelet); - *target_lanelet = left_lanelet.get(); - return true; + if (direction == Direction::NONE || direction == Direction::LEFT) { + if (num > 0) { + if (!!routing_graph_ptr_->left(lanelet)) { + const auto left_lanelet = routing_graph_ptr_->left(lanelet); + *target_lanelet = left_lanelet.get(); + return true; + } } - continue; } } @@ -1212,26 +1215,34 @@ lanelet::ConstLanelets RouteHandler::getClosestLaneletSequence(const Pose & pose return getLaneletSequence(lanelet); } -int RouteHandler::getNumLaneToPreferredLane(const lanelet::ConstLanelet & lanelet) const +int RouteHandler::getNumLaneToPreferredLane( + const lanelet::ConstLanelet & lanelet, const Direction direction) const { - int num = 0; if (exists(preferred_lanelets_, lanelet)) { - return num; + return 0; } - const auto & right_lanes = - lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, lanelet); - for (const auto & right : right_lanes) { - num--; - if (exists(preferred_lanelets_, right)) { - return num; + + if ((direction == Direction::NONE) || (direction == Direction::RIGHT)) { + int num{0}; + const auto & right_lanes = + lanelet::utils::query::getAllNeighborsRight(routing_graph_ptr_, lanelet); + for (const auto & right : right_lanes) { + num--; + if (exists(preferred_lanelets_, right)) { + return num; + } } } - const auto & left_lanes = lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, lanelet); - num = 0; - for (const auto & left : left_lanes) { - num++; - if (exists(preferred_lanelets_, left)) { - return num; + + if ((direction == Direction::NONE) || (direction == Direction::LEFT)) { + const auto & left_lanes = + lanelet::utils::query::getAllNeighborsLeft(routing_graph_ptr_, lanelet); + int num = 0; + for (const auto & left : left_lanes) { + num++; + if (exists(preferred_lanelets_, left)) { + return num; + } } } @@ -1368,7 +1379,7 @@ lanelet::ConstLanelets RouteHandler::getLaneChangeTargetLanes(const Pose & pose) } double RouteHandler::getLaneChangeableDistance( - const Pose & current_pose, const LaneChangeDirection & direction) const + const Pose & current_pose, const Direction & direction) const { lanelet::ConstLanelet current_lane; if (!getClosestLaneletWithinRoute(current_pose, ¤t_lane)) { @@ -1382,12 +1393,12 @@ double RouteHandler::getLaneChangeableDistance( double accumulated_distance = 0; for (const auto & lane : lanelet_sequence) { lanelet::ConstLanelet target_lane; - if (direction == LaneChangeDirection::RIGHT) { + if (direction == Direction::RIGHT) { if (!getRightLaneletWithinRoute(lane, &target_lane)) { break; } } - if (direction == LaneChangeDirection::LEFT) { + if (direction == Direction::LEFT) { if (!getLeftLaneletWithinRoute(lane, &target_lane)) { break; } diff --git a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp index 568384509d4da..ed0d753a68ecc 100644 --- a/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp +++ b/sensing/pointcloud_preprocessor/include/pointcloud_preprocessor/vector_map_filter/lanelet2_map_filter_nodelet.hpp @@ -23,6 +23,7 @@ #include #include +#include #include #include diff --git a/system/component_state_monitor/config/topics.yaml b/system/component_state_monitor/config/topics.yaml index 5a5f2e880bcae..c00203dbb6073 100644 --- a/system/component_state_monitor/config/topics.yaml +++ b/system/component_state_monitor/config/topics.yaml @@ -1,5 +1,5 @@ - module: map - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: launch args: node_name_suffix: vector_map @@ -12,7 +12,7 @@ timeout: 0.0 - module: map - mode: [online] + mode: [online, logging_simulation] type: launch args: node_name_suffix: pointcloud_map @@ -25,7 +25,7 @@ timeout: 0.0 - module: localization - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: initialpose3d @@ -38,7 +38,7 @@ timeout: 0.0 - module: localization - mode: [online] + mode: [online, logging_simulation] type: autonomous args: node_name_suffix: pose_twist_fusion_filter_pose @@ -51,7 +51,7 @@ timeout: 1.0 - module: perception - mode: [online] + mode: [online, logging_simulation] type: launch args: node_name_suffix: obstacle_segmentation_pointcloud @@ -64,7 +64,7 @@ timeout: 1.0 - module: perception - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: object_recognition_objects @@ -77,7 +77,7 @@ timeout: 1.0 - module: planning - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: mission_planning_route @@ -90,7 +90,7 @@ timeout: 0.0 - module: planning - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: scenario_planning_trajectory @@ -103,7 +103,7 @@ timeout: 1.0 - module: control - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: trajectory_follower_control_cmd @@ -116,7 +116,7 @@ timeout: 1.0 - module: control - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: control_command_control_cmd @@ -129,7 +129,7 @@ timeout: 1.0 - module: vehicle - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: vehicle_status_velocity_status @@ -142,7 +142,7 @@ timeout: 1.0 - module: vehicle - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: vehicle_status_steering_status @@ -155,7 +155,7 @@ timeout: 1.0 - module: system - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: launch args: node_name_suffix: system_emergency_control_cmd @@ -168,7 +168,7 @@ timeout: 1.0 - module: localization - mode: [online, planning_simulation] + mode: [online, logging_simulation, planning_simulation] type: autonomous args: node_name_suffix: transform_map_to_base_link diff --git a/system/system_error_monitor/src/system_error_monitor_core.cpp b/system/system_error_monitor/src/system_error_monitor_core.cpp index d6cc771e5dbd2..8a4cf3ecbb8d6 100644 --- a/system/system_error_monitor/src/system_error_monitor_core.cpp +++ b/system/system_error_monitor/src/system_error_monitor_core.cpp @@ -443,7 +443,7 @@ void AutowareErrorMonitor::onTimer() if (!isDataReady()) { if ((this->now() - initialized_time_).seconds() > params_.data_ready_timeout) { RCLCPP_WARN_THROTTLE( - get_logger(), *get_clock(), std::chrono::milliseconds(1000).count(), + get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(), "input data is timeout"); updateTimeoutHazardStatus(); publishHazardStatus(hazard_status_); diff --git a/system/velodyne_monitor/CMakeLists.txt b/system/velodyne_monitor/CMakeLists.txt index d3238a0fd0d5b..c7511bf572f4a 100644 --- a/system/velodyne_monitor/CMakeLists.txt +++ b/system/velodyne_monitor/CMakeLists.txt @@ -2,6 +2,7 @@ cmake_minimum_required(VERSION 3.14) project(velodyne_monitor) find_package(autoware_cmake REQUIRED) +find_package(fmt) autoware_package() ### Target executable