I finally archieved my goal to implement a line detecting algorithm to my robot. Now i want to combine my 2 different scripts. The first one is the line detection function. whenever the robot sees a black line in its path it dirigates to the black line. the second function is basically to control the robot with a joycon from the /joy node
My question now is a bit more theoretical: I want to create a third node. In this node you have basically a subscriber for the joy node. And when you press a certain button (im thinking on the Triangle Button on the PS5 controller) it switches the modes. So basically when i start the launch file, it starts in mode 1. When i press the triangle button, it changes to mode 2 and back. The switch node publishes a variable (either 1 or 2) and the other 2 files subscribe to this and when the other number is published, the node stays in idle
About my 2 existing scripts: Both are defined as classes. in the first one, "line_detection.py":
#! /usr/bin/env python
import rospy
import cv2
import numpy as np
from sensor_msgs.msg import Image, CompressedImage
from cv_bridge import CvBridge, CvBridgeError
from std_msgs.msg import Int32
class LineDetector:
def __init__(self):
self.rate = rospy.Rate(rospy.get_param("/rate/lineDetector"))
self.bridge = CvBridge()
self.mode= rospy.Subscriber("/mode_topic",Int32)
self.image_sub_rpi =rospy.Subscriber("/raspicam_node/image/compressed", CompressedImage, self.image_callback_compressed)
self.image_sub = rospy.Subscriber("/camera/image_raw", Image, self.image_callback_raw)
self.image_pub = rospy.Publisher("processed_image", Image, queue_size=1)
self.image_debug_pub = rospy.Publisher("debug_image", Image, queue_size=1)
def image_callback_raw(self, msg):
try:
cv_image = self.bridge.imgmsg_to_cv2(msg, "passthrough")
self.process_image(cv_image)
except CvBridgeError as e:
rospy.logerr("Raw Error")
rospy.logerr(e)
def image_callback_compressed(self, msg):
try:
np_arr = np.frombuffer(msg.data, np.uint8)
image_np = cv2.imdecode(np_arr, cv2.IMREAD_COLOR)
self.process_image(image_np)
except CvBridgeError as e:
rospy.logerr("Compressed Error")
rospy.logerr(e)
def process_image(self, cv_image):
low_b=np.uint8([51,51,51])
high_b=np.uint8([0,0,0])
cv_image = cv2.resize(cv_image, (256,256), interpolation =cv2.INTER_AREA)
mask = cv2.inRange(cv_image,high_b,low_b)
contours, hierarchy = cv2.findContours(mask,1,cv2.CHAIN_APPROX_NONE)
if len(contours) > 0:
c = max(contours, key=cv2.contourArea)
M=cv2.moments(c)
if M["m00"] !=0 :
cx= int(M["m10"]/M["m00"])
cy= int(M["m01"]/M["m00"])
print("CX: "+ str(cx)+"CY: "+str(cy))
if cx>160:
print("Turn Left")
if cx< 160 and cx > 96:
print("On Track!")
if cx <=96:
print("Turn Right")
cv2.line(cv_image,(128,256),(128,0),(0,0,255),2)
cv2.line(cv_image,(0,128),(256,128),(0,0,255),2)
cv2.circle(cv_image,(cx,cy), 5, (255,255,255), -1)
else:
print("No line in sight. Dang!")
cv_message = self.bridge.cv2_to_imgmsg(cv_image,"passthrough")
cv_debug = self.bridge.cv2_to_imgmsg(mask,"passthrough")
self.image_pub.publish(cv_message)
self.image_debug_pub.publish(cv_debug)
def run(self):
while not rospy.is_shutdown():
self.rate.sleep()
if __name__ == '__main__':
rospy.init_node('coole_linien')
line_detector = LineDetector()
rospy.spin()
and second my motor_control:
#! /usr/bin/env python
import rospy
from sensor_msgs.msg import Joy
import numpy as np
class MotorControl:
def __init__(self):
self.controls_sub = rospy.Subscriber("joy",Joy,self.control)
def control(self):
if self.axes[5] <= 0.95 and self.axes[5] > 0.75:
print("Modus 1")
elif self.axes[5] <= 0.75 and self.axes[5] > 0.2:
print("Modus 2")
elif self.axes[5] <= 0.2 and self.axes[5] > -0.2:
print("Modus 3")
elif self.axes[5] <= -0.2 and self.axes[5] > -1:
print("Modus 5")
elif self.axes[5] == -1:
print("Modus 6")
else:
if self.axes[2] <= 0.95 and self.axes[2] > 0.75:
print("Modus 7")
elif self.axes[2] <= 0.75 and self.axes[2] > 0.2:
print("Modus 8")
elif self.axes[2] <= 0.2 and self.axes[2] > -0.2:
print("Modus 9")
elif self.axes[2] <= -0.2 and self.axes[2] >= -1:
print("Modus 10")
else:
print("Modus 0")
if __name__ == '__main__':
rospy.init_node("control")
mc=MotorControl()
rospy.spin()
How can i implement my idea in the functions? am i thinking too complex? Is it easy to achieve or will it be way too complicated for a little extra feature that isnt really necessary?
Thank you for your suggestions. Sincerely Pascal
i have no really idea how to implement this. I tried creating a third script which changes modes when pressing triangle, but i dont know how to implement it in the other functions
#!/usr/bin/env python
import rospy
from sensor_msgs.msg import Joy
from std_msgs.msg import Int32
current_mode = 1
def switch_callback(data):
global current_mode
if data.buttons[3] == 1:
if current_mode == 1:
current_mode = 2
else:
current_mode = 1
mode_msg =Int32()
mode_msg.data = current_mode
mode_pub.publish(mode_msg)
def switch():
global mode_pub
rospy.init_node('cooler_switch',anonymous=False)
rospy.Subscriber("joy",Joy,switch_callback)
mode_pub = rospy.Publisher("mode_topic",Int32, queue_size=10)
rospy.spin()
if __name__=='__main__':
switch()