【指针检测项目】yolo detect_image函数返回检测结果裁剪后的图片

修改后的代码如下

def detect_image(self, image):
    start = timer()
if self.model_image_size != (None, None):
    assert self.model_image_size[0]%32 == 0, 'Multiples of 32 required'
    assert self.model_image_size[1]%32 == 0, 'Multiples of 32 required'
    boxed_image = letterbox_image(image, tuple(reversed(self.model_image_size)))
else:
    new_image_size = (image.width - (image.width % 32),
                      image.height - (image.height % 32))
    boxed_image = letterbox_image(image, new_image_size)
image_data = np.array(boxed_image, dtype='float32')

#print(image_data.shape)
image_data /= 255.
image_data = np.expand_dims(image_data, 0)  # Add batch dimension.

out_boxes, out_scores, out_classes = self.sess.run(
    [self.boxes, self.scores, self.classes],
    feed_dict={
        self.yolo_model.input: image_data,
        self.input_image_shape: [image.size[1], image.size[0]],
        K.learning_phase(): 0
    })

print('Found {} boxes for {}'.format(len(out_boxes), 'img'))

font = ImageFont.truetype(font='font/FiraMono-Medium.otf',
            size=np.floor(3e-2 * image.size[1] + 0.5).astype('int32'))
thickness = (image.size[0] + image.size[1]) // 300

for i, c in reversed(list(enumerate(out_classes))):
    predicted_class = self.class_names[c]
    box = out_boxes[i]
    score = out_scores[i]

    label = '{} {:.2f}'.format(predicted_class, score)
    draw = ImageDraw.Draw(image)
    #print(image.size)
    label_size = draw.textsize(label, font)

    top, left, bottom, right = box
    top = max(0, np.floor(top + 0.5).astype('int32'))
    left = max(0, np.floor(left + 0.5).astype('int32'))
    bottom = min(image.size[1], np.floor(bottom + 0.5).astype('int32'))
    right = min(image.size[0], np.floor(right + 0.5).astype('int32'))
    print(label, (left, top), (right, bottom))

    if top - label_size[1] >= 0:
        text_origin = np.array([left, top - label_size[1]])
    else:
        text_origin = np.array([left, top + 1])

    # My kingdom for a good redistributable image drawing library.
    for i in range(thickness):
        draw.rectangle(
            [left + i, top + i, right - i, bottom - i],
            outline=255) #outline=self.colors[c]
    draw.rectangle(
        [tuple(text_origin), tuple(text_origin + label_size)],
        fill=255) #fill=self.colors[c]
    draw.text(text_origin, label, fill=0, font=font)# fill=255
    del draw
#img = cv2.imdecode(image, cv2.IMREAD_GRAYSCALE)
#img = cv2.imread(image)
#cropped = img[bottom:top, left:right]
image = image.crop((left, top, right, bottom))
end = timer()
print(end - start)
return image
#return cropped

def close_session(self):
    self.sess.close()

主要是这句话 image = image.crop((left, top, right, bottom)),一开始没搞明白crop的用法,报错SystemError: tile cannot extend outside image,然后百度得知left top 是要裁剪区域的左上角坐标,right bottom是要裁剪区域的右下角坐标。

#!/usr/bin/env python3 # -*- coding: utf-8 -*- import cv2 import torch import rospy import numpy as np from ultralytics import YOLO from time import time from std_msgs.msg import Header from sensor_msgs.msg import Image from yolov8_ros_msgs.msg import BoundingBox, BoundingBoxes from std_msgs.msg import Float64MultiArray class Yolo_Dect: # 像素坐标系转图像坐标系 def undistort_pixel(self, pixel_coord): # 像素坐标 (u, v) u, v = pixel_coord # 相机内参数矩阵 fx, fy = self.intrinsic_matrix[0, 0], self.intrinsic_matrix[1, 1] cx, cy = self.intrinsic_matrix[0, 2], self.intrinsic_matrix[1, 2] # 畸变系数 k1, k2, p1, p2, k3 = self.distortion_coeffs # 归一化像素坐标 x = (u - cx) / fx y = (v - cy) / fy # 径向畸变和切向畸变校正 r2 = x**2 + y**2 x_distorted = x * (1 + k1*r2 + k2*r2**2 + k3*r2**3) + 2*p1*x*y + p2*(r2 + 2*x**2) y_distorted = y * (1 + k1*r2 + k2*r2**2 + k3*r2**3) + p1*(r2 + 2*y**2) + 2*p2*x*y # 返回校正后的归一化像素坐标 return x_distorted, y_distorted # 图像坐标系转相机坐标系 def pixel_to_camera_coord(self, pixel_coord, depth): # 校正像素坐标 x_distorted, y_distorted = self.undistort_pixel(pixel_coord) # 相机内参数矩阵 fx, fy = self.intrinsic_matrix[0, 0], self.intrinsic_matrix[1, 1] cx, cy = self.intrinsic_matrix[0, 2], self.intrinsic_matrix[1, 2] # 计算相机坐标系下的三维坐标 X = depth * (x_distorted - cx) / fx Y = depth * (y_distorted - cy) / fy Z = depth return X, Y, Z def __init__(self): # 标签到话题名称和ID的映射 self.class_to_topic = { "GongYeXiangJi": ("/GongYeXiangJi", 1), "TuXiangChuanGanQi": ("/TuXiangChuanGanQi", 2), "KongZhiXinPian": ("/KongZhiXinPian", 3), "JingTou": ("/JingTou", 4), "SiFuDianJi": ("/SiFuDianJi", 5), "ZhuanZi": ("/ZhuanZi", 6) } # 为每个类别创建发布器 self.class_publishers = {} for class_name, (topic, _) in self.class_to_topic.items(): self.class_publishers[class_name] = rospy.Publisher( topic, Float64MultiArray, queue_size=10 ) # 相机内参矩阵 (示例值,实际应从相机标定获取) self.intrinsic_matrix = np.array([[800, 0, 320], [0, 800, 240], [0, 0, 1]]) # 畸变系数 (示例值) self.distortion_coeffs = (-0.1, 0.01, 0.001, 0.001, -0.02) # 固定深度值 (实际应使用深度图) self.depth = 0.9 # 加载参数 weight_path = rospy.get_param('~weight_path', '') image_topic = rospy.get_param( '~image_topic', '/camera/color/image_raw') pub_topic = rospy.get_param('~pub_topic', '/yolov8/BoundingBoxes') self.camera_frame = rospy.get_param('~camera_frame', '') conf = rospy.get_param('~conf', '0.5') self.visualize = rospy.get_param('~visualize', 'True') # 选择使用的设备 if (rospy.get_param('/use_cpu', 'false')): self.device = 'cpu' else: self.device = 'cuda' self.model = YOLO(weight_path) self.model.fuse() self.model.conf = conf self.color_image = Image() self.getImageStatus = False # 加载类别颜色 self.classes_colors = {} # 图像订阅 self.color_sub = rospy.Subscriber(image_topic, Image, self.image_callback, queue_size=1, buff_size=52428800) # 输出发布器 self.position_pub = rospy.Publisher( pub_topic, BoundingBoxes, queue_size=1) self.image_pub = rospy.Publisher( '/yolov8/detection_image', Image, queue_size=1) # 如果没有图像消息 while (not self.getImageStatus): rospy.loginfo("等待图像...") rospy.sleep(2) def image_callback(self, image): self.boundingBoxes = BoundingBoxes() self.boundingBoxes.header = image.header self.boundingBoxes.image_header = image.header self.getImageStatus = True self.color_image = np.frombuffer(image.data, dtype=np.uint8).reshape( image.height, image.width, -1) self.color_image = cv2.cvtColor(self.color_image, cv2.COLOR_BGR2RGB) results = self.model(self.color_image, show=False, conf=0.3) self.dectshow(results, image.height, image.width) cv2.waitKey(3) def dectshow(self, results, height, width): self.frame = results[0].plot() fps = 1000.0 / results[0].speed['inference'] cv2.putText(self.frame, f'FPS: {int(fps)}', (20,50), cv2.FONT_HERSHEY_SIMPLEX, 0.6, (0, 255, 0), 2, cv2.LINE_AA) for result in results[0].boxes: boundingBox = BoundingBox() # 获取边界框坐标 x1 = np.float64(result.xyxy[0][0].item()) y1 = np.float64(result.xyxy[0][1].item()) x2 = np.float64(result.xyxy[0][2].item()) y2 = np.float64(result.xyxy[0][3].item()) # 计算中心点 center_x = (x1 + x2) / 2 center_y = (y1 + y2) / 2 # 转换到相机坐标系 X, Y, Z = self.pixel_to_camera_coord((center_x, center_y), self.depth) class_name = results[0].names[result.cls.item()] # 发布到类别特定话题 if class_name in self.class_publishers: coords_msg = Float64MultiArray() # 发布中心点3D坐标和边界框尺寸 coords_msg.data = [X, Y, Z, x2-x1, y2-y1] self.class_publishers[class_name].publish(coords_msg) # 保留原有的BoundingBox发布 boundingBox.xmin = x1 boundingBox.ymin = y1 boundingBox.xmax = x2 boundingBox.ymax = y2 boundingBox.Class = class_name boundingBox.probability = result.conf.item() self.boundingBoxes.bounding_boxes.append(boundingBox) self.position_pub.publish(self.boundingBoxes) self.publish_image(self.frame, height, width) if self.visualize: cv2.imshow('YOLOv8', self.frame) def publish_image(self, imgdata, height, width): image_temp = Image() header = Header(stamp=rospy.Time.now()) header.frame_id = self.camera_frame image_temp.height = height image_temp.width = width image_temp.encoding = 'bgr8' image_temp.data = np.array(cv2.cvtColor(imgdata, cv2.COLOR_RGB2BGR)).tobytes() image_temp.header = header image_temp.step = width * 3 self.image_pub.publish(image_temp) def main(): rospy.init_node('yolov8_ros', anonymous=True) yolo_dect = Yolo_Dect() rospy.spin() if __name__ == "__main__": main()此代码发布到#include <rei_robot_cruise/cruise.h> #include "oryxbot_cruise_ex/oryxbot_task.h" #include <geometry_msgs/Twist.h> #include <tf/transform_listener.h> #include <cmath> #include <nav_msgs/Odometry.h> #include <ros/ros.h> #include <std_msgs/Float64MultiArray.h> #include <map> #include <string> #include <thread> // 类别名称到ID的映射 std::map<std::string, int> class_to_id = { {"GongYeXiangJi", 1}, {"TuXiangChuanGanQi", 2}, {"KongZhiXinPian", 3}, {"JingTou", 4}, {"SiFuDianJi", 5}, {"ZhuanZi", 6} }; // 全局变量存储结果 double xmin = 0.0; double ymin = 0.0; double xmax = 0.0; double ymax = 0.0; int class_id = 0; // 全局变量 ros::Publisher* g_vel_pub_ptr = nullptr; geometry_msgs::Twist g_current_vel; // 当前实际速度 geometry_msgs::Twist* g_current_vel_ptr = nullptr; // 速度指针 // 实例化对象 ORYXTask oryxTask; geometry_msgs::Twist workStationAbovePose_; geometry_msgs::Twist boxAbovePose_; std::vector<geometry_msgs::Twist> boxPose_; std::vector<geometry_msgs::Twist> workStationPoses_; std::vector<int> boxState_(2,0); // PID参数 const double KP_LINEAR = 0.5; const double KI_LINEAR = 0.01; const double KD_LINEAR = 0.1; const double KP_ANGULAR = 1.0; const double KI_ANGULAR = 0.01; const double KD_ANGULAR = 0.2; const double TOLERANCE_POS = 0.01; // 位置容差() const double TOLERANCE_ANGLE = 0.0175; // 角度容差(1度) const double DEADZONE_POS = 0.005; // 5mm死区 const double DEADZONE_ANGLE = 0.0087; // 0.5度死区 const double MAX_INTEGRAL = 0.5; // 积分上限 const double DECELERATION_RATE = 0.5; // 减速度(m/s²) const double ANG_DECELERATION = 1.0; // 角减速度(rad/s²) // 速度回调函数 void velocityCallback(const geometry_msgs::Twist::ConstPtr& msg) { g_current_vel = *msg; g_current_vel_ptr = &g_current_vel; } void boundingBoxCallback(const std_msgs::Float64MultiArray::ConstPtr& msg, const std::string& class_name) { if (msg->data.size() >= 4) { xmin = msg->data[0]; ymin = msg->data[1]; xmax = msg->data[2]; ymax = msg->data[3]; // 查找类别ID auto it = class_to_id.find(class_name); if (it != class_to_id.end()) { class_id = it->second; } else { class_id = 0; // 未知类别 ROS_WARN("Unknown class detected: %s", class_name.c_str()); } ROS_INFO("Received %s (ID:%d) - Bounding Box: [%.2f, %.2f, %.2f, %.2f]", class_name.c_str(), class_id, xmin, ymin, xmax, ymax); } else { ROS_ERROR("Received invalid bounding box data"); } } bool chargeCallback(int value){ int taskStep = value; int taskState; std::vector<int> arIds; int placeTime = 0; ros::Rate loop(1); while(ros::ok()){ if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){ rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); break; }else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){ break; } taskState = rei_cruise::RobotCruise::getInstance().GetTaskState(); if(taskState == rei_cruise::TaskState::ACTIVE){ switch (taskStep) { case 0: if(oryxTask.ArAdjust(0)) taskStep = 1; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 1: if(oryxTask.RelativeMove(0.12,0,0,"odom")){ ros::Time start = ros::Time::now(); while (ros::Time::now() - start < ros::Duration(2.0) && ros::ok()) { if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE) { rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); return true; } else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP) { return true; } ros::Duration(0.1).sleep(); } taskStep = 2; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 2: if(oryxTask.RelativeMove(-0.12,0,0,"odom")){ return true; }else return false; break; } }else if(taskState == rei_cruise::TaskState::STOP){ break; } loop.sleep(); } return true; } bool StartCallback1(){ return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z); } bool renwua_aplaceCallback(int value){ int taskStep = value; int taskState; std::vector<int> arIds; int placeTime = 0; ros::Rate loop(1); while(ros::ok()){ if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){ rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); break; }else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){ break; } taskState = rei_cruise::RobotCruise::getInstance().GetTaskState(); if(taskState == rei_cruise::TaskState::ACTIVE){ switch (taskStep) { case 0: if(oryxTask.ArAdjust(0)) taskStep = 1; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 1: if(oryxTask.RelativeMove(0.12,0,0,"odom")){ taskStep = 2; placeTime = 0; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 2: if(oryxTask.GotoPosition(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z)){ taskStep=3; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 3: if(oryxTask.GetArIDs(arIds)) taskStep=4; else if(placeTime>0) taskStep=5; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 4: if(oryxTask.PickPlace(2,70, -165, 10)){ taskStep=5; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 5: if(oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z)){ taskStep=6; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 6: if(oryxTask.RelativeMove(-0.12,0,0,"odom")){ return true; }else return false; break; } }else if(taskState == rei_cruise::TaskState::STOP){ break; } loop.sleep(); } return true; } bool StartCallback2(){ return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z); } bool renwua_bplaceCallback(int value){ int taskStep = value; int taskState; std::vector<int> arIds; double p=3.141592653589793238462643383279502884197169399375105820974944923078164; int placeTime = 0; ros::Rate loop(1); while(ros::ok()){ if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){ rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); break; }else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){ break; } taskState = rei_cruise::RobotCruise::getInstance().GetTaskState(); if(taskState == rei_cruise::TaskState::ACTIVE){ switch (taskStep) { case 0: if(oryxTask.ArAdjust(0)) taskStep = 1; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 1: if(oryxTask.RelativeMove(0.12,0,0,"odom")){ taskStep = 2; placeTime = 0; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 2: if(oryxTask.GotoPosition(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z)){ taskStep=3; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 3: if(oryxTask.GetArIDs(arIds)) taskStep=4; else if(placeTime>0) taskStep=5; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 4: if(oryxTask.PickPlace(4,140, -165, 10)){ taskStep=5; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 5: if(oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z)){ taskStep=6; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 6: if(oryxTask.RelativeMove(-0.12,0,0,"odom")){ return true; }else return false; break; } }else if(taskState == rei_cruise::TaskState::STOP){ break; } loop.sleep(); } return true; } bool StartCallback3(){ return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z); } bool renwub_aplaceCallback(int value){ int taskStep = value; int taskState; std::vector<int> arIds; int placeTime = 0; ros::Rate loop(1); while(ros::ok()){ if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){ rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); break; }else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){ break; } taskState = rei_cruise::RobotCruise::getInstance().GetTaskState(); if(taskState == rei_cruise::TaskState::ACTIVE){ switch (taskStep) { case 0: if(oryxTask.ArAdjust(0)) taskStep = 1; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 1: if(oryxTask.RelativeMove(0.12,0,0,"odom")){ taskStep = 2; placeTime = 0; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 2: if(oryxTask.GotoPosition(workStationAbovePose_.linear.x, workStationAbovePose_.linear.y, workStationAbovePose_.linear.z)){ taskStep=3; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 3: if(oryxTask.GetArIDs(arIds)) taskStep=4; else if(placeTime>0) taskStep=5; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 4: if(oryxTask.PickPlace(7, 105, 120,35)) { taskStep=5; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 5: if(oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z)){ taskStep=6; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 6: if(oryxTask.RelativeMove(-0.12,0,0,"odom")){ return true; }else return false; break; } }else if(taskState == rei_cruise::TaskState::STOP){ break; } loop.sleep(); } return true; } bool renwub_aplaceCallback_stuff(int value){ int taskStep = value; int taskState; std::vector<int> arIds; int placeTime = 0; ros::Rate loop(1); while(ros::ok()){ if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){ rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); break; }else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){ break; } taskState = rei_cruise::RobotCruise::getInstance().GetTaskState(); if(taskState == rei_cruise::TaskState::ACTIVE){ switch (taskStep) { case 0: if(oryxTask.ArAdjust(0)) taskStep = 1; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 1: if(oryxTask.RelativeMove(0.12,0,0,"odom")){ taskStep = 2; placeTime = 0; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 2: if(oryxTask.GotoPosition(workStationAbovePose_.linear.x, workStationAbovePose_.linear.y, workStationAbovePose_.linear.z)){ taskStep=3; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 3: if(1) taskStep=4; break; case 4: for(int i=0; i<100; i++){ if(class_id==4) { oryxTask.ResetArm((xmin+xmax)*50+50, (ymin*100+ymax)/2-225, 90); //taskStep=5; } } break; case 5: if(oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z)){ taskStep=6; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 6: if(oryxTask.RelativeMove(-0.12,0,0,"odom")){ return true; }else return false; break; } }else if(taskState == rei_cruise::TaskState::STOP){ break; } loop.sleep(); } return true; } bool StartCallback4(){ return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z); } bool renwub_bplaceCallback(int value){ int taskStep = value; int taskState; std::vector<int> arIds; int placeTime = 1; double p=3.141592653589793238462643383279502884197169399375105820974944; ros::Rate loop(1); while(ros::ok()){ if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE){ rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); break; }else if(rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP){ break; } taskState = rei_cruise::RobotCruise::getInstance().GetTaskState(); if(taskState == rei_cruise::TaskState::ACTIVE){ switch (taskStep) { case 0: if(oryxTask.ArAdjust(1)) taskStep = 1; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 1: if(oryxTask.RelativeMove(0.12,0,0,"odom")){ taskStep = 2; placeTime = 1; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 2: if(oryxTask.GotoPosition(workStationAbovePose_.linear.x+20, workStationAbovePose_.linear.y, workStationAbovePose_.linear.z)){ taskStep=3; } else rei_cruise::RobotCruise::getInstance().Pause(); break; case 3: if(oryxTask.GetArIDs(arIds)) taskStep=4; else if(placeTime>0) taskStep=5; else rei_cruise::RobotCruise::getInstance().Pause(); break; case 4: if(oryxTask.PickPlace(8, 105, 183,35)) { taskStep=5; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 5: if(oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z)){ taskStep=6; }else rei_cruise::RobotCruise::getInstance().Pause(); break; case 6: if(oryxTask.RelativeMove(-0.12,0,0,"odom")){ return true; }else return false; break; } }else if(taskState == rei_cruise::TaskState::STOP){ break; } loop.sleep(); } return true; } bool StartCallback5(){ return oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, boxAbovePose_.linear.z); } bool pidAdjustCallback(int value) { int taskStep = value; int taskState; ros::Rate loop_rate(10); // 10Hz控制频率 // PID控制变量 double prev_error_x = 0.0, prev_error_y = 0.0, prev_error_yaw = 0.0; double integral_x = 0.0, integral_y = 0.0, integral_yaw = 0.0; // 新增停止控制变量 bool is_stopping = false; // 停止过程标志 int consecutive_success = 0; // 连续满足容差次数 tf::TransformListener listener; while (ros::ok()) { if (rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::PAUSE) { rei_cruise::RobotCruise::getInstance().SetCallBackFuncStep(taskStep); return true; } else if (rei_cruise::RobotCruise::getInstance().GetTaskState() == rei_cruise::STOP) { return true; } taskState = rei_cruise::RobotCruise::getInstance().GetTaskState(); if (taskState == rei_cruise::TaskState::ACTIVE) { switch (taskStep) { case 0: { // 获取当前位置 tf::StampedTransform transform; try { // 获取从odom到base_link的变换 listener.waitForTransform("odom", "base_link", ros::Time(0), ros::Duration(3.0)); listener.lookupTransform("odom", "base_link", ros::Time(0), transform); // 当前位置 double current_x = transform.getOrigin().x(); double current_y = transform.getOrigin().y(); tf::Quaternion q = transform.getRotation(); double current_yaw = tf::getYaw(q); // 计算误差 double error_x = -current_x; // 目标位置是原点(0,0,0) double error_y = -current_y; double error_yaw = -current_yaw; // 目标朝向0弧度 // 角度归一化到[-π, π] while (error_yaw > M_PI) error_yaw -= 2 * M_PI; while (error_yaw < -M_PI) error_yaw += 2 * M_PI; // ========== 新增停止序列检查 ========== if (!is_stopping) { // 检查是否达到精度要求 if (fabs(error_x) < TOLERANCE_POS && fabs(error_y) < TOLERANCE_POS && fabs(error_yaw) < TOLERANCE_ANGLE) { consecutive_success++; } else { consecutive_success = 0; } // 连续3次满足容差进入停止序列 if (consecutive_success >= 3) { ROS_INFO("Reached tolerance zone, initiating stop sequence"); is_stopping = true; // 重置积分项防止过冲 integral_x = 0; integral_y = 0; integral_yaw = 0; } } // ========== 停止序列处理 ========== if (is_stopping) { // 平滑减速停止 double current_vel_x = 0; double current_vel_y = 0; double current_vel_yaw = 0; // 获取当前实际速度 if (g_current_vel_ptr) { current_vel_x = g_current_vel_ptr->linear.x; current_vel_y = g_current_vel_ptr->linear.y; current_vel_yaw = g_current_vel_ptr->angular.z; } // 计算减速度命令 double decel_x = 0, decel_y = 0, decel_yaw = 0; // X方向减速 if (current_vel_x > 0.01) { decel_x = std::max(current_vel_x - DECELERATION_RATE * 0.1, 0.0); } else if (current_vel_x < -0.01) { decel_x = std::min(current_vel_x + DECELERATION_RATE * 0.1, 0.0); } // Y方向减速 if (current_vel_y > 0.01) { decel_y = std::max(current_vel_y - DECELERATION_RATE * 0.1, 0.0); } else if (current_vel_y < -0.01) { decel_y = std::min(current_vel_y + DECELERATION_RATE * 0.1, 0.0); } // 角速度减速 if (current_vel_yaw > 0.01) { decel_yaw = std::max(current_vel_yaw - ANG_DECELERATION * 0.1, 0.0); } else if (current_vel_yaw < -0.01) { decel_yaw = std::min(current_vel_yaw + ANG_DECELERATION * 0.1, 0.0); } // 发送减速命令 geometry_msgs::Twist cmd_vel; cmd_vel.linear.x = decel_x; cmd_vel.linear.y = decel_y; cmd_vel.angular.z = decel_yaw; if (g_vel_pub_ptr) { g_vel_pub_ptr->publish(cmd_vel); } // 检查是否完全停止 if (fabs(decel_x) < 0.001 && fabs(decel_y) < 0.001 && fabs(decel_yaw) < 0.001) { ROS_INFO("Full stop achieved. Position adjustment complete."); // 发送最终停止命令 cmd_vel.linear.x = 0; cmd_vel.linear.y = 0; cmd_vel.angular.z = 0; g_vel_pub_ptr->publish(cmd_vel); return true; } ROS_INFO("Stopping sequence: vx=%.3f, vy=%.3f, vyaw=%.3f", decel_x, decel_y, decel_yaw); } // ========== 常规PID控制 ========== else { // 死区处理 if (fabs(error_x) < DEADZONE_POS) error_x = 0; if (fabs(error_y) < DEADZONE_POS) error_y = 0; if (fabs(error_yaw) < DEADZONE_ANGLE) error_yaw = 0; // PID计算 integral_x += error_x; integral_y += error_y; integral_yaw += error_yaw; // 积分限幅 integral_x = std::max(std::min(integral_x, MAX_INTEGRAL), -MAX_INTEGRAL); integral_y = std::max(std::min(integral_y, MAX_INTEGRAL), -MAX_INTEGRAL); integral_yaw = std::max(std::min(integral_yaw, MAX_INTEGRAL), -MAX_INTEGRAL); double derivative_x = error_x - prev_error_x; double derivative_y = error_y - prev_error_y; double derivative_yaw = error_yaw - prev_error_yaw; double vel_x = KP_LINEAR * error_x + KI_LINEAR * integral_x + KD_LINEAR * derivative_x; double vel_y = KP_LINEAR * error_y + KI_LINEAR * integral_y + KD_LINEAR * derivative_y; double vel_yaw = KP_ANGULAR * error_yaw + KI_ANGULAR * integral_yaw + KD_ANGULAR * derivative_yaw; // 限制速度范围 vel_x = std::max(std::min(vel_x, 0.3), -0.3); vel_y = std::max(std::min(vel_y, 0.3), -0.3); vel_yaw = std::max(std::min(vel_yaw, 0.5), -0.5); // 发布速度命令 geometry_msgs::Twist cmd_vel; cmd_vel.linear.x = vel_x; cmd_vel.linear.y = vel_y; cmd_vel.angular.z = vel_yaw; if (g_vel_pub_ptr) { g_vel_pub_ptr->publish(cmd_vel); } // 更新前次误差 prev_error_x = error_x; prev_error_y = error_y; prev_error_yaw = error_yaw; ROS_INFO("Adjusting position: dx=%.3f, dy=%.3f, dyaw=%.3f", error_x, error_y, error_yaw); } } catch (tf::TransformException &ex) { ROS_ERROR("TF exception: %s", ex.what()); return false; } break; } } } else if (taskState == rei_cruise::TaskState::STOP) { break; } loop_rate.sleep(); } return true; } int main(int argc, char** argv) { ros::init(argc, argv, "cruise_test_node"); // 只初始化一个节点 ros::NodeHandle nh; // 创建订阅者列表 std::vector<ros::Subscriber> subscribers; // 为每个类别创建订阅者 for (const auto& pair : class_to_id) { const std::string& class_name = pair.first; const std::string topic_name = "/" + class_name; // 使用boost::bind传递额外参数(类别名称) ros::Subscriber sub = nh.subscribe<std_msgs::Float64MultiArray>( topic_name, 10, boost::bind(&boundingBoxCallback, _1, class_name)); subscribers.push_back(sub); ROS_INFO("Subscribed to: %s", topic_name.c_str()); } // 创建速度发布者和速度订阅者 ros::Publisher Vel_c = nh.advertise<geometry_msgs::Twist>("cmd_vel", 10); g_vel_pub_ptr = &Vel_c; ros::Subscriber vel_sub = nh.subscribe("odom_velocity", 1, velocityCallback); if (!oryxTask.Init(nh)){ ROS_ERROR("Failed to initialize ORYXTask"); return 1; } // 仿真小车白色盒子观测位置 boxAbovePose_.linear.x = 105.0; boxAbovePose_.linear.y = 120.0; boxAbovePose_.linear.z = 90.0; boxAbovePose_.angular.z = 0.0; if(!oryxTask.ResetArm(boxAbovePose_.linear.x, boxAbovePose_.linear.y, 60, 0)){ ROS_ERROR("Failed to reset arm"); return 1; } // 模拟加工台观测位置 workStationAbovePose_.linear.x = 78.0; workStationAbovePose_.linear.y = -160.0; workStationAbovePose_.linear.z = 80.0; workStationAbovePose_.angular.z = 0.0; //仿真小车两个盒子位置 geometry_msgs::Twist tmpPose; tmpPose.linear.x = 105; tmpPose.linear.y = 123; tmpPose.linear.z = 30; tmpPose.angular.z = 0.0; boxPose_.push_back(tmpPose); tmpPose.linear.x = 105; tmpPose.linear.y = 183; tmpPose.linear.z = 30; tmpPose.angular.z = 0.0; boxPose_.push_back(tmpPose); //模拟加工台的放置位置 tmpPose.linear.x = 65; tmpPose.linear.y = -160; tmpPose.linear.z = 30.0; tmpPose.angular.z = 0.0; workStationPoses_.push_back(tmpPose); tmpPose.linear.x = 125; tmpPose.linear.y = -160; tmpPose.linear.z = 30.0; tmpPose.angular.z = 0.0; workStationPoses_.push_back(tmpPose); rei_cruise::RobotCruise& cruise = rei_cruise::RobotCruise::getInstance(); if(!cruise.Init(nh)) { ROS_ERROR("Failed to initialize RobotCruise"); return 1; } // 注册任务回调 cruise.AddStartCallBack(StartCallback5); if (cruise.AddCallBack("pid_adjust", pidAdjustCallback)) { ROS_INFO("set callback pidAdjustCallback success"); } cruise.AddStartCallBack(StartCallback1); if(cruise.AddCallBack("renwua_a_place", renwua_aplaceCallback)) { ROS_INFO("set callback renwua_aplaceCallback success"); } cruise.AddStartCallBack(StartCallback2); if(cruise.AddCallBack("renwua_b_place", renwua_bplaceCallback)) { ROS_INFO("set callback renwua_bplaceCallback success"); } cruise.AddStartCallBack(StartCallback3); if(cruise.AddCallBack("renwub_a_place", renwub_aplaceCallback_stuff)) { ROS_INFO("set callback WorkStationPickCallback success"); } cruise.AddStartCallBack(StartCallback4); if(cruise.AddCallBack("renwub_b_place", renwub_bplaceCallback)) { ROS_INFO("set callback WorkStationPickCallback success"); } if(cruise.AddCallBack("charge", chargeCallback)) { ROS_INFO("set callback chargeCallback success"); } cruise.RunNavTask(); // 添加服务可用性检查 ROS_INFO("Waiting for /start_stop_nav service to become available..."); ros::service::waitForService("/start_stop_nav", ros::Duration(10.0)); if (!ros::service::exists("/start_stop_nav", false)) { ROS_ERROR("/start_stop_nav service is not available!"); return 1; } ros::spin(); // 保持节点运行 return 0; }的消息中X轴的坐标总是为定值
最新发布
08-12
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

当前余额3.43前往充值 >
需支付:10.00
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值