From 3383b7014af85b0169cc1e5be7ae96ae5bda196a Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Fri, 26 Jul 2024 16:56:46 +0900 Subject: [PATCH 1/5] rebase Signed-off-by: Yuki Takagi --- .../launch/simulator.launch.xml | 50 +++++++++++++- .../simple_planning_simulator_core.hpp | 8 ++- .../simple_planning_simulator.launch.py | 68 ++++++++++++------- .../simple_planning_simulator_core.cpp | 23 ++++++- 4 files changed, 123 insertions(+), 26 deletions(-) diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 5f6632e76cdf..9abf8021b87f 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -5,7 +5,12 @@ + + + + + @@ -130,7 +135,7 @@ - + @@ -148,6 +153,48 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -168,6 +215,7 @@ + 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 0ed603960a6c..547689dc847a 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 afdbeb120e2d..9e0f41ba8a85 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("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", @@ -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 3095e060a71b..4252ace6c192 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; From 863a4bf1acb09bbe7107c9bef669f95dd1b2ae98 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 14 Aug 2024 14:05:11 +0900 Subject: [PATCH 2/5] rebase to new mode name Signed-off-by: Yuki Takagi --- .../tier4_simulator_launch/launch/simulator.launch.xml | 10 +++++++--- .../launch/simple_planning_simulator.launch.py | 8 ++++++-- 2 files changed, 13 insertions(+), 5 deletions(-) diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 9abf8021b87f..af7144817ece 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -154,7 +154,8 @@ - + + @@ -180,7 +181,8 @@ - + + @@ -210,12 +212,14 @@ + + - + 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 9e0f41ba8a85..092afe782cfc 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -65,7 +65,7 @@ def launch_setup(context, *args, **kwargs): ] # Additional remappings - if LaunchConfiguration("launch_localization_for_sim_vehicle").perform(context) == "true": + if LaunchConfiguration("motion_publish_mode").perform(context) == "pose_only": remappings.extend( [ ("output/odometry", "/simulation/debug/localization/kinematic_state"), @@ -73,7 +73,7 @@ def launch_setup(context, *args, **kwargs): ("output/pose", "/localization/pose_estimator/pose_with_covariance"), ] ) - else: + elif LaunchConfiguration("motion_publish_mode").perform(context) == "full_motion": remappings.extend( [ ("output/odometry", "/localization/kinematic_state"), @@ -84,6 +84,10 @@ def launch_setup(context, *args, **kwargs): ), ] ) + else: + raise ValueError( + "Invalid motion_publish_mode specified. Please choose 'pose_only' or 'full_motion'." + ) simple_planning_simulator_node = Node( package="simple_planning_simulator", From 044aa3da38049a2afbcacd28a72f63dbbcfa48ec Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 14 Aug 2024 17:41:19 +0900 Subject: [PATCH 3/5] add commnent Signed-off-by: Yuki Takagi --- launch/tier4_simulator_launch/launch/simulator.launch.xml | 1 + 1 file changed, 1 insertion(+) diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index af7144817ece..8f350f7aff8e 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -212,6 +212,7 @@ + From a96d1247e2fedbad48df5b2d4c8ff4da1bd8c59c Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Wed, 14 Aug 2024 18:15:55 +0900 Subject: [PATCH 4/5] add comment Signed-off-by: Yuki Takagi --- launch/tier4_simulator_launch/launch/simulator.launch.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/launch/tier4_simulator_launch/launch/simulator.launch.xml b/launch/tier4_simulator_launch/launch/simulator.launch.xml index 8f350f7aff8e..5dec24c66f6c 100644 --- a/launch/tier4_simulator_launch/launch/simulator.launch.xml +++ b/launch/tier4_simulator_launch/launch/simulator.launch.xml @@ -16,7 +16,7 @@ From 9b03ebe43297cd8c94d7fc8e2e5a3a91c7c96921 Mon Sep 17 00:00:00 2001 From: Yuki Takagi Date: Thu, 15 Aug 2024 13:20:52 +0900 Subject: [PATCH 5/5] delete exception code Signed-off-by: Yuki Takagi --- .../launch/simple_planning_simulator.launch.py | 4 ---- 1 file changed, 4 deletions(-) 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 092afe782cfc..e6a35bd420bf 100644 --- a/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py +++ b/simulator/simple_planning_simulator/launch/simple_planning_simulator.launch.py @@ -84,10 +84,6 @@ def launch_setup(context, *args, **kwargs): ), ] ) - else: - raise ValueError( - "Invalid motion_publish_mode specified. Please choose 'pose_only' or 'full_motion'." - ) simple_planning_simulator_node = Node( package="simple_planning_simulator",