Skip to content

Commit 95ac60c

Browse files
author
Shota Aoki
authored
CIにLintによるコードスタイルのチェックを追加。スケッチファイルのコード整形を実施。 (#5)
* Add lint.yaml * Set recursive true * Set comliance specification * Add clang-format check * .clang-formatファイルをルートに追加。 STEP1に.clang-formatを追加。 STEP1にArduino-IDEの自動整形を適用 * 自動整形についてREADMEに説明を記載 * .clang-formatのシンボリックリンクをSTEP2-STEP13に追加 * STEP2-8を自動整形 * Use zsh for clang-format * Update for zsh * STEP11-13に自動整形を適用 * メッセージヘッダーファイルが見つからないエラーを解決するため、インクルード順序を変更する。また、これをclang-formatのチェック対象外とする
1 parent 4f41fe9 commit 95ac60c

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

60 files changed

+1036
-982
lines changed

.clang-format

Lines changed: 17 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,17 @@
1+
Language: Cpp
2+
BasedOnStyle: Google
3+
4+
AccessModifierOffset: -2
5+
AlignAfterOpenBracket: AlwaysBreak
6+
BraceWrapping:
7+
AfterClass: true
8+
AfterFunction: true
9+
AfterNamespace: true
10+
AfterStruct: true
11+
BreakBeforeBraces: Custom
12+
ColumnLimit: 100
13+
ConstructorInitializerIndentWidth: 0
14+
ContinuationIndentWidth: 2
15+
DerivePointerAlignment: false
16+
PointerAlignment: Middle
17+
ReflowComments: false

.github/workflows/lint.yaml

Lines changed: 30 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,30 @@
1+
name: Lint
2+
3+
on:
4+
push:
5+
paths-ignore:
6+
- '**.md'
7+
pull_request:
8+
paths-ignore:
9+
- '**.md'
10+
workflow_dispatch:
11+
12+
jobs:
13+
arduino-lint:
14+
runs-on: ubuntu-latest
15+
steps:
16+
- uses: actions/checkout@v3
17+
- uses: arduino/arduino-lint-action@v1
18+
with:
19+
recursive: true
20+
compliance: specification
21+
clang-format:
22+
needs: arduino-lint
23+
runs-on: ubuntu-latest
24+
steps:
25+
- uses: actions/checkout@v3
26+
# 各スケッチファイルの.ino、.hファイルに対してclang-formatによる整形が必要か判定する
27+
# 正規表現を簡単にするためzshを使用する
28+
- run: sudo apt install -y clang-format zsh
29+
- run: clang-format --dry-run -Werror uROS_STEP*/*.(ino|h)
30+
shell: zsh {0}

README.md

Lines changed: 5 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -2,3 +2,8 @@
22

33
[![CI](https://github.com/rt-net/pico_micro_ros_examples/actions/workflows/ci.yaml/badge.svg?branch=main)](https://github.com/rt-net/pico_micro_ros_examples/actions/workflows/ci.yaml)
44

5+
6+
## スケッチファイルの自動整形について
7+
8+
ソースコードのレイアウトを整えるため、各スケッチファイルにはArduino IDEの自動整形を適用しています。
9+
自動整形のルールは[.clang-format](.clang-format) ファイルを参照してください。

uROS_STEP10_twistMsg/.clang-format

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
../.clang-format

uROS_STEP10_twistMsg/interrupt.ino

Lines changed: 17 additions & 19 deletions
Original file line numberDiff line numberDiff line change
@@ -12,40 +12,38 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
void control_interrupt(void){
15+
void control_interrupt(void)
16+
{
17+
double speed_r, speed_l;
1618

17-
double speed_r,speed_l;
18-
19-
if ((speed < 0.001) && (speed > -0.001)
20-
&& (omega < 0.001) && (omega > -0.001)) {
19+
if ((speed < 0.001) && (speed > -0.001) && (omega < 0.001) && (omega > -0.001)) {
2120
motor_move = 0;
2221
} else {
2322
motor_move = 1;
2423
}
2524

26-
speed_r = speed + omega*TREAD_WIDTH/2.0;
27-
speed_l = speed - omega*TREAD_WIDTH/2.0;
25+
speed_r = speed + omega * TREAD_WIDTH / 2.0;
26+
speed_l = speed - omega * TREAD_WIDTH / 2.0;
2827

29-
if(speed_r > 0){
28+
if (speed_r > 0) {
3029
digitalWrite(CW_R, LOW);
31-
}else{
30+
} else {
3231
digitalWrite(CW_R, HIGH);
3332
speed_r *= -1.0;
3433
}
35-
if(speed_r < 0.001){
36-
speed_r=0.0;
37-
}else if(speed_r < MIN_SPEED){
38-
speed_r =MIN_SPEED;
34+
if (speed_r < 0.001) {
35+
speed_r = 0.0;
36+
} else if (speed_r < MIN_SPEED) {
37+
speed_r = MIN_SPEED;
3938
}
40-
RStepHz = (signed short)(speed_r/PULSE);
39+
RStepHz = (signed short)(speed_r / PULSE);
4140

42-
if(speed_l > 0){
41+
if (speed_l > 0) {
4342
digitalWrite(CW_L, LOW);
44-
}else{
43+
} else {
4544
digitalWrite(CW_L, HIGH);
4645
speed_l *= -1.0;
4746
}
48-
if(speed_l < MIN_SPEED)speed_l =MIN_SPEED;
49-
LStepHz = (signed short)(speed_l/PULSE);
50-
47+
if (speed_l < MIN_SPEED) speed_l = MIN_SPEED;
48+
LStepHz = (signed short)(speed_l / PULSE);
5149
}

uROS_STEP10_twistMsg/uROS_STEP10_twistMsg.ino

Lines changed: 41 additions & 33 deletions
Original file line numberDiff line numberDiff line change
@@ -12,16 +12,17 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
15+
// clang-format off
16+
// メッセージヘッダーファイルを見つけるため、micro_ros_arduino.hを先にインクルードすること
1617
#include <micro_ros_arduino.h>
17-
18-
#include <stdio.h>
19-
#include <rcl/rcl.h>
18+
#include <geometry_msgs/msg/twist.h>
2019
#include <rcl/error_handling.h>
21-
#include <rclc/rclc.h>
20+
#include <rcl/rcl.h>
2221
#include <rclc/executor.h>
22+
#include <rclc/rclc.h>
23+
#include <stdio.h>
24+
// clang-format on
2325

24-
#include <geometry_msgs/msg/twist.h>
2526
geometry_msgs__msg__Twist msg;
2627

2728
rcl_subscription_t subscriber;
@@ -47,9 +48,9 @@ rcl_node_t node;
4748
#define MIN_SPEED (MIN_HZ * PULSE)
4849
#define TREAD_WIDTH (65.0)
4950

50-
hw_timer_t* timer0 = NULL;
51-
hw_timer_t* timer2 = NULL;
52-
hw_timer_t* timer3 = NULL;
51+
hw_timer_t * timer0 = NULL;
52+
hw_timer_t * timer2 = NULL;
53+
hw_timer_t * timer3 = NULL;
5354

5455
portMUX_TYPE timerMux = portMUX_INITIALIZER_UNLOCKED;
5556

@@ -70,18 +71,23 @@ double omega = 0;
7071

7172
volatile bool motor_move = 0;
7273

73-
#define RCCHECK(fn) \
74-
{ \
75-
rcl_ret_t temp_rc = fn; \
76-
if ((temp_rc != RCL_RET_OK)) { error_loop(); } \
74+
#define RCCHECK(fn) \
75+
{ \
76+
rcl_ret_t temp_rc = fn; \
77+
if ((temp_rc != RCL_RET_OK)) { \
78+
error_loop(); \
79+
} \
7780
}
78-
#define RCSOFTCHECK(fn) \
79-
{ \
80-
rcl_ret_t temp_rc = fn; \
81-
if ((temp_rc != RCL_RET_OK)) { error_loop(); } \
81+
#define RCSOFTCHECK(fn) \
82+
{ \
83+
rcl_ret_t temp_rc = fn; \
84+
if ((temp_rc != RCL_RET_OK)) { \
85+
error_loop(); \
86+
} \
8287
}
8388

84-
void error_loop() {
89+
void error_loop()
90+
{
8591
while (1) {
8692
digitalWrite(LED0, !digitalRead(LED0));
8793
delay(100);
@@ -90,14 +96,16 @@ void error_loop() {
9096

9197
//割り込み
9298
//目標値の更新周期1kHz
93-
void IRAM_ATTR OnTimer0(void) {
99+
void IRAM_ATTR OnTimer0(void)
100+
{
94101
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
95102
control_interrupt();
96103
portEXIT_CRITICAL_ISR(&timerMux); //割り込み許可
97104
}
98105

99106
//Rモータの周期数割り込み
100-
void IRAM_ATTR IsrR(void) {
107+
void IRAM_ATTR IsrR(void)
108+
{
101109
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
102110
if (motor_move) {
103111
if (RStepHz < 30) RStepHz = 30;
@@ -113,7 +121,8 @@ void IRAM_ATTR IsrR(void) {
113121
}
114122

115123
//Lモータの周期数割り込み
116-
void IRAM_ATTR IsrL(void) {
124+
void IRAM_ATTR IsrL(void)
125+
{
117126
portENTER_CRITICAL_ISR(&timerMux); //割り込み禁止
118127
if (motor_move) {
119128
if (LStepHz < 30) LStepHz = 30;
@@ -128,17 +137,17 @@ void IRAM_ATTR IsrL(void) {
128137
portEXIT_CRITICAL_ISR(&timerMux); //割り込み許可
129138
}
130139

131-
132140
//twist message cb
133-
void subscription_callback(const void* msgin) {
134-
const geometry_msgs__msg__Twist* msg = (const geometry_msgs__msg__Twist*)msgin;
141+
void subscription_callback(const void * msgin)
142+
{
143+
const geometry_msgs__msg__Twist * msg = (const geometry_msgs__msg__Twist *)msgin;
135144

136145
speed = msg->linear.x * 1000.0;
137146
omega = msg->angular.z;
138-
139147
}
140148

141-
void setup() {
149+
void setup()
150+
{
142151
// set_microros_transports();
143152
pinMode(LED0, OUTPUT);
144153
pinMode(LED1, OUTPUT);
@@ -147,7 +156,7 @@ void setup() {
147156

148157
digitalWrite(LED1, HIGH);
149158
set_microros_wifi_transports("使用するWiFiのAP名", "Wi-Fiのパスワード", "PCのIPアドレス", 8888);
150-
159+
151160
digitalWrite(LED2, HIGH);
152161

153162
//motor disable
@@ -190,19 +199,18 @@ void setup() {
190199

191200
// create subscriber
192201
RCCHECK(rclc_subscription_init_default(
193-
&subscriber,
194-
&node,
195-
ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist),
196-
"/cmd_vel"));
202+
&subscriber, &node, ROSIDL_GET_MSG_TYPE_SUPPORT(geometry_msgs, msg, Twist), "/cmd_vel"));
197203

198204
// create executor
199205
RCCHECK(rclc_executor_init(&executor, &support.context, 1, &allocator));
200-
RCCHECK(rclc_executor_add_subscription(&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
206+
RCCHECK(rclc_executor_add_subscription(
207+
&executor, &subscriber, &msg, &subscription_callback, ON_NEW_DATA));
201208

202209
digitalWrite(MOTOR_EN, HIGH);
203210
}
204211

205-
void loop() {
212+
void loop()
213+
{
206214
delay(10);
207215
RCCHECK(rclc_executor_spin_some(&executor, RCL_MS_TO_NS(100)));
208216
}

uROS_STEP11_tfMsg/.clang-format

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1 @@
1+
../.clang-format

uROS_STEP11_tfMsg/intrrupt.ino

Lines changed: 15 additions & 16 deletions
Original file line numberDiff line numberDiff line change
@@ -12,36 +12,36 @@
1212
// See the License for the specific language governing permissions and
1313
// limitations under the License.
1414

15-
void control_interrupt(void) {
15+
void control_interrupt(void)
16+
{
1617
double speed_r, speed_l;
1718

18-
if ((speed < 0.001) && (speed > -0.001)
19-
&& (omega < 0.001) && (omega > -0.001)) {
19+
if ((speed < 0.001) && (speed > -0.001) && (omega < 0.001) && (omega > -0.001)) {
2020
motor_move = 0;
2121
} else {
2222
motor_move = 1;
2323
}
2424

25-
speed_r = speed + omega*TREAD_WIDTH/2.0;
26-
speed_l = speed - omega*TREAD_WIDTH/2.0;
25+
speed_r = speed + omega * TREAD_WIDTH / 2.0;
26+
speed_l = speed - omega * TREAD_WIDTH / 2.0;
2727

28-
if((speed_r >0.001)&&(speed_r<MIN_SPEED)){
28+
if ((speed_r > 0.001) && (speed_r < MIN_SPEED)) {
2929
speed_r = MIN_SPEED;
30-
}else if((speed_r < -0.001)&&(speed_r> (-1.0*MIN_SPEED))){
30+
} else if ((speed_r < -0.001) && (speed_r > (-1.0 * MIN_SPEED))) {
3131
speed_r = -1.0 * MIN_SPEED;
3232
}
33-
if((speed_l >0.001)&&(speed_l<MIN_SPEED)){
33+
if ((speed_l > 0.001) && (speed_l < MIN_SPEED)) {
3434
speed_l = MIN_SPEED;
35-
}else if((speed_l < -0.001)&&(speed_l> (-1.0*MIN_SPEED))){
35+
} else if ((speed_l < -0.001) && (speed_l > (-1.0 * MIN_SPEED))) {
3636
speed_l = -1.0 * MIN_SPEED;
3737
}
38-
speed= (speed_r+speed_l)/2.0;
38+
speed = (speed_r + speed_l) / 2.0;
3939

40-
odom_x += speed *0.001 * cos(odom_theta) * 0.001;
41-
odom_y += speed *0.001 * sin(odom_theta) * 0.001;
42-
odom_theta += omega *0.001;
43-
position_r += speed_r*0.001 /(48 * PI) * 2 * PI;
44-
position_l -= speed_l*0.001 /(48 * PI) * 2 * PI;
40+
odom_x += speed * 0.001 * cos(odom_theta) * 0.001;
41+
odom_y += speed * 0.001 * sin(odom_theta) * 0.001;
42+
odom_theta += omega * 0.001;
43+
position_r += speed_r * 0.001 / (48 * PI) * 2 * PI;
44+
position_l -= speed_l * 0.001 / (48 * PI) * 2 * PI;
4545

4646
if (speed_r > 0) {
4747
digitalWrite(CW_R, LOW);
@@ -57,4 +57,3 @@ void control_interrupt(void) {
5757
}
5858
L_STEP_HZ = abs((signed short)(speed_l / PULSE));
5959
}
60-

0 commit comments

Comments
 (0)