Skip to content

Commit

Permalink
rebase
Browse files Browse the repository at this point in the history 
Signed-off-by: Yuki Takagi <yuki.takagi@tier4.jp>
  • Loading branch information
@yuki-takagi-66
yuki-takagi-66 committed Jul 26, 2024
1 parent 95aef0d commit 67cc848
Show file tree
Hide file tree
Showing 4 changed files with 136 additions and 35 deletions.
72 changes: 62 additions & 10 deletions launch/tier4_simulator_launch/launch/simulator.launch.xml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,11 +5,17 @@
<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="launch_dummy_localization"/>
<arg name="launch_localization_for_sim_vehicle"/>
<arg name="launch_dummy_doors"/>
<arg name="launch_diagnostic_converter"/>
<arg name="vehicle_info_param_file"/>
Expand Down Expand Up @@ -129,16 +135,61 @@

<!-- Dummy localization -->
<group if="$(var launch_dummy_localization)">
<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="false"/>
<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 unless="$(var launch_localization_for_sim_vehicle)">
<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="false"/>
<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 if="$(var launch_localization_for_sim_vehicle)">
<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>
</group>

<!-- Dummy doors -->
Expand All @@ -161,6 +212,7 @@
<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="launch_localization_for_sim_vehicle" value="$(var launch_localization_for_sim_vehicle)"/>
</include>
</group>
</launch>
8 changes: 7 additions & 1 deletion ...e_planning_simulator/include/simple_planning_simulator/simple_planning_simulator_core.hpp
View file
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
68 changes: 45 additions & 23 deletions simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py
View file
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("launch_localization_for_sim_vehicle").perform(context) == "true":
remappings.extend(
[
("output/odometry", "/simulation/debug/localization/kinematic_state"),
("output/acceleration", "/simulation/debug/localization/acceleration"),
("output/pose", "/localization/pose_estimator/pose_with_covariance"),
]
)
else:
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
23 changes: 22 additions & 1 deletion ...imple_planning_simulator/src/simple_planning_simulator/simple_planning_simulator_core.cpp
View file
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 67cc848

Please sign in to comment.