modified for gazebo

ROBOTIS-GIT · Jun 29, 2018 · 72f9bc7 · 72f9bc7
1 parent 7e01e0d
commit 72f9bc7
Show file tree

Hide file tree

Showing 19 changed files with 92 additions and 90 deletions.
diff --git a/turtlebot3_autorace/package.xml b/turtlebot3_autorace/package.xml
@@ -6,7 +6,7 @@
     AutoRace ROS packages for AutoRace with TurtleBot3 (meta package)
   </description>
   <license>Apache 2.0</license>
-  <author email="rwjung@robotis.com">Leon Jung</author>
+  <author email="kkjong@robotis.com">Gilbert</author>
   <maintainer email="pyo@robotis.com">Pyo</maintainer>
   <url type="website">http://wiki.ros.org/turtlebot3_autorace</url>
   <url type="emanual">http://turtlebot3.robotis.com</url>

diff --git a/turtlebot3_autorace_camera/nodes/image_projection b/turtlebot3_autorace_camera/nodes/image_projection
@@ -17,7 +17,7 @@
 # limitations under the License.
 ################################################################################
 
-# Authors: Leon Jung, [AuTURBO] Ki Hoon Kim (https://github.com/auturbo)
+# Authors: Leon Jung, [AuTURBO] Ki Hoon Kim (https://github.com/auturbo), Gilbert
 
 import rospy
 import numpy as np
@@ -29,9 +29,9 @@ from turtlebot3_autorace_camera.cfg import ImageProjectionParamsConfig
 
 class ImageProjection():
     def __init__(self):
-        self.top_x = rospy.get_param("/camera/extrinsic_camera_calibration/top_x", 60)
-        self.top_y = rospy.get_param("/camera/extrinsic_camera_calibration/top_y", 50)
-        self.bottom_x = rospy.get_param("/camera/extrinsic_camera_calibration/bottom_x", 140)
+        self.top_x = rospy.get_param("/camera/extrinsic_camera_calibration/top_x", 75)
+        self.top_y = rospy.get_param("/camera/extrinsic_camera_calibration/top_y", 35)
+        self.bottom_x = rospy.get_param("/camera/extrinsic_camera_calibration/bottom_x", 165)
         self.bottom_y = rospy.get_param("/camera/extrinsic_camera_calibration/bottom_y", 120)
 
         self.is_calibration_mode = rospy.get_param("~is_extrinsic_camera_calibration_mode", False)

diff --git a/turtlebot3_autorace_camera/package.xml b/turtlebot3_autorace_camera/package.xml
@@ -7,7 +7,7 @@
   </description>
   <license>Apache 2.0</license>
   <author>Leon Jung</author>
-  <maintainer email="pyo@robotis.com">Pyo</maintainer>
+  <author email="kkjong@robotis.com">Gilbert</author>
   <url type="website">http://wiki.ros.org/turtlebot3_autorace_camera</url>
   <url type="emanual">http://turtlebot3.robotis.com</url>
   <url type="repository">https://github.com/ROBOTIS-GIT/turtlebot3_autorace</url>

diff --git a/turtlebot3_autorace_control/nodes/control_lane b/turtlebot3_autorace_control/nodes/control_lane
@@ -17,7 +17,7 @@
 # limitations under the License.
 ################################################################################
 
-# Author: Leon Jung
+# Author: Leon Jung, Gilbert
 
 import rospy
 import numpy as np
@@ -31,7 +31,7 @@ class ControlLane():
         self.pub_cmd_vel = rospy.Publisher('/control/cmd_vel', Twist, queue_size = 1)
 
         self.lastError = 0
-        self.MAX_VEL = 0.19
+        self.MAX_VEL = 0.12
 
         rospy.on_shutdown(self.fnShutDown)
 
@@ -43,22 +43,20 @@ class ControlLane():
 
         error = center - 500
 
-        Kp = 0.0035
+        Kp = 0.0025
         Kd = 0.007
 
         angular_z = Kp * error + Kd * (error - self.lastError)
         self.lastError = error
-
         twist = Twist()
-        twist.linear.x = self.MAX_VEL * ((1 - abs(error) / 500) ** 2) 
+        twist.linear.x = min(self.MAX_VEL * ((1 - abs(error) / 500) ** 2.2), 0.2)
         twist.linear.y = 0
         twist.linear.z = 0
         twist.angular.x = 0
         twist.angular.y = 0
-        twist.angular.z = -angular_z
+        twist.angular.z = -max(angular_z, -2.0) if angular_z < 0 else -min(angular_z, 2.0)
         self.pub_cmd_vel.publish(twist)
 
-
     def fnShutDown(self):
         rospy.loginfo("Shutting down. cmd_vel will be 0")
 

diff --git a/turtlebot3_autorace_control/package.xml b/turtlebot3_autorace_control/package.xml
@@ -6,7 +6,7 @@
     TurtleBot3 AutoRace ROS package that controls TurtleBot3 Auto
   </description>
   <license>Apache 2.0</license>
-  <author>Leon Jung</author>
+  <author email="kkjong@robotis.com">Gilbert</author>
   <maintainer email="pyo@robotis.com">Pyo</maintainer>
   <url type="website">http://wiki.ros.org/turtlebot3_autorace_control</url>
   <url type="emanual">http://turtlebot3.robotis.com</url>

diff --git a/turtlebot3_autorace_core/CMakeLists.txt b/turtlebot3_autorace_core/CMakeLists.txt
@@ -37,6 +37,7 @@ catkin_package()
 catkin_install_python(PROGRAMS
   nodes/core_mode_decider
   nodes/core_node_controller
+  nodes/core_node_mission
   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 

diff --git a/turtlebot3_autorace_core/nodes/core_mode_decider b/turtlebot3_autorace_core/nodes/core_mode_decider
@@ -17,7 +17,7 @@
 # limitations under the License.
 ################################################################################
 
-# Author: Leon Jung
+# Author: Leon Jung, Gilbert
 
 import rospy
 import numpy as np
@@ -45,7 +45,6 @@ class CoreModeDecider():
     def cbInvokedByTrafficSign(self, traffic_sign_type_msg):
         self.fnDecideMode(self.InvokedObject.traffic_sign.value, traffic_sign_type_msg)
         rospy.loginfo("Traffic sign detected")
-
     # Invoke if traffic light is detected
     # def cbInvokedByTrafficLight(self, traffic_light_type_msg):
     #     self.fnDecideMode(self.InvokedObject.traffic_light.value, traffic_light_type_msg)

diff --git a/turtlebot3_autorace_core/nodes/core_node_controller b/turtlebot3_autorace_core/nodes/core_node_controller
@@ -17,7 +17,7 @@
 # limitations under the License.
 ################################################################################
 
-# Authors: Leon Jung, [AuTURBO] Kihoon Kim (https://github.com/auturbo)
+# Authors: Leon Jung, [AuTURBO] Kihoon Kim (https://github.com/auturbo), Gilbert
 
 import rospy, roslaunch
 import numpy as np
@@ -116,7 +116,7 @@ class CoreNodeController():
 
         rospy.loginfo("into %d", self.current_step_level_crossing)
 
-        if self.current_step_level_crossing == self.StepOfLevelCrossing.searching_stop_sign.value:
+        if self.current_step_level_crossing == self.StepOfLevelCrossing.pass_level.value:
             self.current_mode = self.CurrentMode.level_crossing.value
             msg_mode_return = UInt8()
             msg_mode_return.data = self.current_mode
@@ -324,13 +324,13 @@ class CoreNodeController():
                 self.fnLaunch(self.Launcher.launch_detect_sign.value, True)
                 self.fnLaunch(self.Launcher.launch_detect_lane.value, True)
                 self.fnLaunch(self.Launcher.launch_control_lane.value, True)
-                self.fnLaunch(self.Launcher.launch_detect_level.value, False)
-                self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, True)
+                self.fnLaunch(self.Launcher.launch_detect_level.value, True)
+                self.fnLaunch(self.Launcher.launch_detect_traffic_light.value, False)
 
             rospy.sleep(2)
 
             self.pub_level_crossing_order.publish(msg_pub_level_crossing_order)
-        
+
         # tunnel
         elif self.current_mode == self.CurrentMode.tunnel.value:
             rospy.loginfo("New trigger for tunnel")
@@ -573,7 +573,7 @@ class CoreNodeController():
                     self.launch_control_tunnel_launched = False
                     self.launch_control_tunnel.shutdown()
                 else:
-                    pass                 
+                    pass
 
     def main(self):
         rospy.spin()

diff --git a/turtlebot3_autorace_core/nodes/core_node_mission b/turtlebot3_autorace_core/nodes/core_node_mission
@@ -1,5 +1,6 @@
 #!/usr/bin/env python
 # -*- coding: utf-8 -*-
+
 #################################################################################
 # Copyright 2018 ROBOTIS CO., LTD.
 #

diff --git a/turtlebot3_autorace_core/package.xml b/turtlebot3_autorace_core/package.xml
@@ -6,7 +6,7 @@
     TurtleBot3 AutoRace ROS package that TurtleBot3 Auto's core
   </description>
   <license>Apache 2.0</license>
-  <author>Leon Jung</author>
+  <author email="kkjong@robotis.com">Gilbert</author>
   <maintainer email="pyo@robotis.com">Pyo</maintainer>
   <url type="website">http://wiki.ros.org/turtlebot3_autorace_core</url>
   <url type="emanual">http://turtlebot3.robotis.com</url>

diff --git a/turtlebot3_autorace_detect/CMakeLists.txt b/turtlebot3_autorace_detect/CMakeLists.txt
@@ -46,12 +46,11 @@ catkin_package(
 catkin_install_python(PROGRAMS
   nodes/detect_lane
   nodes/detect_level
-  nodes/detect_obstacle
   nodes/detect_parking
   nodes/detect_sign
   nodes/detect_traffic_light
   nodes/detect_tunnel
- 	DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 )
 
 install(DIRECTORY cfg file launch param

diff --git a/turtlebot3_autorace_detect/nodes/detect_lane b/turtlebot3_autorace_detect/nodes/detect_lane
@@ -17,7 +17,7 @@
 # limitations under the License.
 ################################################################################
 
-# Authors: Leon Jung, Special Thanks : Roger Sacchelli
+# Authors: Leon Jung, Gilbert, Special Thanks : Roger Sacchelli
 
 import rospy
 import numpy as np
@@ -30,18 +30,18 @@ from turtlebot3_autorace_detect.cfg import DetectLaneParamsConfig
 
 class DetectLane():
     def __init__(self):
-        self.hue_white_l = rospy.get_param("~detect/lane/white/hue_l", 90)
-        self.hue_white_h = rospy.get_param("~detect/lane/white/hue_h", 180)
-        self.saturation_white_l = rospy.get_param("~detect/lane/white/saturation_l", 128)
-        self.saturation_white_h = rospy.get_param("~detect/lane/white/saturation_h", 255)
-        self.lightness_white_l = rospy.get_param("~detect/lane/white/lightness_l", 128)
+        self.hue_white_l = rospy.get_param("~detect/lane/white/hue_l", 0)
+        self.hue_white_h = rospy.get_param("~detect/lane/white/hue_h", 25)
+        self.saturation_white_l = rospy.get_param("~detect/lane/white/saturation_l", 0)
+        self.saturation_white_h = rospy.get_param("~detect/lane/white/saturation_h", 36)
+        self.lightness_white_l = rospy.get_param("~detect/lane/white/lightness_l", 180)
         self.lightness_white_h = rospy.get_param("~detect/lane/white/lightness_h", 255)
 
-        self.hue_yellow_l = rospy.get_param("~detect/lane/yellow/hue_l", 90)
-        self.hue_yellow_h = rospy.get_param("~detect/lane/yellow/hue_h", 180)
-        self.saturation_yellow_l = rospy.get_param("~detect/lane/yellow/saturation_l", 128)
+        self.hue_yellow_l = rospy.get_param("~detect/lane/yellow/hue_l", 27)
+        self.hue_yellow_h = rospy.get_param("~detect/lane/yellow/hue_h", 41)
+        self.saturation_yellow_l = rospy.get_param("~detect/lane/yellow/saturation_l", 130)
         self.saturation_yellow_h = rospy.get_param("~detect/lane/yellow/saturation_h", 255)
-        self.lightness_yellow_l = rospy.get_param("~detect/lane/yellow/lightness_l", 128)
+        self.lightness_yellow_l = rospy.get_param("~detect/lane/yellow/lightness_l", 160)
         self.lightness_yellow_h = rospy.get_param("~detect/lane/yellow/lightness_h", 255)
 
         self.is_calibration_mode = rospy.get_param("~is_detection_calibration_mode", False)
@@ -101,7 +101,6 @@ class DetectLane():
         rospy.loginfo("saturation_white_h : %d", config.saturation_white_h)
         rospy.loginfo("lightness_white_l : %d", config.lightness_white_l)
         rospy.loginfo("lightness_white_h : %d", config.lightness_white_h)
-
         rospy.loginfo("hue_yellow_l : %d", config.hue_yellow_l)
         rospy.loginfo("hue_yellow_h : %d", config.hue_yellow_h)
         rospy.loginfo("saturation_yellow_l : %d", config.saturation_yellow_l)

diff --git a/turtlebot3_autorace_detect/nodes/detect_level b/turtlebot3_autorace_detect/nodes/detect_level
@@ -17,7 +17,7 @@
 # limitations under the License.
 ################################################################################
 
-# Authors: Leon Jung, [AuTURBO] Kihoon Kim (https://github.com/auturbo)
+# Authors: Leon Jung, [AuTURBO] Kihoon Kim (https://github.com/auturbo), Gilbert
 
 import rospy
 import numpy as np
@@ -246,7 +246,7 @@ class DetectLevel():
             rospy.loginfo("GO~~")
 
             msg_pub_max_vel = Float64()
-            msg_pub_max_vel.data = 0.19
+            msg_pub_max_vel.data = 0.04
             self.pub_max_vel.publish(msg_pub_max_vel)
 
             pub_level_crossing_return.data = self.StepOfLevelCrossing.stop.value
@@ -379,7 +379,7 @@ class DetectLevel():
 
                 # publishing topic
                 self.stop_bar_count = 40
-                if distance_bar2car > 1.5:
+                if distance_bar2car > 0.8:
                     is_level_detected = True
                     self.stop_bar_state = 'slowdown'
                     self.state = "detected"

diff --git a/turtlebot3_autorace_detect/nodes/detect_obstacle b/turtlebot3_autorace_detect/nodes/detect_obstacle
diff --git a/turtlebot3_autorace_detect/nodes/detect_parking b/turtlebot3_autorace_detect/nodes/detect_parking
@@ -17,7 +17,7 @@
 # limitations under the License.
 ################################################################################
 
-# Author: Leon Jung
+# Author: Leon Jung, Gilbert
 
 import rospy
 import numpy as np
@@ -86,21 +86,24 @@ class DetectParking():
         self.img2 = cv2.imread(dir_path + 'parking_allowed.png', -1)
 
     def cbGetImage(self, image_msg):
-        if self.sub_image_type == "compressed":
-            np_arr = np.fromstring(image_msg.data, np.uint8)
-            self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
-        else:
-            self.cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8")
+        if image_msg is not None:
+            if self.sub_image_type == "compressed":
+                np_arr = np.fromstring(image_msg.data, np.uint8)
+                self.cv_image = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
+            else:
+                self.cv_image = self.cvBridge.imgmsg_to_cv2(image_msg, "bgr8")
 
-        self.fnBlinkParkSign()
+            self.fnBlinkParkSign()
 
-        if self.pub_image_type == "compressed":
-            # publishes parking image in compressed type
-            self.pub_image_parking.publish(self.cvBridge.cv2_to_compressed_imgmsg(self.cv_image, "jpg"))
+            if self.pub_image_type == "compressed":
+                # publishes parking image in compressed type
+                self.pub_image_parking.publish(self.cvBridge.cv2_to_compressed_imgmsg(self.cv_image, "jpg"))
 
-        elif self.pub_image_type == "raw":
-            # publishes parking image in raw type
-            self.pub_image_parking.publish(self.cvBridge.cv2_to_imgmsg(self.cv_image, "bgr8"))
+            elif self.pub_image_type == "raw":
+                # publishes parking image in raw type
+                self.pub_image_parking.publish(self.cvBridge.cv2_to_imgmsg(self.cv_image, "bgr8"))
+        else:
+            pass
 
     def cbParkingLotOrder(self, order):
         msg_pub_parking_lot_return = UInt8()
@@ -228,7 +231,7 @@ class DetectParking():
             if self.blink_count % 8 == 0:
                 self.blink_trigger = 1 - self.blink_trigger
                 self.blink_count = 0
-
+            '''
             if self.is_parking_allowed == False:
                 if self.blink_trigger == 1:
                     x_offset = 874
@@ -258,7 +261,7 @@ class DetectParking():
                     for c in range(0, 3):
                         self.cv_image[y1:y2, x1:x2, c] = (alpha_s * self.img2[:, :, c] +
                                                 alpha_l * self.cv_image[y1:y2, x1:x2, c])
-
+            '''
 
     def main(self):
         rospy.spin()

diff --git a/turtlebot3_autorace_detect/nodes/detect_sign b/turtlebot3_autorace_detect/nodes/detect_sign
@@ -17,7 +17,7 @@
 # limitations under the License.
 ################################################################################
 
-# Author: Leon Jung
+# Author: Leon Jung, Gilbert
 
 import rospy
 import numpy as np
@@ -63,7 +63,7 @@ class DetectSign():
         self.sift = cv2.xfeatures2d.SIFT_create()
 
         dir_path = os.path.dirname(os.path.realpath(__file__))
-        dir_path = dir_path.replace('turtlebot3_autorace_detect/src', 'turtlebot3_autorace_detect/')
+        dir_path = dir_path.replace('turtlebot3_autorace_detect/nodes', 'turtlebot3_autorace_detect/')
         dir_path += 'file/detect_sign/'
 
         self.img2 = cv2.imread(dir_path + 'stop.png',0)         # trainImage1
@@ -170,7 +170,6 @@ class DetectSign():
         for m,n in matches4:
             if m.distance < 0.7*n.distance:
                 good4.append(m)
-
         if len(good4)>MIN_MATCH_COUNT:
             src_pts = np.float32([ kp1[m.queryIdx].pt for m in good4 ]).reshape(-1,1,2)
             dst_pts = np.float32([ self.kp4[m.trainIdx].pt for m in good4 ]).reshape(-1,1,2)