Skip to content

Commit 9c17194

Browse files
style(pre-commit): autofix
1 parent 70c10fe commit 9c17194

File tree

8 files changed

+47
-50
lines changed

8 files changed

+47
-50
lines changed

system/emergency_handler/README.md

+11-11
Original file line numberDiff line numberDiff line change
@@ -9,14 +9,14 @@ Emergency Handler is a node to select proper MRM from system failure state conta
99
Emergency Handler has MRM operators inside.
1010
Currently, there are two types of MRM:
1111

12-
- Emergency Stop Operator: publishes `/system/emergency/control_cmd` to make an emergency stop
13-
- Comfortable Stop Operator: publishes `/planning/scenario_planning/max_velocity_candidates` to make a comfortable stop
12+
- Emergency Stop Operator: publishes `/system/emergency/control_cmd` to make an emergency stop
13+
- Comfortable Stop Operator: publishes `/planning/scenario_planning/max_velocity_candidates` to make a comfortable stop
1414

1515
These operators have `operate()`, `cancel()` and `onTimer` public functions.
1616

17-
- `operate()`: executes the MRM operation
18-
- `cancel()`: cancels the MRM operation
19-
- `onTimer()`: executes each operator's timer callback
17+
- `operate()`: executes the MRM operation
18+
- `cancel()`: cancels the MRM operation
19+
- `onTimer()`: executes each operator's timer callback
2020

2121
### State Transitions
2222

@@ -26,12 +26,12 @@ These operators have `operate()`, `cancel()` and `onTimer` public functions.
2626

2727
### Input
2828

29-
| Name | Type | Description |
30-
| ----------------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- |
31-
| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus |
32-
| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not |
33-
| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual |
34-
| `/control/command/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used to calculate emergency control commands in emergency_stop_operator |
29+
| Name | Type | Description |
30+
| --------------------------------- | ---------------------------------------------------------- | ----------------------------------------------------------------------------- |
31+
| `/system/emergency/hazard_status` | `autoware_auto_system_msgs::msg::HazardStatusStamped` | Used to select proper MRM from system failure state contained in HazardStatus |
32+
| `/localization/kinematic_state` | `nav_msgs::msg::Odometry` | Used to decide whether vehicle is stopped or not |
33+
| `/vehicle/status/control_mode` | `autoware_auto_vehicle_msgs::msg::ControlModeReport` | Used to check vehicle mode: autonomous or manual |
34+
| `/control/command/control_cmd` | `autoware_auto_control_msgs::msg::AckermannControlCommand` | Used to calculate emergency control commands in emergency_stop_operator |
3535

3636
### Output
3737

system/emergency_handler/include/emergency_handler/emergency_handler_core.hpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -37,8 +37,8 @@
3737
#include <nav_msgs/msg/odometry.hpp>
3838

3939
// Operators
40-
#include "operators/emergency_stop.hpp"
4140
#include "operators/comfortable_stop.hpp"
41+
#include "operators/emergency_stop.hpp"
4242

4343
namespace emergency_handler
4444
{
@@ -134,6 +134,6 @@ class EmergencyHandler : public rclcpp::Node
134134
bool isEmergency();
135135
};
136136

137-
} // namespace emergency_handler
137+
} // namespace emergency_handler
138138

139139
#endif // EMERGENCY_HANDLER__EMERGENCY_HANDLER_CORE_HPP_

system/emergency_handler/include/operators/comfortable_stop.hpp

+7-7
Original file line numberDiff line numberDiff line change
@@ -12,14 +12,14 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef COMFORTABLE_STOP_HPP_
16-
#define COMFORTABLE_STOP_HPP_
15+
#ifndef OPERATORS__COMFORTABLE_STOP_HPP_
16+
#define OPERATORS__COMFORTABLE_STOP_HPP_
17+
18+
#include <rclcpp/rclcpp.hpp>
1719

1820
#include <tier4_planning_msgs/msg/velocity_limit.hpp>
1921
#include <tier4_planning_msgs/msg/velocity_limit_clear_command.hpp>
2022

21-
#include <rclcpp/rclcpp.hpp>
22-
2323
namespace emergency_handler
2424
{
2525

@@ -57,8 +57,8 @@ class ComfortableStopOperator
5757
rclcpp::Node * node_;
5858
};
5959

60-
} // namespace comfortable_stop_operator
60+
} // namespace comfortable_stop_operator
6161

62-
} // namespace emergency_handler
62+
} // namespace emergency_handler
6363

64-
#endif // COMFORTABLE_STOP_HPP_
64+
#endif // OPERATORS__COMFORTABLE_STOP_HPP_

system/emergency_handler/include/operators/emergency_stop.hpp

+6-7
Original file line numberDiff line numberDiff line change
@@ -12,13 +12,12 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
#ifndef EMERGENCY_STOP_HPP_
16-
#define EMERGENCY_STOP_HPP_
17-
18-
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
15+
#ifndef OPERATORS__EMERGENCY_STOP_HPP_
16+
#define OPERATORS__EMERGENCY_STOP_HPP_
1917

2018
#include <rclcpp/rclcpp.hpp>
2119

20+
#include <autoware_auto_control_msgs/msg/ackermann_control_command.hpp>
2221

2322
namespace emergency_handler
2423
{
@@ -61,8 +60,8 @@ class EmergencyStopOperator
6160
rclcpp::Node * node_;
6261
};
6362

64-
} // namespace emergency_stop_operator
63+
} // namespace emergency_stop_operator
6564

66-
} // namespace emergency_handler
65+
} // namespace emergency_handler
6766

68-
#endif // EMERGENCY_STOP_HPP_
67+
#endif // OPERATORS__EMERGENCY_STOP_HPP_

system/emergency_handler/schema/emergency_handler.schema.json

+2-9
Original file line numberDiff line numberDiff line change
@@ -51,10 +51,7 @@
5151
"default": -1.5
5252
}
5353
},
54-
"required": [
55-
"target_acceleration",
56-
"target_jerk"
57-
]
54+
"required": ["target_acceleration", "target_jerk"]
5855
},
5956
"comfortable_stop": {
6057
"type": "object",
@@ -75,11 +72,7 @@
7572
"default": -0.3
7673
}
7774
},
78-
"required": [
79-
"min_acceleration",
80-
"max_jerk",
81-
"min_jerk"
82-
]
75+
"required": ["min_acceleration", "max_jerk", "min_jerk"]
8376
}
8477
},
8578
"required": [

system/emergency_handler/src/emergency_handler/emergency_handler_core.cpp

+3-2
Original file line numberDiff line numberDiff line change
@@ -61,7 +61,8 @@ EmergencyHandler::EmergencyHandler() : Node("emergency_handler")
6161

6262
// Opertors
6363
emergency_stop_operator_ = std::make_unique<emergency_stop_operator::EmergencyStopOperator>(this);
64-
comfortable_stop_operator_ = std::make_unique<comfortable_stop_operator::ComfortableStopOperator>(this);
64+
comfortable_stop_operator_ =
65+
std::make_unique<comfortable_stop_operator::ComfortableStopOperator>(this);
6566

6667
// Timer
6768
const auto update_period_ns = rclcpp::Rate(param_.update_rate).period();
@@ -400,4 +401,4 @@ bool EmergencyHandler::isStopped()
400401
return false;
401402
}
402403

403-
} // namespace emergency_handler
404+
} // namespace emergency_handler

system/emergency_handler/src/operators/comfortable_stop.cpp

+6-5
Original file line numberDiff line numberDiff line change
@@ -35,22 +35,23 @@ ComfortableStopOperator::ComfortableStopOperator(rclcpp::Node * node) : node_(no
3535
"~/output/velocity_limit/clear", rclcpp::QoS{1}.transient_local());
3636
}
3737

38-
void ComfortableStopOperator::onTimer() {
38+
void ComfortableStopOperator::onTimer()
39+
{
3940
// do nothing
4041
}
4142

4243
bool ComfortableStopOperator::operate()
4344
{
4445
publishVelocityLimit();
45-
46+
4647
// Currently, ComfortableStopOperator does not return false
4748
return true;
4849
}
4950

5051
bool ComfortableStopOperator::cancel()
5152
{
5253
publishVelocityLimitClearCommand();
53-
54+
5455
// Currently, ComfortableStopOperator does not return false
5556
return true;
5657
}
@@ -79,6 +80,6 @@ void ComfortableStopOperator::publishVelocityLimitClearCommand()
7980
pub_velocity_limit_clear_command_->publish(velocity_limit_clear_command);
8081
}
8182

82-
} // namespace comfortable_stop_operator
83+
} // namespace comfortable_stop_operator
8384

84-
} // namespace emergency_handler
85+
} // namespace emergency_handler

system/emergency_handler/src/operators/emergency_stop.cpp

+10-7
Original file line numberDiff line numberDiff line change
@@ -23,7 +23,8 @@ namespace emergency_stop_operator
2323
EmergencyStopOperator::EmergencyStopOperator(rclcpp::Node * node) : node_(node)
2424
{
2525
// Parameter
26-
params_.target_acceleration = node_->declare_parameter<double>("emergency_stop.target_acceleration");
26+
params_.target_acceleration =
27+
node_->declare_parameter<double>("emergency_stop.target_acceleration");
2728
params_.target_jerk = node_->declare_parameter<double>("emergency_stop.target_jerk");
2829

2930
// Subscriber
@@ -45,14 +46,15 @@ void EmergencyStopOperator::onControlCommand(AckermannControlCommand::ConstShare
4546
is_prev_control_cmd_subscribed_ = true;
4647
}
4748

48-
void EmergencyStopOperator::onTimer() {
49+
void EmergencyStopOperator::onTimer()
50+
{
4951
pub_control_cmd_->publish(calcNextControlCmd());
5052
}
5153

5254
bool EmergencyStopOperator::operate()
5355
{
5456
pub_control_cmd_->publish(calcNextControlCmd());
55-
57+
5658
// Currently, EmergencyStopOperator does not return false
5759
return true;
5860
}
@@ -92,7 +94,8 @@ AckermannControlCommand EmergencyStopOperator::calcNextControlCmd()
9294
const auto dt = (now - prev_control_cmd_.stamp).seconds();
9395
control_cmd.longitudinal.stamp = now;
9496
control_cmd.longitudinal.speed = static_cast<float>(std::max(
95-
prev_control_cmd_.longitudinal.speed + prev_control_cmd_.longitudinal.acceleration * dt, 0.0));
97+
prev_control_cmd_.longitudinal.speed + prev_control_cmd_.longitudinal.acceleration * dt,
98+
0.0));
9699
control_cmd.longitudinal.acceleration = static_cast<float>(std::max(
97100
prev_control_cmd_.longitudinal.acceleration + params_.target_jerk * dt,
98101
params_.target_acceleration));
@@ -104,10 +107,10 @@ AckermannControlCommand EmergencyStopOperator::calcNextControlCmd()
104107

105108
// lateral: keep previous lateral command
106109
}
107-
110+
108111
return control_cmd;
109112
}
110113

111-
} // namespace emergency_stop_operator
114+
} // namespace emergency_stop_operator
112115

113-
} // namespace emergency_handler
116+
} // namespace emergency_handler

0 commit comments

Comments
 (0)