From 74da89e5d670f9f6d7b13e9d97f06cc869e417b4 Mon Sep 17 00:00:00 2001
From: Zhang Xuan <xuan.zhang@ensta-paristech.fr>
Date: Tue, 10 Oct 2017 14:27:43 +0200
Subject: [PATCH] skelet for the tutorial

---
 src/find_color.py   |  9 ++++---
 src/follow_color.py | 64 +++++----------------------------------------
 2 files changed, 13 insertions(+), 60 deletions(-)

diff --git a/src/find_color.py b/src/find_color.py
index 0dcddf9..8167170 100755
--- a/src/find_color.py
+++ b/src/find_color.py
@@ -22,12 +22,13 @@ def nothing(x):
 
 class FindColor:
     def __init__(self, debug=False):
+        # Converter of ros image to opencv image
         self.bridge = CvBridge()
         # Subscribe to the camera
-        self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
+        #TODO
 
         # Create the center_deviation topic where we will publish the error
-        self.center_pub = rospy.Publisher('center_deviation', Float32, queue_size=10)
+        #TODO
         # Create the ball_radius topic where we will publish the radius of the ball
         self.ball_radius_pub = rospy.Publisher('ball_radius', Float32, queue_size = 10)
         # Dummy images
@@ -121,13 +122,15 @@ class FindColor:
             max_deviation = image_width // 2
             center_deviation = (center[0] - max_deviation) / max_deviation
             # print("Center deviation : {}".format(center_deviation))
+
             # Publish the error to the corresponding topic
-            self.center_pub.publish(center_deviation)
+            #TODO
 
         # Publish the ball_radius
         if radius > MIN_RADIUS_SIZE:
             self.ball_radius_pub.publish(radius)
 
+
         if self.debug:
             # Show the images
             cv2.imshow(self.image_name, self.cv_image)
diff --git a/src/follow_color.py b/src/follow_color.py
index 09a433d..e191e0b 100755
--- a/src/follow_color.py
+++ b/src/follow_color.py
@@ -8,70 +8,20 @@ import numpy as np
 from geometry_msgs.msg import Twist
 from std_msgs.msg import Float32
 
-vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
-Kp_w = 2
-Kd = 1
-Ki = 0
-last_time = None
-
-Kp_x = 0.2
-
-vel_x = 0
-omega_z = 0
+#   Publisher of velocity
+#TODO
 
+# callback of the Subscriber
 def turnCallback(data):
+    #TODO
+    pass
 
-    global errorD, errorI, last_time, omega_z
-    # Error (deviation to the center rescaled to [-1, 1])
-    error = data.data
-    # print("Error: {}".format(error))
-
-
-    if last_time is not None:
-        errorD = error - last_error
-        dt = time.time() - last_time
-        last_time = time.time()
-    else:
-        errorI, errorD = 0, 0
-        dt = 1
-    # PID Control
-    omega_z = - (Kp_w * error + Kd * (errorD / dt) + Ki * dt * errorI)
-    # Update integral error
-    errorI += error
-
-    # Init twist message (speed + orientation command)
-    vel_msg = Twist()
-    vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = vel_x, 0, 0
-    vel_msg.angular.x, vel_msg.angular.y, vel_msg.angular.z = 0, 0, omega_z
-    # Publish the Twist message
-    vel_pub.publish(vel_msg)
-
-def translateCallback(raw_data):
-    data=np.mean(raw_data.data)
-
-    global vel_x
-    # Error (deviation to the center rescaled to [-1, 1])
-    error = (data - 45)/20
-    # print("Error: {}".format(error))
-
-    # PID Control
-    vel_x = - Kp_x * error
-
-
-    # Init twist message (speed + orientation command)
-    vel_msg = Twist()
-    vel_msg.linear.x, vel_msg.linear.y, vel_msg.linear.z = vel_x, 0, 0
-    vel_msg.angular.x, vel_msg.angular.y, vel_msg.angular.z= 0, 0, omega_z
-    # Publish the Twist message
-    vel_pub.publish(vel_msg)
 
 if __name__ == "__main__":
     print("Starting follow_color node")
     rospy.init_node('follow_color', anonymous=True)
-    turn_sub = rospy.Subscriber("center_deviation", Float32, turnCallback)
-    translate_sub = message_filters.Subscriber("ball_radius", Float32)
-    translate_cache = message_filters.Cache(translate_sub,10,allow_headerless=True)
-    translate_cache.registerCallback(translateCallback)
+    # Declare the Subscriber to center_deviationturn
+    #TODO
     try:
         rospy.spin()
     except KeyboardInterrupt:
-- 
GitLab