Skip to content

Commit a136f58

Browse files
anhnv3991anhnv3991
authored
Merge branch 'main' into feature/zero_block_ndt_update
2 parents 90535c7 + 754742e commit a136f58

File tree

27 files changed

+565
-105
lines changed

27 files changed

+565
-105
lines changed

‎control/joy_controller/README.md

+15
Original file line numberDiff line numberDiff line change
@@ -4,6 +4,21 @@
44

55
`joy_controller` is the package to convert a joy msg to autoware commands (e.g. steering wheel, shift, turn signal, engage) for a vehicle.
66

7+
## Usage
8+
9+
### ROS 2 launch
10+
11+
```bash
12+
# With default config (ds4)
13+
ros2 launch joy_controller joy_controller.launch.xml
14+
15+
# Default config but select from the existing parameter files
16+
ros2 launch joy_controller joy_controller_param_selection.launch.xml joy_type:=ds4 # or g29, p65, xbox
17+
18+
# Override the param file
19+
ros2 launch joy_controller joy_controller.launch.xml config_file:=/path/to/your/param.yaml
20+
```
21+
722
## Input / Output
823

924
### Input topics

‎control/joy_controller/config/joy_controller.param.yaml ‎control/joy_controller/config/joy_controller_ds4.param.yaml

+1-1
Original file line numberDiff line numberDiff line change
@@ -1,6 +1,6 @@
11
/**:
22
ros__parameters:
3-
joy_type: $(var joy_type)
3+
joy_type: DS4
44
update_rate: 10.0
55
accel_ratio: 3.0
66
brake_ratio: 5.0

‎control/joy_controller/config/joy_controller_g29.param.yaml

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
/**:
2+
ros__parameters:
3+
joy_type: G29
4+
update_rate: 10.0
5+
accel_ratio: 3.0
6+
brake_ratio: 5.0
7+
steer_ratio: 0.5
8+
steering_angle_velocity: 0.1
9+
accel_sensitivity: 1.0
10+
brake_sensitivity: 1.0
11+
control_command:
12+
raw_control: false
13+
velocity_gain: 3.0
14+
max_forward_velocity: 20.0
15+
max_backward_velocity: 3.0
16+
backward_accel_ratio: 1.0

‎control/joy_controller/config/joy_controller_p65.param.yaml

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
/**:
2+
ros__parameters:
3+
joy_type: P65
4+
update_rate: 10.0
5+
accel_ratio: 3.0
6+
brake_ratio: 5.0
7+
steer_ratio: 0.5
8+
steering_angle_velocity: 0.1
9+
accel_sensitivity: 1.0
10+
brake_sensitivity: 1.0
11+
control_command:
12+
raw_control: false
13+
velocity_gain: 3.0
14+
max_forward_velocity: 20.0
15+
max_backward_velocity: 3.0
16+
backward_accel_ratio: 1.0

‎control/joy_controller/config/joy_controller_xbox.param.yaml

+16
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,16 @@
1+
/**:
2+
ros__parameters:
3+
joy_type: XBOX
4+
update_rate: 10.0
5+
accel_ratio: 3.0
6+
brake_ratio: 5.0
7+
steer_ratio: 0.5
8+
steering_angle_velocity: 0.1
9+
accel_sensitivity: 1.0
10+
brake_sensitivity: 1.0
11+
control_command:
12+
raw_control: false
13+
velocity_gain: 3.0
14+
max_forward_velocity: 20.0
15+
max_backward_velocity: 3.0
16+
backward_accel_ratio: 1.0

‎control/joy_controller/launch/joy_controller.launch.xml

+1-2
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,4 @@
11
<launch>
2-
<arg name="joy_type" default="DS4" description="options: DS4, G29, P65, XBOX"/>
32
<arg name="external_cmd_source" default="remote" description="options: local, remote"/>
43

54
<arg name="input_joy" default="/joy"/>
@@ -13,7 +12,7 @@
1312
<arg name="output_gate_mode" default="/control/gate_mode_cmd"/>
1413
<arg name="output_vehicle_engage" default="/vehicle/engage"/>
1514

16-
<arg name="config_file" default="$(find-pkg-share joy_controller)/config/joy_controller.param.yaml"/>
15+
<arg name="config_file" default="$(find-pkg-share joy_controller)/config/joy_controller_ds4.param.yaml"/>
1716

1817
<node pkg="joy_controller" exec="joy_controller" name="joy_controller" output="screen">
1918
<remap from="input/joy" to="$(var input_joy)"/>

‎control/joy_controller/launch/joy_controller_param_selection.launch.xml

+7
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,7 @@
1+
<launch>
2+
<arg name="joy_type" default="ds4" description="options: ds4, g29, p65, xbox"/>
3+
4+
<include file="$(find-pkg-share joy_controller)/launch/joy_controller.launch.xml">
5+
<arg name="config_file" value="$(find-pkg-share joy_controller)/config/joy_controller_$(var joy_type).param.yaml"/>
6+
</include>
7+
</launch>

‎perception/probabilistic_occupancy_grid_map/src/pointcloud_based_occupancy_grid_map/pointcloud_based_occupancy_grid_map_node.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -170,7 +170,7 @@ void PointcloudBasedOccupancyGridMapNode::onPointcloudWithObstacleAndRaw(
170170
Pose gridmap_origin{};
171171
Pose scan_origin{};
172172
try {
173-
robot_pose = utils::getPose(input_raw_msg->header, *tf2_, map_frame_);
173+
robot_pose = utils::getPose(input_raw_msg->header.stamp, *tf2_, base_link_frame_, map_frame_);
174174
gridmap_origin =
175175
utils::getPose(input_raw_msg->header.stamp, *tf2_, gridmap_origin_frame_, map_frame_);
176176
scan_origin =

‎perception/radar_crossing_objects_noise_filter/CMakeLists.txt

+8
Original file line numberDiff line numberDiff line change
@@ -17,9 +17,17 @@ rclcpp_components_register_node(radar_crossing_objects_noise_filter_node_compone
1717

1818
# Tests
1919
if(BUILD_TESTING)
20+
find_package(ament_cmake_ros REQUIRED)
2021
list(APPEND AMENT_LINT_AUTO_EXCLUDE ament_cmake_uncrustify)
2122
find_package(ament_lint_auto REQUIRED)
2223
ament_lint_auto_find_test_dependencies()
24+
25+
file(GLOB_RECURSE test_files test/**/*.cpp)
26+
ament_add_ros_isolated_gtest(radar_crossing_objects_noise_filter ${test_files})
27+
28+
target_link_libraries(radar_crossing_objects_noise_filter
29+
radar_crossing_objects_noise_filter_node_component
30+
)
2331
endif()
2432

2533
# Package

‎perception/radar_crossing_objects_noise_filter/include/radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -58,6 +58,7 @@ class RadarCrossingObjectsNoiseFilterNode : public rclcpp::Node
5858
// Parameter
5959
NodeParam node_param_{};
6060

61+
public:
6162
// Core
6263
bool isNoise(const DetectedObject & object);
6364
};

‎perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_filter.cpp

+26
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,26 @@
1+
// Copyright 2024 TierIV
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include <rclcpp/rclcpp.hpp>
16+
17+
#include <gtest/gtest.h>
18+
19+
int main(int argc, char * argv[])
20+
{
21+
testing::InitGoogleTest(&argc, argv);
22+
rclcpp::init(argc, argv);
23+
bool result = RUN_ALL_TESTS();
24+
rclcpp::shutdown();
25+
return result;
26+
}

‎perception/radar_crossing_objects_noise_filter/test/test_radar_crossing_objects_noise_filter/test_radar_crossing_objects_filter_is_noise.cpp

+175
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,175 @@
1+
// Copyright 2024 Tier IV, Inc.
2+
//
3+
// Licensed under the Apache License, Version 2.0 (the "License");
4+
// you may not use this file except in compliance with the License.
5+
// You may obtain a copy of the License at
6+
//
7+
// http://www.apache.org/licenses/LICENSE-2.0
8+
//
9+
// Unless required by applicable law or agreed to in writing, software
10+
// distributed under the License is distributed on an "AS IS" BASIS,
11+
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12+
// See the License for the specific language governing permissions and
13+
// limitations under the License.
14+
15+
#include "radar_crossing_objects_noise_filter/radar_crossing_objects_noise_filter_node.hpp"
16+
#include "tier4_autoware_utils/geometry/geometry.hpp"
17+
#include "tier4_autoware_utils/math/unit_conversion.hpp"
18+
19+
#include <geometry_msgs/msg/point32.hpp>
20+
21+
#include <gtest/gtest.h>
22+
23+
std::shared_ptr<radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode> get_node(
24+
double angle_threshold, double velocity_threshold)
25+
{
26+
rclcpp::NodeOptions node_options;
27+
28+
node_options.parameter_overrides(
29+
{{"angle_threshold", angle_threshold}, {"velocity_threshold", velocity_threshold}});
30+
auto node =
31+
std::make_shared<radar_crossing_objects_noise_filter::RadarCrossingObjectsNoiseFilterNode>(
32+
node_options);
33+
return node;
34+
}
35+
36+
autoware_auto_perception_msgs::msg::DetectedObject get_object(
37+
geometry_msgs::msg::Vector3 velocity, geometry_msgs::msg::Point position,
38+
geometry_msgs::msg::Quaternion orientation)
39+
{
40+
autoware_auto_perception_msgs::msg::DetectedObject object;
41+
object.kinematics.twist_with_covariance.twist.linear = velocity;
42+
object.kinematics.pose_with_covariance.pose.position = position;
43+
object.kinematics.pose_with_covariance.pose.orientation = orientation;
44+
return object;
45+
}
46+
47+
TEST(RadarCrossingObjectsFilter, IsNoise)
48+
{
49+
rclcpp::init(0, nullptr);
50+
{
51+
auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0);
52+
auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0);
53+
auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0);
54+
auto object = get_object(velocity, position, orientation);
55+
{
56+
double velocity_threshold = 40.0;
57+
double angle_threshold = 1.0472;
58+
auto node = get_node(angle_threshold, velocity_threshold);
59+
EXPECT_TRUE(node->isNoise(object));
60+
}
61+
{
62+
double velocity_threshold = 40.0;
63+
double angle_threshold = -1.0472;
64+
65+
auto node = get_node(angle_threshold, velocity_threshold);
66+
EXPECT_TRUE(node->isNoise(object));
67+
}
68+
{
69+
double velocity_threshold = -40.0;
70+
double angle_threshold = 1.0472;
71+
72+
auto node = get_node(angle_threshold, velocity_threshold);
73+
EXPECT_TRUE(node->isNoise(object));
74+
}
75+
{
76+
double velocity_threshold = -40.0;
77+
double angle_threshold = -1.0472;
78+
auto node = get_node(angle_threshold, velocity_threshold);
79+
EXPECT_TRUE(node->isNoise(object));
80+
}
81+
}
82+
83+
{
84+
auto velocity = tier4_autoware_utils::createVector3(40.0, 30.0, 0.0);
85+
auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0);
86+
auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0);
87+
auto object = get_object(velocity, position, orientation);
88+
{
89+
double velocity_threshold = 40.0;
90+
double angle_threshold = 1.0472;
91+
auto node = get_node(angle_threshold, velocity_threshold);
92+
EXPECT_FALSE(node->isNoise(object));
93+
}
94+
{
95+
double velocity_threshold = 40.0;
96+
double angle_threshold = -1.0472;
97+
auto node = get_node(angle_threshold, velocity_threshold);
98+
EXPECT_FALSE(node->isNoise(object));
99+
}
100+
{
101+
double velocity_threshold = -40.0;
102+
double angle_threshold = 1.0472;
103+
auto node = get_node(angle_threshold, velocity_threshold);
104+
EXPECT_FALSE(node->isNoise(object));
105+
}
106+
{
107+
double velocity_threshold = -40.0;
108+
double angle_threshold = -1.0472;
109+
auto node = get_node(angle_threshold, velocity_threshold);
110+
EXPECT_FALSE(node->isNoise(object));
111+
}
112+
}
113+
114+
{
115+
auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0);
116+
auto position = tier4_autoware_utils::createPoint(1.0, 0.0, 0.0);
117+
auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0);
118+
auto object = get_object(velocity, position, orientation);
119+
{
120+
double velocity_threshold = 40.0;
121+
double angle_threshold = 1.0472;
122+
auto node = get_node(angle_threshold, velocity_threshold);
123+
EXPECT_FALSE(node->isNoise(object));
124+
}
125+
{
126+
double velocity_threshold = 40.0;
127+
double angle_threshold = -1.0472;
128+
auto node = get_node(angle_threshold, velocity_threshold);
129+
EXPECT_FALSE(node->isNoise(object));
130+
}
131+
{
132+
double velocity_threshold = -40.0;
133+
double angle_threshold = 1.0472;
134+
auto node = get_node(angle_threshold, velocity_threshold);
135+
EXPECT_TRUE(node->isNoise(object));
136+
}
137+
{
138+
double velocity_threshold = -40.0;
139+
double angle_threshold = -1.0472;
140+
auto node = get_node(angle_threshold, velocity_threshold);
141+
EXPECT_TRUE(node->isNoise(object));
142+
}
143+
}
144+
145+
{
146+
auto velocity = tier4_autoware_utils::createVector3(24.0, 18.0, 0.0);
147+
auto position = tier4_autoware_utils::createPoint(1.0, 2.0, 0.0);
148+
auto orientation = tier4_autoware_utils::createQuaternion(1.0, 1.0, 1.0, 0.0);
149+
auto object = get_object(velocity, position, orientation);
150+
{
151+
double velocity_threshold = 40.0;
152+
double angle_threshold = 1.0472;
153+
auto node = get_node(angle_threshold, velocity_threshold);
154+
EXPECT_FALSE(node->isNoise(object));
155+
}
156+
{
157+
double velocity_threshold = 40.0;
158+
double angle_threshold = -1.0472;
159+
auto node = get_node(angle_threshold, velocity_threshold);
160+
EXPECT_FALSE(node->isNoise(object));
161+
}
162+
{
163+
double velocity_threshold = -40.0;
164+
double angle_threshold = 1.0472;
165+
auto node = get_node(angle_threshold, velocity_threshold);
166+
EXPECT_FALSE(node->isNoise(object));
167+
}
168+
{
169+
double velocity_threshold = -40.0;
170+
double angle_threshold = -1.0472;
171+
auto node = get_node(angle_threshold, velocity_threshold);
172+
EXPECT_FALSE(node->isNoise(object));
173+
}
174+
}
175+
}

‎perception/tensorrt_yolox/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -114,4 +114,5 @@ endif()
114114

115115
ament_auto_package(INSTALL_TO_SHARE
116116
launch
117+
config
117118
)

‎perception/tensorrt_yolox/config/yolox_s_plus_opt.param.yaml

+15
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
/**:
2+
ros__parameters:
3+
model_path: "$(var data_path)/tensorrt_yolox/$(var model_name).onnx"
4+
label_path: "$(var data_path)/tensorrt_yolox/label.txt"
5+
score_threshold: 0.35
6+
nms_threshold: 0.7
7+
precision: "int8" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8].
8+
calibration_algorithm: "Entropy" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax].
9+
dla_core_id: -1 # If positive ID value is specified, the node assign inference task to the DLA core.
10+
quantize_first_layer: false # If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8.
11+
quantize_last_layer: false # If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8.
12+
profile_per_layer: false # If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose.
13+
clip_value: 6.0 # If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration.
14+
preprocess_on_gpu: true # If true, pre-processing is performed on GPU.
15+
calibration_image_list_path: "" # Path to a file which contains path to images. Those images will be used for int8 quantization.

‎perception/tensorrt_yolox/config/yolox_tiny.param.yaml

+15
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,15 @@
1+
/**:
2+
ros__parameters:
3+
model_path: "$(var data_path)/tensorrt_yolox/$(var model_name).onnx"
4+
label_path: "$(var data_path)/tensorrt_yolox/label.txt"
5+
score_threshold: 0.35
6+
nms_threshold: 0.7
7+
precision: "fp16" # Operation precision to be used on inference. Valid value is one of: [fp32, fp16, int8].
8+
calibration_algorithm: "MinMax" # Calibration algorithm to be used for quantization when precision==int8. Valid value is one of: [Entropy, (Legacy | Percentile), MinMax].
9+
dla_core_id: -1 # If positive ID value is specified, the node assign inference task to the DLA core.
10+
quantize_first_layer: false # If true, set the operating precision for the first (input) layer to be fp16. This option is valid only when precision==int8.
11+
quantize_last_layer: false # If true, set the operating precision for the last (output) layer to be fp16. This option is valid only when precision==int8.
12+
profile_per_layer: false # If true, profiler function will be enabled. Since the profile function may affect execution speed, it is recommended to set this flag true only for development purpose.
13+
clip_value: 0.0 # If positive value is specified, the value of each layer output will be clipped between [0.0, clip_value]. This option is valid only when precision==int8 and used to manually specify the dynamic range instead of using any calibration.
14+
preprocess_on_gpu: true # If true, pre-processing is performed on GPU.
15+
calibration_image_list_path: "" # Path to a file which contains path to images. Those images will be used for int8 quantization.

0 commit comments

Comments
 (0)