#!/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轴的坐标总是为定值
最新发布
本文介绍了一种LCD彩色图片转换工具BMP_to_H的使用方法，包括如何生成.C文件、声明数组及在LCD初始化中调用显示图片，适用于mini2440硬件平台与UCOS2操作系统。
扫一扫
5856
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
