From cc5d3ae580e35eaa3f5033baba45d11a6cd732ab Mon Sep 17 00:00:00 2001 From: s-hosoai Date: Sat, 14 May 2022 16:17:21 +0900 Subject: [PATCH 1/5] Reformat spaces --- ros2/workspace/src/tb3/src/tb3ctrl.cpp | 42 ++++++++++++++------------ 1 file changed, 23 insertions(+), 19 deletions(-) diff --git a/ros2/workspace/src/tb3/src/tb3ctrl.cpp b/ros2/workspace/src/tb3/src/tb3ctrl.cpp index 650a0bf..f3f9a53 100644 --- a/ros2/workspace/src/tb3/src/tb3ctrl.cpp +++ b/ros2/workspace/src/tb3/src/tb3ctrl.cpp @@ -37,6 +37,7 @@ static float get_foward_distance(void) { // printf("foward: %lf\n", min); return min; } + static float get_right_distance(void) { int i; float min = 100.0f; @@ -48,6 +49,7 @@ static float get_right_distance(void) { // printf("right: %lf\n", min); return min; } + static float sarch_all(void) { int i; float min = 100.0f; @@ -75,17 +77,17 @@ static bool do_foward(void) { #if 0 static bool turn_left(void) { - bool is_stop = false; - cmd_vel.angular.z = 0; - if (get_right_distance() < 0.05f) { - cmd_vel.angular.z = 5; - is_stop = true; - } - else { - cmd_vel.angular.z = 0; - } - - return is_stop; + bool is_stop = false; + cmd_vel.angular.z = 0; + if (get_right_distance() < 0.05f) { + cmd_vel.angular.z = 5; + is_stop = true; + } + else { + cmd_vel.angular.z = 0; + } + + return is_stop; } #endif @@ -111,27 +113,29 @@ static void do_control(void) { } return; } + using namespace std::chrono_literals; typedef enum { RoboMode_INIT = 0, RoboMode_RUN, } RoboModeType; + int main(int argc, char **argv) { RoboModeType mode = RoboMode_RUN; char buffer[3][4096]; if (argc > 1) { - sprintf(buffer[0], "%s_tb3_node", argv[1]); - sprintf(buffer[1], "%s_cmd_vel", argv[1]); - sprintf(buffer[2], "%s_scan", argv[1]); - printf("START: %s\n", argv[1]); + sprintf(buffer[0], "%s_tb3_node", argv[1]); + sprintf(buffer[1], "%s_cmd_vel", argv[1]); + sprintf(buffer[2], "%s_scan", argv[1]); + printf("START: %s\n", argv[1]); } else { - sprintf(buffer[0], "tb3_node"); - sprintf(buffer[1], "cmd_vel"); - sprintf(buffer[2], "scan"); - printf("START\n"); + sprintf(buffer[0], "tb3_node"); + sprintf(buffer[1], "cmd_vel"); + sprintf(buffer[2], "scan"); + printf("START\n"); } rclcpp::init(argc, argv); From 1e4663308da4e7a27d50541828bd2d7e899997be Mon Sep 17 00:00:00 2001 From: s-hosoai Date: Sat, 14 May 2022 17:44:25 +0900 Subject: [PATCH 2/5] Fix typo --- ros2/workspace/src/tb3/src/tb3ctrl.cpp | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/ros2/workspace/src/tb3/src/tb3ctrl.cpp b/ros2/workspace/src/tb3/src/tb3ctrl.cpp index f3f9a53..479cc95 100644 --- a/ros2/workspace/src/tb3/src/tb3ctrl.cpp +++ b/ros2/workspace/src/tb3/src/tb3ctrl.cpp @@ -21,7 +21,7 @@ static void scanCallback(const sensor_msgs::msg::LaserScan::SharedPtr msg) { static geometry_msgs::msg::Twist cmd_vel; -static float get_foward_distance(void) { +static float get_forward_distance(void) { int i; float min = 100.0f; for (i = 0; i < 15; i++) { @@ -34,7 +34,7 @@ static float get_foward_distance(void) { min = scan_data.ranges[i]; } } - // printf("foward: %lf\n", min); + // printf("forward: %lf\n", min); return min; } @@ -50,7 +50,7 @@ static float get_right_distance(void) { return min; } -static float sarch_all(void) { +static float search_all(void) { int i; float min = 100.0f; for (i = 0; i < 360; i++) { @@ -61,10 +61,10 @@ static float sarch_all(void) { return min; } -static bool do_foward(void) { +static bool do_forward(void) { bool is_stop = false; cmd_vel.linear.x = 0; - if (get_foward_distance() < 0.2f) { + if (get_forward_distance() < 0.2f) { cmd_vel.linear.x = 0; is_stop = true; } else { @@ -105,7 +105,7 @@ static bool turn_right(void) { } static void do_control(void) { - (void)do_foward(); + (void)do_forward(); (void)turn_right(); if (cmd_vel.linear.x == 0 && cmd_vel.angular.z == 0) { @@ -149,7 +149,7 @@ int main(int argc, char **argv) { while (rclcpp::ok()) { if (mode == RoboMode_INIT) { - float d = sarch_all(); + float d = search_all(); if (d > 0.0f && d <= 0.08f) { printf("d=%f MOVE\n", d); mode = RoboMode_RUN; From da118e60c972e9be3811e69db6e8513ba7d87d00 Mon Sep 17 00:00:00 2001 From: s-hosoai Date: Sat, 14 May 2022 17:45:45 +0900 Subject: [PATCH 3/5] Remove #if comment out --- ros2/workspace/src/tb3/src/tb3ctrl.cpp | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/ros2/workspace/src/tb3/src/tb3ctrl.cpp b/ros2/workspace/src/tb3/src/tb3ctrl.cpp index 479cc95..212bb99 100644 --- a/ros2/workspace/src/tb3/src/tb3ctrl.cpp +++ b/ros2/workspace/src/tb3/src/tb3ctrl.cpp @@ -74,7 +74,6 @@ static bool do_forward(void) { return is_stop; } -#if 0 static bool turn_left(void) { bool is_stop = false; @@ -89,7 +88,6 @@ static bool turn_left(void) return is_stop; } -#endif static bool turn_right(void) { bool is_stop = false; @@ -149,7 +147,7 @@ int main(int argc, char **argv) { while (rclcpp::ok()) { if (mode == RoboMode_INIT) { - float d = search_all(); + float d = search_all(); if (d > 0.0f && d <= 0.08f) { printf("d=%f MOVE\n", d); mode = RoboMode_RUN; From 94ac032e0bd11a2417c25fc73884375920cd7034 Mon Sep 17 00:00:00 2001 From: s-hosoai Date: Sat, 14 May 2022 18:54:33 +0900 Subject: [PATCH 4/5] Remove unused code --- ros2/workspace/src/tb3/src/tb3ctrl.cpp | 47 ++------------------------ 1 file changed, 3 insertions(+), 44 deletions(-) diff --git a/ros2/workspace/src/tb3/src/tb3ctrl.cpp b/ros2/workspace/src/tb3/src/tb3ctrl.cpp index 212bb99..ad791ca 100644 --- a/ros2/workspace/src/tb3/src/tb3ctrl.cpp +++ b/ros2/workspace/src/tb3/src/tb3ctrl.cpp @@ -50,17 +50,6 @@ static float get_right_distance(void) { return min; } -static float search_all(void) { - int i; - float min = 100.0f; - for (i = 0; i < 360; i++) { - if (scan_data.ranges[i] < min) { - min = scan_data.ranges[i]; - } - } - return min; -} - static bool do_forward(void) { bool is_stop = false; cmd_vel.linear.x = 0; @@ -74,21 +63,6 @@ static bool do_forward(void) { return is_stop; } -static bool turn_left(void) -{ - bool is_stop = false; - cmd_vel.angular.z = 0; - if (get_right_distance() < 0.05f) { - cmd_vel.angular.z = 5; - is_stop = true; - } - else { - cmd_vel.angular.z = 0; - } - - return is_stop; -} - static bool turn_right(void) { bool is_stop = false; cmd_vel.angular.z = 0; @@ -114,13 +88,7 @@ static void do_control(void) { using namespace std::chrono_literals; -typedef enum { - RoboMode_INIT = 0, - RoboMode_RUN, -} RoboModeType; - int main(int argc, char **argv) { - RoboModeType mode = RoboMode_RUN; char buffer[3][4096]; if (argc > 1) { @@ -146,18 +114,9 @@ int main(int argc, char **argv) { rclcpp::WallRate rate(10ms); while (rclcpp::ok()) { - if (mode == RoboMode_INIT) { - float d = search_all(); - if (d > 0.0f && d <= 0.08f) { - printf("d=%f MOVE\n", d); - mode = RoboMode_RUN; - } else { - printf("WATING d=%f\n", d); - } - } else { - do_control(); - publisher->publish(cmd_vel); - } + do_control(); + publisher->publish(cmd_vel); + rclcpp::spin_some(node); rate.sleep(); } From ba2827d8fdc66f4314c0b9b5122e42a57f59ef4d Mon Sep 17 00:00:00 2001 From: s-hosoai Date: Sat, 14 May 2022 19:09:29 +0900 Subject: [PATCH 5/5] Align indent --- ros2/workspace/src/tb3/src/tb3ctrl.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/ros2/workspace/src/tb3/src/tb3ctrl.cpp b/ros2/workspace/src/tb3/src/tb3ctrl.cpp index ad791ca..2bf762a 100644 --- a/ros2/workspace/src/tb3/src/tb3ctrl.cpp +++ b/ros2/workspace/src/tb3/src/tb3ctrl.cpp @@ -92,15 +92,15 @@ int main(int argc, char **argv) { char buffer[3][4096]; if (argc > 1) { - sprintf(buffer[0], "%s_tb3_node", argv[1]); - sprintf(buffer[1], "%s_cmd_vel", argv[1]); - sprintf(buffer[2], "%s_scan", argv[1]); + sprintf(buffer[0], "%s_tb3_node", argv[1]); + sprintf(buffer[1], "%s_cmd_vel", argv[1]); + sprintf(buffer[2], "%s_scan", argv[1]); printf("START: %s\n", argv[1]); } else { - sprintf(buffer[0], "tb3_node"); - sprintf(buffer[1], "cmd_vel"); - sprintf(buffer[2], "scan"); + sprintf(buffer[0], "tb3_node"); + sprintf(buffer[1], "cmd_vel"); + sprintf(buffer[2], "scan"); printf("START\n"); } rclcpp::init(argc, argv);