Merge pull request #32 from toppers/refactor-tb3ctrl

Refactor tb3ctrl

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,9 +34,10 @@ static float get_foward_distance(void) {
       min = scan_data.ranges[i];
     }
   }
-  // printf("foward: %lf\n", min);
+  // printf("forward: %lf\n", min);
   return min;
 }
+
 static float get_right_distance(void) {
   int i;
   float min = 100.0f;
@@ -48,21 +49,11 @@ static float get_right_distance(void) {
   // printf("right: %lf\n", min);
   return min;
 }
-static float sarch_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_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 {
@@ -72,23 +63,6 @@ static bool do_foward(void) {
   return is_stop;
 }
 
-#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;
-}
-#endif
-
 static bool turn_right(void) {
   bool is_stop = false;
   cmd_vel.angular.z = 0;
@@ -103,35 +77,31 @@ 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) {
     cmd_vel.angular.z = -1.0f;
   }
   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);
 
@@ -144,18 +114,9 @@ int main(int argc, char **argv) {
   rclcpp::WallRate rate(10ms);
 
   while (rclcpp::ok()) {
-    if (mode == RoboMode_INIT) {
-      float d = sarch_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();
   }