答:
首先，确保你已经安装了ROS2和OpenCV。你可以使用以下命令安装OpenCV：
bash
pip install opencv-python-headless
然后，创建一个Python脚本，例如color_ball_detector.py，并添加以下代码：
python
import cv2
import numpy as np
import rclpy
from rclpy.node import Node
from sensor_msgs.msg import Image
from sensor_msgs.imgmsg_codec import encode_data, decode_data
from std_srvs.srv import SetBool, Trigger
from std_msgs.msg import Bool, String
class ColorBallDetector(Node):
def __init__(self):
super().__init__('color_ball_detector')
self.subscription = self.create_subscription(
Image,
'balls_img',
self.listener_callback,
10
)
self.publication = self.create_publisher(
Image,
'balls_img_detected',
10
)
self.color_lower = np.array([35, 100, 100]) # Example color range for a blue ball in HSV
self.color_upper = np.array([85, 255, 255])
self.change_color_service = self.create_service(
Trigger,
'change_color_service',
self.change_color_callback
)
self.change_color_srv = self.create_client(
SetBool,
'ChangeColor'
)
self.timer = self.create_timer(1.0, self.timer_callback)
def listener_callback(self, msg):
try:
nparr = decode_data(msg.data, msg.format)
# Convert the image to HSV color space
hsv = cv2.cvtColor(nparr, cv2.COLOR_BGR2HSV)
# Create a mask using the color range
mask = cv2.inRange(hsv, self.color_lower, self.color_upper)
# Find contours in the mask
contours, _ = cv2.findContours(mask, cv2.RETR_EXTERNAL, cv2.CHAIN_APPROX_SIMPLE)
# Draw bounding boxes around the contours
for contour in contours:
x, y, w, h = cv2.boundingRect(contour)
cv2.rectangle(nparr, (x, y), (x + w, y + h), (0, 255, 0), 2)
# Encode the image back to a ROS message
success, encoded_img = encode_data(nparr, format='bgr8')
if success:
self.publication.publish(Image(header=msg.header, height=msg.height, width=msg.width,
step=msg.step, encoding=msg.encoding, data=encoded_img))
except Exception as e:
self.get_logger().error(f"Error processing image: {e}")
def change_color_callback(self, request, response):
try:
color = request.data.strip().lower()
if color == 'red':
self.color_lower = np.array([0, 100, 100])
self.color_upper = np.array([10, 255, 255])
elif color == 'green':
self.color_lower = np.array([40, 40, 40])
……
关注
