Skip to content

Commit

Permalink
feat(psim)!: preapre settings to launch localization modules on psim (a…
Browse files Browse the repository at this point in the history
…utowarefoundation#8212)

Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
  • Loading branch information
yuki-takagi-66 committed Sep 17, 2024
1 parent c9fb044 commit 13e04cc
Show file tree
Hide file tree
Showing 4 changed files with 129 additions and 27 deletions.
57 changes: 55 additions & 2 deletions launch/tier4_simulator_launch/launch/simulator.launch.xml
Original file line number Diff line number Diff line change
Expand Up @@ -5,13 +5,18 @@
<arg name="laserscan_based_occupancy_grid_map_param_path"/>
<arg name="occupancy_grid_map_updater"/>
<arg name="occupancy_grid_map_updater_param_path"/>

<arg name="localization_error_monitor_param_path"/>
<arg name="ekf_localizer_param_path"/>
<arg name="pose_initializer_param_path"/>
<arg name="twist2accel_param_path"/>

<arg name="launch_simulator_perception_modules" default="true"/>
<arg name="launch_dummy_perception"/>
<arg name="launch_dummy_vehicle"/>
<arg
name="localization_sim_mode"
description="Select localization mode. Options are 'none' or 'api'. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process."
description="Select localization mode. Options are 'none', 'api' or 'pose_twist_estimator'. 'pose_twist_estimator' starts most of the localization modules except for the ndt_scan_matcher. 'api' starts an external API for initial position estimation. 'none' does not start any localization-related process."
/>
<arg name="launch_dummy_doors"/>
<arg name="launch_diagnostic_converter"/>
Expand Down Expand Up @@ -129,7 +134,7 @@
</group>
</group>

<!-- Dummy localization -->
<!-- localization -->
<group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;none&quot;')">
<!-- Do nothing -->
</group>
Expand All @@ -147,6 +152,50 @@
</include>
</group>

<group if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')">
<group>
<!-- start name space localization -->
<push-ros-namespace namespace="localization"/>
<group>
<push-ros-namespace namespace="util"/>
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
<arg name="user_defined_initial_pose/enable" value="false"/>
<arg name="user_defined_initial_pose/pose" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]"/>
<arg name="ndt_enabled" value="false"/>
<arg name="gnss_enabled" value="false"/>
<arg name="ekf_enabled" value="true"/>
<arg name="yabloc_enabled" value="false"/>
<arg name="stop_check_enabled" value="false"/>
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
</include>
</group>

<group>
<push-ros-namespace namespace="twist_estimator"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/gyro_odometer.launch.xml"/>
</group>

<!-- pose_twist_fusion_filter module -->
<group>
<push-ros-namespace namespace="pose_twist_fusion_filter"/>
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
</group>
</group>
<!-- end name space localization -->
<group>
<push-ros-namespace namespace="sensing"/>
<arg name="input_vehicle_velocity_topic" default="/vehicle/status/velocity_status"/>
<arg name="output_twist_with_covariance" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
<arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.param.yaml"/>

<node pkg="vehicle_velocity_converter" exec="vehicle_velocity_converter_node" output="both">
<param from="$(var config_file)"/>
<remap from="velocity_status" to="$(var input_vehicle_velocity_topic)"/>
<remap from="twist_with_covariance" to="$(var output_twist_with_covariance)"/>
</node>
</group>
</group>

<!-- Dummy doors -->
<group if="$(var launch_dummy_doors)">
<include file="$(find-pkg-share vehicle_door_simulator)/launch/vehicle_door_simulator.launch.xml"/>
Expand All @@ -162,11 +211,15 @@
<!-- Simulator model -->
<group if="$(var launch_dummy_vehicle)">
<arg name="simulator_model" default="$(var vehicle_model_pkg)/config/simulator_model.param.yaml" description="path to the file of simulator model"/>
<!-- 'pose_only' mode publishes the pose topic as an alternative to ndt_localizer. 'full_motion' mode publishes the odometry and acceleration topics as an alternative to localization modules. -->
<let name="motion_publish_mode" value="pose_only" if="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
<let name="motion_publish_mode" value="full_motion" unless="$(eval '&quot;$(var localization_sim_mode)&quot;==&quot;pose_twist_estimator&quot;')"/>
<include file="$(find-pkg-share simple_planning_simulator)/launch/simple_planning_simulator.launch.py">
<arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
<arg name="simulator_model_param_file" value="$(var simulator_model)"/>
<arg name="initial_engage_state" value="$(var initial_engage_state)"/>
<arg name="raw_vehicle_cmd_converter_param_path" value="$(var raw_vehicle_cmd_converter_param_path)"/>
<arg name="motion_publish_mode" value="$(var motion_publish_mode)"/>
</include>
</group>
</launch>
Original file line number Diff line number Diff line change
Expand Up @@ -137,7 +137,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
rclcpp::Publisher<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_current_pose_;

rclcpp::Subscription<GearCommand>::SharedPtr sub_gear_cmd_;
rclcpp::Subscription<GearCommand>::SharedPtr sub_manual_gear_cmd_;
Expand Down Expand Up @@ -346,6 +346,12 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
*/
void publish_odometry(const Odometry & odometry);

/**
* @brief publish pose
* @param [in] odometry The odometry to publish its pose
*/
void publish_pose(const Odometry & odometry);

/**
* @brief publish steering
* @param [in] steer The steering to publish
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -41,6 +41,50 @@ def launch_setup(context, *args, **kwargs):
param_file=simulator_model_param_path, allow_substs=True
)

# Base remappings
remappings = [
("input/vector_map", "/map/vector_map"),
("input/initialpose", "/initialpose3d"),
("input/ackermann_control_command", "/control/command/control_cmd"),
("input/actuation_command", "/control/command/actuation_cmd"),
("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
("input/gear_command", "/control/command/gear_cmd"),
("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
("input/trajectory", "/planning/scenario_planning/trajectory"),
("input/engage", "/vehicle/engage"),
("input/control_mode_request", "/control/control_mode_request"),
("output/twist", "/vehicle/status/velocity_status"),
("output/imu", "/sensing/imu/imu_data"),
("output/steering", "/vehicle/status/steering_status"),
("output/gear_report", "/vehicle/status/gear_status"),
("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
("output/control_mode_report", "/vehicle/status/control_mode"),
]

# Additional remappings
if LaunchConfiguration("motion_publish_mode").perform(context) == "pose_only":
remappings.extend(
[
("output/odometry", "/simulation/debug/localization/kinematic_state"),
("output/acceleration", "/simulation/debug/localization/acceleration"),
("output/pose", "/localization/pose_estimator/pose_with_covariance"),
]
)
elif LaunchConfiguration("motion_publish_mode").perform(context) == "full_motion":
remappings.extend(
[
("output/odometry", "/localization/kinematic_state"),
("output/acceleration", "/localization/acceleration"),
(
"output/pose",
"/simulation/debug/localization/pose_estimator/pose_with_covariance",
),
]
)

simple_planning_simulator_node = Node(
package="simple_planning_simulator",
executable="simple_planning_simulator_exe",
Expand All @@ -55,29 +99,7 @@ def launch_setup(context, *args, **kwargs):
"initial_engage_state": LaunchConfiguration("initial_engage_state"),
},
],
remappings=[
("input/vector_map", "/map/vector_map"),
("input/initialpose", "/initialpose3d"),
("input/ackermann_control_command", "/control/command/control_cmd"),
("input/actuation_command", "/control/command/actuation_cmd"),
("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
("input/gear_command", "/control/command/gear_cmd"),
("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
("input/trajectory", "/planning/scenario_planning/trajectory"),
("input/engage", "/vehicle/engage"),
("input/control_mode_request", "/control/control_mode_request"),
("output/twist", "/vehicle/status/velocity_status"),
("output/odometry", "/localization/kinematic_state"),
("output/acceleration", "/localization/acceleration"),
("output/imu", "/sensing/imu/imu_data"),
("output/steering", "/vehicle/status/steering_status"),
("output/gear_report", "/vehicle/status/gear_status"),
("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
("output/control_mode_report", "/vehicle/status/control_mode"),
],
remappings=remappings,
)

# Determine if we should launch raw_vehicle_cmd_converter based on the vehicle_model_type
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -165,7 +165,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
create_publisher<TurnIndicatorsReport>("output/turn_indicators_report", QoS{1});
pub_hazard_lights_report_ =
create_publisher<HazardLightsReport>("output/hazard_lights_report", QoS{1});
pub_current_pose_ = create_publisher<PoseStamped>("output/debug/pose", QoS{1});
pub_current_pose_ = create_publisher<PoseWithCovarianceStamped>("output/pose", QoS{1});
pub_velocity_ = create_publisher<VelocityReport>("output/twist", QoS{1});
pub_odom_ = create_publisher<Odometry>("output/odometry", QoS{1});
pub_steer_ = create_publisher<SteeringReport>("output/steering", QoS{1});
Expand Down Expand Up @@ -444,6 +444,7 @@ void SimplePlanningSimulator::on_timer()

// publish vehicle state
publish_odometry(current_odometry_);
publish_pose(current_odometry_);
publish_velocity(current_velocity_);
publish_steering(current_steer_);
publish_acceleration();
Expand Down Expand Up @@ -749,6 +750,26 @@ void SimplePlanningSimulator::publish_odometry(const Odometry & odometry)
pub_odom_->publish(msg);
}

void SimplePlanningSimulator::publish_pose(const Odometry & odometry)
{
geometry_msgs::msg::PoseWithCovarianceStamped msg;

msg.pose = odometry.pose;
using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
constexpr auto COV_POS = 0.0225; // same value as current ndt output
constexpr auto COV_ANGLE = 0.000625; // same value as current ndt output
msg.pose.covariance.at(COV_IDX::X_X) = COV_POS;
msg.pose.covariance.at(COV_IDX::Y_Y) = COV_POS;
msg.pose.covariance.at(COV_IDX::Z_Z) = COV_POS;
msg.pose.covariance.at(COV_IDX::ROLL_ROLL) = COV_ANGLE;
msg.pose.covariance.at(COV_IDX::PITCH_PITCH) = COV_ANGLE;
msg.pose.covariance.at(COV_IDX::YAW_YAW) = COV_ANGLE;

msg.header.frame_id = origin_frame_id_;
msg.header.stamp = get_clock()->now();
pub_current_pose_->publish(msg);
}

void SimplePlanningSimulator::publish_steering(const SteeringReport & steer)
{
SteeringReport msg = steer;
Expand Down

0 comments on commit 13e04cc

Please sign in to comment.