Skip to content

Commit

Permalink
Merge pull request #1368 from tier4/fix/mock-test-launch-test
Browse files Browse the repository at this point in the history 
fix: mock test launch
  • Loading branch information
@hakuturu583
hakuturu583 authored Sep 11, 2024
2 parents 33a329d + 8c90aa4 commit 3f25017
Show file tree
Hide file tree
Showing 3 changed files with 188 additions and 109 deletions.
2 changes: 1 addition & 1 deletion mock/cpp_mock_scenarios/include/cpp_mock_scenarios/cpp_scenario_node.hpp
View file
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
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
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

0 comments on commit 3f25017

Please sign in to comment.