Skip to content

Commit 60178c6

Browse files
karishma1911karishma1911
authored
Merge branch 'main' into traffic-light-classifier
2 parents 9baac21 + ed8ffea commit 60178c6

File tree

38 files changed

+701
-289
lines changed

38 files changed

+701
-289
lines changed

‎common/tier4_state_rviz_plugin/src/velocity_steering_factors_panel.cpp

-3
Original file line numberDiff line numberDiff line change
@@ -174,9 +174,6 @@ void VelocitySteeringFactorsPanel::onSteeringFactors(const SteeringFactorArray::
174174
case SteeringFactor::APPROACHING:
175175
label->setText("APPROACHING");
176176
break;
177-
case SteeringFactor::TRYING:
178-
label->setText("TRYING");
179-
break;
180177
case SteeringFactor::TURNING:
181178
label->setText("TURNING");
182179
break;

‎common/traffic_light_utils/src/traffic_light_utils.cpp

+1-1
Original file line numberDiff line numberDiff line change
@@ -46,7 +46,7 @@ void setSignalUnknown(tier4_perception_msgs::msg::TrafficSignal & signal, float
4646
signal.elements[0].shape = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
4747
signal.elements[0].color = tier4_perception_msgs::msg::TrafficLightElement::UNKNOWN;
4848
// the default value is -1, which means to not set confidence
49-
if (confidence >= 0) {
49+
if (confidence > 0) {
5050
signal.elements[0].confidence = confidence;
5151
}
5252
}

‎control/joy_controller/README.md

+23
Original file line numberDiff line numberDiff line change
@@ -37,6 +37,7 @@
3737
| `steering_angle_velocity` | double | steering angle velocity for operation |
3838
| `accel_sensitivity` | double | sensitivity to calculate acceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) |
3939
| `brake_sensitivity` | double | sensitivity to calculate deceleration for external API (commanded acceleration is pow(operation, 1 / sensitivity)) |
40+
| `raw_control` | bool | skip input odometry if true |
4041
| `velocity_gain` | double | ratio to calculate velocity by acceleration |
4142
| `max_forward_velocity` | double | absolute max velocity to go forward |
4243
| `max_backward_velocity` | double | absolute max velocity to go backward |
@@ -85,3 +86,25 @@
8586
| Autoware Disengage | ○ |
8687
| Vehicle Engage | △ |
8788
| Vehicle Disengage | △ |
89+
90+
## XBOX Joystick Key Map
91+
92+
| Action | Button |
93+
| -------------------- | --------------------- |
94+
| Acceleration | RT |
95+
| Brake | LT |
96+
| Steering | Left Stick Left Right |
97+
| Shift up | Cursor Up |
98+
| Shift down | Cursor Down |
99+
| Shift Drive | Cursor Left |
100+
| Shift Reverse | Cursor Right |
101+
| Turn Signal Left | LB |
102+
| Turn Signal Right | RB |
103+
| Clear Turn Signal | A |
104+
| Gate Mode | B |
105+
| Emergency Stop | View |
106+
| Clear Emergency Stop | Menu |
107+
| Autoware Engage | X |
108+
| Autoware Disengage | Y |
109+
| Vehicle Engage | Left Stick Button |
110+
| Vehicle Disengage | Right Stick Button |

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

+1
Original file line numberDiff line numberDiff line change
@@ -9,6 +9,7 @@
99
accel_sensitivity: 1.0
1010
brake_sensitivity: 1.0
1111
control_command:
12+
raw_control: false
1213
velocity_gain: 3.0
1314
max_forward_velocity: 20.0
1415
max_backward_velocity: 3.0

‎control/joy_controller/include/joy_controller/joy_controller.hpp

+1
Original file line numberDiff line numberDiff line change
@@ -59,6 +59,7 @@ class AutowareJoyControllerNode : public rclcpp::Node
5959
double brake_sensitivity_;
6060

6161
// ControlCommand Parameter
62+
bool raw_control_;
6263
double velocity_gain_;
6364
double max_forward_velocity_;
6465
double max_backward_velocity_;

‎control/joy_controller/include/joy_controller/joy_converter/xbox_joy_converter.hpp

+81
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,81 @@
1+
// Copyright 2023 The Autoware Contributors
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+
#ifndef JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_
16+
#define JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_
17+
18+
#include "joy_controller/joy_converter/joy_converter_base.hpp"
19+
20+
#include <algorithm>
21+
22+
namespace joy_controller
23+
{
24+
class XBOXJoyConverter : public JoyConverterBase
25+
{
26+
public:
27+
explicit XBOXJoyConverter(const sensor_msgs::msg::Joy & j) : j_(j) {}
28+
29+
float accel() const { return std::max(0.0f, -((RT() - 1.0f) / 2.0f)); }
30+
31+
float brake() const { return std::max(0.0f, -((LT() - 1.0f) / 2.0f)); }
32+
33+
float steer() const { return LStickLeftRight(); }
34+
35+
bool shift_up() const { return CursorUpDown() == 1.0f; }
36+
bool shift_down() const { return CursorUpDown() == -1.0f; }
37+
bool shift_drive() const { return CursorLeftRight() == 1.0f; }
38+
bool shift_reverse() const { return CursorLeftRight() == -1.0f; }
39+
40+
bool turn_signal_left() const { return LB(); }
41+
bool turn_signal_right() const { return RB(); }
42+
bool clear_turn_signal() const { return A(); }
43+
44+
bool gate_mode() const { return B(); }
45+
46+
bool emergency_stop() const { return ChangeView(); }
47+
bool clear_emergency_stop() const { return Menu(); }
48+
49+
bool autoware_engage() const { return X(); }
50+
bool autoware_disengage() const { return Y(); }
51+
52+
bool vehicle_engage() const { return LStickButton(); }
53+
bool vehicle_disengage() const { return RStickButton(); }
54+
55+
private:
56+
float LStickLeftRight() const { return j_.axes.at(0); }
57+
float LStickUpDown() const { return j_.axes.at(1); }
58+
float RStickLeftRight() const { return j_.axes.at(2); }
59+
float RStickUpDown() const { return j_.axes.at(3); }
60+
float RT() const { return j_.axes.at(4); }
61+
float LT() const { return j_.axes.at(5); }
62+
float CursorLeftRight() const { return j_.axes.at(6); }
63+
float CursorUpDown() const { return j_.axes.at(7); }
64+
65+
bool A() const { return j_.buttons.at(0); }
66+
bool B() const { return j_.buttons.at(1); }
67+
bool X() const { return j_.buttons.at(3); }
68+
bool Y() const { return j_.buttons.at(4); }
69+
bool LB() const { return j_.buttons.at(6); }
70+
bool RB() const { return j_.buttons.at(7); }
71+
bool Menu() const { return j_.buttons.at(11); }
72+
bool LStickButton() const { return j_.buttons.at(13); }
73+
bool RStickButton() const { return j_.buttons.at(14); }
74+
bool ChangeView() const { return j_.buttons.at(15); }
75+
bool Xbox() const { return j_.buttons.at(16); }
76+
77+
const sensor_msgs::msg::Joy j_;
78+
};
79+
} // namespace joy_controller
80+
81+
#endif // JOY_CONTROLLER__JOY_CONVERTER__XBOX_JOY_CONVERTER_HPP_

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

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

55
<arg name="input_joy" default="/joy"/>

‎control/joy_controller/schema/joy_controller.schema.json

+7-1
Original file line numberDiff line numberDiff line change
@@ -10,7 +10,7 @@
1010
"type": "string",
1111
"description": "Joy controller type",
1212
"default": "DS4",
13-
"enum": ["P65", "DS4", "G29"]
13+
"enum": ["P65", "DS4", "G29", "XBOX"]
1414
},
1515
"update_rate": {
1616
"type": "number",
@@ -55,6 +55,11 @@
5555
"control_command": {
5656
"type": "object",
5757
"properties": {
58+
"raw_control": {
59+
"type": "boolean",
60+
"description": "Whether to skip input odometry",
61+
"default": false
62+
},
5863
"velocity_gain": {
5964
"type": "number",
6065
"description": "Ratio to calculate velocity by acceleration",
@@ -79,6 +84,7 @@
7984
}
8085
},
8186
"required": [
87+
"raw_control",
8288
"velocity_gain",
8389
"max_forward_velocity",
8490
"max_backward_velocity",

‎control/joy_controller/src/joy_controller/joy_controller_node.cpp

+13-5
Original file line numberDiff line numberDiff line change
@@ -16,6 +16,7 @@
1616
#include "joy_controller/joy_converter/ds4_joy_converter.hpp"
1717
#include "joy_controller/joy_converter/g29_joy_converter.hpp"
1818
#include "joy_controller/joy_converter/p65_joy_converter.hpp"
19+
#include "joy_controller/joy_converter/xbox_joy_converter.hpp"
1920

2021
#include <tier4_api_utils/tier4_api_utils.hpp>
2122

@@ -154,6 +155,8 @@ void AutowareJoyControllerNode::onJoy(const sensor_msgs::msg::Joy::ConstSharedPt
154155
joy_ = std::make_shared<const G29JoyConverter>(*msg);
155156
} else if (joy_type_ == "DS4") {
156157
joy_ = std::make_shared<const DS4JoyConverter>(*msg);
158+
} else if (joy_type_ == "XBOX") {
159+
joy_ = std::make_shared<const XBOXJoyConverter>(*msg);
157160
} else {
158161
joy_ = std::make_shared<const P65JoyConverter>(*msg);
159162
}
@@ -217,7 +220,7 @@ bool AutowareJoyControllerNode::isDataReady()
217220
}
218221

219222
// Twist
220-
{
223+
if (!raw_control_) {
221224
if (!twist_) {
222225
RCLCPP_WARN_THROTTLE(
223226
get_logger(), *get_clock(), std::chrono::milliseconds(5000).count(),
@@ -458,6 +461,7 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions &
458461
steering_angle_velocity_ = declare_parameter<double>("steering_angle_velocity");
459462
accel_sensitivity_ = declare_parameter<double>("accel_sensitivity");
460463
brake_sensitivity_ = declare_parameter<double>("brake_sensitivity");
464+
raw_control_ = declare_parameter<bool>("control_command.raw_control");
461465
velocity_gain_ = declare_parameter<double>("control_command.velocity_gain");
462466
max_forward_velocity_ = declare_parameter<double>("control_command.max_forward_velocity");
463467
max_backward_velocity_ = declare_parameter<double>("control_command.max_backward_velocity");
@@ -477,10 +481,14 @@ AutowareJoyControllerNode::AutowareJoyControllerNode(const rclcpp::NodeOptions &
477481
sub_joy_ = this->create_subscription<sensor_msgs::msg::Joy>(
478482
"input/joy", 1, std::bind(&AutowareJoyControllerNode::onJoy, this, std::placeholders::_1),
479483
subscriber_option);
480-
sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
481-
"input/odometry", 1,
482-
std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1),
483-
subscriber_option);
484+
if (!raw_control_) {
485+
sub_odom_ = this->create_subscription<nav_msgs::msg::Odometry>(
486+
"input/odometry", 1,
487+
std::bind(&AutowareJoyControllerNode::onOdometry, this, std::placeholders::_1),
488+
subscriber_option);
489+
} else {
490+
twist_ = std::make_shared<geometry_msgs::msg::TwistStamped>();
491+
}
484492

485493
// Publisher
486494
pub_control_command_ =

‎control/pid_longitudinal_controller/src/pid_longitudinal_controller.cpp

+3-5
Original file line numberDiff line numberDiff line change
@@ -555,9 +555,7 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
555555
return;
556556
};
557557

558-
const auto info_throttle = [this](const auto & s) {
559-
RCLCPP_INFO_SKIPFIRST_THROTTLE(logger_, *clock_, 5000, "%s", s);
560-
};
558+
const auto info_once = [this](const auto & s) { RCLCPP_INFO_ONCE(logger_, "%s", s); };
561559

562560
const bool is_under_control = m_current_operation_mode.is_autoware_control_enabled &&
563561
m_current_operation_mode.mode == OperationModeState::AUTONOMOUS;
@@ -623,10 +621,10 @@ void PidLongitudinalController::updateControlState(const ControlData & control_d
623621
if (m_control_state == ControlState::STOPPED) {
624622
// -- debug print --
625623
if (has_nonzero_target_vel && !departure_condition_from_stopped) {
626-
info_throttle("target speed > 0, but departure condition is not met. Keep STOPPED.");
624+
info_once("target speed > 0, but departure condition is not met. Keep STOPPED.");
627625
}
628626
if (has_nonzero_target_vel && keep_stopped_condition) {
629-
info_throttle("target speed > 0, but keep stop condition is met. Keep STOPPED.");
627+
info_once("target speed > 0, but keep stop condition is met. Keep STOPPED.");
630628
}
631629
// ---------------
632630

‎launch/tier4_perception_launch/launch/perception.launch.xml

+13-2
Original file line numberDiff line numberDiff line change
@@ -86,13 +86,18 @@
8686
default="$(var data_path)/traffic_light_fine_detector"
8787
description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6"
8888
/>
89-
<arg name="traffic_light_fine_detector_model_name" default="tlr_yolox_s_batch_6" description="options: `tlr_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6"/>
89+
<arg name="traffic_light_fine_detector_model_name" default="tlr_car_ped_yolox_s_batch_6" description="options: `tlr_car_ped_yolox_s_batch_**`. The batch number must be either one of 1, 4, 6"/>
9090
<arg name="traffic_light_classifier_model_path" default="$(var data_path)/traffic_light_classifier" description="classifier onnx model path"/>
9191
<arg
92-
name="traffic_light_classifier_model_name"
92+
name="car_traffic_light_classifier_model_name"
9393
default="traffic_light_classifier_mobilenetv2_batch_6"
9494
description="options: `traffic_light_classifier_mobilenetv2_batch_*` or `traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6"
9595
/>
96+
<arg
97+
name="pedestrian_traffic_light_classifier_model_name"
98+
default="ped_traffic_light_classifier_mobilenetv2_batch_4"
99+
description="options: `ped_traffic_light_classifier_mobilenetv2_batch_*` or `ped_traffic_light_classifier_efficientNet_b1_batch_*`. The batch number must be either one of 1, 4, 6"
100+
/>
96101

97102
<!-- Camera-Lidar fusion parameters -->
98103
<arg name="remove_unknown" default="true"/>
@@ -231,6 +236,12 @@
231236
<arg name="enable_fine_detection" value="$(var traffic_light_recognition/enable_fine_detection)"/>
232237
<arg name="fusion_only" value="$(var traffic_light_recognition/fusion_only)"/>
233238
<arg name="image_number" value="$(var traffic_light_image_number)"/>
239+
<arg name="traffic_light_arbiter_param_path" value="$(var traffic_light_arbiter_param_path)"/>
240+
<arg name="traffic_light_fine_detector_model_path" value="$(var traffic_light_fine_detector_model_path)"/>
241+
<arg name="traffic_light_fine_detector_model_name" value="$(var traffic_light_fine_detector_model_name)"/>
242+
<arg name="traffic_light_classifier_model_path" value="$(var traffic_light_classifier_model_path)"/>
243+
<arg name="car_traffic_light_classifier_model_name" value="$(var car_traffic_light_classifier_model_name)"/>
244+
<arg name="pedestrian_traffic_light_classifier_model_name" value="$(var pedestrian_traffic_light_classifier_model_name)"/>
234245
</include>
235246
</group>
236247
</group>

0 commit comments

Comments
 (0)