Skip to content

Commit e670841

Browse files
yuki-takagi-66yuki-takagi-66
committed
add localizatoin on psim
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
1 parent 73ed8b8 commit e670841

File tree

4 files changed

+136
-34
lines changed

4 files changed

+136
-34
lines changed

‎launch/tier4_simulator_launch/launch/simulator.launch.xml

+62-10
Original file line numberDiff line numberDiff line change
@@ -5,11 +5,17 @@
55
<arg name="laserscan_based_occupancy_grid_map_param_path"/>
66
<arg name="occupancy_grid_map_updater"/>
77
<arg name="occupancy_grid_map_updater_param_path"/>
8+
9+
<arg name="localization_error_monitor_param_path"/>
10+
<arg name="ekf_localizer_param_path"/>
811
<arg name="pose_initializer_param_path"/>
12+
<arg name="twist2accel_param_path"/>
13+
914
<arg name="launch_simulator_perception_modules" default="true"/>
1015
<arg name="launch_dummy_perception"/>
1116
<arg name="launch_dummy_vehicle"/>
1217
<arg name="launch_dummy_localization"/>
18+
<arg name="launch_localization_for_sim_vehicle"/>
1319
<arg name="launch_dummy_doors"/>
1420
<arg name="launch_diagnostic_converter"/>
1521
<arg name="vehicle_info_param_file"/>
@@ -127,16 +133,61 @@
127133

128134
<!-- Dummy localization -->
129135
<group if="$(var launch_dummy_localization)">
130-
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
131-
<arg name="user_defined_initial_pose/enable" value="false"/>
132-
<arg name="user_defined_initial_pose/pose" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]"/>
133-
<arg name="ndt_enabled" value="false"/>
134-
<arg name="gnss_enabled" value="false"/>
135-
<arg name="ekf_enabled" value="false"/>
136-
<arg name="yabloc_enabled" value="false"/>
137-
<arg name="stop_check_enabled" value="false"/>
138-
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
139-
</include>
136+
<group unless="$(var launch_localization_for_sim_vehicle)">
137+
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
138+
<arg name="user_defined_initial_pose/enable" value="false"/>
139+
<arg name="user_defined_initial_pose/pose" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]"/>
140+
<arg name="ndt_enabled" value="false"/>
141+
<arg name="gnss_enabled" value="false"/>
142+
<arg name="ekf_enabled" value="false"/>
143+
<arg name="yabloc_enabled" value="false"/>
144+
<arg name="stop_check_enabled" value="false"/>
145+
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
146+
</include>
147+
</group>
148+
<group if="$(var launch_localization_for_sim_vehicle)">
149+
<group>
150+
<!-- start name space localization -->
151+
<push-ros-namespace namespace="localization"/>
152+
<group>
153+
<push-ros-namespace namespace="util"/>
154+
<include file="$(find-pkg-share pose_initializer)/launch/pose_initializer.launch.xml">
155+
<arg name="user_defined_initial_pose/enable" value="false"/>
156+
<arg name="user_defined_initial_pose/pose" value="[0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 1.0]"/>
157+
<arg name="ndt_enabled" value="false"/>
158+
<arg name="gnss_enabled" value="false"/>
159+
<arg name="ekf_enabled" value="true"/>
160+
<arg name="yabloc_enabled" value="false"/>
161+
<arg name="stop_check_enabled" value="false"/>
162+
<arg name="config_file" value="$(var pose_initializer_param_path)"/>
163+
</include>
164+
</group>
165+
166+
<group>
167+
<push-ros-namespace namespace="twist_estimator"/>
168+
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_estimator/gyro_odometer.launch.xml"/>
169+
</group>
170+
171+
<!-- pose_twist_fusion_filter module -->
172+
<group>
173+
<push-ros-namespace namespace="pose_twist_fusion_filter"/>
174+
<include file="$(find-pkg-share tier4_localization_launch)/launch/pose_twist_fusion_filter/pose_twist_fusion_filter.launch.xml"/>
175+
</group>
176+
</group>
177+
<!-- end name space localization -->
178+
<group>
179+
<push-ros-namespace namespace="sensing"/>
180+
<arg name="input_vehicle_velocity_topic" default="/vehicle/status/velocity_status"/>
181+
<arg name="output_twist_with_covariance" default="/sensing/vehicle_velocity_converter/twist_with_covariance"/>
182+
<arg name="config_file" default="$(find-pkg-share vehicle_velocity_converter)/config/vehicle_velocity_converter.param.yaml"/>
183+
184+
<node pkg="vehicle_velocity_converter" exec="vehicle_velocity_converter_node" output="both">
185+
<param from="$(var config_file)"/>
186+
<remap from="velocity_status" to="$(var input_vehicle_velocity_topic)"/>
187+
<remap from="twist_with_covariance" to="$(var output_twist_with_covariance)"/>
188+
</node>
189+
</group>
190+
</group>
140191
</group>
141192

142193
<!-- Dummy doors -->
@@ -158,6 +209,7 @@
158209
<arg name="vehicle_info_param_file" value="$(var vehicle_info_param_file)"/>
159210
<arg name="simulator_model_param_file" value="$(var simulator_model)"/>
160211
<arg name="initial_engage_state" value="$(var initial_engage_state)"/>
212+
<arg name="launch_localization_for_sim_vehicle" value="$(var launch_localization_for_sim_vehicle)"/>
161213
</include>
162214
</group>
163215
</launch>

‎simulator/simple_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp

+7-1
Original file line numberDiff line numberDiff line change
@@ -132,7 +132,7 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
132132
rclcpp::Publisher<TurnIndicatorsReport>::SharedPtr pub_turn_indicators_report_;
133133
rclcpp::Publisher<HazardLightsReport>::SharedPtr pub_hazard_lights_report_;
134134
rclcpp::Publisher<tf2_msgs::msg::TFMessage>::SharedPtr pub_tf_;
135-
rclcpp::Publisher<PoseStamped>::SharedPtr pub_current_pose_;
135+
rclcpp::Publisher<PoseWithCovarianceStamped>::SharedPtr pub_current_pose_;
136136

137137
rclcpp::Subscription<GearCommand>::SharedPtr sub_gear_cmd_;
138138
rclcpp::Subscription<GearCommand>::SharedPtr sub_manual_gear_cmd_;
@@ -331,6 +331,12 @@ class PLANNING_SIMULATOR_PUBLIC SimplePlanningSimulator : public rclcpp::Node
331331
*/
332332
void publish_odometry(const Odometry & odometry);
333333

334+
/**
335+
* @brief publish pose
336+
* @param [in] odometry The odometry to publish its pose
337+
*/
338+
void publish_pose(const Odometry & odometry);
339+
334340
/**
335341
* @brief publish steering
336342
* @param [in] steer The steering to publish

‎simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py

+45-22
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,50 @@ def launch_setup(context, *args, **kwargs):
3939
param_file=simulator_model_param_path, allow_substs=True
4040
)
4141

42+
# Base remappings
43+
remappings = [
44+
("input/vector_map", "/map/vector_map"),
45+
("input/initialpose", "/initialpose3d"),
46+
("input/ackermann_control_command", "/control/command/control_cmd"),
47+
("input/actuation_command", "/control/command/actuation_cmd"),
48+
("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
49+
("input/gear_command", "/control/command/gear_cmd"),
50+
("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
51+
("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
52+
("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
53+
("input/trajectory", "/planning/scenario_planning/trajectory"),
54+
("input/engage", "/vehicle/engage"),
55+
("input/control_mode_request", "/control/control_mode_request"),
56+
("output/twist", "/vehicle/status/velocity_status"),
57+
("output/imu", "/sensing/imu/imu_data"),
58+
("output/steering", "/vehicle/status/steering_status"),
59+
("output/gear_report", "/vehicle/status/gear_status"),
60+
("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
61+
("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
62+
("output/control_mode_report", "/vehicle/status/control_mode"),
63+
]
64+
65+
# Additional remappings
66+
if LaunchConfiguration("launch_localization_for_sim_vehicle").perform(context) == "true":
67+
remappings.extend(
68+
[
69+
("output/odometry", "/simulation/debug/localization/kinematic_state"),
70+
("output/acceleration", "/simulation/debug/localization/acceleration"),
71+
("output/pose", "/localization/pose_estimator/pose_with_covariance"),
72+
]
73+
)
74+
else:
75+
remappings.extend(
76+
[
77+
("output/odometry", "/localization/kinematic_state"),
78+
("output/acceleration", "/localization/acceleration"),
79+
(
80+
"output/pose",
81+
"/simulation/debug/localization/pose_estimator/pose_with_covariance",
82+
),
83+
]
84+
)
85+
4286
simple_planning_simulator_node = Node(
4387
package="simple_planning_simulator",
4488
executable="simple_planning_simulator_exe",
@@ -53,28 +97,7 @@ def launch_setup(context, *args, **kwargs):
5397
"initial_engage_state": LaunchConfiguration("initial_engage_state"),
5498
},
5599
],
56-
remappings=[
57-
("input/vector_map", "/map/vector_map"),
58-
("input/initialpose", "/initialpose3d"),
59-
("input/ackermann_control_command", "/control/command/control_cmd"),
60-
("input/manual_ackermann_control_command", "/vehicle/command/manual_control_cmd"),
61-
("input/gear_command", "/control/command/gear_cmd"),
62-
("input/manual_gear_command", "/vehicle/command/manual_gear_command"),
63-
("input/turn_indicators_command", "/control/command/turn_indicators_cmd"),
64-
("input/hazard_lights_command", "/control/command/hazard_lights_cmd"),
65-
("input/trajectory", "/planning/scenario_planning/trajectory"),
66-
("input/engage", "/vehicle/engage"),
67-
("input/control_mode_request", "/control/control_mode_request"),
68-
("output/twist", "/vehicle/status/velocity_status"),
69-
("output/odometry", "/localization/kinematic_state"),
70-
("output/acceleration", "/localization/acceleration"),
71-
("output/imu", "/sensing/imu/imu_data"),
72-
("output/steering", "/vehicle/status/steering_status"),
73-
("output/gear_report", "/vehicle/status/gear_status"),
74-
("output/turn_indicators_report", "/vehicle/status/turn_indicators_status"),
75-
("output/hazard_lights_report", "/vehicle/status/hazard_lights_status"),
76-
("output/control_mode_report", "/vehicle/status/control_mode"),
77-
],
100+
remappings=remappings,
78101
)
79102

80103
return [simple_planning_simulator_node]

‎simulator/simple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp

+22-1
Original file line numberDiff line numberDiff line change
@@ -151,7 +151,7 @@ SimplePlanningSimulator::SimplePlanningSimulator(const rclcpp::NodeOptions & opt
151151
create_publisher<TurnIndicatorsReport>("output/turn_indicators_report", QoS{1});
152152
pub_hazard_lights_report_ =
153153
create_publisher<HazardLightsReport>("output/hazard_lights_report", QoS{1});
154-
pub_current_pose_ = create_publisher<PoseStamped>("output/debug/pose", QoS{1});
154+
pub_current_pose_ = create_publisher<PoseWithCovarianceStamped>("output/pose", QoS{1});
155155
pub_velocity_ = create_publisher<VelocityReport>("output/twist", QoS{1});
156156
pub_odom_ = create_publisher<Odometry>("output/odometry", QoS{1});
157157
pub_steer_ = create_publisher<SteeringReport>("output/steering", QoS{1});
@@ -411,6 +411,7 @@ void SimplePlanningSimulator::on_timer()
411411

412412
// publish vehicle state
413413
publish_odometry(current_odometry_);
414+
publish_pose(current_odometry_);
414415
publish_velocity(current_velocity_);
415416
publish_steering(current_steer_);
416417
publish_acceleration();
@@ -684,6 +685,26 @@ void SimplePlanningSimulator::publish_odometry(const Odometry & odometry)
684685
pub_odom_->publish(msg);
685686
}
686687

688+
void SimplePlanningSimulator::publish_pose(const Odometry & odometry)
689+
{
690+
geometry_msgs::msg::PoseWithCovarianceStamped msg;
691+
692+
msg.pose = odometry.pose;
693+
using COV_IDX = autoware::universe_utils::xyzrpy_covariance_index::XYZRPY_COV_IDX;
694+
constexpr auto COV_POS = 0.0225; // same value as current ndt output
695+
constexpr auto COV_ANGLE = 0.000625; // same value as current ndt output
696+
msg.pose.covariance.at(COV_IDX::X_X) = COV_POS;
697+
msg.pose.covariance.at(COV_IDX::Y_Y) = COV_POS;
698+
msg.pose.covariance.at(COV_IDX::Z_Z) = COV_POS;
699+
msg.pose.covariance.at(COV_IDX::ROLL_ROLL) = COV_ANGLE;
700+
msg.pose.covariance.at(COV_IDX::PITCH_PITCH) = COV_ANGLE;
701+
msg.pose.covariance.at(COV_IDX::YAW_YAW) = COV_ANGLE;
702+
703+
msg.header.frame_id = origin_frame_id_;
704+
msg.header.stamp = get_clock()->now();
705+
pub_current_pose_->publish(msg);
706+
}
707+
687708
void SimplePlanningSimulator::publish_steering(const SteeringReport & steer)
688709
{
689710
SteeringReport msg = steer;

0 commit comments

Comments
 (0)