Skip to content
New issue

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

Sign up for GitHub

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

Already on GitHub? Sign in to your account

Jump to bottom

fix: mock test launch #1368

Merged
merged 6 commits into from
Sep 11, 2024
+188 −109
Merged

fix: mock test launch #1368

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
6 commits
Select commit Hold shift + click to select a range
846960f
fix: load necessary parameters
satoshi-ota Sep 10, 2024
2e3adec
fix: make it possible to change hard-coded parameters
satoshi-ota Sep 10, 2024
a8b8a60
fix: use global timeout
satoshi-ota Sep 10, 2024
3045ccd
fix: missing param
satoshi-ota Sep 10, 2024
4b79839
fix: set default rviz config
satoshi-ota Sep 10, 2024
8c90aa4
fix: global frame rate 30.0 -> 20.0
satoshi-ota Sep 10, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
2 changes: 1 addition & 1 deletion mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -68,7 +68,7 @@ class CppScenarioNode : public rclcpp::Node
virtual void onUpdate() = 0;
virtual void onInitialize() = 0;
rclcpp::TimerBase::SharedPtr update_timer_;
double timeout_;
int timeout_;
auto configure(
const std::string & map_path, const std::string & lanelet2_map_file,
const std::string & scenario_filename, const bool verbose) -> traffic_simulator::Configuration
Expand Down
286 changes: 181 additions & 105 deletions mock/cpp_mock_scenarios/launch/mock_test.launch.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -30,7 +30,7 @@
from launch.events import Shutdown
from launch.event_handlers import OnProcessExit, OnProcessIO

from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction
from launch.actions import EmitEvent, RegisterEventHandler, LogInfo, TimerAction, OpaqueFunction
from launch.actions.declare_launch_argument import DeclareLaunchArgument
from launch.substitutions.launch_configuration import LaunchConfiguration

Expand All @@ -40,7 +40,6 @@

from pathlib import Path


class Color:
BLACK = "\033[30m"
RED = "\033[31m"
Expand Down Expand Up @@ -70,113 +69,190 @@ def on_stdout_output(event: launch.Event) -> None:
if lines[0] == "cpp_scenario:success":
print(Color.GREEN + "Scenario Succeed" + Color.END)

def architecture_types():
return ["awf/universe", "awf/universe/20230906"]

def generate_launch_description():
timeout = LaunchConfiguration("timeout", default=10.0)
scenario = LaunchConfiguration("scenario", default="")
scenario_package = LaunchConfiguration("package", default="cpp_mock_scenarios")
junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml")
launch_rviz = LaunchConfiguration("launch_rviz", default=False)
vehicle_model = LaunchConfiguration("vehicle_model", default="sample_vehicle")
sensor_model = LaunchConfiguration("sensor_model", default="sample_sensor_kit")
consider_pose_by_road_slope = LaunchConfiguration("consider_pose_by_road_slope", default=True)
scenario_node = Node(
package=scenario_package,
executable=scenario,
name="scenario_node",
output="screen",
arguments=[("__log_level:=info")],
parameters=[
{
"junit_path": junit_path,
"timeout": timeout,
"architecture_type": "awf/universe",
"autoware_launch_file": "planning_simulator.launch.xml",
"autoware_launch_package": "autoware_launch",
"launch_autoware": True,
"vehicle_model": vehicle_model,
"sensor_model": sensor_model,
"initialize_duration": 900,
"consider_pose_by_road_slope": consider_pose_by_road_slope
}
],
)

def default_autoware_launch_package_of(architecture_type):
if architecture_type not in architecture_types():
raise KeyError(
f"architecture_type := {architecture_type} is not supported. Choose one of {architecture_types()}."
)
return {
"awf/universe": "autoware_launch",
"awf/universe/20230906": "autoware_launch",
}[architecture_type]


def default_autoware_launch_file_of(architecture_type):
if architecture_type not in architecture_types():
raise KeyError(
f"architecture_type := {architecture_type} is not supported. Choose one of {architecture_types()}."
)
return {
"awf/universe": "planning_simulator.launch.xml",
"awf/universe/20230906": "planning_simulator.launch.xml",
}[architecture_type]

def default_rviz_config_file():
return Path(get_package_share_directory("traffic_simulator")) / "config/scenario_simulator_v2.rviz"

def launch_setup(context, *args, **kwargs):
# fmt: off
architecture_type = LaunchConfiguration("architecture_type", default="awf/universe/20230906")
autoware_launch_file = LaunchConfiguration("autoware_launch_file", default=default_autoware_launch_file_of(architecture_type.perform(context)))
autoware_launch_package = LaunchConfiguration("autoware_launch_package", default=default_autoware_launch_package_of(architecture_type.perform(context)))
consider_acceleration_by_road_slope = LaunchConfiguration("consider_acceleration_by_road_slope", default=False)
consider_pose_by_road_slope = LaunchConfiguration("consider_pose_by_road_slope", default=True)
global_frame_rate = LaunchConfiguration("global_frame_rate", default=20.0)
global_real_time_factor = LaunchConfiguration("global_real_time_factor", default=1.0)
global_timeout = LaunchConfiguration("global_timeout", default=180)
initialize_duration = LaunchConfiguration("initialize_duration", default=300)
launch_autoware = LaunchConfiguration("launch_autoware", default=True)
launch_rviz = LaunchConfiguration("launch_rviz", default=False)
launch_simple_sensor_simulator = LaunchConfiguration("launch_simple_sensor_simulator", default=True)
output_directory = LaunchConfiguration("output_directory", default=Path("/tmp"))
port = LaunchConfiguration("port", default=5555)
publish_empty_context = LaunchConfiguration("publish_empty_context", default=False)
record = LaunchConfiguration("record", default=False)
rviz_config = LaunchConfiguration("rviz_config", default=default_rviz_config_file())
scenario = LaunchConfiguration("scenario", default=Path("/dev/null"))
sensor_model = LaunchConfiguration("sensor_model", default="")
sigterm_timeout = LaunchConfiguration("sigterm_timeout", default=8)
use_sim_time = LaunchConfiguration("use_sim_time", default=False)
vehicle_model = LaunchConfiguration("vehicle_model", default="")
scenario_package = LaunchConfiguration("package", default="cpp_mock_scenarios")
junit_path = LaunchConfiguration("junit_path", default="/tmp/output.xunit.xml")
# fmt: on

print(f"architecture_type := {architecture_type.perform(context)}")
print(f"autoware_launch_file := {autoware_launch_file.perform(context)}")
print(f"autoware_launch_package := {autoware_launch_package.perform(context)}")
print(f"consider_acceleration_by_road_slope := {consider_acceleration_by_road_slope.perform(context)}")
print(f"consider_pose_by_road_slope := {consider_pose_by_road_slope.perform(context)}")
print(f"global_frame_rate := {global_frame_rate.perform(context)}")
print(f"global_real_time_factor := {global_real_time_factor.perform(context)}")
print(f"global_timeout := {global_timeout.perform(context)}")
print(f"initialize_duration := {initialize_duration.perform(context)}")
print(f"launch_autoware := {launch_autoware.perform(context)}")
print(f"launch_rviz := {launch_rviz.perform(context)}")
print(f"output_directory := {output_directory.perform(context)}")
print(f"port := {port.perform(context)}")
print(f"publish_empty_context := {publish_empty_context.perform(context)}")
print(f"record := {record.perform(context)}")
print(f"rviz_config := {rviz_config.perform(context)}")
print(f"scenario := {scenario.perform(context)}")
print(f"sensor_model := {sensor_model.perform(context)}")
print(f"sigterm_timeout := {sigterm_timeout.perform(context)}")
print(f"use_sim_time := {use_sim_time.perform(context)}")
print(f"vehicle_model := {vehicle_model.perform(context)}")
print(f"scenario_package := {scenario_package.perform(context)}")
print(f"junit_path := {junit_path.perform(context)}")

def make_parameters():
parameters = [
{"architecture_type": architecture_type},
{"autoware_launch_file": autoware_launch_file},
{"autoware_launch_package": autoware_launch_package},
{"consider_acceleration_by_road_slope": consider_acceleration_by_road_slope},
{"consider_pose_by_road_slope": consider_pose_by_road_slope},
{"initialize_duration": initialize_duration},
{"launch_autoware": launch_autoware},
{"port": port},
{"publish_empty_context" : publish_empty_context},
{"record": record},
{"rviz_config": rviz_config},
{"sensor_model": sensor_model},
{"sigterm_timeout": sigterm_timeout},
{"vehicle_model": vehicle_model},
{"global_real_time_factor": global_real_time_factor},
{"global_frame_rate": global_frame_rate},
{"global_timeout": global_timeout},
{"junit_path": junit_path},
]
parameters += make_vehicle_parameters()
return parameters

def make_vehicle_parameters():
parameters = []

def description():
return get_package_share_directory(
vehicle_model.perform(context) + "_description"
)

if vehicle_model.perform(context):
parameters.append(description() + "/config/vehicle_info.param.yaml")
parameters.append(description() + "/config/simulator_model.param.yaml")
return parameters

cpp_scenario_node = Node(
package=scenario_package,
executable=scenario,
name="scenario_node",
output="screen",
arguments=[("__log_level:=info")],
parameters=make_parameters() + [{"use_sim_time": use_sim_time}],
)
io_handler = OnProcessIO(
target_action=scenario_node,
target_action=cpp_scenario_node,
on_stderr=on_stderr_output,
on_stdout=on_stdout_output,
)
shutdown_handler = OnProcessExit(
target_action=scenario_node, on_exit=[EmitEvent(event=Shutdown())]
target_action=cpp_scenario_node, on_exit=[EmitEvent(event=Shutdown())]
)
description = LaunchDescription(
[
DeclareLaunchArgument(
"scenario", default_value=scenario, description="Name of the scenario."
),
DeclareLaunchArgument(
"package",
default_value=scenario_package,
description="Name of package your scenario exists",
),
DeclareLaunchArgument(
"timeout", default_value=timeout, description="Timeout in seconds."
),
DeclareLaunchArgument(
"junit_path",
default_value=junit_path,
description="Path of the junit output.",
),
DeclareLaunchArgument(
"launch_rviz",
default_value=launch_rviz,
description="If true, launch with rviz.",
),
DeclareLaunchArgument(
"vehicle_model",
default_value=vehicle_model,
description="Vehicle model of the Autoware",
),
DeclareLaunchArgument(
"sensor_model",
default_value=sensor_model,
description="Sensor model of the Autoware",
),
DeclareLaunchArgument(
"consider_pose_by_road_slope",
default_value=consider_pose_by_road_slope ,
description="Consideration of lanelet slope in matching position",
),
scenario_node,
RegisterEventHandler(event_handler=io_handler),
RegisterEventHandler(event_handler=shutdown_handler),
Node(
package="simple_sensor_simulator",
executable="simple_sensor_simulator_node",
name="simple_sensor_simulator_node",
output="log",
arguments=[("__log_level:=warn")],
),
Node(
package="traffic_simulator",
executable="visualization_node",
name="visualizer",
output="screen",
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output={"stderr": "log", "stdout": "log"},
condition=IfCondition(launch_rviz),
arguments=[
"-d",
str(
Path(get_package_share_directory("cpp_mock_scenarios"))
/ "rviz/mock_test.rviz"
),
],
),
]
)
return description

return [
# fmt: off
DeclareLaunchArgument("architecture_type", default_value=architecture_type ),
DeclareLaunchArgument("autoware_launch_file", default_value=autoware_launch_file ),
DeclareLaunchArgument("autoware_launch_package", default_value=autoware_launch_package ),
DeclareLaunchArgument("consider_acceleration_by_road_slope", default_value=consider_acceleration_by_road_slope),
DeclareLaunchArgument("consider_pose_by_road_slope", default_value=consider_pose_by_road_slope ),
DeclareLaunchArgument("global_frame_rate", default_value=global_frame_rate ),
DeclareLaunchArgument("global_real_time_factor", default_value=global_real_time_factor ),
DeclareLaunchArgument("global_timeout", default_value=global_timeout ),
DeclareLaunchArgument("launch_autoware", default_value=launch_autoware ),
DeclareLaunchArgument("launch_rviz", default_value=launch_rviz ),
DeclareLaunchArgument("publish_empty_context", default_value=publish_empty_context ),
DeclareLaunchArgument("output_directory", default_value=output_directory ),
DeclareLaunchArgument("rviz_config", default_value=rviz_config ),
DeclareLaunchArgument("scenario", default_value=scenario ),
DeclareLaunchArgument("sensor_model", default_value=sensor_model ),
DeclareLaunchArgument("sigterm_timeout", default_value=sigterm_timeout ),
DeclareLaunchArgument("use_sim_time", default_value=use_sim_time ),
DeclareLaunchArgument("vehicle_model", default_value=vehicle_model ),
DeclareLaunchArgument("scenario_package", default_value=scenario_package ),
DeclareLaunchArgument("junit_path", default_value=junit_path ),
# fmt: on
cpp_scenario_node,
Node(
package="simple_sensor_simulator",
executable="simple_sensor_simulator_node",
namespace="simulation",
output="screen",
parameters=make_parameters() + [{"use_sim_time": use_sim_time}],
condition=IfCondition(launch_simple_sensor_simulator),
),
Node(
package="traffic_simulator",
executable="visualization_node",
namespace="simulation",
name="visualizer",
output="screen",
),
Node(
package="rviz2",
executable="rviz2",
name="rviz2",
output={"stderr": "log", "stdout": "log"},
condition=IfCondition(launch_rviz),
arguments=["-d", str(default_rviz_config_file())],
),
RegisterEventHandler(event_handler=io_handler),
RegisterEventHandler(event_handler=shutdown_handler),
]

def generate_launch_description():
return LaunchDescription([OpaqueFunction(function=launch_setup)])
9 changes: 6 additions & 3 deletions mock/cpp_mock_scenarios/src/cpp_scenario_node.cpp
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -22,14 +22,17 @@ CppScenarioNode::CppScenarioNode(
const std::string & lanelet2_map_file, const std::string & scenario_filename, const bool verbose,
const rclcpp::NodeOptions & option)
: Node(node_name, option),
api_(this, configure(map_path, lanelet2_map_file, scenario_filename, verbose), 1.0, 20),
api_(
this, configure(map_path, lanelet2_map_file, scenario_filename, verbose),
declare_parameter<double>("global_real_time_factor", 1.0),
declare_parameter<double>("global_frame_rate", 20.0)),
scenario_filename_(scenario_filename),
exception_expect_(false)
{
declare_parameter<std::string>("junit_path", "/tmp");
get_parameter<std::string>("junit_path", junit_path_);
declare_parameter<double>("timeout", 10.0);
get_parameter<double>("timeout", timeout_);
declare_parameter<int>("global_timeout", 10.0);
get_parameter<int>("global_timeout", timeout_);

traffic_simulator::lanelet_pose::CanonicalizedLaneletPose::setConsiderPoseByRoadSlope([&]() {
if (not has_parameter("consider_pose_by_road_slope")) {
Expand Down
Loading