x264_param_t参数注解

本文详细介绍了x264编码器的各种参数配置,包括CPU标志、视频属性、线程控制、视频可用性信息、帧率控制、熵编码、量化、去块滤波等。此外还介绍了分析参数、码率控制、切片参数等高级设置。

原始来源不详

typedef struct x264_param_t

{

/* CPU 标志位 */

unsigned int cpu;

int i_threads; /* 并行编码多帧 */

int b_deterministic; /*是否允许非确定性时线程优化*/

int i_sync_lookahead; /* 线程超前缓冲 */

/* 视频属性 */

int i_width; /* 宽度*/

int i_height; /* 高度*/

int i_csp; /* 编码比特流的CSP,仅支持i420,色彩空间设置 */

int i_level_idc; /* level值的设置*/

int i_frame_total; /* 编码帧的总数, 默认 0 */

/*Vui参数集视频可用性信息视频标准化选项 */

struct

{

/* they will be reduced to be 0 < x <= 65535 and prime */

int i_sar_height;

int i_sar_width; /* 设置长宽比 */

int i_overscan; /* 0=undef, 1=no overscan, 2=overscan 过扫描线,默认"undef"(不设置),可选项:show(观看)/crop(去除)*/

/*见以下的值h264附件E */

Int i_vidformat;/* 视频格式,默认"undef",component/pal/ntsc/secam/mac/undef*/

int b_fullrange; /*Specify full range samples setting,默认"off",可选项:off/on*/

int i_colorprim; /*原始色度格式,默认"undef",可选项:undef/bt709/bt470m/bt470bg,smpte170m/smpte240m/film*/

int i_transfer; /*转换方式,默认"undef",可选项:undef/bt709/bt470m/bt470bg/linear,log100/log316/smpte170m/smpte240m*/

int i_colmatrix; /*色度矩阵设置,默认"undef",undef/bt709/fcc/bt470bg,smpte170m/smpte240m/GBR/YCgCo*/

int i_chroma_loc; /* both top & bottom色度样本指定,范围0~5,默认0 */

} vui;

int i_fps_num;

int i_fps_den;

/*这两个参数是由fps帧率确定的,赋值的过程见下:

{ float fps;

if( sscanf( value, "%d/%d", &p->i_fps_num, &p->i_fps_den ) == 2 )

;

else if( sscanf( value, "%f", &fps ) )

{

p->i_fps_num = (int)(fps * 1000 + .5);

p->i_fps_den = 1000;

}

else

b_error = 1;

}

Value的值就是fps。*/

/*流参数 */

int i_frame_reference; /* 参考帧最大数目 */

int i_keyint_max; /* 在此间隔设置IDR关键帧 */

int i_keyint_min; /* 场景切换少于次值编码位I, 而不是 IDR. */

int i_scenecut_threshold; /*如何积极地插入额外的I帧 */

int i_bframe; /*两个相关图像间P帧的数目 */

int i_bframe_adaptive; /*自适应B帧判定*/

int i_bframe_bias; /*控制插入B帧判定,范围-100~+100,越高越容易插入B帧,默认0*/

int b_bframe_pyramid; /*允许部分B为参考帧 */

/*去块滤波器需要的参数*/

int b_deblocking_filter;

int i_deblocking_filter_alphac0; /* [-6, 6] -6 light filter, 6 strong */

int i_deblocking_filter_beta; /* [-6, 6] idem */

/*熵编码 */

int b_cabac;

int i_cabac_init_idc;

int b_interlaced; /* 隔行扫描 */

/*量化 */

int i_cqm_preset; /*自定义量化矩阵(CQM),初始化量化模式为flat*/

char *psz_cqm_file; /* JM format读取JM格式的外部量化矩阵文件,自动忽略其他—cqm 选项*/

uint8_t cqm_4iy[16]; /* used only if i_cqm_preset == X264_CQM_CUSTOM */

uint8_t cqm_4ic[16];

uint8_t cqm_4py[16];

uint8_t cqm_4pc[16];

uint8_t cqm_8iy[64];

uint8_t cqm_8py[64];

/* 日志 */

void (*pf_log)( void *, int i_level, const char *psz, va_list );

void *p_log_private;

int i_log_level;

int b_visualize;

char *psz_dump_yuv; /* 重建帧的名字 */

/* 编码分析参数*/

struct

{

unsigned int intra; /* 帧间分区*/

unsigned int inter; /* 帧内分区 */

int b_transform_8x8; /* 帧间分区*/

int b_weighted_bipred; /*为b帧隐式加权 */

int i_direct_mv_pred; /*时间空间队运动预测 */

int i_chroma_qp_offset; /*色度量化步长偏移量 */

int i_me_method; /* 运动估计算法 (X264_ME_*) */

int i_me_range; /* 整像素运动估计搜索范围 (from predicted mv) */

int i_mv_range; /* 运动矢量最大长度(in pixels). -1 = auto, based on level */

int i_mv_range_thread; /* 线程之间的最小空间. -1 = auto, based on number of threads. */

int i_subpel_refine; /* 亚像素运动估计质量 */

int b_chroma_me; /* 亚像素色度运动估计和P帧的模式选择 */

int b_mixed_references; /*允许每个宏块的分区在P帧有它自己的参考号*/

int i_trellis; /* Trellis量化,对每个8x8的块寻找合适的量化值,需要CABAC,默认0 0:关闭1:只在最后编码时使用2:一直使用*/

int b_fast_pskip; /*快速P帧跳过检测*/

int b_dct_decimate; /* 在P-frames转换参数域 */

int i_noise_reduction; /*自适应伪盲区 */

float f_psy_rd; /* Psy RD strength */

float f_psy_trellis; /* Psy trellis strength */

int b_psy; /* Toggle all psy optimizations */

/*,亮度量化中使用的无效区大小*/

int i_luma_deadzone[2]; /* {帧间, 帧内} */

int b_psnr; /* 计算和打印PSNR信息 */

int b_ssim; /*计算和打印SSIM信息*/

} analyse;

/* 码率控制参数 */

struct

{

int i_rc_method; /* X264_RC_* */

int i_qp_constant; /* 0-51 */

int i_qp_min; /*允许的最小量化值 */

int i_qp_max; /*允许的最大量化值*/

int i_qp_step; /*帧间最大量化步长 */

int i_bitrate; /*设置平均码率 */

float f_rf_constant; /* 1pass VBR, nominal QP */

float f_rate_tolerance;

int i_vbv_max_bitrate; /*平均码率模式下,最大瞬时码率,默认0(与-B设置相同) */

int i_vbv_buffer_size; /*码率控制缓冲区的大小,单位kbit,默认0 */

float f_vbv_buffer_init; /* <=1: fraction of buffer_size. >1: kbit码率控制缓冲区数据保留的最大数据量与缓冲区大小之比,范围0~1.0,默认0.9*/

float f_ip_factor;

float f_pb_factor;

int i_aq_mode; /* psy adaptive QP. (X264_AQ_*) */

float f_aq_strength;

int b_mb_tree; /* Macroblock-tree ratecontrol. */

int i_lookahead;

/* 2pass 多次压缩码率控制 */

int b_stat_write; /* Enable stat writing in psz_stat_out */

char *psz_stat_out;

int b_stat_read; /* Read stat from psz_stat_in and use it */

char *psz_stat_in;

/* 2pass params (same as ffmpeg ones) */

float f_qcompress; /* 0.0 => cbr, 1.0 => constant qp */

float f_qblur; /*时间上模糊量化 */

float f_complexity_blur; /* 时间上模糊复杂性 */

x264_zone_t *zones; /* 码率控制覆盖 */

int i_zones; /* number of zone_t's */

char *psz_zones; /*指定区的另一种方法*/

} rc;

/* Muxing parameters */

int b_aud; /*生成访问单元分隔符*/

int b_repeat_headers; /* 在每个关键帧前放置SPS/PPS*/

int i_sps_id; /* SPS 和 PPS id 号 */

/*切片(像条)参数 */

int i_slice_max_size; /* 每片字节的最大数,包括预计的NAL开销. */

int i_slice_max_mbs; /* 每片宏块的最大数,重写 i_slice_count */

int i_slice_count; /* 每帧的像条数目: 设置矩形像条. */

/* Optional callback for freeing this x264_param_t when it is done being used.

* Only used when the x264_param_t sits in memory for an indefinite period of time,

* i.e. when an x264_param_t is passed to x264_t in an x264_picture_t or in zones.

* Not used when x264_encoder_reconfig is called directly. */

void (*param_free)( void* );

} x264_param_t;

#!/usr/bin/python3 # -*- coding: utf-8 -*- # 脚本解释器声明:指定使用 Python3 运行,编码格式为 UTF-8(支持中文注释和字符串) # 导入依赖库 import re # 正则表达式库:用于处理 DeepSeek 输出的字符串格式 import ast # 抽象语法树库:用于将字符串转为列表/字典(解析任务指令) import cv2 # OpenCV 库:图像处理(此处用于接收相机图像) import time # 时间库:计时、延时 import copy # 复制库:深拷贝检测结果,避免原数据被修改 import numpy as np # 数值计算库:矩阵运算、三角函数等 from scipy.spatial.transform import Rotation # 姿态转换库:用于旋转矩阵/欧拉角转换 from utils import * # 自定义工具库:包含计数、坐标转换等通用函数(需提前实现) from arm_motions import * # 机械臂运动库:包含抓取(grab)、放置(put)等核心动作函数(需提前实现) # ROS 相关库导入 import rospy # ROS 核心库:节点初始化、话题订阅/发布等 from std_msgs.msg import Bool, String, Int16 # ROS 标准消息类型:布尔、字符串、16位整数 from sensor_msgs.msg import CameraInfo, Image, JointState # 传感器消息:相机参数、图像、关节状态 from cv_bridge import CvBridge # 图像格式转换库:ROS Image ↔ OpenCV 图像 from geometry_msgs.msg import Twist, PoseStamped # 几何消息:速度指令、位姿目标 from user_msgs.msg import CarCmd, ExitCondition, ObjDet, ObjDets # 自定义消息:车辆命令、退出条件、目标检测结果 # ========================== 全局字典定义(数据映射)========================== # 颜色名称 → 编号映射(用于将文字指令转为代码可识别的数字) hostage_dict = { "红": 0, "蓝": 1, } # 编号 → 颜色名称映射(用于日志打印,将数字转回文字) hostage_dict_ = { 0: "红", 1: "蓝", } # 放置位名称 → 编号映射(一号/二号放置位 → 0/1) place_dict = { "一号": 0, "二号": 1, } # 编号 → 放置位名称映射(日志打印用) place_dict_ = { 0: "一号", 1: "二号" } # ========================== 可调参数(提前配置,方便调试)========================== LATERAL_SPEED = 0.20 # 车辆侧向平移速度 (m/s):正值=向左,负值=向右 FORWARD_SPEED = 0.32 # 车辆前进/后退速度(m/s):正值=前进,负值=后退 PTS = 0.2 # 预留参数(未使用,可根据需求扩展) # ============================================================================== FDT1 = 3.6 # 第一段前进持续时间(秒) FDT2 = 1.0 # 预留前进时间参数(未使用) TPS1 = 0.58 # 第一段侧向平移持续时间(秒) TPS2 = 1.0 # 预留平移时间参数(未使用) TPS3 = 0.5 # 车位间平移时间(秒):从一个车位中心到相邻车位中心 # ============================================================================== # ========================== 全局变量定义(跨函数共享数据)========================== img_raw = None # 存储原始相机图像(从相机话题订阅) car_cmd = CarCmd() # 存储车辆当前命令状态(导航/保持/到达等) arrived = False # 导航到达标志位:True=已到达目标点,False=未到达 cylinder_dets = ObjDets() # 存储圆柱目标检测结果(包含目标颜色、位置、尺寸) # 左臂逆运动学计算结果:关节角度 left_arm_ik_servo_degree = JointState() left_arm_get_cmd_ik = False # 左臂逆解结果接收标志:True=已收到有效关节角度 # 右臂逆运动学计算结果:关节角度 right_arm_ik_servo_degree = JointState() right_arm_get_cmd_ik = False # 右臂逆解结果接收标志 success_rescure_count = 0 # 成功救援(抓取+放置)计数:最多救援2个目标 # ========================== 话题回调函数(数据接收)========================== def image_raw_cb(msg:Image): """ 相机图像回调函数:订阅相机原始图像,转换为 OpenCV 格式并存储到全局变量 :param msg: ROS 图像消息(/ascamera_hp60c/rgb0/image 话题) """ global img_raw # 声明使用全局变量 bridge = CvBridge() # 创建图像转换实例 img_raw = bridge.imgmsg_to_cv2(msg, 'bgr8') # ROS Image → OpenCV BGR 图像 def car_cmd_cb(msg:CarCmd): """ 车辆命令回调函数:订阅车辆状态命令,更新全局变量并设置到达标志位 :param msg: 自定义车辆命令消息(/car_cmd 话题) """ global car_cmd, arrived # 逻辑:如果之前未到达,且车辆从“导航/保持”状态切换为“到达”状态 → 置位 arrived if not arrived and (car_cmd.cmd == CarCmd.NAVIGATION or car_cmd.cmd == CarCmd.HOLD) and msg.cmd == CarCmd.ARRIVE: arrived = True # 逻辑:如果已到达,且车辆切换为“导航”状态 → 复位 arrived elif arrived and msg.cmd == CarCmd.NAVIGATION: arrived = False car_cmd = msg # 更新全局车辆命令 def cylinder_dets_cb(msg:ObjDets): """ 圆柱检测结果回调函数:订阅 YOLO 检测到的圆柱目标,存储到全局变量 :param msg: 自定义目标检测消息(/usb_cam/obj_dets_with_pose 话题) """ global cylinder_dets cylinder_dets = msg # 更新全局检测结果 def left_arm_ik_servo_degree_cb(msg:JointState): """ 左臂逆解结果回调函数:订阅左臂逆运动学计算出的关节角度 :param msg: 关节状态消息(/arm_left/arm_ik_servo_degree 话题) """ global left_arm_ik_servo_degree, left_arm_get_cmd_ik left_arm_ik_servo_degree = msg # 更新左臂关节角度 left_arm_get_cmd_ik = True # 置位“已收到逆解结果”标志 def right_arm_ik_servo_degree_cb(msg:JointState): """ 右臂逆解结果回调函数:订阅右臂逆运动学计算出的关节角度 :param msg: 关节状态消息(/arm_right/arm_ik_servo_degree 话题) """ global right_arm_ik_servo_degree, right_arm_get_cmd_ik right_arm_ik_servo_degree = msg # 更新右臂关节角度 right_arm_get_cmd_ik = True # 置位“已收到逆解结果”标志 # ========================== 核心工具函数(车辆控制)========================== def car_lateral_translation(speed, duration, car_cmd_pub, cmd_vel_pub): """ 控制车辆进行侧向平移(左右移动) :param speed: 平移速度 (m/s):正值=向左,负值=向右 :param duration: 平移持续时间 (秒):控制移动距离(距离=速度×时间) :param car_cmd_pub: 车辆命令发布者(发布 /car_cmd 话题) :param cmd_vel_pub: 速度指令发布者(发布 /cmd_vel 话题) """ # 1. 切换车辆为“遥控模式”(只有遥控模式才能接收速度指令) car_cmd = CarCmd() car_cmd.cmd = CarCmd.REMOTE # 连续发布20次(确保车辆控制器稳定接收模式切换指令) for _ in range(20): car_cmd_pub.publish(car_cmd) rospy.sleep(0.01) # 每次发布后延时0.01秒 # 日志打印:提示平移方向、速度、持续时间(便于调试) direction = "左" if speed > 0 else "右" rospy.loginfo(f"开始向 {direction} 平移,速度: {abs(speed):.2f} m/s, 持续时间: {duration:.2f} s...") # 2. 发布侧向平移速度指令(ROS Twist 消息:linear.y 对应侧向运动) twist = Twist() twist.linear.y = speed # 设置侧向速度 start_time = time.time() # 记录开始时间 rate = rospy.Rate(10) # 设定发布频率:10 Hz(每秒发布10次指令,避免指令丢失) # 循环发布指令,直到达到指定持续时间或节点关闭 while time.time() - start_time < duration and not rospy.is_shutdown(): cmd_vel_pub.publish(twist) rate.sleep() # 按设定频率休眠 # 3. 停止移动:发布零速度指令 twist.linear.y = 0 cmd_vel_pub.publish(twist) # 4. 切换车辆为“保持模式”(停止后锁定当前状态,防止误动) car_cmd.cmd = CarCmd.HOLD car_cmd_pub.publish(car_cmd) rospy.loginfo("车辆平移完成。") def car_forward_backward(speed, duration, car_cmd_pub, cmd_vel_pub): """ 控制车辆进行前进或后退 :param speed: 移动速度 (m/s):正值=前进,负值=后退 :param duration: 移动持续时间 (秒):控制移动距离(距离=速度×时间) :param car_cmd_pub: 车辆命令发布者(发布 /car_cmd 话题) :param cmd_vel_pub: 速度指令发布者(发布 /cmd_vel 话题) """ # 1. 切换车辆为“遥控模式”(接收速度指令的前提) car_cmd = CarCmd() car_cmd.cmd = CarCmd.REMOTE # 连续发布20次(确保模式切换成功) for _ in range(20): car_cmd_pub.publish(car_cmd) rospy.sleep(0.01) # 日志打印:提示移动方向、速度、持续时间 direction = "前进" if speed > 0 else "后退" rospy.loginfo(f"开始 {direction},速度: {abs(speed):.2f} m/s, 持续时间: {duration:.2f} s...") # 2. 发布前进/后退速度指令(ROS Twist 消息:linear.x 对应前后运动) twist = Twist() twist.linear.x = speed # 设置前后速度 start_time = time.time() # 记录开始时间 rate = rospy.Rate(10) # 10 Hz 发布频率 # 循环发布指令,直到达到指定时间 while time.time() - start_time < duration and not rospy.is_shutdown(): cmd_vel_pub.publish(twist) rate.sleep() # 3. 停止移动:发布零速度指令 twist.linear.x = 0 cmd_vel_pub.publish(twist) rospy.loginfo("车辆停止。") # 4. 切换回“保持模式”,交还控制权 car_cmd.cmd = CarCmd.HOLD car_cmd_pub.publish(car_cmd) rospy.loginfo("车辆前进/后退动作完成,已切换回保持模式。") # ========================== 核心工具函数(检测辅助)========================== def get_current_detected_colors(cylinder_dets, timeout=2.0): """ 实时获取当前检测到的圆柱目标颜色(从全局检测结果中提取) :param cylinder_dets: 全局目标检测结果(ObjDets 类型) :param timeout: 超时时间(秒):超过时间未检测到则返回空列表 :return: list:检测到的颜色列表,如 ['红', '蓝'] 或 [] """ start_time = time.time() detected_colors = [] # 存储检测到的颜色 # 循环检测,直到超时或检测到目标 while not rospy.is_shutdown() and (time.time() - start_time < timeout): if cylinder_dets.num > 0: # 如果检测到目标(num=目标数量) detected_colors = [] # 清空之前的结果(避免重复) # 遍历所有检测到的目标 for det in cylinder_dets.dets: det: ObjDet # 类型注解:明确 det 是 ObjDet 类型 # 根据目标的 class_id 映射到颜色名称 if det.class_id in hostage_dict_: color = hostage_dict_[det.class_id] detected_colors.append(color) # 如果检测到颜色,立即返回结果 if detected_colors: rospy.loginfo(f"实时检测到颜色: {detected_colors}") return detected_colors rospy.sleep(0.1) # 每0.1秒检测一次,避免占用过多CPU # 超时或未检测到目标,打印警告并返回空列表 if not detected_colors: rospy.logwarn("未检测到任何物体颜色") return detected_colors # ========================== 主函数(核心任务逻辑)========================== def main(): # 声明使用全局变量(跨函数修改需要声明) global arrived, left_arm_get_cmd_ik, right_arm_get_cmd_ik global success_rescure_count # 1. 初始化 ROS 节点:节点名=mission_manage_node,anonymous=True→自动添加随机后缀(避免节点名冲突) rospy.init_node('mission_manage_node', anonymous=True) # 2. 从 ROS 参数服务器读取配置参数参数在 launch 文件中通过 rosparam 加载) # 2.1 读取导航目标点参数(救援位置、过渡点、放置位置、返回点、停车位置) go_to_grab_num = rospy.get_param('/mission_magage/go_to_grab/wp_num', 0) # 救援位置点数量 go_to_grab_wp = [] # 存储救援位置点列表 for i in range(go_to_grab_num): # 读取每个救援位置点(参数名格式:/mission_magage/go_to_grab/wp_1, wp_2...) go_to_grab_wp.append(rospy.get_param(f'/mission_magage/go_to_grab/wp_{i+1}')) transit_wp = rospy.get_param('/mission_magage/transit_pt/wp_1') # 过渡点(救援→放置的中间导航点) grab_num = rospy.get_param('/mission_magage/grab/wp_num', 0) # 抓取位置点数量 grab_wp = [] # 存储抓取位置点列表 for i in range(grab_num): grab_wp.append(rospy.get_param(f'/mission_magage/grab/wp_{i+1}')) put_num = rospy.get_param('/mission_magage/put/wp_num', 0) # 放置位置点数量 put_wp = [] # 存储放置位置点列表 for i in range(put_num): put_wp.append(rospy.get_param(f'/mission_magage/put/wp_{i+1}')) go_back_wp = rospy.get_param('/mission_magage/go_back/wp_1') # 返回点(放置→停车的中间点) park_num = rospy.get_param('/mission_magage/park/wp_num', 0) # 停车位置点数量 park_wp = [] # 存储停车位置点列表 for i in range(park_num): park_wp.append(rospy.get_param(f'/mission_magage/park/wp_{i+1}')) # 2.2 初始化机械臂抓取参数(左右臂分别配置) grab_param_l = GrabParam() # 左臂抓取参数实例(自定义类,包含相机内参、坐标转换参数等) grab_param_r = GrabParam() # 右臂抓取参数实例 # 2.3 读取坐标转换参数(各部件相对车体的位置/姿态) t_larm_2_car = rospy.get_param('/arm_left/translation', [0, 0, 0]) # 左臂→车体的平移向量(x,y,z) t_rarm_2_car = rospy.get_param('/arm_right/translation', [0, 0, 0]) # 右臂→车体的平移向量 t_cam_2_car = rospy.get_param('/ascamera/translation', [0, 0, 0]) # 相机→车体的平移向量 rpy_cam_2_car = rospy.get_param('/ascamera/rotation', [-90, 0, -90]) # 相机→车体的姿态(欧拉角,度) # 2.4 读取抓取相关参数(视觉识别区域、目标距离、机械臂爪部参数) sight_middle_h = rospy.get_param('/grab_param/sight_middle_h', 300) # 视觉识别区域中线(高度方向像素) sight_middle_w = rospy.get_param('/grab_param/sight_middle_w', [269, 457]) # 视觉识别区域左右边界(宽度方向像素) x_obj_to_cam_close = rospy.get_param('/grab_param/x_obj_to_cam_close', 0.205) # 目标→相机的近距离(m) x_obj_to_cam_middle = rospy.get_param('/grab_param/x_obj_to_cam_middle', 0.26) # 目标→相机的中距离(m) x_obj_to_cam_far = rospy.get_param('/grab_param/x_obj_to_cam_far', 0.34) # 目标→相机的远距离(m) x_j5_2_claw = rospy.get_param('/grab_param/x_j5_2_claw', 0.085) # 机械臂关节5→爪部的x方向距离(m) z_j5_2_claw = rospy.get_param('/grab_param/z_j5_2_claw', 0.0115) # 机械臂关节5→爪部的z方向距离(m) l_j5_2_claw = np.sqrt(x_j5_2_claw**2 + z_j5_2_claw**2) # 关节5→爪部的直线距离(m) theta_j5_2_claw = np.rad2deg(np.arctan2(z_j5_2_claw, x_j5_2_claw)) # 关节5→爪部的角度(度) # 2.5 读取机械臂关节角度参数(左右臂独立配置) left_arm_front = rospy.get_param('turn_front', 90) # 左臂向前转动角度(度) left_arm_middle = rospy.get_param('turn_middle', 0) # 左臂中间位置角度(度) left_arm_back = rospy.get_param('turn_back', -90) # 左臂向后转动角度(度) left_arm_tight = rospy.get_param('/arm_left/claw_tight', 30) # 左臂爪部夹紧角度(度) left_arm_loose = rospy.get_param('/arm_left/claw_loose', 60) # 左臂爪部松开角度(度) right_arm_front = rospy.get_param('/arm_right/turn_front', 90) # 右臂向前转动角度(度) right_arm_middle = rospy.get_param('/arm_right/turn_middle', 0) # 右臂中间位置角度(度) right_arm_back = rospy.get_param('/arm_right/turn_back', -90) # 右臂向后转动角度(度) right_arm_tight = rospy.get_param('/arm_right/claw_tight', 30) # 右臂爪部夹紧角度(度) right_arm_loose = rospy.get_param('/arm_right/claw_loose', 60) # 右臂爪部松开角度(度) # 2.6 读取任务时间参数(总任务时间,预留25秒返回时间) mission_time = rospy.get_param('/mission/mission_time', 300) - 25 # 3. 创建 ROS 话题订阅者(接收数据) image_raw_sub = rospy.Subscriber('/ascamera_hp60c/rgb0/image', Image, image_raw_cb) # 订阅相机图像 car_cmd_sub = rospy.Subscriber('/car_cmd', CarCmd, car_cmd_cb) # 订阅车辆命令 yolov5_dets_sub = rospy.Subscriber('/usb_cam/obj_dets_with_pose', ObjDets, cylinder_dets_cb) # 订阅目标检测结果 left_arm_servo_degree_sub = rospy.Subscriber("/arm_left/arm_ik_servo_degree", JointState, left_arm_ik_servo_degree_cb) # 订阅左臂逆解结果 right_arm_servo_degree_sub = rospy.Subscriber("/arm_right/arm_ik_servo_degree", JointState, right_arm_ik_servo_degree_cb) # 订阅右臂逆解结果 # 4. 创建 ROS 话题发布者(发送指令) ds_task_pub = rospy.Publisher('/deepseek/task', Bool, queue_size=1) # 控制 DeepSeek 节点启停 ds_input_pub = rospy.Publisher('/deepseek/input_text', String, queue_size=1) # 向 DeepSeek 发送任务指令 at_target_pub = rospy.Publisher('/at_target_id', Int16, queue_size=1) # 发布 AprilTag 目标ID car_target_pub = rospy.Publisher('/car_target', PoseStamped, queue_size=1) # 发布车辆导航目标位姿 exit_condition_pub = rospy.Publisher('exit_condition', ExitCondition, queue_size=1) # 发布任务退出条件 car_cmd_pub = rospy.Publisher('/car_cmd', CarCmd, queue_size=3) # 发布车辆控制命令 cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=3) # 发布车辆速度指令 left_arm_target_pose_pub = rospy.Publisher('/arm_left/arm_target', PoseStamped, queue_size=3) # 发布左臂目标位姿 left_arm_servo_degree_pub = rospy.Publisher('/arm_left/arm_joint_states', JointState, queue_size=3) # 发布左臂关节角度指令 right_arm_target_pose_pub = rospy.Publisher('/arm_right/arm_target', PoseStamped, queue_size=3) # 发布右臂目标位姿 right_arm_servo_degree_pub = rospy.Publisher('/arm_right/arm_joint_states', JointState, queue_size=3) # 发布右臂关节角度指令 # ========================== 任务指令解析(核心逻辑1)========================== # 说明:此处注释了 DeepSeek 交互逻辑,直接手动指定任务指令(仅供调试) # 实际使用时可取消注释,通过输入文本指令让 DeepSeek 生成任务列表 # ################################################################################ # 输入任务指令 等待deepseek输出 # ############################## # ds_input = String() # input_temp = input("输入任务指令\n>>>") # ds_input.data = "在此填入自定义附加提示词" \ # + input_temp + "。" # ds_input_pub.publish(ds_input) # t0 = time.time() # rospy.loginfo("任务开始\n") # 等待deepseek处理得到的结果 # ds_output: String = rospy.wait_for_message('/deepseek/output_text', String, timeout=None) # ################################################################################ # ################################################################################ # 手动指定结果 仅供调试使用(核心:任务指令→[[颜色, 放置位], [颜色, 放置位]]) # 示例:[[蓝色,一号], [红色, 二号]] → 蓝色目标放到一号位,红色目标放到二号位 # ################################################################################ t0 = time.time() # 记录任务开始时间 ds_output = "[[蓝色,一号], [红色, 二号]]" # 手动指定任务指令 # ################################################################################ # 解析任务指令:将字符串转为列表(如 "[[蓝色,一号], [红色,二号]]" → [['蓝色','一号'], ['红色','二号']]) orig_str = ds_output if "色" in orig_str: orig_str = orig_str.replace("色", "") # 去除“色”字(如“蓝色”→“蓝”,适配 hostage_dict) try: mission = ast.literal_eval(orig_str) # 字符串→列表(正常格式直接解析) print(mission) except ValueError: # 格式异常时,用正则表达式给中文添加引号(如 [蓝色,一号] → ["蓝色","一号"]) processed = re.sub(r'(?<=[\[,\s])([\u4e00-\u9fa5]+)(?=[,\]\s])', r'"\1"', orig_str) mission = ast.literal_eval(processed) # 重新解析 print(mission) # 将任务列表中的文字转为编号(如 "蓝色"→1,"一号"→0),便于代码处理 grab_mission = mission print(grab_mission) # 打印转换前的任务列表 for item in grab_mission: if isinstance(item, list) and len(item) == 2: if item[0] in hostage_dict: item[0] = hostage_dict[item[0]] # 颜色文字→编号 if item[1] in place_dict: item[1] = place_dict[item[1]] # 放置位文字→编号 # 提取救援目标颜色和放置位指令(便于后续匹配) hostage_cmd = [grab_mission[i][0] for i in range(2)] # 救援目标颜色编号列表:[1,0](对应蓝色、红色) place_cmd = [grab_mission[i][1] for i in range(2)] # 放置位编号列表:[0,1](对应一号、二号) print("救援放置任务配对:", grab_mission) # 打印最终任务:[[1,0], [0,1]] # 结束 DeepSeek 节点,释放内存(调试模式下无作用,实际使用时启用) ds_task_pub.publish(Bool(False)) # 5. 初始化机械臂抓取参数(从相机参数中获取内参) # 等待相机参数话题(/ascamera_hp60c/rgb0/camera_info),超时则报错 camera_info_msg: CameraInfo = rospy.wait_for_message('/ascamera_hp60c/rgb0/camera_info', CameraInfo, timeout=None) # 给左右臂抓取参数赋值(相机内参:cx/cy=主点坐标,fx/fy=焦距) grab_param_l.cx = camera_info_msg.K[2] grab_param_l.cy = camera_info_msg.K[5] grab_param_l.fx = camera_info_msg.K[0] grab_param_l.fy = camera_info_msg.K[4] grab_param_l.t_arm_2_car = t_larm_2_car grab_param_l.t_cam_2_car = t_cam_2_car grab_param_l.rpy_cam_2_car = rpy_cam_2_car grab_param_l.sight_middle_h = sight_middle_h grab_param_l.sight_middle_w = sight_middle_w grab_param_l.x_obj_to_cam_close = x_obj_to_cam_close grab_param_l.x_obj_to_cam_middle = x_obj_to_cam_middle grab_param_l.x_obj_to_cam_far = x_obj_to_cam_far grab_param_l.l_j5_2_claw = l_j5_2_claw grab_param_l.theta_j5_2_claw = theta_j5_2_claw # 右臂参数与左臂一致(如需差异配置可单独修改) grab_param_r.cx = camera_info_msg.K[2] grab_param_r.cy = camera_info_msg.K[5] grab_param_r.fx = camera_info_msg.K[0] grab_param_r.fy = camera_info_msg.K[4] grab_param_r.t_arm_2_car = t_rarm_2_car grab_param_r.t_cam_2_car = t_cam_2_car grab_param_r.rpy_cam_2_car = rpy_cam_2_car grab_param_r.sight_middle_h = sight_middle_h grab_param_r.sight_middle_w = sight_middle_w grab_param_r.x_obj_to_cam_close = x_obj_to_cam_close grab_param_r.x_obj_to_cam_middle = x_obj_to_cam_middle grab_param_r.x_obj_to_cam_far = x_obj_to_cam_far grab_param_r.l_j5_2_claw = l_j5_2_claw grab_param_r.theta_j5_2_claw = theta_j5_2_claw # 初始化机械臂关节状态消息(6个关节:joint1~joint6) arm_servo_degree = JointState() arm_servo_degree.name = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] # ========================== 初始动作:离开起始区(核心逻辑2)========================== print("\n离开起始区...\n") rospy.loginfo("开始执行前进动作") car_forward_backward(FORWARD_SPEED, FDT1, car_cmd_pub, cmd_vel_pub) # 前进FDT1秒 rospy.loginfo("车辆开始向右平移") car_lateral_translation(-LATERAL_SPEED, TPS1, car_cmd_pub, cmd_vel_pub) # 向右平移TPS1秒(速度为负) rospy.sleep(1) # 延时1秒,稳定车辆状态 # 检测当前视野内的目标颜色,判断救援位置顺序 current_colors = get_current_detected_colors(cylinder_dets) print(f"当前检测到的颜色: {current_colors}") # 逻辑:匹配检测到的颜色与任务指令,确定第一个救援位置 if hostage_dict[current_colors[0]] == hostage_cmd[0]: print(f"{current_colors} 和 {hostage_cmd} 匹配,选择第一个救援位置") flag=True grab_wp_cnt = 0 # 救援位置计数器:0→第一个救援点 else: print(f"{current_colors} 和 {hostage_cmd} 不匹配,选择第二个救援位置") flag=False grab_wp_cnt = 1 # 救援位置计数器:1→第二个救援点 # 如果不匹配,调整车辆位置(向右平移4秒+小幅前进) if flag==False: rospy.loginfo("车辆开始向右平移") car_lateral_translation(-LATERAL_SPEED, 4.0, car_cmd_pub, cmd_vel_pub) rospy.loginfo("开始执行前进动作") car_forward_backward(FORWARD_SPEED, 0.1, car_cmd_pub, cmd_vel_pub) flag=True # ========================== 救援循环:抓取+放置(核心逻辑3)========================== # 初始化 TF 监听器(用于坐标转换,如相机坐标系→车体坐标系) tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) count=0 # 单次救援计数(0→第一个目标,1→第二个目标) # 循环条件:未超时(任务时间)、未救援2个目标、节点未关闭 while not rospy.is_shutdown() and (time.time() - t0 < mission_time) and count<2 and success_rescure_count < 2: # 1. 导航到救援位置 print(f"\n前往{grab_wp_cnt+1}号救援位置...\n") arrived = False # 复位到达标志 # 发布救援位置导航目标(pub_wp 是自定义函数,用于发布导航点) pub_wp(grab_wp[grab_wp_cnt], car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) # 等待车辆到达目标位置 while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break # 2. 检测救援目标(圆柱) print(f"\n到达 {grab_wp_cnt+1}号 救援位置 开始检测\n") t1 = time.time() # 记录检测开始时间 cylinder_dets_ = None # 存储当前救援位置的检测结果 # 等待检测结果(超时2秒未检测到则跳过) while not rospy.is_shutdown() and cylinder_dets_ is None: if time.time() - t1 > 2: rospy.logwarn("检测超时,未发现目标") break if not cylinder_dets.num: # 如果未检测到目标,继续等待 rospy.Rate(1).sleep() continue cylinder_dets_ = copy.copy(cylinder_dets) # 深拷贝检测结果(避免原数据被修改) # 如果未检测到目标,跳过当前救援位置 if cylinder_dets_ is None: print(f"\n{grab_wp_cnt+1}号 救援位置无目标,跳过\n") continue # 3. 机械臂抓取目标 print("\n检测到人质 准备抓取\n") assign_left = False # 左臂分配标志(未使用,可扩展) assign_right = False # 右臂分配标志(未使用,可扩展) hostage_in_arm = [-1, -1] # 记录臂部抓取结果:[右臂结果, 左臂结果],-1=未抓取 # 遍历所有检测到的目标 for det in cylinder_dets_.dets: det: ObjDet # 只处理任务指令中指定的目标颜色(过滤无关目标) if det.class_id not in hostage_cmd: continue # 计算目标中心在图像中的像素坐标 target_center_pix = [det.bbox.x, det.bbox.y] ########################################## # 逻辑:根据目标在图像中的位置,判断抓取优先级(middle/top/bottom) if det.bbox.y <= sight_middle_h and sight_middle_w[0] <= det.bbox.x <= sight_middle_w[1]: position_level = "middle" # 目标在中间区域(优先抓取) elif det.bbox.y <= sight_middle_h: position_level = "top" # 目标在上半区域 print("目标在上半区域") else: position_level = "bottom" # 目标在下半区域 # 逻辑:根据目标在图像中的左右位置,分配机械臂(左/右) if target_center_pix[0] < sight_middle_w[1]: lr = True # True=左臂 print("分配左臂抓取") arm_servo_degree_pub = left_arm_servo_degree_pub # 左臂关节角度发布者 loose = left_arm_loose # 左臂松开角度 tight = left_arm_tight # 左臂夹紧角度 else : lr = False # False=右臂 print("分配右臂抓取") arm_servo_degree_pub = right_arm_servo_degree_pub # 右臂关节角度发布者 loose = right_arm_loose # 右臂松开角度 tight = right_arm_tight # 右臂夹紧角度 # 准备抓取:拷贝检测结果,避免被后续回调修改 dets_to_process = copy.copy(cylinder_dets_.dets) init_fruit_count = count_fruits(cylinder_dets_) # 初始目标数量(自定义函数:统计检测到的目标数) print("初始识别到的目标数量:", init_fruit_count) after_fruit_count = init_fruit_count # 抓取后目标数量(初始化为初始数量) grab_degree=[] # 存储抓取时的关节角度(由 grab 函数填充) # 循环抓取:直到抓取成功(目标数量减少)或退出 while(True): # 调用抓取函数(自定义函数:arm_motions.py 中实现,控制机械臂完成抓取动作) grab(grab_degree, loose, tight, lr, arm_servo_degree, arm_servo_degree_pub, position_level) # 抓取后重新读取检测结果,判断是否抓取成功 yolov5_dets_after = copy.copy(cylinder_dets) after_fruit_count = count_fruits(yolov5_dets_after) print("抓取后识别到的目标数量:", after_fruit_count) # 逻辑:如果抓取后目标数量减少 → 抓取成功 if after_fruit_count < init_fruit_count: if lr: hostage_in_arm[0] = det.class_id # 左臂抓取到的目标颜色编号 success_rescure_count += 1 # 成功救援计数+1 count += 1 # 单次循环计数+1 # 切换下一个救援位置(0→1,1→0) if grab_wp_cnt == 1: grab_wp_cnt = 0 elif grab_wp_cnt == 0: grab_wp_cnt = 1 print(f"左臂抓取成功,目标颜色编号:{hostage_in_arm[0]}") else: hostage_in_arm[1] = det.class_id # 右臂抓取到的目标颜色编号 print(f"右臂抓取成功,目标颜色编号:{hostage_in_arm[1]}") success_rescure_count += 1 # 成功救援计数+1 count += 1 # 单次循环计数+1 break # 抓取成功,退出循环 # 如果抓取失败,重新导航到当前救援位置,再次尝试 print(f"抓取失败,重新前往{grab_wp_cnt+1}号救援位置...\n") arrived = False pub_wp(grab_wp[grab_wp_cnt], car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break ########################################################## # 4. 导航到过渡点(救援→放置的中间点,稳定车辆姿态) print("\n前往中间点...\n") arrived = False pub_wp(transit_wp, car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break # 5. 导航到放置位置并放置目标 # 配置放置参数(此处默认用左臂放置,可根据实际抓取的臂部调整) lr = True loose = left_arm_loose tight = left_arm_tight arm_servo_degree_pub = left_arm_servo_degree_pub # 根据当前救援计数,确定放置位(count-1→0/1,对应一号/二号放置位) place = place_cmd[count-1] print(f"\n前往 {place+1} 号放置位...\n") # 导航到放置位 arrived = False pub_wp(put_wp[place], car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break # 调用放置函数(自定义函数:arm_motions.py 中实现,控制机械臂松开爪部放置目标) put(1, loose, tight, lr, arm_servo_degree, arm_servo_degree_pub) # ========================== 任务结束:返回并停车(核心逻辑4)========================== print("\n所有救援任务完成,开始返回...\n") # 车辆掉头(自定义函数:控制车辆旋转190度,准备返回) chassis_turn(190, car_cmd_pub, cmd_vel_pub) rospy.loginfo("开始执行返回前进动作") car_forward_backward(FORWARD_SPEED, FDT1, car_cmd_pub, cmd_vel_pub) # 前进返回 print(f"\n开始停入车位...\n") # 导航到停车位置 arrived = False pub_wp(park_wp[0], car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break # 可选:微调停车位置(侧向平移,根据实际车位调整) # rospy.loginfo("车辆开始向右平移微调") # car_lateral_translation(-0.4, 7, car_cmd_pub, cmd_vel_pub) # ========================== 程序入口 ========================== if __name__ == "__main__": main() # 启动主函数
11-30
#!/usr/bin/python3 # -*- coding: utf-8 -*- # 脚本解释器声明:指定使用 Python3 运行,编码格式为 UTF-8(支持中文注释和字符串) # 导入依赖库 import cv2 # OpenCV 库:图像处理、窗口显示、鼠标事件 import time # 时间库:延时、计时 import copy # 复制库:深拷贝数据,避免原数据被修改 import numpy as np # 数值计算库:矩阵运算、三角函数 import threading # 多线程库:独立线程处理图像显示(避免阻塞主逻辑) import queue # 队列库:线程间数据通信(图像显示队列、鼠标点击队列) from utils import * # 自定义工具库:包含抓取参数类(GrabParam)、坐标转换等函数 # ROS 相关库导入 import rospy # ROS 核心库:节点初始化、话题订阅/发布 from cv_bridge import CvBridge # 图像格式转换:ROS Image ↔ OpenCV 图像 from std_msgs.msg import Bool, String # ROS 标准消息:布尔、字符串 from sensor_msgs.msg import CameraInfo, Image, JointState # 传感器消息:相机参数、图像、关节状态 from user_msgs.msg import ObjDet, ObjDets # 自定义消息:单个目标检测结果、多个目标检测结果 from geometry_msgs.msg import PoseStamped # 几何消息:机械臂目标位姿 # ========================== 全局变量定义(跨函数/线程共享数据)========================== img_raw = None # 存储原始相机图像(从 ROS 话题订阅) cylinder_dets = None # 存储圆柱目标检测结果(从 YOLO 检测话题订阅) arm_ik_servo_degree = JointState() # 存储机械臂逆运动学计算出的关节角度 get_cmd_ik = False # 逆解结果接收标志:True=已收到有效关节角度 # 线程锁:解决多线程并发访问全局变量的冲突(图像和检测结果为多线程共享) img_lock = threading.Lock() # 图像数据锁:保护 img_raw 的读写 det_lock = threading.Lock() # 检测结果锁:保护 cylinder_dets 的读写 # 线程间通信队列 display_queue = queue.Queue(maxsize=1) # 图像显示队列(限制最大1个元素,避免缓存堆积) click_queue = queue.Queue() # 鼠标点击队列:存储鼠标左键点击的坐标(x,y) exit_event = threading.Event() # 线程退出事件:用于优雅关闭所有线程 # ========================== ROS 话题回调函数(数据接收)========================== def image_raw_cb(msg: Image): """ 相机图像回调函数:订阅相机原始图像,转换为 OpenCV 格式并存储到全局变量 :param msg: ROS 图像消息(/ascamera_hp60c/rgb0/image 话题) """ global img_raw bridge = CvBridge() # 创建图像转换实例 with img_lock: # 加锁:确保同一时间只有一个线程读写 img_raw img_raw = bridge.imgmsg_to_cv2(msg, 'bgr8') # ROS Image → OpenCV BGR 图像 def cylinder_dets_cb(msg: ObjDets): """ 圆柱检测结果回调函数:订阅 YOLO 检测到的圆柱目标,存储到全局变量 :param msg: 自定义消息(/usb_cam/obj_dets_with_pose 话题),包含所有检测到的圆柱信息 """ global cylinder_dets with det_lock: # 加锁:确保同一时间只有一个线程读写 cylinder_dets cylinder_dets = msg def arm_ik_servo_degree_cb(msg:JointState): """ 机械臂逆解结果回调函数:订阅机械臂逆运动学节点计算出的关节角度 :param msg: 关节状态消息(当前命名空间下的 arm_ik_servo_degree 话题) """ global arm_ik_servo_degree, get_cmd_ik arm_ik_servo_degree = msg # 更新关节角度数据 get_cmd_ik = True # 置位“已收到逆解结果”标志 # ========================== 图像处理与显示函数(多线程相关)========================== def draw_detections(img): """ 在图像上绘制检测结果:给每个圆柱目标画 bounding box 和标签 :param img: 原始 OpenCV 图像 :return: 绘制了检测结果的图像副本(不修改原图像) """ img_copy = img.copy() # 深拷贝图像,避免修改原数据 with det_lock: # 加锁:安全访问检测结果 if cylinder_dets is not None: # 如果有检测结果 for det in cylinder_dets.dets: # 遍历每个检测到的圆柱 det: ObjDet # 类型注解:明确 det 是 ObjDet 类型 # 计算 bounding box 左上角(tl)和右下角(br)坐标(像素) tl = (int(det.bbox.x1), int(det.bbox.y1)) br = (int(det.bbox.x2), int(det.bbox.y2)) # 设置颜色:红色圆柱→红色(0,0,255),蓝色圆柱→蓝色(255,0,0) color = (0, 0, 255) if det.label == "red_cylinder" else (255, 0, 0) # 绘制矩形框(线宽2) cv2.rectangle(img_copy, tl, br, color, 2) # 绘制标签文字(在矩形框左上角上方,字体、大小0.5、线宽1) cv2.putText(img_copy, det.label, (tl[0], tl[1]-5), cv2.FONT_HERSHEY_SIMPLEX, 0.5, color, 1) return img_copy # 返回绘制后的图像 def mouse_callback(event, x, y, flags, param): """ 鼠标回调函数:绑定到显示窗口,监听鼠标左键点击事件 :param event: 鼠标事件类型(如左键点击、移动等) :param x: 鼠标点击的 x 坐标(像素) :param y: 鼠标点击的 y 坐标(像素) :param flags: 鼠标按键状态(如是否按住) :param param: 额外参数(此处未使用) """ if event == cv2.EVENT_LBUTTONDOWN: # 如果是鼠标左键按下 click_queue.put((x, y)) # 将点击坐标放入队列,供主线程处理 def t_display(): """ 图像显示线程函数:独立线程运行,负责显示相机图像和检测结果,监听鼠标/键盘事件 作用:避免图像显示阻塞主逻辑(如机械臂控制、目标抓取) """ # 创建 OpenCV 显示窗口,窗口名=Camera cv2.namedWindow('Camera') # 绑定鼠标回调函数:将鼠标事件与 mouse_callback 关联 cv2.setMouseCallback('Camera', mouse_callback) # 循环运行,直到收到退出事件 while not exit_event.is_set(): try: current_img = None # 存储当前要显示的图像 with img_lock: # 加锁读取图像 if img_raw is not None: current_img = img_raw.copy() # 拷贝图像,避免原数据被修改 if current_img is None: # 如果没有收到图像,短暂休眠后继续 time.sleep(0.01) continue # 在图像上绘制检测结果 img_with_dets = draw_detections(current_img) # 显示图像 cv2.imshow('Camera', img_with_dets) # 监听键盘事件(等待1ms,避免阻塞) key = cv2.waitKey(1) & 0xff if key == 27: # 按下 ESC 键(ASCII 码27),触发退出事件 exit_event.set() break except Exception as e: # 捕获异常,打印错误日志并退出 rospy.logerr(f"Display thread error: {str(e)}") exit_event.set() break # 线程退出时,关闭所有 OpenCV 窗口 cv2.destroyAllWindows() # ========================== 主函数(核心逻辑:参数初始化+线程管理+抓取控制)========================== def main(): # 声明使用全局变量 global img_raw, cylinder_dets, arm_ik_servo_degree, get_cmd_ik # 1. 初始化 ROS 节点:节点名=grab_cylinder_node,anonymous=True→自动添加随机后缀(避免节点名冲突) rospy.init_node('grab_cylinder_node', anonymous=True) # 2. 从 ROS 命名空间判断机械臂方向(左/右) arm_ns = rospy.get_namespace() # 获取当前节点的命名空间(如 /arm_left/ 或 /arm_right/) arm_dir = arm_ns[5:-1] # 提取命名空间中的方向字段:/arm_left/ → 切片后为 left(索引5到-1,去除前缀/arm_和后缀/) # 判断机械臂方向:left→左臂(lr=True),right→右臂(lr=False),默认左臂 if arm_dir in ['l', 'L', 'left', 'Left', 'LEFT']: lr = True # 左臂标志 elif arm_dir in ['r', 'R', 'right', 'Right', 'RIGHT']: lr = False # 右臂标志 else: lr = True # 默认左臂 # 3. 初始化抓取参数(GrabParam 为自定义类,存储相机内参、坐标转换参数等) grab_param = GrabParam() # 等待并获取相机内参消息(/ascamera_hp60c/rgb0/camera_info 话题),超时则报错 camera_info_msg: CameraInfo = rospy.wait_for_message('/ascamera_hp60c/rgb0/camera_info', CameraInfo, timeout=None) # 相机内参赋值(K矩阵:fx, fy 为焦距;cx, cy 为主点坐标) grab_param.cx = camera_info_msg.K[2] grab_param.cy = camera_info_msg.K[5] grab_param.fx = camera_info_msg.K[0] grab_param.fy = camera_info_msg.K[4] # 从 ROS 参数服务器读取坐标转换参数(launch 文件中加载) grab_param.t_arm_2_car = rospy.get_param('translation', [0, 0, 0]) # 机械臂→车体的平移向量(x,y,z) grab_param.t_cam_2_car = rospy.get_param('/ascamera/translation', [0, 0, 0]) # 相机→车体的平移向量 grab_param.rpy_cam_2_car = rospy.get_param('/ascamera/rotation', [-90, 0, -90]) # 相机→车体的姿态(欧拉角,度) # 读取抓取相关参数(视觉识别区域、目标距离、机械臂爪部参数) grab_param.sight_middle_h = rospy.get_param('/grab_param/sight_middle_h', 300) # 视觉区域中线(高度方向像素) grab_param.sight_middle_w = rospy.get_param('/grab_param/sight_middle_w', [269, 457]) # 视觉区域左右边界(像素) grab_param.x_obj_to_cam_close = rospy.get_param('/grab_param/x_obj_to_cam_close', 0.205) # 目标→相机近距离(m) grab_param.x_obj_to_cam_middle = rospy.get_param('/grab_param/x_obj_to_cam_middle', 0.26) # 目标→相机中距离(m) grab_param.x_obj_to_cam_far = rospy.get_param('/grab_param/x_obj_to_cam_far', 0.34) # 目标→相机远距离(m) x_j5_2_claw = rospy.get_param('/grab_param/x_j5_2_claw', 0.085) # 关节5→爪部的x方向距离(m) z_j5_2_claw = rospy.get_param('/grab_param/z_j5_2_claw', 0.0115) # 关节5→爪部的z方向距离(m) # 计算关节5→爪部的直线距离和角度(用于逆运动学计算) grab_param.l_j5_2_claw = np.sqrt(x_j5_2_claw**2 + z_j5_2_claw**2) grab_param.theta_j5_2_claw = np.rad2deg(np.arctan2(z_j5_2_claw, x_j5_2_claw)) # 读取爪部开合角度参数(从参数服务器获取,左右臂独立配置) tight = rospy.get_param('claw_tight', 0) # 爪部夹紧角度(度) loose = rospy.get_param('claw_loose', 70) # 爪部松开角度(度) # 4. 初始化机械臂关节状态消息(6个关节:joint1~joint6) arm_servo_degree = JointState() arm_servo_degree.name = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] # 5. 创建 ROS 话题订阅者(接收数据) image_raw_sub = rospy.Subscriber('/ascamera_hp60c/rgb0/image', Image, image_raw_cb) # 订阅相机图像 cylinder_dets_sub = rospy.Subscriber('/usb_cam/obj_dets_with_pose', ObjDets, cylinder_dets_cb) # 订阅圆柱检测结果 arm_servo_degree_sub = rospy.Subscriber("arm_ik_servo_degree", JointState, arm_ik_servo_degree_cb) # 订阅逆解结果 # 6. 创建 ROS 话题发布者(发送指令) arm_target_pose_pub = rospy.Publisher('arm_target', PoseStamped, queue_size=3) # 发布机械臂目标位姿 arm_servo_degree_pub = rospy.Publisher('arm_joint_states', JointState, queue_size=3) # 发布机械臂关节角度指令 # 7. 初始化 TF 监听器(用于坐标转换,如相机坐标系→机械臂基坐标系) tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) # 8. 启动图像显示线程(后台运行,不阻塞主逻辑) display_thread = threading.Thread(target=t_display, daemon=True) # daemon=True→主线程退出时子线程自动退出 display_thread.start() # 9. 主循环:监听鼠标点击→选择目标→计算位姿→控制机械臂抓取 while not exit_event.is_set() and not rospy.is_shutdown(): try: # 检查鼠标点击队列是否有数据(用户是否点击了图像中的圆柱) if not click_queue.empty(): # 获取鼠标点击坐标(x,y 为像素坐标) x, y = click_queue.get(timeout=0.1) # 安全读取检测结果(加锁) with det_lock: if cylinder_dets is None: # 无检测结果,跳过 continue else: dets_copy: ObjDets = copy.copy(cylinder_dets) # 深拷贝,避免原数据被修改 # 初始化目标中心像素坐标和选中的检测结果 center_pix = [x, y] selected_det = None # 遍历所有检测结果,判断点击坐标是否在某个圆柱的 bounding box 内 for det in dets_copy.dets: det: ObjDet # 条件:点击坐标在当前圆柱的矩形框内(x1≤x≤x2 且 y1≤y≤y2) if (det.bbox.x1 <= x <= det.bbox.x2) and (det.bbox.y1 <= y <= det.bbox.y2): center_pix = [det.bbox.x, det.bbox.y] # 更新为圆柱的中心像素坐标 selected_det = det # 标记当前圆柱为选中目标 break # 找到选中目标,退出循环 # 如果未点击到任何圆柱,打印提示并继续等待 if selected_det is None: print("未点击到任何圆柱体,重新点击...") continue # 打印选中目标的信息(标签+中心像素坐标) print(f"\n选中圆柱体: {selected_det.label} ,中心像素坐标 ({center_pix[0]}, {center_pix[1]})\n") # 机械臂抓取范围检查:左臂只能抓左侧圆柱,右臂只能抓右侧圆柱 if lr: # 左臂 if center_pix[0] < grab_param.sight_middle_w[1]: # 圆柱在左半边视野(符合左臂抓取范围) pass else: print("左臂无法抓取右侧圆柱体,请点击左侧目标!") continue else: # 右臂 if center_pix[0] > grab_param.sight_middle_w[0]: # 圆柱在右半边视野(符合右臂抓取范围) pass else: print("右臂无法抓取左侧圆柱体,请点击右侧目标!") continue # 10. 计算机械臂抓取位姿 # calc_j5_from_claw:自定义函数(utils.py),根据像素坐标计算关节5的目标位姿 pose = calc_j5_from_claw(lr, center_pix, 0, grab_param) # calc_grab:自定义函数(utils.py),将关节5位姿转换为机械臂基坐标系下的目标位姿 pose_in_arm_base = calc_grab(arm_ns, pose, tf_buffer) # 发布机械臂目标位姿(供逆运动学节点计算关节角度) arm_target_pose_pub.publish(pose_in_arm_base) # 等待逆运动学计算结果(超时2秒) t0 = time.time() while not get_cmd_ik and (time.time() - t0 < 2): pass # 循环等待,直到收到逆解结果或超时 # 根据圆柱在图像中的位置,判断抓取优先级(middle/top/bottom) if det.bbox.y <= grab_param.sight_middle_h and grab_param.sight_middle_w[0] <= det.bbox.x <= grab_param.sight_middle_w[1]: position_level = "middle" # 中间区域(优先抓取) elif det.bbox.y <= grab_param.sight_middle_h: position_level = "top" # 上半区域 else: position_level = "bottom" # 下半区域 # 如果收到逆解结果,控制机械臂抓取 if get_cmd_ik: grab_degree = arm_ik_servo_degree.position # 获取逆解计算出的关节角度 get_cmd_ik = False # 复位逆解接收标志 print(f"开始抓取 {selected_det.label},抓取区域:{position_level}\n") # grab:自定义函数(arm_motions.py),控制机械臂按关节角度执行抓取动作 grab(grab_degree, loose, tight, lr, arm_servo_degree, arm_servo_degree_pub, position_level) else: # 没有鼠标点击,短暂休眠(避免占用过多CPU) time.sleep(0.01) except queue.Empty: # 队列空异常(正常情况,继续循环) continue except Exception as e: # 其他异常:打印错误日志并触发退出 rospy.logerr(f"Main thread error: {str(e)}") exit_event.set() # 11. 退出清理:触发退出事件,等待显示线程结束 exit_event.set() display_thread.join(timeout=1.0) # 等待1秒,确保显示线程正常关闭 # ========================== 程序入口 ========================== if __name__ == "__main__": main() # 启动主函数修改后完整代码发我
最新发布
12-02
#!/usr/bin/python3 # -*- coding: utf-8 -*- # 脚本解释器声明:指定使用 Python3 运行,编码格式为 UTF-8(支持中文注释和字符串) # 导入依赖库 import re # 正则表达式库:用于处理 DeepSeek 输出的字符串格式 import ast # 抽象语法树库:用于将字符串转为列表/字典(解析任务指令) import cv2 # OpenCV 库:图像处理(此处用于接收相机图像) import time # 时间库:计时、延时 import copy # 复制库:深拷贝检测结果,避免原数据被修改 import numpy as np # 数值计算库:矩阵运算、三角函数等 from scipy.spatial.transform import Rotation # 姿态转换库:用于旋转矩阵/欧拉角转换 from utils import * # 自定义工具库:包含计数、坐标转换等通用函数(需提前实现) from arm_motions import * # 机械臂运动库:包含抓取(grab)、放置(put)等核心动作函数(需提前实现) # ROS 相关库导入 import rospy # ROS 核心库:节点初始化、话题订阅/发布等 from std_msgs.msg import Bool, String, Int16 # ROS 标准消息类型:布尔、字符串、16位整数 from sensor_msgs.msg import CameraInfo, Image, JointState # 传感器消息:相机参数、图像、关节状态 from cv_bridge import CvBridge # 图像格式转换库:ROS Image ↔ OpenCV 图像 from geometry_msgs.msg import Twist, PoseStamped # 几何消息:速度指令、位姿目标 from user_msgs.msg import CarCmd, ExitCondition, ObjDet, ObjDets # 自定义消息:车辆命令、退出条件、目标检测结果 import tf2_ros # TF 坐标转换库 # ========================== 全局字典定义(数据映射)========================== # 颜色名称 → 编号映射(用于将文字指令转为代码可识别的数字) hostage_dict = { "红": 0, "蓝": 1, } # 编号 → 颜色名称映射(用于日志打印,将数字转回文字) hostage_dict_ = { 0: "红", 1: "蓝", } # 放置位名称 → 编号映射(一号/二号放置位 → 0/1) place_dict = { "一号": 0, "二号": 1, } # 编号 → 放置位名称映射(日志打印用) place_dict_ = { 0: "一号", 1: "二号" } # ========================== 可调参数(提前配置,方便调试)========================== LATERAL_SPEED = 0.20 # 车辆侧向平移速度 (m/s):正值=向左,负值=向右 FORWARD_SPEED = 0.32 # 车辆前进/后退速度(m/s):正值=前进,负值=后退 PTS = 0.2 # 预留参数(未使用,可根据需求扩展) # ============================================================================== FDT1 = 3.6 # 第一段前进持续时间(秒) FDT2 = 1.0 # 预留前进时间参数(未使用) TPS1 = 0.58 # 第一段侧向平移持续时间(秒) TPS2 = 1.0 # 预留平移时间参数(未使用) TPS3 = 0.5 # 车位间平移时间(秒):从一个车位中心到相邻车位中心 # ============================================================================== # ========================== 新增:位置调节参数(自动对齐圆柱用)========================== DX_THRESHOLD = 30 # 左右偏差阈值(像素):小于该值视为对齐 DY_THRESHOLD = 50 # 上下偏差阈值(像素):小于该值视为对齐 DISTANCE_THRESHOLD = 0.05 # 距离偏差阈值(m):小于该值视为距离合适 CAR_ADJUST_SPEED_LAT = 0.08 # 小车微调侧向速度(m/s):比正常平移慢,保证精度 CAR_ADJUST_SPEED_FWD = 0.06 # 小车微调前后速度(m/s):比正常速度慢 ADJUST_TIMEOUT = 8 # 位置调节超时时间(s):避免无限调节 CYLINDER_ACTUAL_DIAMETER = 0.1 # 圆柱实际直径(m):需根据真实物体校准 # ========================== 全局变量定义(跨函数共享数据)========================== img_raw = None # 存储原始相机图像(从相机话题订阅) car_cmd = CarCmd() # 存储车辆当前命令状态(导航/保持/到达等) arrived = False # 导航到达标志位:True=已到达目标点,False=未到达 cylinder_dets = ObjDets() # 存储圆柱目标检测结果(包含目标颜色、位置、尺寸) # 左臂逆运动学计算结果:关节角度 left_arm_ik_servo_degree = JointState() left_arm_get_cmd_ik = False # 左臂逆解结果接收标志:True=已收到有效关节角度 # 右臂逆运动学计算结果:关节角度 right_arm_ik_servo_degree = JointState() right_arm_get_cmd_ik = False # 右臂逆解结果接收标志 success_rescure_count = 0 # 成功救援(抓取+放置)计数:最多救援2个目标 target_cylinder = None # 自动调节时选中的目标圆柱 grab_param_global = None # 全局抓取参数(供调节函数使用) # ========================== 话题回调函数(数据接收)========================== def image_raw_cb(msg:Image): """ 相机图像回调函数:订阅相机原始图像,转换为 OpenCV 格式并存储到全局变量 :param msg: ROS 图像消息(/ascamera_hp60c/rgb0/image 话题) """ global img_raw # 声明使用全局变量 bridge = CvBridge() # 创建图像转换实例 img_raw = bridge.imgmsg_to_cv2(msg, 'bgr8') # ROS Image → OpenCV BGR 图像 def car_cmd_cb(msg:CarCmd): """ 车辆命令回调函数:订阅车辆状态命令,更新全局变量并设置到达标志位 :param msg: 自定义车辆命令消息(/car_cmd 话题) """ global car_cmd, arrived # 逻辑:如果之前未到达,且车辆从“导航/保持”状态切换为“到达”状态 → 置位 arrived if not arrived and (car_cmd.cmd == CarCmd.NAVIGATION or car_cmd.cmd == CarCmd.HOLD) and msg.cmd == CarCmd.ARRIVE: arrived = True # 逻辑:如果已到达,且车辆切换为“导航”状态 → 复位 arrived elif arrived and msg.cmd == CarCmd.NAVIGATION: arrived = False car_cmd = msg # 更新全局车辆命令 def cylinder_dets_cb(msg:ObjDets): """ 圆柱检测结果回调函数:订阅 YOLO 检测到的圆柱目标,存储到全局变量 :param msg: 自定义目标检测消息(/usb_cam/obj_dets_with_pose 话题) """ global cylinder_dets cylinder_dets = msg # 更新全局检测结果 def left_arm_ik_servo_degree_cb(msg:JointState): """ 左臂逆解结果回调函数:订阅左臂逆运动学计算出的关节角度 :param msg: 关节状态消息(/arm_left/arm_ik_servo_degree 话题) """ global left_arm_ik_servo_degree, left_arm_get_cmd_ik left_arm_ik_servo_degree = msg # 更新左臂关节角度 left_arm_get_cmd_ik = True # 置位“已收到逆解结果”标志 def right_arm_ik_servo_degree_cb(msg:JointState): """ 右臂逆解结果回调函数:订阅右臂逆运动学计算出的关节角度 :param msg: 关节状态消息(/arm_right/arm_ik_servo_degree 话题) """ global right_arm_ik_servo_degree, right_arm_get_cmd_ik right_arm_ik_servo_degree = msg # 更新右臂关节角度 right_arm_get_cmd_ik = True # 置位“已收到逆解结果”标志 # ========================== 核心工具函数(车辆控制)========================== def car_lateral_translation(speed, duration, car_cmd_pub, cmd_vel_pub): """ 控制车辆进行侧向平移(左右移动) :param speed: 平移速度 (m/s):正值=向左,负值=向右 :param duration: 平移持续时间 (秒):控制移动距离(距离=速度×时间) :param car_cmd_pub: 车辆命令发布者(发布 /car_cmd 话题) :param cmd_vel_pub: 速度指令发布者(发布 /cmd_vel 话题) """ # 1. 切换车辆为“遥控模式”(只有遥控模式才能接收速度指令) car_cmd = CarCmd() car_cmd.cmd = CarCmd.REMOTE # 连续发布20次(确保车辆控制器稳定接收模式切换指令) for _ in range(20): car_cmd_pub.publish(car_cmd) rospy.sleep(0.01) # 每次发布后延时0.01秒 # 日志打印:提示平移方向、速度、持续时间(便于调试) direction = "左" if speed > 0 else "右" rospy.loginfo(f"开始向 {direction} 平移,速度: {abs(speed):.2f} m/s, 持续时间: {duration:.2f} s...") # 2. 发布侧向平移速度指令(ROS Twist 消息:linear.y 对应侧向运动) twist = Twist() twist.linear.y = speed # 设置侧向速度 start_time = time.time() # 记录开始时间 rate = rospy.Rate(10) # 设定发布频率:10 Hz(每秒发布10次指令,避免指令丢失) # 循环发布指令,直到达到指定持续时间或节点关闭 while time.time() - start_time < duration and not rospy.is_shutdown(): cmd_vel_pub.publish(twist) rate.sleep() # 按设定频率休眠 # 3. 停止移动:发布零速度指令 twist.linear.y = 0 cmd_vel_pub.publish(twist) # 4. 切换车辆为“保持模式”(停止后锁定当前状态,防止误动) car_cmd.cmd = CarCmd.HOLD car_cmd_pub.publish(car_cmd) rospy.loginfo("车辆平移完成。") def car_forward_backward(speed, duration, car_cmd_pub, cmd_vel_pub): """ 控制车辆进行前进或后退 :param speed: 移动速度 (m/s):正值=前进,负值=后退 :param duration: 移动持续时间 (秒):控制移动距离(距离=速度×时间) :param car_cmd_pub: 车辆命令发布者(发布 /car_cmd 话题) :param cmd_vel_pub: 速度指令发布者(发布 /cmd_vel 话题) """ # 1. 切换车辆为“遥控模式”(接收速度指令的前提) car_cmd = CarCmd() car_cmd.cmd = CarCmd.REMOTE # 连续发布20次(确保模式切换成功) for _ in range(20): car_cmd_pub.publish(car_cmd) rospy.sleep(0.01) # 日志打印:提示移动方向、速度、持续时间 direction = "前进" if speed > 0 else "后退" rospy.loginfo(f"开始 {direction},速度: {abs(speed):.2f} m/s, 持续时间: {duration:.2f} s...") # 2. 发布前进/后退速度指令(ROS Twist 消息:linear.x 对应前后运动) twist = Twist() twist.linear.x = speed # 设置前后速度 start_time = time.time() # 记录开始时间 rate = rospy.Rate(10) # 10 Hz 发布频率 # 循环发布指令,直到达到指定时间 while time.time() - start_time < duration and not rospy.is_shutdown(): cmd_vel_pub.publish(twist) rate.sleep() # 3. 停止移动:发布零速度指令 twist.linear.x = 0 cmd_vel_pub.publish(twist) rospy.loginfo("车辆停止。") # 4. 切换回“保持模式”,交还控制权 car_cmd.cmd = CarCmd.HOLD car_cmd_pub.publish(car_cmd) rospy.loginfo("车辆前进/后退动作完成,已切换回保持模式。") # ========================== 新增:位置调节核心函数(自动对齐圆柱)========================== def calc_cylinder_pose(det: ObjDet, grab_param): """ 计算圆柱的像素偏差和距离(基于相机内参) :param det: 单个圆柱检测结果 :param grab_param: 抓取参数(含相机内参) :return: dx(左右偏差), dy(上下偏差), distance(目标到相机距离m) """ # 1. 计算像素偏差(以机械臂最佳抓取区域中心为基准) target_center_x = (grab_param.sight_middle_w[0] + grab_param.sight_middle_w[1]) / 2 # 左右中心(像素) target_center_y = grab_param.sight_middle_h # 上下中心(像素) cylinder_center_x = det.bbox.x # 圆柱实际中心x(像素) cylinder_center_y = det.bbox.y # 圆柱实际中心y(像素) # 偏差定义:dx>0→圆柱在右侧→小车需向左移;dx<0→圆柱在左侧→小车需向右移 dx = cylinder_center_x - target_center_x dy = cylinder_center_y - target_center_y # 2. 计算目标到相机的距离(通过圆柱像素宽度估算) cylinder_pixel_width = det.bbox.x2 - det.bbox.x1 # 圆柱在图像中的宽度(像素) if cylinder_pixel_width < 10: # 避免除数为0(检测异常) distance = grab_param.x_obj_to_cam_middle # 默认中距离 else: # 透视投影公式:distance = (实际直径 × 焦距) / 像素宽度 distance = (CYLINDER_ACTUAL_DIAMETER * grab_param.fx) / cylinder_pixel_width # 可选:如果检测结果带深度信息(ObjDet 有 z 字段),直接使用深度值 # if hasattr(det, 'z') and det.z > 0: # distance = det.z return dx, dy, distance def adjust_car_position(car_cmd_pub, cmd_vel_pub): """ 根据圆柱偏差调节小车位置,使圆柱进入最佳抓取区域 :param car_cmd_pub: 车辆命令发布者 :param cmd_vel_pub: 速度指令发布者 :return: bool:True=调节成功,False=超时/失败 """ global target_cylinder, grab_param_global if target_cylinder is None or grab_param_global is None: rospy.logerr("调节失败:未选中目标圆柱或未初始化抓取参数") return False # 计算初始偏差和距离 dx, dy, distance = calc_cylinder_pose(target_cylinder, grab_param_global) rospy.loginfo(f"开始调节小车位置:dx={dx:.1f}px, dy={dy:.1f}px, 距离={distance:.3f}m") # 1. 切换小车为遥控模式(确保能接收速度指令) car_cmd = CarCmd() car_cmd.cmd = CarCmd.REMOTE for _ in range(10): # 连续发布10次,确保模式切换成功 car_cmd_pub.publish(car_cmd) time.sleep(0.01) start_time = time.time() rate = rospy.Rate(10) # 10Hz 调节频率,确保实时响应 while not rospy.is_shutdown() and (time.time() - start_time < ADJUST_TIMEOUT): # 检查是否已满足抓取条件(所有偏差小于阈值) if (abs(dx) < DX_THRESHOLD and abs(dy) < DY_THRESHOLD and abs(distance - grab_param_global.x_obj_to_cam_middle) < DISTANCE_THRESHOLD): rospy.loginfo("小车位置调节完成,圆柱进入最佳抓取区域!") # 停止小车 twist = Twist() twist.linear.x = 0 twist.linear.y = 0 cmd_vel_pub.publish(twist) # 切换小车为保持模式 car_cmd.cmd = CarCmd.HOLD car_cmd_pub.publish(car_cmd) return True # 计算调节速度(比例控制,避免调节过猛) # 左右调节:dx>0→圆柱在右→小车向左移(正速度);dx<0→向右移(负速度) lat_speed = - (dx / DX_THRESHOLD) * CAR_ADJUST_SPEED_LAT lat_speed = np.clip(lat_speed, -CAR_ADJUST_SPEED_LAT, CAR_ADJUST_SPEED_LAT) # 限制最大速度 # 前后调节:距离过远(>中距离+阈值)→前进;过近→后退 fwd_speed = 0.0 distance_error = distance - grab_param_global.x_obj_to_cam_middle if distance_error > DISTANCE_THRESHOLD: fwd_speed = CAR_ADJUST_SPEED_FWD # 距离过远→前进 elif distance_error < -DISTANCE_THRESHOLD: fwd_speed = -CAR_ADJUST_SPEED_FWD # 距离过近→后退 # 发布小车速度指令 twist = Twist() twist.linear.x = fwd_speed twist.linear.y = lat_speed cmd_vel_pub.publish(twist) # 重新获取当前圆柱的偏差和距离(实时更新) global cylinder_dets with copy.copy(cylinder_dets) as dets: if dets is None or dets.num == 0: rospy.logwarn("调节过程中丢失目标,重新查找...") time.sleep(0.5) continue # 遍历检测结果,找到之前选中的目标(通过class_id和位置匹配) det_found = False for det in dets.dets: if det.class_id == target_cylinder.class_id and abs(det.bbox.x - target_cylinder.bbox.x) < 50: target_cylinder = det # 更新目标信息 dx, dy, distance = calc_cylinder_pose(det, grab_param_global) det_found = True break if not det_found: rospy.logwarn("选中的目标丢失,调节失败") break rate.sleep() # 调节超时或目标丢失,停止小车 twist = Twist() twist.linear.x = 0 twist.linear.y = 0 cmd_vel_pub.publish(twist) car_cmd.cmd = CarCmd.HOLD car_cmd_pub.publish(car_cmd) rospy.logerr(f"小车位置调节失败(超时{ADJUST_TIMEOUT}s或目标丢失)") return False # ========================== 核心工具函数(检测辅助)========================== def get_current_detected_colors(cylinder_dets, timeout=2.0): """ 实时获取当前检测到的圆柱目标颜色(从全局检测结果中提取) :param cylinder_dets: 全局目标检测结果(ObjDets 类型) :param timeout: 超时时间(秒):超过时间未检测到则返回空列表 :return: list:检测到的颜色列表,如 ['红', '蓝'] 或 [] """ start_time = time.time() detected_colors = [] # 存储检测到的颜色 # 循环检测,直到超时或检测到目标 while not rospy.is_shutdown() and (time.time() - start_time < timeout): if cylinder_dets.num > 0: # 如果检测到目标(num=目标数量) detected_colors = [] # 清空之前的结果(避免重复) # 遍历所有检测到的目标 for det in cylinder_dets.dets: det: ObjDet # 类型注解:明确 det 是 ObjDet 类型 # 根据目标的 class_id 映射到颜色名称 if det.class_id in hostage_dict_: color = hostage_dict_[det.class_id] detected_colors.append(color) # 如果检测到颜色,立即返回结果 if detected_colors: rospy.loginfo(f"实时检测到颜色: {detected_colors}") return detected_colors rospy.sleep(0.1) # 每0.1秒检测一次,避免占用过多CPU # 超时或未检测到目标,打印警告并返回空列表 if not detected_colors: rospy.logwarn("未检测到任何物体颜色") return detected_colors # ========================== 主函数(核心任务逻辑)========================== def main(): # 声明使用全局变量(跨函数修改需要声明) global arrived, left_arm_get_cmd_ik, right_arm_get_cmd_ik global success_rescure_count, target_cylinder, grab_param_global global at_target # 新增:修复原代码中 at_target 未定义的问题(假设为Int16()) global car_target # 新增:修复原代码中 car_target 未定义的问题(假设为PoseStamped()) global exit_condition # 新增:修复原代码中 exit_condition 未定义的问题(假设为ExitCondition()) # 初始化未定义的全局变量(原代码遗漏) at_target = Int16() car_target = PoseStamped() exit_condition = ExitCondition() # 1. 初始化 ROS 节点:节点名=mission_manage_node,anonymous=True→自动添加随机后缀(避免节点名冲突) rospy.init_node('mission_manage_node', anonymous=True) # 2. 从 ROS 参数服务器读取配置参数参数在 launch 文件中通过 rosparam 加载) # 2.1 读取导航目标点参数(救援位置、过渡点、放置位置、返回点、停车位置) go_to_grab_num = rospy.get_param('/mission_magage/go_to_grab/wp_num', 0) # 救援位置点数量 go_to_grab_wp = [] # 存储救援位置点列表 for i in range(go_to_grab_num): # 读取每个救援位置点(参数名格式:/mission_magage/go_to_grab/wp_1, wp_2...) go_to_grab_wp.append(rospy.get_param(f'/mission_magage/go_to_grab/wp_{i+1}')) transit_wp = rospy.get_param('/mission_magage/transit_pt/wp_1') # 过渡点(救援→放置的中间导航点) grab_num = rospy.get_param('/mission_magage/grab/wp_num', 0) # 抓取位置点数量 grab_wp = [] # 存储抓取位置点列表 for i in range(grab_num): grab_wp.append(rospy.get_param(f'/mission_magage/grab/wp_{i+1}')) put_num = rospy.get_param('/mission_magage/put/wp_num', 0) # 放置位置点数量 put_wp = [] # 存储放置位置点列表 for i in range(put_num): put_wp.append(rospy.get_param(f'/mission_magage/put/wp_{i+1}')) go_back_wp = rospy.get_param('/mission_magage/go_back/wp_1') # 返回点(放置→停车的中间点) park_num = rospy.get_param('/mission_magage/park/wp_num', 0) # 停车位置点数量 park_wp = [] # 存储停车位置点列表 for i in range(park_num): park_wp.append(rospy.get_param(f'/mission_magage/park/wp_{i+1}')) # 2.2 初始化机械臂抓取参数(左右臂分别配置) grab_param_l = GrabParam() # 左臂抓取参数实例(自定义类,包含相机内参、坐标转换参数等) grab_param_r = GrabParam() # 右臂抓取参数实例 grab_param_global = grab_param_l # 全局抓取参数(默认用左臂参数,可根据臂部切换) # 2.3 读取坐标转换参数(各部件相对车体的位置/姿态) t_larm_2_car = rospy.get_param('/arm_left/translation', [0, 0, 0]) # 左臂→车体的平移向量(x,y,z) t_rarm_2_car = rospy.get_param('/arm_right/translation', [0, 0, 0]) # 右臂→车体的平移向量 t_cam_2_car = rospy.get_param('/ascamera/translation', [0, 0, 0]) # 相机→车体的平移向量 rpy_cam_2_car = rospy.get_param('/ascamera/rotation', [-90, 0, -90]) # 相机→车体的姿态(欧拉角,度) # 2.4 读取抓取相关参数(视觉识别区域、目标距离、机械臂爪部参数) sight_middle_h = rospy.get_param('/grab_param/sight_middle_h', 300) # 视觉识别区域中线(高度方向像素) sight_middle_w = rospy.get_param('/grab_param/sight_middle_w', [269, 457]) # 视觉识别区域左右边界(宽度方向像素) x_obj_to_cam_close = rospy.get_param('/grab_param/x_obj_to_cam_close', 0.205) # 目标→相机的近距离(m) x_obj_to_cam_middle = rospy.get_param('/grab_param/x_obj_to_cam_middle', 0.26) # 目标→相机的中距离(m) x_obj_to_cam_far = rospy.get_param('/grab_param/x_obj_to_cam_far', 0.34) # 目标→相机的远距离(m) x_j5_2_claw = rospy.get_param('/grab_param/x_j5_2_claw', 0.085) # 机械臂关节5→爪部的x方向距离(m) z_j5_2_claw = rospy.get_param('/grab_param/z_j5_2_claw', 0.0115) # 机械臂关节5→爪部的z方向距离(m) l_j5_2_claw = np.sqrt(x_j5_2_claw**2 + z_j5_2_claw**2) # 关节5→爪部的直线距离(m) theta_j5_2_claw = np.rad2deg(np.arctan2(z_j5_2_claw, x_j5_2_claw)) # 关节5→爪部的角度(度) # 2.5 读取机械臂关节角度参数(左右臂独立配置) left_arm_front = rospy.get_param('turn_front', 90) # 左臂向前转动角度(度) left_arm_middle = rospy.get_param('turn_middle', 0) # 左臂中间位置角度(度) left_arm_back = rospy.get_param('turn_back', -90) # 左臂向后转动角度(度) left_arm_tight = rospy.get_param('/arm_left/claw_tight', 30) # 左臂爪部夹紧角度(度) left_arm_loose = rospy.get_param('/arm_left/claw_loose', 60) # 左臂爪部松开角度(度) right_arm_front = rospy.get_param('/arm_right/turn_front', 90) # 右臂向前转动角度(度) right_arm_middle = rospy.get_param('/arm_right/turn_middle', 0) # 右臂中间位置角度(度) right_arm_back = rospy.get_param('/arm_right/turn_back', -90) # 右臂向后转动角度(度) right_arm_tight = rospy.get_param('/arm_right/claw_tight', 30) # 右臂爪部夹紧角度(度) right_arm_loose = rospy.get_param('/arm_right/claw_loose', 60) # 右臂爪部松开角度(度) # 2.6 读取任务时间参数(总任务时间,预留25秒返回时间) mission_time = rospy.get_param('/mission/mission_time', 300) - 25 # 3. 创建 ROS 话题订阅者(接收数据) image_raw_sub = rospy.Subscriber('/ascamera_hp60c/rgb0/image', Image, image_raw_cb) # 订阅相机图像 car_cmd_sub = rospy.Subscriber('/car_cmd', CarCmd, car_cmd_cb) # 订阅车辆命令 yolov5_dets_sub = rospy.Subscriber('/usb_cam/obj_dets_with_pose', ObjDets, cylinder_dets_cb) # 订阅目标检测结果 left_arm_servo_degree_sub = rospy.Subscriber("/arm_left/arm_ik_servo_degree", JointState, left_arm_ik_servo_degree_cb) # 订阅左臂逆解结果 right_arm_servo_degree_sub = rospy.Subscriber("/arm_right/arm_ik_servo_degree", JointState, right_arm_ik_servo_degree_cb) # 订阅右臂逆解结果 # 4. 创建 ROS 话题发布者(发送指令) ds_task_pub = rospy.Publisher('/deepseek/task', Bool, queue_size=1) # 控制 DeepSeek 节点启停 ds_input_pub = rospy.Publisher('/deepseek/input_text', String, queue_size=1) # 向 DeepSeek 发送任务指令 at_target_pub = rospy.Publisher('/at_target_id', Int16, queue_size=1) # 发布 AprilTag 目标ID(已无用,保留兼容) car_target_pub = rospy.Publisher('/car_target', PoseStamped, queue_size=1) # 发布车辆导航目标位姿 exit_condition_pub = rospy.Publisher('exit_condition', ExitCondition, queue_size=1) # 发布任务退出条件 car_cmd_pub = rospy.Publisher('/car_cmd', CarCmd, queue_size=3) # 发布车辆控制命令 cmd_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=3) # 发布车辆速度指令 left_arm_target_pose_pub = rospy.Publisher('/arm_left/arm_target', PoseStamped, queue_size=3) # 发布左臂目标位姿 left_arm_servo_degree_pub = rospy.Publisher('/arm_left/arm_joint_states', JointState, queue_size=3) # 发布左臂关节角度指令 right_arm_target_pose_pub = rospy.Publisher('/arm_right/arm_target', PoseStamped, queue_size=3) # 发布右臂目标位姿 right_arm_servo_degree_pub = rospy.Publisher('/arm_right/arm_joint_states', JointState, queue_size=3) # 发布右臂关节角度指令 # ========================== 任务指令解析(核心逻辑1)========================== # 说明:此处注释了 DeepSeek 交互逻辑,直接手动指定任务指令(仅供调试) t0 = time.time() # 记录任务开始时间 ds_output = "[[蓝色,一号], [红色, 二号]]" # 手动指定任务指令 # 解析任务指令:将字符串转为列表(如 "[[蓝色,一号], [红色,二号]]" → [['蓝色','一号'], ['红色','二号']]) orig_str = ds_output if "色" in orig_str: orig_str = orig_str.replace("色", "") # 去除“色”字(如“蓝色”→“蓝”,适配 hostage_dict) try: mission = ast.literal_eval(orig_str) # 字符串→列表(正常格式直接解析) print(mission) except ValueError: # 格式异常时,用正则表达式给中文添加引号(如 [蓝色,一号] → ["蓝色","一号"]) processed = re.sub(r'(?<=[\[,\s])([\u4e00-\u9fa5]+)(?=[,\]\s])', r'"\1"', orig_str) mission = ast.literal_eval(processed) # 重新解析 print(mission) # 将任务列表中的文字转为编号(如 "蓝色"→1,"一号"→0),便于代码处理 grab_mission = mission print(grab_mission) # 打印转换前的任务列表 for item in grab_mission: if isinstance(item, list) and len(item) == 2: if item[0] in hostage_dict: item[0] = hostage_dict[item[0]] # 颜色文字→编号 if item[1] in place_dict: item[1] = place_dict[item[1]] # 放置位文字→编号 # 提取救援目标颜色和放置位指令(便于后续匹配) hostage_cmd = [grab_mission[i][0] for i in range(2)] # 救援目标颜色编号列表:[1,0](对应蓝色、红色) place_cmd = [grab_mission[i][1] for i in range(2)] # 放置位编号列表:[0,1](对应一号、二号) print("救援放置任务配对:", grab_mission) # 打印最终任务:[[1,0], [0,1]] # 结束 DeepSeek 节点,释放内存(调试模式下无作用,实际使用时启用) ds_task_pub.publish(Bool(False)) # 5. 初始化机械臂抓取参数(从相机参数中获取内参) # 等待相机参数话题(/ascamera_hp60c/rgb0/camera_info),超时则报错 camera_info_msg: CameraInfo = rospy.wait_for_message('/ascamera_hp60c/rgb0/camera_info', CameraInfo, timeout=None) # 给左右臂抓取参数赋值(相机内参:cx/cy=主点坐标,fx/fy=焦距) grab_param_l.cx = camera_info_msg.K[2] grab_param_l.cy = camera_info_msg.K[5] grab_param_l.fx = camera_info_msg.K[0] grab_param_l.fy = camera_info_msg.K[4] grab_param_l.t_arm_2_car = t_larm_2_car grab_param_l.t_cam_2_car = t_cam_2_car grab_param_l.rpy_cam_2_car = rpy_cam_2_car grab_param_l.sight_middle_h = sight_middle_h grab_param_l.sight_middle_w = sight_middle_w grab_param_l.x_obj_to_cam_close = x_obj_to_cam_close grab_param_l.x_obj_to_cam_middle = x_obj_to_cam_middle grab_param_l.x_obj_to_cam_far = x_obj_to_cam_far grab_param_l.l_j5_2_claw = l_j5_2_claw grab_param_l.theta_j5_2_claw = theta_j5_2_claw # 右臂参数与左臂一致(如需差异配置可单独修改) grab_param_r.cx = camera_info_msg.K[2] grab_param_r.cy = camera_info_msg.K[5] grab_param_r.fx = camera_info_msg.K[0] grab_param_r.fy = camera_info_msg.K[4] grab_param_r.t_arm_2_car = t_rarm_2_car grab_param_r.t_cam_2_car = t_cam_2_car grab_param_r.rpy_cam_2_car = rpy_cam_2_car grab_param_r.sight_middle_h = sight_middle_h grab_param_r.sight_middle_w = sight_middle_w grab_param_r.x_obj_to_cam_close = x_obj_to_cam_close grab_param_r.x_obj_to_cam_middle = x_obj_to_cam_middle grab_param_r.x_obj_to_cam_far = x_obj_to_cam_far grab_param_r.l_j5_2_claw = l_j5_2_claw grab_param_r.theta_j5_2_claw = theta_j5_2_claw # 初始化机械臂关节状态消息(6个关节:joint1~joint6) arm_servo_degree = JointState() arm_servo_degree.name = ["joint1", "joint2", "joint3", "joint4", "joint5", "joint6"] # ========================== 初始动作:离开起始区(核心逻辑2)========================== print("\n离开起始区...\n") rospy.loginfo("开始执行前进动作") car_forward_backward(FORWARD_SPEED, FDT1, car_cmd_pub, cmd_vel_pub) # 前进FDT1秒 rospy.loginfo("车辆开始向右平移") car_lateral_translation(-LATERAL_SPEED, TPS1, car_cmd_pub, cmd_vel_pub) # 向右平移TPS1秒(速度为负) rospy.sleep(1) # 延时1秒,稳定车辆状态 # 检测当前视野内的目标颜色,判断救援位置顺序 current_colors = get_current_detected_colors(cylinder_dets) print(f"当前检测到的颜色: {current_colors}") # 逻辑:匹配检测到的颜色与任务指令,确定第一个救援位置 if current_colors and hostage_dict.get(current_colors[0], -1) == hostage_cmd[0]: print(f"{current_colors} 和 {hostage_cmd} 匹配,选择第一个救援位置") flag=True grab_wp_cnt = 0 # 救援位置计数器:0→第一个救援点 else: print(f"{current_colors} 和 {hostage_cmd} 不匹配,选择第二个救援位置") flag=False grab_wp_cnt = 1 # 救援位置计数器:1→第二个救援点 # 如果不匹配,调整车辆位置(向右平移4秒+小幅前进) if flag==False: rospy.loginfo("车辆开始向右平移") car_lateral_translation(-LATERAL_SPEED, 4.0, car_cmd_pub, cmd_vel_pub) rospy.loginfo("开始执行前进动作") car_forward_backward(FORWARD_SPEED, 0.1, car_cmd_pub, cmd_vel_pub) flag=True # ========================== 救援循环:抓取+放置(核心逻辑3)========================== # 初始化 TF 监听器(用于坐标转换,如相机坐标系→车体坐标系) tf_buffer = tf2_ros.Buffer() tf_listener = tf2_ros.TransformListener(tf_buffer) count=0 # 单次救援计数(0→第一个目标,1→第二个目标) # 循环条件:未超时(任务时间)、未救援2个目标、节点未关闭 while not rospy.is_shutdown() and (time.time() - t0 < mission_time) and count<2 and success_rescure_count < 2: # 1. 导航到救援位置 print(f"\n前往{grab_wp_cnt+1}号救援位置...\n") arrived = False # 复位到达标志 # 发布救援位置导航目标(pub_wp 是自定义函数,用于发布导航点) pub_wp(grab_wp[grab_wp_cnt], car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) # 等待车辆到达目标位置 while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break # 2. 检测救援目标(圆柱) print(f"\n到达 {grab_wp_cnt+1}号 救援位置 开始检测\n") t1 = time.time() # 记录检测开始时间 cylinder_dets_ = None # 存储当前救援位置的检测结果 # 等待检测结果(超时2秒未检测到则跳过) while not rospy.is_shutdown() and cylinder_dets_ is None: if time.time() - t1 > 2: rospy.logwarn("检测超时,未发现目标") break if not cylinder_dets.num: # 如果未检测到目标,继续等待 rospy.Rate(1).sleep() continue cylinder_dets_ = copy.copy(cylinder_dets) # 深拷贝检测结果(避免原数据被修改) # 如果未检测到目标,跳过当前救援位置 if cylinder_dets_ is None: print(f"\n{grab_wp_cnt+1}号 救援位置无目标,跳过\n") continue # 3. 新增:自动选择任务指令中的目标圆柱(过滤无关颜色) target_cylinder = None for det in cylinder_dets_.dets: det: ObjDet # 只选择任务指令中指定的目标颜色 if det.class_id in hostage_cmd: target_cylinder = det rospy.loginfo(f"选中目标圆柱:颜色={hostage_dict_[det.class_id]}, 中心坐标=({det.bbox.x:.1f}, {det.bbox.y:.1f})") break if target_cylinder is None: rospy.logwarn("未检测到任务指定颜色的圆柱,跳过当前位置") continue # 4. 新增:自动调节小车位置(对齐圆柱到最佳抓取区域) adjust_success = adjust_car_position(car_cmd_pub, cmd_vel_pub) if not adjust_success: rospy.logwarn("位置调节失败,尝试直接抓取") # 调节失败仍尝试抓取,避免任务中断 pass # 5. 机械臂抓取目标 print("\n准备抓取目标...\n") assign_left = False # 左臂分配标志(未使用,可扩展) assign_right = False # 右臂分配标志(未使用,可扩展) hostage_in_arm = [-1, -1] # 记录臂部抓取结果:[右臂结果, 左臂结果],-1=未抓取 # 遍历所有检测到的目标(实际只处理已选中的 target_cylinder) for det in cylinder_dets_.dets: det: ObjDet # 只处理选中的目标(避免重复抓取) if det.class_id != target_cylinder.class_id or abs(det.bbox.x - target_cylinder.bbox.x) > 50: continue # 计算目标中心在图像中的像素坐标 target_center_pix = [det.bbox.x, det.bbox.y] ########################################## # 逻辑:根据目标在图像中的位置,判断抓取优先级(middle/top/bottom) if det.bbox.y <= sight_middle_h and sight_middle_w[0] <= det.bbox.x <= sight_middle_w[1]: position_level = "middle" # 目标在中间区域(优先抓取) elif det.bbox.y <= sight_middle_h: position_level = "top" # 目标在上半区域 print("目标在上半区域") else: position_level = "bottom" # 目标在下半区域 # 逻辑:根据目标在图像中的左右位置,分配机械臂(左/右) if target_center_pix[0] < sight_middle_w[1]: lr = True # True=左臂 print("分配左臂抓取") arm_servo_degree_pub = left_arm_servo_degree_pub # 左臂关节角度发布者 loose = left_arm_loose # 左臂松开角度 tight = left_arm_tight # 左臂夹紧角度 grab_param_global = grab_param_l # 更新全局抓取参数为左臂 else : lr = False # False=右臂 print("分配右臂抓取") arm_servo_degree_pub = right_arm_servo_degree_pub # 右臂关节角度发布者 loose = right_arm_loose # 右臂松开角度 tight = right_arm_tight # 右臂夹紧角度 grab_param_global = grab_param_r # 更新全局抓取参数为右臂 # 准备抓取:拷贝检测结果,避免被后续回调修改 dets_to_process = copy.copy(cylinder_dets_.dets) init_fruit_count = count_fruits(cylinder_dets_) # 初始目标数量(自定义函数:统计检测到的目标数) print("初始识别到的目标数量:", init_fruit_count) after_fruit_count = init_fruit_count # 抓取后目标数量(初始化为初始数量) grab_degree=[] # 存储抓取时的关节角度(由 grab 函数填充) # 循环抓取:直到抓取成功(目标数量减少)或退出 while(True): # 调用抓取函数(自定义函数:arm_motions.py 中实现,控制机械臂完成抓取动作) grab(grab_degree, loose, tight, lr, arm_servo_degree, arm_servo_degree_pub, position_level) # 抓取后重新读取检测结果,判断是否抓取成功 yolov5_dets_after = copy.copy(cylinder_dets) after_fruit_count = count_fruits(yolov5_dets_after) print("抓取后识别到的目标数量:", after_fruit_count) # 逻辑:如果抓取后目标数量减少 → 抓取成功 if after_fruit_count < init_fruit_count: if lr: hostage_in_arm[0] = det.class_id # 左臂抓取到的目标颜色编号 success_rescure_count += 1 # 成功救援计数+1 count += 1 # 单次循环计数+1 # 切换下一个救援位置(0→1,1→0) if grab_wp_cnt == 1: grab_wp_cnt = 0 elif grab_wp_cnt == 0: grab_wp_cnt = 1 print(f"左臂抓取成功,目标颜色:{hostage_dict_[hostage_in_arm[0]]}") else: hostage_in_arm[1] = det.class_id # 右臂抓取到的目标颜色编号 print(f"右臂抓取成功,目标颜色:{hostage_dict_[hostage_in_arm[1]]}") success_rescure_count += 1 # 成功救援计数+1 count += 1 # 单次循环计数+1 target_cylinder = None # 重置目标,避免重复抓取 break # 抓取成功,退出循环 # 如果抓取失败,重新导航到当前救援位置,再次尝试 print(f"抓取失败,重新前往{grab_wp_cnt+1}号救援位置...\n") arrived = False pub_wp(grab_wp[grab_wp_cnt], car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break ########################################################## # 6. 导航到过渡点(救援→放置的中间点,稳定车辆姿态) print("\n前往中间点...\n") arrived = False pub_wp(transit_wp, car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break # 7. 导航到放置位置并放置目标 # 配置放置参数(根据实际抓取的臂部调整) if hostage_in_arm[0] != -1: # 左臂抓取成功 lr = True loose = left_arm_loose tight = left_arm_tight arm_servo_degree_pub = left_arm_servo_degree_pub elif hostage_in_arm[1] != -1: # 右臂抓取成功 lr = False loose = right_arm_loose tight = right_arm_tight arm_servo_degree_pub = right_arm_servo_degree_pub else: lr = True # 默认左臂 loose = left_arm_loose tight = left_arm_tight arm_servo_degree_pub = left_arm_servo_degree_pub # 根据当前救援计数,确定放置位(count-1→0/1,对应一号/二号放置位) if count > 0: place = place_cmd[count-1] else: place = place_cmd[0] # 默认第一个放置位 print(f"\n前往 {place_dict_[place]} 放置位...\n") # 导航到放置位 arrived = False pub_wp(put_wp[place], car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break # 调用放置函数(自定义函数:arm_motions.py 中实现,控制机械臂松开爪部放置目标) put(1, loose, tight, lr, arm_servo_degree, arm_servo_degree_pub) print(f"\n{place_dict_[place]} 放置位放置完成!\n") # ========================== 任务结束:返回并停车(核心逻辑4)========================== print("\n所有救援任务完成,开始返回...\n") # 车辆掉头(自定义函数:控制车辆旋转190度,准备返回) chassis_turn(190, car_cmd_pub, cmd_vel_pub) rospy.loginfo("开始执行返回前进动作") car_forward_backward(FORWARD_SPEED, FDT1, car_cmd_pub, cmd_vel_pub) # 前进返回 print(f"\n开始停入车位...\n") # 导航到停车位置 arrived = False pub_wp(park_wp[0], car_cmd, at_target, car_target, exit_condition, car_cmd_pub, at_target_pub, exit_condition_pub, car_target_pub) while not rospy.is_shutdown(): if not arrived: time.sleep(1) else: break # 可选:微调停车位置(侧向平移,根据实际车位调整) # rospy.loginfo("车辆开始向右平移微调") # car_lateral_translation(-0.4, 7, car_cmd_pub, cmd_vel_pub) # ========================== 程序入口 ========================== if __name__ == "__main__": main() # 启动主函数
11-30
评论
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值