Merge pull request #20 from dawan0111/feature/navi

navi and yac servo update

md/src/com.cpp

@@ -26,7 +26,7 @@ extern void PubRobotPose(void);
 int InitSerialComm(void)
 {
     try {
-        ser.setPort("/dev/yacyac/motor");
+        ser.setPort("/dev/ttyUSB0");
         ser.setBaudrate(robotParamData.nBaudrate);
         serial::Timeout to = serial::Timeout::simpleTimeout(1667); // 1667 when baud is 57600, 0.6ms
         ser.setTimeout(to);                                        // 2857 when baud is 115200, 0.35ms

yacyac_navigation2/param/yacyac.yaml

@@ -14,7 +14,7 @@ amcl:
     global_frame_id: "map"
     lambda_short: 0.1
     laser_likelihood_max_dist: 2.0
-    laser_max_range: 100.0
+    laser_max_range: 12.0
     laser_min_range: -1.0
     laser_model_type: "likelihood_field"
     max_beams: 60
@@ -126,11 +126,11 @@ controller_server:
       debug_trajectory_details: True
       min_vel_x: 0.0
       min_vel_y: 0.0
-      max_vel_x: 0.22
+      max_vel_x: 0.4
       max_vel_y: 0.0
       max_vel_theta: 1.0
       min_speed_xy: 0.0
-      max_speed_xy: 0.22
+      max_speed_xy: 0.4
       min_speed_theta: 0.0
       # Add high threshold velocity for turtlebot 3 issue.
       # https://github.com/ROBOTIS-GIT/turtlebot3_simulations/issues/75
@@ -179,7 +179,7 @@ local_costmap:
       width: 3
       height: 3
       resolution: 0.05
-      robot_radius: 0.1
+      robot_radius: 0.233
       plugins: ["obstacle_layer", "voxel_layer", "inflation_layer"]
       inflation_layer:
         plugin: "nav2_costmap_2d::InflationLayer"
@@ -347,4 +347,4 @@ waypoint_follower:
     wait_at_waypoint:
       plugin: "nav2_waypoint_follower::WaitAtWaypoint"
       enabled: True
-      waypoint_pause_duration: 200
+      waypoint_pause_duration: 200

yacyac_servo/launch/yacyac_servo.launch.py

@@ -4,20 +4,17 @@
 from launch_ros.actions import Node
 
 def generate_launch_description():
-    # servo_ctrl 노드를 실행하는 런치 액션
     servo_ctrl_node = Node(
-            package='yacyac_servo',
-            executable='servo_ctrl',
-            name='servo_ctrl_node',
-            output='screen',
-        ),
-    # read_write_node 노드를 실행하는 런치 액션
+        package='yacyac_servo',
+        executable='servo_ctrl',
+        name='servo_ctrl_node',
+        output='screen',
+    )
     read_write_node = Node(
         package="dynamixel_sdk_examples",
         executable="read_write_node",
         output="screen",
-        # parameters=[{"use_sim_time": False}],  # 필요한 경우 추가적인 파라미터 설정
     )
-
+    
     return LaunchDescription([servo_ctrl_node, read_write_node])
 

yacyac_servo/yacyac_servo/servo_ctrl.py

@@ -33,8 +33,10 @@ def __init__(self):
         self.position_cnt =  [0, 0, 0, 0, 0, 0, 0, 0, 0]
         print ("init positioning...")
         for i in range(8):
-            # self.init_position(i)
-            self.reset_position(i)
+            # 근처 포지션으로 이동합니다.
+            self.init_position(i)
+            # 원점 포지션으로 이동합니다. 
+            # self.reset_position(i)
         print ("init positioning done!!!")
 
 
@@ -49,38 +51,53 @@ def init_position(self, id):
 
         if future.result() is not None:
             response = future.result()
-
+            # print("response")
             closest_idx = 0
 
             min_diff = 10000
-            for i, val in enumerate(self.close_position_set):
+
+
+            cmd_pose = SetPosition()
+            cmd_pose.id = id
+            flag = 0
+            for i, val in enumerate(self.cw_position_set):
                 diff = abs(response.position - val)
                 if diff < min_diff:
                     min_diff = diff
                     closest_idx = i
-
-            cmd_pose = SetPosition()
-            cmd_pose.id = id
-            cmd_pose.position = self.close_position_set[closest_idx]
+                    cmd_pose.position = self.cw_position_set[closest_idx]
+                    flag = 1
+            for i, val in enumerate(self.ccw_position_set):
+                diff = abs(response.position - val)
+                if diff < min_diff:
+                    min_diff = diff
+                    closest_idx = i
+                    cmd_pose.position = self.ccw_position_set[closest_idx]
+                    flag = 0
+            if flag == 0:
+                self.position_flag[id] = 1
+                # print("cw")
+            else:
+                self.position_flag[id] = 0
+                # print("ccw")
             # 리스트의 요소값과 같은 값이 있으면 그 인덱스를 반환
-            for i, val in enumerate(self.position_set):
-                if val == cmd_pose.position:
-                    self.position_cnt[id] = i
-                    break
-            # closest_idx이 중앙값을 넘는경우 
-            if closest_idx > 2:
-                # 반시계 방향으로 돌아야함
-                self.position_flag[closest_idx] = 1
+            if flag: 
+                for i, val in enumerate(self.cw_position_set):
+                    if val == cmd_pose.position:
+                        self.position_cnt[id] = i
+                        break
             else:
-                # 시계 방향으로 돌아야함
-                self.position_flag[closest_idx] = 0
-
+                for i, val in enumerate(self.ccw_position_set):
+                    if val == cmd_pose.position:
+                        self.position_cnt[id] = i
+                        break
             self.pub.publish(cmd_pose)
             time.sleep(0.1)
         else:
             self.get_logger().info(f"Service call for ID {id} failed")
 
 
+
     def control_position(self, id, servo):
         # 제조할 약의 개수 
         yac_num = servo
@@ -92,8 +109,8 @@ def control_position(self, id, servo):
             yac_num -= 1
             yac_cnt += 1
             # yac_cnt 
-            if yac_cnt % 2 == 0:
-                print(id, "번 약 ", servo, "개중 ", yac_cnt // 2, "개 배출중...")
+
+            print(id, "번 약 ", servo, "개중 ", yac_cnt, "개 배출중...")
             if self.position_flag[id] == 1:
                 cmd_pose = SetPosition()
                 self.position_cnt[id] += 1