Skip to content
GitLab
Commit 1542bdca authored by Antonin Raffin's avatar Antonin Raffin
Browse files

Rename files

parent 8ac74635
Hide whitespace changes
Inline Side-by-side
src/TurnBot.py deleted 100755 → 0
View file @ 8ac74635
#!/usr/bin/env python
from __future__ import print_function, division
import rospy
from geometry_msgs.msg import Twist
from std_msgs.msg import Int32
vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
MAX_DEVIATION = 320
def turnCallback(data):
deviation = data.data
kp = None
vel_msg = Twist()
vel_msg.linear.x = 0
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
if abs(deviation) > MAX_DEVIATION / 2:
vel_msg.angular.z = - deviation / abs(deviation)
else:
vel_msg.angular.z = -deviation * 2.0 / 320.0
vel_pub.publish(vel_msg)
if __name__ == "__main__":
print("Start")
rospy.init_node('turnBot', anonymous=True)
turn_sub = rospy.Subscriber("center_deviation", Int32, turnCallback)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
src/find_color.py 0 → 100755
View file @ 1542bdca
#!/usr/bin/env python
from __future__ import print_function, division
import cv2
import numpy as np
import rospy
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from std_msgs.msg import Int32
class FindColor:
def __init__(self):
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
self.center_pub = rospy.Publisher('center_deviation', Int32, queue_size=10)
self.cv_image = np.zeros((100, 100, 3), np.uint8)
self.hsv = np.zeros((100, 100, 3), np.uint8)
self.h_min = 0
self.s_min = 0
self.v_min = 0
self.h_max = 179
self.s_max = 255
self.v_max = 255
cv2.namedWindow('RGB')
cv2.namedWindow('Mask')
# Creating track bar
cv2.createTrackbar('h_min', 'Mask', 0, 179, self.nothing)
cv2.createTrackbar('s_min', 'Mask', 0, 255, self.nothing)
cv2.createTrackbar('v_min', 'Mask', 0, 255, self.nothing)
cv2.createTrackbar('h_max', 'Mask', 0, 179, self.nothing)
cv2.createTrackbar('s_max', 'Mask', 0, 255, self.nothing)
cv2.createTrackbar('v_max', 'Mask', 0, 255, self.nothing)
# Initial value, for green
cv2.setTrackbarPos('h_min', 'Mask', 29)
cv2.setTrackbarPos('s_min', 'Mask', 86)
cv2.setTrackbarPos('v_min', 'Mask', 6)
cv2.setTrackbarPos('h_max', 'Mask', 84)
cv2.setTrackbarPos('s_max', 'Mask', 255)
cv2.setTrackbarPos('v_max', 'Mask', 255)
def nothing(self, x):
pass
def callback(self, data):
try:
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# get info from track bar and appy to result
self.h_min = cv2.getTrackbarPos('h_min', 'Mask')
self.s_min = cv2.getTrackbarPos('s_min', 'Mask')
self.v_min = cv2.getTrackbarPos('v_min', 'Mask')
self.h_max = cv2.getTrackbarPos('h_max', 'Mask')
self.s_max = cv2.getTrackbarPos('s_max', 'Mask')
self.v_max = cv2.getTrackbarPos('v_max', 'Mask')
# Convert to HSV color space
self.hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_RGB2HSV)
# Normal masking algorithm
mask = None
if self.h_min <= self.h_max and self.s_min <= self.s_max and self.v_min <= self.v_max:
lower = np.array([self.h_min, self.s_min, self.v_min])
upper = np.array([self.h_max, self.s_max, self.v_max])
mask = cv2.inRange(self.hsv, lower, upper)
# Erosion and dilation to remove noise
kernel = np.ones((7, 7), np.uint8)
mask = cv2.erode(mask, kernel, iterations=2)
mask = cv2.dilate(mask, kernel, iterations=2)
# Find contours in the mask
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
radius = 0
if len(cnts) > 0:
# Find largest contour and compute the minimum eclosing circle
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
self.mask_image = cv2.bitwise_and(self.cv_image, self.cv_image, mask=mask)
# Only proceed if the radius meets a minimum size
if radius > 10:
# Draw the circle
cv2.circle(self.mask_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(self.mask_image, center, 5, (0, 0, 255), -1)
cv2.circle(self.cv_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(self.cv_image, center, 5, (0, 0, 255), -1)
cv2.imshow("RGB", self.cv_image)
cv2.imshow("Mask", self.mask_image)
print(center)
# Publish horizon deviation from the middle
if center is not None:
center_deviation = int(center[0] - 320)
print(center_deviation)
self.center_pub.publish(center_deviation)
cv2.waitKey(3)
if __name__ == '__main__':
ic = FindColor()
print("start")
rospy.init_node('FindColor', anonymous=True)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
src/follow_color.py
View file @ 1542bdca
#!/usr/bin/env python
from __future__ import print_function, division
import cv2
import numpy as np
import rospy
from cv_bridge import CvBridge, CvBridgeError
from sensor_msgs.msg import Image
from geometry_msgs.msg import Twist
from std_msgs.msg import Int32
vel_pub = rospy.Publisher("/mobile_base/commands/velocity", Twist, queue_size=10)
MAX_DEVIATION = 320
class ImageConverter:
def __init__(self):
self.bridge = CvBridge()
self.image_sub = rospy.Subscriber("/camera/rgb/image_raw", Image, self.callback)
self.center_pub = rospy.Publisher('center_deviation', Int32, queue_size=10)
self.cv_image = np.zeros((100, 100, 3), np.uint8)
self.hsv = np.zeros((100, 100, 3), np.uint8)
self.h_min = 0
self.s_min = 0
self.v_min = 0
self.h_max = 179
self.s_max = 255
self.v_max = 255
cv2.namedWindow('RGB')
cv2.namedWindow('Mask')
# Creating track bar
cv2.createTrackbar('h_min', 'Mask', 0, 179, self.nothing)
cv2.createTrackbar('s_min', 'Mask', 0, 255, self.nothing)
cv2.createTrackbar('v_min', 'Mask', 0, 255, self.nothing)
cv2.createTrackbar('h_max', 'Mask', 0, 179, self.nothing)
cv2.createTrackbar('s_max', 'Mask', 0, 255, self.nothing)
cv2.createTrackbar('v_max', 'Mask', 0, 255, self.nothing)
# Initial value, for green
cv2.setTrackbarPos('h_min', 'Mask', 29)
cv2.setTrackbarPos('s_min', 'Mask', 86)
cv2.setTrackbarPos('v_min', 'Mask', 6)
cv2.setTrackbarPos('h_max', 'Mask', 84)
cv2.setTrackbarPos('s_max', 'Mask', 255)
cv2.setTrackbarPos('v_max', 'Mask', 255)
def nothing(self, x):
pass
def callback(self, data):
try:
self.cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
except CvBridgeError as e:
print(e)
# get info from track bar and appy to result
self.h_min = cv2.getTrackbarPos('h_min', 'Mask')
self.s_min = cv2.getTrackbarPos('s_min', 'Mask')
self.v_min = cv2.getTrackbarPos('v_min', 'Mask')
self.h_max = cv2.getTrackbarPos('h_max', 'Mask')
self.s_max = cv2.getTrackbarPos('s_max', 'Mask')
self.v_max = cv2.getTrackbarPos('v_max', 'Mask')
# Convert to HSV color space
self.hsv = cv2.cvtColor(self.cv_image, cv2.COLOR_RGB2HSV)
# Normal masking algorithm
mask = None
if self.h_min <= self.h_max and self.s_min <= self.s_max and self.v_min <= self.v_max:
lower = np.array([self.h_min, self.s_min, self.v_min])
upper = np.array([self.h_max, self.s_max, self.v_max])
mask = cv2.inRange(self.hsv, lower, upper)
# Erosion and dilation to remove noise
kernel = np.ones((7, 7), np.uint8)
mask = cv2.erode(mask, kernel, iterations=2)
mask = cv2.dilate(mask, kernel, iterations=2)
# Find contours in the mask
cnts = cv2.findContours(mask.copy(), cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)[-2]
center = None
radius = 0
if len(cnts) > 0:
# Find largest contour and compute the minimum eclosing circle
c = max(cnts, key=cv2.contourArea)
((x, y), radius) = cv2.minEnclosingCircle(c)
M = cv2.moments(c)
center = (int(M["m10"] / M["m00"]), int(M["m01"] / M["m00"]))
self.mask_image = cv2.bitwise_and(self.cv_image, self.cv_image, mask=mask)
# Only proceed if the radius meets a minimum size
if radius > 10:
# Draw the circle
cv2.circle(self.mask_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(self.mask_image, center, 5, (0, 0, 255), -1)
cv2.circle(self.cv_image, (int(x), int(y)), int(radius), (0, 255, 255), 2)
cv2.circle(self.cv_image, center, 5, (0, 0, 255), -1)
cv2.imshow("RGB", self.cv_image)
cv2.imshow("Mask", self.mask_image)
print(center)
# Publish horizon deviation from the middle
if center is not None:
center_deviation = int(center[0] - 320)
print(center_deviation)
self.center_pub.publish(center_deviation)
cv2.waitKey(3)
def turnCallback(data):
deviation = data.data
kp = None
vel_msg = Twist()
vel_msg.linear.x = 0
vel_msg.linear.y = 0
vel_msg.linear.z = 0
vel_msg.angular.x = 0
vel_msg.angular.y = 0
if abs(deviation) > MAX_DEVIATION / 2:
vel_msg.angular.z = - deviation / abs(deviation)
else:
vel_msg.angular.z = -deviation * 2.0 / 320.0
vel_pub.publish(vel_msg)
if __name__ == '__main__':
ic = ImageConverter()
print("start")
rospy.init_node('ImageConverter', anonymous=True)
if __name__ == "__main__":
print("Start")
rospy.init_node('turnBot', anonymous=True)
turn_sub = rospy.Subscriber("center_deviation", Int32, turnCallback)
try:
rospy.spin()
except KeyboardInterrupt:
print("Shutting down")
cv2.destroyAllWindows()
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment