diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml
index 5f6632e76cdfa..5dec24c66f6ca 100644
--- a/launch/tier4_simulator_launch/launch/simulator.launch.xml
+++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml
@@ -5,13 +5,18 @@
+
+
+
+
+
@@ -130,7 +135,7 @@
-
+
@@ -148,6 +153,50 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
@@ -163,11 +212,15 @@
+
+
+
+
diff --git a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
index 0ed603960a6c5..547689dc847a0 100644
--- a/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
+++ b/simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
@@ -137,7 +137,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
rclcpp::Publisher::SharedPtr pub_turn_indicators_report_;
rclcpp::Publisher::SharedPtr pub_hazard_lights_report_;
rclcpp::Publisher::SharedPtr pub_tf_;
- rclcpp::Publisher::SharedPtr pub_current_pose_;
+ rclcpp::Publisher::SharedPtr pub_current_pose_;
rclcpp::Subscription::SharedPtr sub_gear_cmd_;
rclcpp::Subscription::SharedPtr sub_manual_gear_cmd_;
@@ -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
diff --git a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
index afdbeb120e2d3..e6a35bd420bf3 100644
--- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
+++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
@@ -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",
@@ -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
diff --git a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
index 3095e060a71bf..4252ace6c1920 100644
--- a/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
+++ b/simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
@@ -165,7 +165,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
create_publisher("output/turn_indicators_report", QoS{1});
pub_hazard_lights_report_ =
create_publisher("output/hazard_lights_report", QoS{1});
- pub_current_pose_ = create_publisher("output/debug/pose", QoS{1});
+ pub_current_pose_ = create_publisher("output/pose", QoS{1});
pub_velocity_ = create_publisher("output/twist", QoS{1});
pub_odom_ = create_publisher("output/odometry", QoS{1});
pub_steer_ = create_publisher("output/steering", QoS{1});
@@ -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();
@@ -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;