Skip to content

Commit

Permalink
feat: add scenario selector package (autowarefoundation#41)
Browse files Browse the repository at this point in the history
* release v0.4.0

* remove ROS1 packages temporarily

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Revert "remove ROS1 packages temporarily"

This reverts commit 7f877f288151212d5ee578f070fc7e11bd15ad9f.

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* add COLCON_IGNORE to ros1 packages

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Rename launch files to launch.xml (autowarefoundation#28)

* ROS2 Porting: scenario_selector (autowarefoundation#101)

* Add cmake and package.xml
 - Remove colcon ignore

* First pass:
 - Remove pubs and subs
 - Remove lane esoteric lanelet method calls

* Add parameter fetching and setting

* Add publishers, logging and transforms
 - Fix createCallback

* Remove duration field from lookuptransform

* Add topic_tools dependency
 - launching of rostopic in ROS1 hasn't been ported?

* Remove comments from cmake

* Remove boost dependency

* Remove test parameters

* Address PR comments:
 - Add const reference back
 - Use initializer list for parameter fetching
 - Fix fromBinMap reference
 - Launch publishing from executable tag in launch file

* Address PR comment:
 - Fix launch file launching of ros2 topic publisher

* Rename h files to hpp (autowarefoundation#142)

* Change includes

* Rename files

* Adjustments to make things compile

* Other packages

* Adjust copyright notice on 532 out of 699 source files (autowarefoundation#143)

* Use quotes for includes where appropriate (autowarefoundation#144)

* Use quotes for includes where appropriate

* Fix lint tests

* Make tests pass hopefully

* fixing trasient_local in ROS2 packages (autowarefoundation#160)

* [scenario_selector_node] fix timer frequency (autowarefoundation#182)

Signed-off-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>

* Add linters to scenario_selector (autowarefoundation#157)

* Added linters to scenario_selector

* Removed duplicate dependencies

* Add linters to CMake

* Only add cppcheck as linter

* Ros2 v0.8.0 scenario selector (autowarefoundation#294)

* add use_sim-time option (autowarefoundation#454)

* Format launch files (autowarefoundation#1219)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* update first tf (autowarefoundation#1223)

* update first tf

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* update

Signed-off-by: Kosuke Murakami <kosuke.murakami@tier4.jp>

* Make planning modules components (autowarefoundation#1263)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* Remove use_sim_time for set_parameter (autowarefoundation#1260)

Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

* change trajectory callback implementation (autowarefoundation#1338) (autowarefoundation#1355)

* change trajectory callback implementation

* change name of callback function

* remove unnecessary function

* improve readability

Co-authored-by: satoshi-ota <satoshi.ota@gmail.com>

Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: satoshi-ota <satoshi.ota@gmail.com>

* Select LaneDriving scenario if vehicle is in lane and goal is in lane (autowarefoundation#1459) (autowarefoundation#1485)

Signed-off-by: Azumi Suzuki <azumi.suzuki@tier4.jp>
Signed-off-by: wep21 <border_goldenmarket@yahoo.co.jp>

Co-authored-by: s-azumi <38061530+s-azumi@users.noreply.github.com>

* Add markdownlint and prettier (autowarefoundation#1661)

* Add markdownlint and prettier

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore .param.yaml

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix -Wunused-parameter (autowarefoundation#1836)

* Fix -Wunused-parameter

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix mistake

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* fix spell

* Fix lint issues

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Ignore flake8 warnings

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>

* add sort-package-xml hook in pre-commit (autowarefoundation#1881)

* add sort xml hook in pre-commit

* change retval to exit_status

* rename

* add prettier plugin-xml

* use early return

* add license note

* add tier4 license

* restore prettier

* change license order

* move local hooks to public repo

* move prettier-xml to pre-commit-hooks-ros

* update version for bug-fix

* apply pre-commit

* Change formatter to clang-format and black (autowarefoundation#2332)

* Revert "Temporarily comment out pre-commit hooks"

This reverts commit 748e9cdb145ce12f8b520bcbd97f5ff899fc28a3.

* Replace ament_lint_common with autoware_lint_common

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Remove ament_cmake_uncrustify and ament_clang_format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply Black

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix for cpplint

* Fix include double quotes to angle brackets

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Apply clang-format

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Fix build errors

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* Add COLCON_IGNORE (autowarefoundation#500)

Signed-off-by: Kenji Miyake <kenji.miyake@tier4.jp>

* port scenario_selector (autowarefoundation#482)

* port scenario_selector

* remove COLCON_IGNORE

* use odom

* add nav_msgs dependency

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* [apply_predicted_obj_type] adapt to autoware auto msgs (autowarefoundation#564)

* fix obj shape

* fix obj shape

* fix goal pose

* rename topic name twist -> odometry (autowarefoundation#568)

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* update iv_msgs -> auto_msgs in planning readme (autowarefoundation#576)

* update iv_msgs -> auto_msgs in planning readme

* minor change

* some fix

* some fix

Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>

* fix scenario selector (autowarefoundation#632)

* ci(pre-commit): autofix

* fix: package title

Co-authored-by: mitsudome-r <ryohsuke.mitsudome@tier4.jp>
Co-authored-by: Nikolai Morin <nnmmgit@gmail.com>
Co-authored-by: Jilada Eccleston <jilada.eccleston@gmail.com>
Co-authored-by: nik-tier4 <71747268+nik-tier4@users.noreply.github.com>
Co-authored-by: Ryohsuke Mitsudome <43976834+mitsudome-r@users.noreply.github.com>
Co-authored-by: Esteve Fernandez <esteve@apache.org>
Co-authored-by: Daisuke Nishimatsu <42202095+wep21@users.noreply.github.com>
Co-authored-by: tkimura4 <tomoya.kimura@tier4.jp>
Co-authored-by: Kenji Miyake <31987104+kenji-miyake@users.noreply.github.com>
Co-authored-by: Kosuke Murakami <kosuke.murakami@tier4.jp>
Co-authored-by: Satoshi OTA <44889564+satoshi-ota@users.noreply.github.com>
Co-authored-by: satoshi-ota <satoshi.ota@gmail.com>
Co-authored-by: s-azumi <38061530+s-azumi@users.noreply.github.com>
Co-authored-by: Hiroki OTA <hiroki.ota@tier4.jp>
Co-authored-by: Keisuke Shima <19993104+KeisukeShima@users.noreply.github.com>
Co-authored-by: Takayuki Murooka <takayuki5168@gmail.com>
Co-authored-by: Takayuki Murooka <takayuki.murooka@tier4.jp>
Co-authored-by: pre-commit-ci[bot] <66853113+pre-commit-ci[bot]@users.noreply.github.com>
Co-authored-by: Takeshi Miura <57553950+1222-takeshi@users.noreply.github.com>
  • Loading branch information
20 people committed Dec 6, 2021
1 parent e5d305b commit c53da99
Show file tree
Hide file tree
Showing 8 changed files with 743 additions and 0 deletions.
42 changes: 42 additions & 0 deletions planning/scenario_selector/CMakeLists.txt
Original file line number Diff line number Diff line change
@@ -0,0 +1,42 @@
cmake_minimum_required(VERSION 3.5)
project(scenario_selector)

### Compile options
if(NOT CMAKE_CXX_STANDARD)
set(CMAKE_CXX_STANDARD 14)
set(CMAKE_CXX_STANDARD_REQUIRED ON)
set(CMAKE_CXX_EXTENSIONS OFF)
endif()
if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang")
add_compile_options(-Wall -Wextra -Wpedantic)
endif()

## Dependencies
find_package(ament_cmake_auto REQUIRED)
ament_auto_find_build_dependencies()

# Target

## Target executable
set(SCENARIO_SELECTOR_SRC
src/scenario_selector_node/scenario_selector_node.cpp
)

## scenario_selector_node
ament_auto_add_library(scenario_selector_node SHARED
${SCENARIO_SELECTOR_SRC}
)

rclcpp_components_register_node(scenario_selector_node
PLUGIN "ScenarioSelectorNode"
EXECUTABLE scenario_selector
)

if(BUILD_TESTING)
find_package(ament_lint_auto REQUIRED)
ament_lint_auto_find_test_dependencies()
endif()

ament_auto_package(INSTALL_TO_SHARE
launch
)
121 changes: 121 additions & 0 deletions planning/scenario_selector/README.md
Original file line number Diff line number Diff line change
@@ -0,0 +1,121 @@
# scenario_selector

## scenario_selector_node

`scenario_selector_node` is a node that switches trajectories from each scenario.

### Input topics

| Name | Type | Description |
| -------------------------------- | ---------------------------------------- | ----------------------------------------------------- |
| `~input/lane_driving/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of LaneDriving scenario |
| `~input/parking/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory of Parking scenario |
| `~input/lanelet_map` | autoware_auto_mapping_msgs::HADMapBin | |
| `~input/route` | autoware_auto_planning_msgs::HADMapRoute | route and goal pose |
| `~input/odometry` | nav_msgs::Odometry | for checking whether vehicle is stopped |
| `is_parking_completed` | bool (implemented as rosparam) | whether all split trajectory of Parking are published |

### Output topics

| Name | Type | Description |
| -------------------- | --------------------------------------- | ---------------------------------------------- |
| `~output/scenario` | autoware_planning_msgs::Scenario | current scenario and scenarios to be activated |
| `~output/trajectory` | autoware_auto_planning_msgs::Trajectory | trajectory to be followed |

### Output TFs

None

### How to launch

1. Write your remapping info in `scenario_selector.launch` or add args when executing `roslaunch`
2. `roslaunch scenario_selector scenario_selector.launch`
- If you would like to use only a single scenario, `roslaunch scenario_selector dummy_scenario_selector_{scenario_name}.launch`

### Parameters

| Parameter | Type | Description |
| -------------------------- | ------ | ------------------------------------------------------------------------------- |
| `update_rate` | double | timer's update rate |
| `th_max_message_delay_sec` | double | threshold time of input messages' maximum delay |
| `th_arrived_distance_m` | double | threshold distance to check if vehicle has arrived at the trajectory's endpoint |
| `th_stopped_time_sec` | double | threshold time to check if vehicle is stopped |
| `th_stopped_velocity_mps` | double | threshold velocity to check if vehicle is stopped |

### Flowchart

```plantuml
@startuml
title onTimer
start
:get current pose;
if (all input data are ready?) then (yes)
else (no)
stop
endif
if (scenario is initialized?) then (yes)
else (no)
:initialize scenario;
endif
:select scenario;
:publish scenario;
:extract scenario trajectory;
if (scenario trajectory is empty?) then (yes)
else (no)
:publish trajectory;
endif
stop
@enduml
```

```plantuml
@startuml
title Scenario Transition
start
if (current_scenario is completed?\n()) then (yes)
else (no)
stop
endif
' Empty
if (scenario is initialized?) then (yes)
else (no)
if (is in lane?) then (yes)
:set LaneDriving;
else (no)
:set Parking;
endif
stop
endif
' LaneDriving
if (current scenario is LaneDriving?) then (yes)
if (is in parking lot & goal is not in lane?) then (yes)
:set Parking;
stop
endif
endif
' Parking
if (current scenario is Parking?) then (yes)
if (parking is completed and is in lane?) then (yes)
:set LaneDriving;
stop
endif
endif
:continue previous scenario;
stop
@enduml
```
Original file line number Diff line number Diff line change
@@ -0,0 +1,95 @@
// Copyright 2020 Tier IV, Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_
#define SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_

#include <rclcpp/rclcpp.hpp>

#include <autoware_auto_mapping_msgs/msg/had_map_bin.hpp>
#include <autoware_auto_planning_msgs/msg/had_map_route.hpp>
#include <autoware_auto_planning_msgs/msg/trajectory.hpp>
#include <autoware_planning_msgs/msg/scenario.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>

#include <lanelet2_core/LaneletMap.h>
#include <lanelet2_routing/RoutingGraph.h>
#include <lanelet2_traffic_rules/TrafficRules.h>
#include <tf2_geometry_msgs/tf2_geometry_msgs.h>
#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <deque>
#include <memory>
#include <string>

class ScenarioSelectorNode : public rclcpp::Node
{
public:
explicit ScenarioSelectorNode(const rclcpp::NodeOptions & node_options);

void onMap(const autoware_auto_mapping_msgs::msg::HADMapBin::ConstSharedPtr msg);
void onRoute(const autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr msg);
void onOdom(const nav_msgs::msg::Odometry::ConstSharedPtr msg);

void onTimer();
void onLaneDrivingTrajectory(
const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
void onParkingTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);
void publishTrajectory(const autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr msg);

void updateCurrentScenario();
std::string selectScenarioByPosition();
autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr getScenarioTrajectory(
const std::string & scenario);

private:
rclcpp::TimerBase::SharedPtr timer_;

tf2_ros::Buffer tf_buffer_;
tf2_ros::TransformListener tf_listener_;

rclcpp::Subscription<autoware_auto_mapping_msgs::msg::HADMapBin>::SharedPtr sub_lanelet_map_;
rclcpp::Subscription<autoware_auto_planning_msgs::msg::HADMapRoute>::SharedPtr sub_route_;
rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr sub_odom_;
rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
sub_lane_driving_trajectory_;
rclcpp::Subscription<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr
sub_parking_trajectory_;
rclcpp::Publisher<autoware_auto_planning_msgs::msg::Trajectory>::SharedPtr pub_trajectory_;
rclcpp::Publisher<autoware_planning_msgs::msg::Scenario>::SharedPtr pub_scenario_;

autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr lane_driving_trajectory_;
autoware_auto_planning_msgs::msg::Trajectory::ConstSharedPtr parking_trajectory_;
autoware_auto_planning_msgs::msg::HADMapRoute::ConstSharedPtr route_;
geometry_msgs::msg::PoseStamped::ConstSharedPtr current_pose_;
geometry_msgs::msg::TwistStamped::ConstSharedPtr twist_;

std::string current_scenario_;
std::deque<geometry_msgs::msg::TwistStamped::ConstSharedPtr> twist_buffer_;

std::shared_ptr<lanelet::LaneletMap> lanelet_map_ptr_;
std::shared_ptr<lanelet::routing::RoutingGraph> routing_graph_ptr_;
std::shared_ptr<lanelet::traffic_rules::TrafficRules> traffic_rules_ptr_;

// Parameters
double update_rate_;
double th_max_message_delay_sec_;
double th_arrived_distance_m_;
double th_stopped_time_sec_;
double th_stopped_velocity_mps_;
};

#endif // SCENARIO_SELECTOR__SCENARIO_SELECTOR_NODE_HPP_
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
<launch>
<arg name="input_lane_driving_trajectory" />
<arg name="output_scenario" />
<arg name="output_trajectory" />

<arg name="input_parking_trajectory" default=""/>
<arg name="input_lanelet_map" default="" />
<arg name="input_route" default="" />
<arg name="input_odometry" default="" />
<arg name="is_parking_completed" default="" />

<node pkg="topic_tools" exec="relay" name="scenario_trajectory_relay" output="log">
<param name="input_topic" value="$(var input_lane_driving_trajectory)" />
<param name="output_topic" value="$(var output_trajectory)" />
<param name="type" value="autoware_planning_msgs/msg/Trajectory" />
</node>

<arg name="cmd" default="ros2 topic pub $(var output_scenario) autoware_planning_msgs/msg/Scenario '{current_scenario: LaneDriving, activating_scenarios: [LaneDriving]}'" />
<executable cmd="$(var cmd)" name="scenario_pub" output="screen" shell="true"/>
</launch>
Original file line number Diff line number Diff line change
@@ -0,0 +1,20 @@
<launch>
<arg name="input_parking_trajectory" />
<arg name="output_scenario" />
<arg name="output_trajectory" />

<arg name="input_lane_driving_trajectory" default="" />
<arg name="input_lanelet_map" default="" />
<arg name="input_route" default="" />
<arg name="input_odometry" default="" />
<arg name="is_parking_completed" default="" />

<node pkg="topic_tools" exec="relay" name="scenario_trajectory_relay" output="log">
<param name="input_topic" value="$(var input_parking_trajectory)" />
<param name="output_topic" value="$(var output_trajectory)" />
<param name="type" value="autoware_planning_msgs/msg/Trajectory" />
</node>

<arg name="cmd" default="ros2 topic pub $(var output_scenario) autoware_planning_msgs/msg/Scenario '{current_scenario: Parking, activating_scenarios: [Parking]}'" />
<executable cmd="$(var cmd)" name="scenario_pub" output="screen" shell="true"/>
</launch>
29 changes: 29 additions & 0 deletions planning/scenario_selector/launch/scenario_selector.launch.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,29 @@
<launch>
<arg name="input_lane_driving_trajectory" />
<arg name="input_parking_trajectory" />
<arg name="input_lanelet_map" />
<arg name="input_route" />
<arg name="input_odometry" />
<arg name="is_parking_completed" />

<arg name="output_scenario" />
<arg name="output_trajectory" />

<node pkg="scenario_selector" exec="scenario_selector" name="scenario_selector" output="screen">
<remap from="input/lane_driving/trajectory" to="$(var input_lane_driving_trajectory)"/>
<remap from="input/parking/trajectory" to="$(var input_parking_trajectory)"/>
<remap from="input/lanelet_map" to="$(var input_lanelet_map)"/>
<remap from="input/route" to="$(var input_route)"/>
<remap from="input/odometry" to="$(var input_odometry)"/>
<remap from="is_parking_completed" to="$(var is_parking_completed)"/>

<remap from="output/scenario" to="$(var output_scenario)"/>
<remap from="output/trajectory" to="$(var output_trajectory)"/>

<param name="update_rate" value="10.0" />
<param name="th_max_message_delay_sec" value="1.0" />
<param name="th_arrived_distance_m" value="1.0" />
<param name="th_stopped_time_sec" value="1.0" />
<param name="th_stopped_velocity_mps" value="0.01" />
</node>
</launch>
32 changes: 32 additions & 0 deletions planning/scenario_selector/package.xml
Original file line number Diff line number Diff line change
@@ -0,0 +1,32 @@
<?xml version="1.0"?>
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
<package format="3">
<name>scenario_selector</name>
<version>0.1.0</version>
<description>The scenario_selector ROS2 package</description>
<maintainer email="kenji.miyake@tier4.jp">Kenji Miyake</maintainer>
<license>Apache License 2.0</license>

<buildtool_depend>ament_cmake_auto</buildtool_depend>

<depend>autoware_auto_mapping_msgs</depend>
<depend>autoware_auto_planning_msgs</depend>
<depend>autoware_planning_msgs</depend>
<depend>lanelet2_extension</depend>
<depend>nav_msgs</depend>
<depend>rclcpp</depend>
<depend>rclcpp_components</depend>
<depend>tf2</depend>
<depend>tf2_geometry_msgs</depend>
<depend>tf2_ros</depend>

<exec_depend>ros2cli</exec_depend>
<exec_depend>topic_tools</exec_depend>

<test_depend>ament_lint_auto</test_depend>
<test_depend>autoware_lint_common</test_depend>

<export>
<build_type>ament_cmake</build_type>
</export>
</package>
Loading

0 comments on commit c53da99

Please sign in to comment.