Skip to content

Commit 83d6a99

Browse files
isamu-takagiisamu-takagi
committed
Merge branch 'docs/update-diag-graph-agg-docs' into feat/diag-graph-agg-tools
2 parents 5651715 + fd32dac commit 83d6a99

File tree

5 files changed

+144
-96
lines changed

5 files changed

+144
-96
lines changed

‎control/pid_longitudinal_controller/include/pid_longitudinal_controller/pid_longitudinal_controller.hpp

+11-1
Original file line numberDiff line numberDiff line change
@@ -25,6 +25,7 @@
2525
#include "tf2/utils.h"
2626
#include "tf2_ros/buffer.h"
2727
#include "tf2_ros/transform_listener.h"
28+
#include "tier4_autoware_utils/ros/marker_helper.hpp"
2829
#include "trajectory_follower_base/longitudinal_controller_base.hpp"
2930
#include "vehicle_info_util/vehicle_info_util.hpp"
3031

@@ -39,6 +40,7 @@
3940
#include "nav_msgs/msg/odometry.hpp"
4041
#include "tf2_msgs/msg/tf_message.hpp"
4142
#include "tier4_debug_msgs/msg/float32_multi_array_stamped.hpp"
43+
#include "visualization_msgs/msg/marker.hpp"
4244

4345
#include <deque>
4446
#include <memory>
@@ -50,6 +52,11 @@
5052
namespace autoware::motion::control::pid_longitudinal_controller
5153
{
5254
using autoware_adapi_v1_msgs::msg::OperationModeState;
55+
using tier4_autoware_utils::createDefaultMarker;
56+
using tier4_autoware_utils::createMarkerColor;
57+
using tier4_autoware_utils::createMarkerScale;
58+
using visualization_msgs::msg::Marker;
59+
5360
namespace trajectory_follower = ::autoware::motion::control::trajectory_follower;
5461

5562
/// \class PidLongitudinalController
@@ -97,6 +104,7 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
97104
// ros variables
98105
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_slope;
99106
rclcpp::Publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>::SharedPtr m_pub_debug;
107+
rclcpp::Publisher<Marker>::SharedPtr m_pub_stop_reason_marker;
100108

101109
rclcpp::Node::OnSetParametersCallbackHandle::SharedPtr m_set_param_res;
102110
rcl_interfaces::msg::SetParametersResult paramCallback(
@@ -109,7 +117,9 @@ class PidLongitudinalController : public trajectory_follower::LongitudinalContro
109117
OperationModeState m_current_operation_mode;
110118

111119
// vehicle info
112-
double m_wheel_base;
120+
double m_wheel_base{0.0};
121+
double m_vehicle_width{0.0};
122+
double m_front_overhang{0.0};
113123
bool m_prev_vehicle_is_under_control{false};
114124
std::shared_ptr<rclcpp::Time> m_under_control_starting_time{nullptr};
115125

‎control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

+13-2
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,8 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
3939
m_longitudinal_ctrl_period = node.get_parameter("ctrl_period").as_double();
4040

4141
m_wheel_base = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().wheel_base_m;
42+
m_vehicle_width = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().vehicle_width_m;
43+
m_front_overhang = vehicle_info_util::VehicleInfoUtil(node).getVehicleInfo().front_overhang_m;
4244

4345
// parameters for delay compensation
4446
m_delay_compensation_time = node.declare_parameter<double>("delay_compensation_time"); // [s]
@@ -204,6 +206,7 @@ PidLongitudinalController::PidLongitudinalController(rclcpp::Node & node)
204206
"~/output/slope_angle", rclcpp::QoS{1});
205207
m_pub_debug = node.create_publisher<tier4_debug_msgs::msg::Float32MultiArrayStamped>(
206208
"~/output/longitudinal_diagnostic", rclcpp::QoS{1});
209+
m_pub_stop_reason_marker = node.create_publisher<Marker>("~/output/stop_reason", rclcpp::QoS{1});
207210

208211
// set parameter callback
209212
m_set_param_res = node.add_on_set_parameters_callback(
@@ -597,6 +600,16 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
597600
const bool keep_stopped_condition = std::fabs(current_vel) < vel_epsilon &&
598601
m_enable_keep_stopped_until_steer_convergence &&
599602
!lateral_sync_data_.is_steer_converged;
603+
if (keep_stopped_condition) {
604+
auto marker = createDefaultMarker(
605+
"map", clock_->now(), "stop_reason", 0, Marker::TEXT_VIEW_FACING,
606+
createMarkerScale(0.0, 0.0, 1.0), createMarkerColor(1.0, 1.0, 1.0, 0.999));
607+
marker.pose = tier4_autoware_utils::calcOffsetPose(
608+
m_current_kinematic_state.pose.pose, m_wheel_base + m_front_overhang,
609+
m_vehicle_width / 2 + 2.0, 1.5);
610+
marker.text = "steering not\nconverged";
611+
m_pub_stop_reason_marker->publish(marker);
612+
}
600613

601614
const bool stopping_condition = stop_dist < p.stopping_state_stop_dist;
602615

@@ -662,7 +675,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
662675
if (emergency_condition) {
663676
return changeState(ControlState::EMERGENCY);
664677
}
665-
666678
if (!is_under_control && stopped_condition && keep_stopped_condition) {
667679
// NOTE: When the ego is stopped on manual driving, since the driving state may transit to
668680
// autonomous, keep_stopped_condition should be checked.
@@ -692,7 +704,6 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
692704
if (emergency_condition) {
693705
return changeState(ControlState::EMERGENCY);
694706
}
695-
696707
if (stopped_condition) {
697708
return changeState(ControlState::STOPPED);
698709
}

‎launch/tier4_control_launch/launch/control.launch.py

+1
Original file line numberDiff line numberDiff line change
@@ -81,6 +81,7 @@ def launch_setup(context, *args, **kwargs):
8181
("~/output/lateral_diagnostic", "lateral/diagnostic"),
8282
("~/output/slope_angle", "longitudinal/slope_angle"),
8383
("~/output/longitudinal_diagnostic", "longitudinal/diagnostic"),
84+
("~/output/stop_reason", "longitudinal/stop_reason"),
8485
("~/output/control_cmd", "control_cmd"),
8586
],
8687
parameters=[

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

+87-84
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
<arg name="occupancy_grid_map_updater"/>
77
<arg name="occupancy_grid_map_updater_param_path"/>
88
<arg name="pose_initializer_param_path"/>
9-
9+
<arg name="launch_simulator_perception_modules" default="true"/>
1010
<arg name="launch_dummy_perception"/>
1111
<arg name="launch_dummy_vehicle"/>
1212
<arg name="launch_dummy_localization"/>
@@ -26,97 +26,100 @@
2626

2727
<let name="vehicle_model_pkg" value="$(find-pkg-share $(var vehicle_model)_description)"/>
2828

29-
<group if="$(var scenario_simulation)">
30-
<include file="$(find-pkg-share fault_injection)/launch/fault_injection.launch.xml">
31-
<arg name="config_file" value="$(var fault_injection_param_path)"/>
32-
</include>
33-
</group>
29+
<group if="$(var launch_simulator_perception_modules)">
30+
<group if="$(var scenario_simulation)">
31+
<include file="$(find-pkg-share fault_injection)/launch/fault_injection.launch.xml">
32+
<arg name="config_file" value="$(var fault_injection_param_path)"/>
33+
</include>
34+
</group>
3435

35-
<!-- Dummy Perception -->
36-
<group if="$(var launch_dummy_perception)">
37-
<include file="$(find-pkg-share dummy_perception_publisher)/launch/dummy_perception_publisher.launch.xml">
38-
<arg name="real" value="$(var perception/enable_detection_failure)"/>
39-
<arg name="use_object_recognition" value="$(var perception/enable_object_recognition)"/>
40-
<arg name="use_base_link_z" value="$(var perception/use_base_link_z)"/>
41-
<arg name="visible_range" value="$(var sensing/visible_range)"/>
42-
</include>
43-
</group>
44-
<group unless="$(var scenario_simulation)">
45-
<!-- Occupancy Grid -->
46-
<push-ros-namespace namespace="occupancy_grid_map"/>
47-
<include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
48-
<arg name="input_obstacle_pointcloud" value="true"/>
49-
<arg name="input_obstacle_and_raw_pointcloud" value="false"/>
50-
<arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud"/>
51-
<arg name="output" value="/perception/occupancy_grid_map/map"/>
52-
<arg name="occupancy_grid_map_method" value="laserscan_based_occupancy_grid_map"/>
53-
<arg name="occupancy_grid_map_param_path" value="$(var laserscan_based_occupancy_grid_map_param_path)"/>
54-
<arg name="occupancy_grid_map_updater" value="$(var occupancy_grid_map_updater)"/>
55-
<arg name="occupancy_grid_map_updater_param_path" value="$(var occupancy_grid_map_updater_param_path)"/>
56-
</include>
57-
</group>
36+
<!-- Dummy Perception -->
37+
<group if="$(var launch_dummy_perception)">
38+
<include file="$(find-pkg-share dummy_perception_publisher)/launch/dummy_perception_publisher.launch.xml">
39+
<arg name="real" value="$(var perception/enable_detection_failure)"/>
40+
<arg name="use_object_recognition" value="$(var perception/enable_object_recognition)"/>
41+
<arg name="use_base_link_z" value="$(var perception/use_base_link_z)"/>
42+
<arg name="visible_range" value="$(var sensing/visible_range)"/>
43+
</include>
44+
</group>
5845

59-
<!-- perception module -->
60-
<group>
61-
<push-ros-namespace namespace="perception"/>
62-
<!-- object recognition -->
63-
<group if="$(var perception/enable_object_recognition)">
64-
<push-ros-namespace namespace="object_recognition"/>
65-
<!-- tracking module -->
66-
<group>
67-
<push-ros-namespace namespace="tracking"/>
68-
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/tracking/tracking.launch.xml">
69-
<arg
70-
name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"
71-
value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"
72-
/>
73-
<arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
74-
<arg name="object_recognition_tracking_radar_object_tracker_node_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
75-
<arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
76-
<arg name="object_recognition_tracking_object_merger_node_param_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
77-
</include>
78-
</group>
79-
<!-- prediction module -->
80-
<group>
81-
<push-ros-namespace namespace="prediction"/>
82-
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/prediction/prediction.launch.xml">
83-
<arg name="use_vector_map" value="true"/>
84-
</include>
85-
</group>
46+
<group unless="$(var scenario_simulation)">
47+
<!-- Occupancy Grid -->
48+
<push-ros-namespace namespace="occupancy_grid_map"/>
49+
<include file="$(find-pkg-share tier4_perception_launch)/launch/occupancy_grid_map/probabilistic_occupancy_grid_map.launch.xml">
50+
<arg name="input_obstacle_pointcloud" value="true"/>
51+
<arg name="input_obstacle_and_raw_pointcloud" value="false"/>
52+
<arg name="input/obstacle_pointcloud" value="/perception/obstacle_segmentation/pointcloud"/>
53+
<arg name="output" value="/perception/occupancy_grid_map/map"/>
54+
<arg name="occupancy_grid_map_method" value="laserscan_based_occupancy_grid_map"/>
55+
<arg name="occupancy_grid_map_param_path" value="$(var laserscan_based_occupancy_grid_map_param_path)"/>
56+
<arg name="occupancy_grid_map_updater" value="$(var occupancy_grid_map_updater)"/>
57+
<arg name="occupancy_grid_map_updater_param_path" value="$(var occupancy_grid_map_updater_param_path)"/>
58+
</include>
8659
</group>
8760

88-
<!-- perception evaluator -->
61+
<!-- perception module -->
8962
<group>
90-
<include file="$(find-pkg-share perception_online_evaluator)/launch/perception_online_evaluator.launch.xml"/>
91-
</group>
63+
<push-ros-namespace namespace="perception"/>
64+
<!-- object recognition -->
65+
<group if="$(var perception/enable_object_recognition)">
66+
<push-ros-namespace namespace="object_recognition"/>
67+
<!-- tracking module -->
68+
<group>
69+
<push-ros-namespace namespace="tracking"/>
70+
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/tracking/tracking.launch.xml">
71+
<arg
72+
name="object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path"
73+
value="$(var object_recognition_tracking_radar_object_tracker_data_association_matrix_param_path)"
74+
/>
75+
<arg name="object_recognition_tracking_radar_object_tracker_tracking_setting_param_path" value="$(var object_recognition_tracking_radar_object_tracker_tracking_setting_param_path)"/>
76+
<arg name="object_recognition_tracking_radar_object_tracker_node_param_path" value="$(var object_recognition_tracking_radar_object_tracker_node_param_path)"/>
77+
<arg name="object_recognition_tracking_object_merger_data_association_matrix_param_path" value="$(var object_recognition_tracking_object_merger_data_association_matrix_param_path)"/>
78+
<arg name="object_recognition_tracking_object_merger_node_param_path" value="$(var object_recognition_tracking_object_merger_node_param_path)"/>
79+
</include>
80+
</group>
81+
<!-- prediction module -->
82+
<group>
83+
<push-ros-namespace namespace="prediction"/>
84+
<include file="$(find-pkg-share tier4_perception_launch)/launch/object_recognition/prediction/prediction.launch.xml">
85+
<arg name="use_vector_map" value="true"/>
86+
</include>
87+
</group>
88+
</group>
9289

93-
<!-- publish empty objects instead of object recognition module -->
94-
<group unless="$(var perception/enable_object_recognition)">
95-
<push-ros-namespace namespace="object_recognition"/>
96-
<node pkg="dummy_perception_publisher" exec="empty_objects_publisher" name="empty_objects_publisher" output="screen">
97-
<remap from="~/output/objects" to="/perception/object_recognition/objects"/>
98-
</node>
99-
</group>
90+
<!-- perception evaluator -->
91+
<group>
92+
<include file="$(find-pkg-share perception_online_evaluator)/launch/perception_online_evaluator.launch.xml"/>
93+
</group>
10094

101-
<group if="$(var perception/enable_elevation_map)">
102-
<push-ros-namespace namespace="obstacle_segmentation/elevation_map"/>
103-
<node pkg="elevation_map_loader" exec="elevation_map_loader" name="elevation_map_loader" output="screen">
104-
<remap from="output/elevation_map" to="map"/>
105-
<remap from="input/pointcloud_map" to="/map/pointcloud_map"/>
106-
<remap from="input/vector_map" to="/map/vector_map"/>
107-
<param name="use_lane_filter" value="false"/>
108-
<param name="use_inpaint" value="true"/>
109-
<param name="inpaint_radius" value="1.0"/>
110-
<param name="param_file_path" value="$(var obstacle_segmentation_ground_segmentation_elevation_map_param_path)"/>
111-
<param name="elevation_map_directory" value="$(find-pkg-share elevation_map_loader)/data/elevation_maps"/>
112-
<param name="use_elevation_map_cloud_publisher" value="false"/>
113-
</node>
114-
</group>
95+
<!-- publish empty objects instead of object recognition module -->
96+
<group unless="$(var perception/enable_object_recognition)">
97+
<push-ros-namespace namespace="object_recognition"/>
98+
<node pkg="dummy_perception_publisher" exec="empty_objects_publisher" name="empty_objects_publisher" output="screen">
99+
<remap from="~/output/objects" to="/perception/object_recognition/objects"/>
100+
</node>
101+
</group>
115102

116-
<!-- traffic light module -->
117-
<group if="$(var perception/enable_traffic_light)">
118-
<push-ros-namespace namespace="traffic_light_recognition"/>
119-
<include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml"/>
103+
<group if="$(var perception/enable_elevation_map)">
104+
<push-ros-namespace namespace="obstacle_segmentation/elevation_map"/>
105+
<node pkg="elevation_map_loader" exec="elevation_map_loader" name="elevation_map_loader" output="screen">
106+
<remap from="output/elevation_map" to="map"/>
107+
<remap from="input/pointcloud_map" to="/map/pointcloud_map"/>
108+
<remap from="input/vector_map" to="/map/vector_map"/>
109+
<param name="use_lane_filter" value="false"/>
110+
<param name="use_inpaint" value="true"/>
111+
<param name="inpaint_radius" value="1.0"/>
112+
<param name="param_file_path" value="$(var obstacle_segmentation_ground_segmentation_elevation_map_param_path)"/>
113+
<param name="elevation_map_directory" value="$(find-pkg-share elevation_map_loader)/data/elevation_maps"/>
114+
<param name="use_elevation_map_cloud_publisher" value="false"/>
115+
</node>
116+
</group>
117+
118+
<!-- traffic light module -->
119+
<group if="$(var perception/enable_traffic_light)">
120+
<push-ros-namespace namespace="traffic_light_recognition"/>
121+
<include file="$(find-pkg-share tier4_perception_launch)/launch/traffic_light_recognition/traffic_light.launch.xml"/>
122+
</group>
120123
</group>
121124
</group>
122125

‎perception/occupancy_grid_map_outlier_filter/src/occupancy_grid_map_outlier_filter_nodelet.cpp

+32-9
Original file line numberDiff line numberDiff line change
@@ -239,19 +239,42 @@ OccupancyGridMapOutlierFilterComponent::OccupancyGridMapOutlierFilterComponent(
239239
void OccupancyGridMapOutlierFilterComponent::splitPointCloudFrontBack(
240240
const PointCloud2::ConstSharedPtr & input_pc, PointCloud2 & front_pc, PointCloud2 & behind_pc)
241241
{
242-
PclPointCloud tmp_behind_pc;
243-
PclPointCloud tmp_front_pc;
244-
for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"), y(*input_pc, "y"),
245-
z(*input_pc, "z");
246-
x != x.end(); ++x, ++y, ++z) {
242+
size_t front_count = 0;
243+
size_t behind_count = 0;
244+
245+
for (sensor_msgs::PointCloud2ConstIterator<float> x(*input_pc, "x"); x != x.end(); ++x) {
247246
if (*x < 0.0) {
248-
tmp_behind_pc.push_back(pcl::PointXYZ(*x, *y, *z));
247+
behind_count++;
249248
} else {
250-
tmp_front_pc.push_back(pcl::PointXYZ(*x, *y, *z));
249+
front_count++;
251250
}
252251
}
253-
pcl::toROSMsg(tmp_front_pc, front_pc);
254-
pcl::toROSMsg(tmp_behind_pc, behind_pc);
252+
253+
sensor_msgs::PointCloud2Modifier front_pc_modifier(front_pc);
254+
sensor_msgs::PointCloud2Modifier behind_pc_modifier(behind_pc);
255+
front_pc_modifier.setPointCloud2FieldsByString(1, "xyz");
256+
behind_pc_modifier.setPointCloud2FieldsByString(1, "xyz");
257+
front_pc_modifier.resize(front_count);
258+
behind_pc_modifier.resize(behind_count);
259+
260+
sensor_msgs::PointCloud2Iterator<float> fr_iter(front_pc, "x");
261+
sensor_msgs::PointCloud2Iterator<float> be_iter(behind_pc, "x");
262+
263+
for (sensor_msgs::PointCloud2ConstIterator<float> in_iter(*input_pc, "x");
264+
in_iter != in_iter.end(); ++in_iter) {
265+
if (*in_iter < 0.0) {
266+
*be_iter = in_iter[0];
267+
be_iter[1] = in_iter[1];
268+
be_iter[2] = in_iter[2];
269+
++be_iter;
270+
} else {
271+
*fr_iter = in_iter[0];
272+
fr_iter[1] = in_iter[1];
273+
fr_iter[2] = in_iter[2];
274+
++fr_iter;
275+
}
276+
}
277+
255278
front_pc.header = input_pc->header;
256279
behind_pc.header = input_pc->header;
257280
}

0 commit comments

Comments
 (0)