活动介绍

set min_area_markers [dbGet top.markers.subType "Minimal Area" -p] if {[llength $min_area_markers] == 0} { puts "INFO: No Minimal Area DRC violations found." } else { puts "INFO: Found [llength $min_area_markers] Minimal Area DRC violations." } foreach marker $min_area_markers { set layer [dbGet $marker.layer.name] if {[string match "M3" $layer]} { set box [dbGet $marker.box] lassign $box x1 y1 x2 y2 set current_area [expr ($x2 - $x1) * ($y2 - $y1)] if {$current_area < 0.017} { set patch_side [expr sqrt(0.017)] set patch_llx [expr $x1 - $patch_side/2] set patch_lly [expr $y1 - $patch_side/2] set patch_urx [expr $x2 + $patch_side/2] set patch_ury [expr $y2 + $patch_side/2] setEditMode -type patch -layer M3 -width $patch_side getEditMode -quiet -force_patch -layer set layerNameNoAbbreviation 1 editAddRoute $x1 $y1 editAddRoute $patch_urx $y2 editCommitRoute $patch_urx $y2 } } } 请问上面这段用于 innovus 的脚本哪里有问题?工具为什么会报:missing operator at _@_ in expression "( - 115.755 _@_462.975 115.865 463.02..."

时间: 2025-07-14 15:41:45 浏览: 9
<think>好的,用户遇到了Innovus Tcl脚本中的“missing operator at _@_”错误,需要帮助排查原因。我需要根据提供的引用内容和系统级指令来分析可能的问题点。 首先,这个错误通常与表达式语法有关,尤其是在使用变量或数学运算时。用户提到涉及像box坐标这样的变量,可能是在处理几何数据时出现的。接下来,我需要结合引用中的Tcl命令使用情况来考虑可能的问题。 引用[1]提到Innovus Tcl中数据获取和处理的方法,比如dbGet、get_property,以及字符、列表、集合操作。用户可能在获取数据后进行了处理,而处理过程中可能使用了expr表达式,如果表达式中的运算符或变量使用不当,就会引发错误。例如,变量未正确替换,或者运算符周围缺少空格。 引用[2]中的report_property和get_property用于获取对象属性,如果在获取属性后直接用于计算,而没有正确引用或转换类型,也可能导致问题。例如,属性返回的是字符串而非数值,直接参与运算会出错。 引用[3]中的检查配置可能涉及变量设置,如果坐标值含有特殊字符或格式错误,在expr中使用时会解析失败,导致操作符缺失的错误。 可能的错误点包括: 1. 变量替换问题:使用变量时未用花括号或双引号包裹,导致特殊字符被解析。 2. expr使用不当:在表达式中缺少空格,或者运算符周围有语法错误。 3. 数据类型不匹配:例如字符串参与数学运算而未转换。 4. 括号或引号不匹配:导致表达式被截断,出现意外字符。 5. 坐标格式错误:box坐标可能包含非数字字符,如单位符号,影响解析。 需要检查用户的代码中是否有这些情况,特别是涉及expr和变量替换的地方。建议用户检查相关行的语法,确保变量正确展开,运算符使用正确,并处理可能的数据类型问题。</think>在排查Innovus Tcl脚本中出现的`missing operator at _@_`错误时,需重点关注数学表达式和变量处理的语法问题。以下是逐步排查指南: --- ### **1. 检查expr表达式格式** 当使用`expr`进行数学运算时,必须注意: - **运算符周围必须保留空格**:例如`{$x1 + $x2}`正确,`{$x1+$x2}`可能引发解析错误 - **变量类型需统一**:确保坐标变量(如`box llx`)为数值类型,非字符串 ```tcl # 错误示例(缺少空格) set width [expr $x2-$x1] # 正确示例 set width [expr {$x2 - $x1}] # 注意空格和花括号 ``` --- ### **2. 变量替换与花括号使用** 在坐标计算时(如`dbGet`获取的几何数据): - **优先使用`{}`包裹表达式**防止变量中的特殊字符(如`[ ]`)被解析 - **避免直接拼接字符串变量**,推荐用`list`或格式化命令 ```tcl # 错误示例(未处理变量中的特殊符号) set area [expr $width * $height] # 正确示例(类型检查和花括号隔离) if {[string is double $width] && [string is double $height]} { set area [expr {$width * $height}] } ``` --- ### **3. 坐标值格式验证** 通过`dbGet`获取的几何数据可能包含隐藏符号: - 使用`get_property`确认属性值的类型[^2] ```tcl # 检查坐标是否为数值类型 set llx [get_property $cell llx] if {![string is double $llx]} { puts "Error: Invalid coordinate value $llx" } ``` --- ### **4. 运算符优先级与括号匹配** 复杂表达式需显式定义优先级: ```tcl # 错误示例(运算符优先级混淆) set center [expr $x1 + $x2 / 2] # 正确示例(显式括号) set center [expr {($x1 + $x2) / 2.0}] ``` --- ### **5. 调试技巧** - **分段执行**:将长表达式拆分为多个`set`语句逐步验证 - **打印中间变量**:用`puts "DEBUG: x1=$x1"`检查变量实际值 - **try-catch捕获异常**(需Innovus支持): ```tcl if {[catch {expr {$x1 + $x2}} result]} { puts "Expression error: $result" } ``` --- ### **常见错误场景** | 场景 | 错误示例 | 修复方案 | |------|---------|----------| | 变量含单位 | `set x1 "5um"` | 用`regexp`提取数值部分 | | 缺失运算符 | `expr {$x1 $x2}` | 检查运算符如`+ - * /`是否遗漏 | | 字符串拼接 | `expr "$x1+$x2"` | 改用`[expr {$x1 + $x2}]` | ---
阅读全文

相关推荐

def cell_counter(image, min_area=20): """细胞计数""" # for s in image: df = pd.DataFrame() image =cv2.imread(image) gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY) ret, thresh = cv2.threshold(gray, 100, 255, cv2.THRESH_BINARY + cv2.THRESH_OTSU) kernel = cv2.getStructuringElement(cv2.MORPH_RECT, (3, 3)) opening = cv2.morphologyEx(thresh, cv2.MORPH_OPEN, kernel, iterations=2) distance = ndi.distance_transform_edt(opening) coords = peak_local_max(distance, min_distance=9, footprint=np.ones((7, 7)), labels=opening) mask = np.zeros(distance.shape, dtype=bool) mask[tuple(coords.T)] = True markers, _ = ndi.label(mask) labels = watershed(-distance, markers, mask=opening, watershed_line=True) labels_area = [region.area for region in regionprops(labels) if region.area > min_area] cell_num = len(labels_area) print(cell_num) df = df.append(pd.DataFrame({(file_path,cell_num)}, index=[0]), ignore_index=True) print(df) # return cell_num # df.to_excel('1.xlsx', index=False) if __name__ == '__main__': path = r'D:\0531test' slide_path = os.listdir(path) # df =pd.DataFrame(slide_path) # df.to_excel('1.xlsx',index=False) for i in slide_path: slide_name = os.path.basename(i) #slide_name 样本名称 file_path = os.path.join(path,slide_name) images = os.listdir(file_path) f = glob.glob(os.path.join(file_path, '*.jpg')) for image in f: # print(s) # for s in images: # image_name = os.path.basename(s) # name = image_name.replace('.jpg','') # df = df.append(pd.DataFrame({(file_path,name[:-8])}, index=[0]), ignore_index=True) cell_counter(image) # df.to_excel('1.xlsx',index=False)

“#include "wall_detection.h" // 全局变量定义 tf2_ros::Buffer* tfBuffer = nullptr; ros::Publisher pointcloud_pub; ros::Publisher lines_pub; ros::Publisher normals_pub; ros::Publisher nearest_point_pub; ros::Publisher tf_points_pub; ros::Publisher min_distance_pub; ros::Publisher nearest_distance_pub; //最近距离 // ros::Publisher left_front_nearest_distance_pub; // 左前侧垂直距离 ros::Publisher right_front_nearest_distance_pub; // 右前侧垂直距离 ros::Publisher left_back_nearest_distance_pub; // 左后侧垂直距离 ros::Publisher right_back_nearest_distance_pub; // 右后侧垂直距离 ros::Publisher foot_points_pub;//垂足 // bool isLeftWall(const DetectedLine& wall) { // 计算墙面中点Y坐标,大于0视为左侧墙面 return (wall.start.y + wall.end.y) / 2.0 > 0; } geometry_msgs::Point left_front_ref, left_back_ref; geometry_msgs::Point right_front_ref, right_back_ref; std::string choose_wall ; bool getPointParameters(ros::NodeHandle& nh) { // 为每个点的每个分量设置默认值 double left_front_default_x = 0.0, left_front_default_y = 0.0, left_front_default_z = 0.0; double left_back_default_x = 0.0, left_back_default_y = 0.0, left_back_default_z = 0.0; double right_front_default_x = 0.0, right_front_default_y = 0.0, right_front_default_z = 0.0; double right_back_default_x = 0.0, right_back_default_y = 0.0, right_back_default_z = 0.0; std::string inital_choose = "all"; // 获取左前点参数 nh.param<double>("left_front_ref_x", left_front_ref.x, left_front_default_x); nh.param<double>("left_front_ref_y", left_front_ref.y, left_front_default_y); nh.param<double>("left_front_ref_z", left_front_ref.z, left_front_default_z); // 获取左后点参数 nh.param<double>("left_back_ref_x", left_back_ref.x, left_back_default_x); nh.param<double>("left_back_ref_y", left_back_ref.y, left_back_default_y); nh.param<double>("left_back_ref_z", left_back_ref.z, left_back_default_z); // 获取右前点参数 nh.param<double>("right_front_ref_x", right_front_ref.x, right_front_default_x); nh.param<double>("right_front_ref_y", right_front_ref.y, right_front_default_y); nh.param<double>("right_front_ref_z", right_front_ref.z, right_front_default_z); // 获取右后点参数 nh.param<double>("right_back_ref_x", right_back_ref.x, right_back_default_x); nh.param<double>("right_back_ref_y", right_back_ref.y, right_back_default_y); nh.param<double>("right_back_ref_z", right_back_ref.z, right_back_default_z); nh.param<std::string>("choose_wall",choose_wall, "inital_choose"); // 打印获取的值 ROS_INFO("Left Front Ref: (%.2f, %.2f, %.2f)", left_front_ref.x, left_front_ref.y, left_front_ref.z); ROS_INFO("Left Back Ref: (%.2f, %.2f, %.2f)", left_back_ref.x, left_back_ref.y, left_back_ref.z); ROS_INFO("Right Front Ref: (%.2f, %.2f, %.2f)", right_front_ref.x, right_front_ref.y, right_front_ref.z); ROS_INFO("Right Back Ref: (%.2f, %.2f, %.2f)", right_back_ref.x, right_back_ref.y, right_back_ref.z); ROS_INFO("Choose_wall_type: %s", choose_wall.c_str()); return true; } void setupStaticTFs() { static tf2_ros::StaticTransformBroadcaster static_broadcaster; // 位于激光雷达左前方的tf坐标 geometry_msgs::TransformStamped left_tf; left_tf.header.stamp = ros::Time::now(); //left_tf.header.stamp = ros::Time(0); left_tf.header.frame_id = "base_footprint"; left_tf.child_frame_id = "laser_left_front"; left_tf.transform.translation.x = left_front_ref.x; left_tf.transform.translation.y = left_front_ref.y; left_tf.transform.translation.z = left_front_ref.z; left_tf.transform.rotation.x = 0.0; left_tf.transform.rotation.y = 0.0; left_tf.transform.rotation.z = 0.0; left_tf.transform.rotation.w = 1.0; // 位于激光雷达右前方的tf坐标 geometry_msgs::TransformStamped right_tf; right_tf.header.stamp = ros::Time::now(); //right_tf.header.stamp = ros::Time(0); right_tf.header.frame_id = "base_footprint"; right_tf.child_frame_id = "laser_right_front"; right_tf.transform.translation.x = right_front_ref.x; right_tf.transform.translation.y = right_front_ref.y; right_tf.transform.translation.z = right_front_ref.z; right_tf.transform.rotation.x = 0.0; right_tf.transform.rotation.y = 0.0; right_tf.transform.rotation.z = 0.0; right_tf.transform.rotation.w = 1.0; //左后方TF坐标 geometry_msgs::TransformStamped left_back_tf; left_back_tf.header.stamp = ros::Time::now(); //left_back_tf.header.stamp = ros::Time(0); left_back_tf.header.frame_id = "base_footprint"; left_back_tf.child_frame_id = "laser_left_back"; left_back_tf.transform.translation.x = left_back_ref.x; left_back_tf.transform.translation.y = left_back_ref.y; left_back_tf.transform.translation.z = left_back_ref.z; left_back_tf.transform.rotation.x = 0.0; left_back_tf.transform.rotation.y = 0.0; left_back_tf.transform.rotation.z = 0.0; left_back_tf.transform.rotation.w = 1.0; // 右后方TF坐标 geometry_msgs::TransformStamped right_back_tf; right_back_tf.header.stamp = ros::Time::now(); //right_back_tf.header.stamp = ros::Time(0); right_back_tf.header.frame_id = "base_footprint"; right_back_tf.child_frame_id = "laser_right_back"; right_back_tf.transform.translation.x = right_back_ref.x; right_back_tf.transform.translation.y = right_back_ref.y; right_back_tf.transform.translation.z = right_back_ref.z; right_back_tf.transform.rotation.x = 0.0; right_back_tf.transform.rotation.y = 0.0; right_back_tf.transform.rotation.z = 0.0; right_back_tf.transform.rotation.w = 1.0; static_broadcaster.sendTransform(left_tf); static_broadcaster.sendTransform(right_tf); static_broadcaster.sendTransform(left_back_tf); static_broadcaster.sendTransform(right_back_tf); } double pointToLineDistance(const geometry_msgs::Point& point, const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end) { Eigen::Vector3f pt(point.x, point.y, point.z); Eigen::Vector3f line_vec(line_end.x - line_start.x, line_end.y - line_start.y, line_end.z - line_start.z); Eigen::Vector3f pt_vec(pt.x() - line_start.x, pt.y() - line_start.y, pt.z() - line_start.z); double line_length = line_vec.norm(); if (line_length < 1e-6) { return pt_vec.norm(); } Eigen::Vector3f normalized_line = line_vec / line_length; double projection = pt_vec.dot(normalized_line); // 限制投影在直线范围内 projection = std::max(0.0, std::min(line_length, projection)); Eigen::Vector3f closest_point = Eigen::Vector3f(line_start.x, line_start.y, line_start.z) + projection * normalized_line; return (pt - closest_point).norm(); } // 计算点到直线的垂足点 geometry_msgs::Point pointToLineProjection(const geometry_msgs::Point& point, const geometry_msgs::Point& line_start, const geometry_msgs::Point& line_end) { Eigen::Vector3f pt(point.x, point.y, point.z); Eigen::Vector3f line_vec(line_end.x - line_start.x, line_end.y - line_start.y, line_end.z - line_start.z); Eigen::Vector3f pt_vec(pt.x() - line_start.x, pt.y() - line_start.y, pt.z() - line_start.z); double line_length = line_vec.norm(); if (line_length < 1e-6) { return line_start; // 如果直线长度为零,返回起点 } Eigen::Vector3f normalized_line = line_vec / line_length; double projection = pt_vec.dot(normalized_line); // 限制投影在直线范围内 projection = std::max(0.0, std::min(line_length, projection)); Eigen::Vector3f closest_point = Eigen::Vector3f(line_start.x, line_start.y, line_start.z) + projection * normalized_line; geometry_msgs::Point foot_point; foot_point.x = closest_point.x(); foot_point.y = closest_point.y(); foot_point.z = closest_point.z(); return foot_point; } void publishDetectedLines(const std::vector<DetectedLine>& lines, const std::string& frame_id) { visualization_msgs::Marker line_marker; line_marker.header.frame_id = frame_id; line_marker.header.stamp = ros::Time::now(); line_marker.ns = "detected_line"; line_marker.id = 0; line_marker.type = visualization_msgs::Marker::LINE_LIST; line_marker.pose.orientation.w = 1.0; line_marker.scale.x = 0.15; line_marker.color.r = 0.0; line_marker.color.g = 0.0; line_marker.color.b = 1.0; line_marker.color.a = 1.0; for (const auto& line : lines) { geometry_msgs::Point p1, p2; p1.x = line.start.x; p1.y = line.start.y; p1.z = line.start.z; p2.x = line.end.x; p2.y = line.end.y; p2.z = line.end.z; line_marker.points.push_back(p1); line_marker.points.push_back(p2); } lines_pub.publish(line_marker); visualization_msgs::MarkerArray normal_markers; int id = 0; for (const auto& line : lines) { visualization_msgs::Marker normal_marker; normal_marker.header.frame_id = frame_id; normal_marker.header.stamp = ros::Time::now(); normal_marker.ns = "normals"; normal_marker.id = id++; normal_marker.type = visualization_msgs::Marker::ARROW; normal_marker.action = visualization_msgs::Marker::ADD; normal_marker.pose.orientation.w = 1.0; normal_marker.scale.x = 0.02; normal_marker.scale.y = 0.04; normal_marker.scale.z = 0.0; normal_marker.color.r = 1.0; normal_marker.color.g = 0.0; normal_marker.color.b = 0.0; normal_marker.color.a = 1.0; geometry_msgs::Point mid_point; mid_point.x = (line.start.x + line.end.x) / 2.0; mid_point.y = (line.start.y + line.end.y) / 2.0; mid_point.z = (line.start.z + line.end.z) / 2.0; geometry_msgs::Point normal_end; normal_end.x = mid_point.x + line.normal.normal_x * 0.5; normal_end.y = mid_point.y + line.normal.normal_y * 0.5; normal_end.z = mid_point.z + line.normal.normal_z * 0.5; normal_marker.points.push_back(mid_point); normal_marker.points.push_back(normal_end); normal_markers.markers.push_back(normal_marker); } normals_pub.publish(normal_markers); } void publishNearestPointMarker(const geometry_msgs::Point& point,float distance,const std::string& frame_id, const std::string& ref_name) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "nearest_point" + ref_name; // marker.id = 0; marker.type = visualization_msgs::Marker::SPHERE; marker.action = visualization_msgs::Marker::ADD; marker.pose.position = point; marker.pose.orientation.w = 1.0; marker.scale.x = 0.15; marker.scale.y = 0.15; marker.scale.z = 0.15; marker.color.r = 1.0; marker.color.g = 0.0; marker.color.b = 0.0; marker.color.a = 1.0; marker.lifetime = ros::Duration(0.1); nearest_point_pub.publish(marker); //std_msgs::Float64 dist; //dist.data =(point.y < 0 ? -1.0 : 1.0) *(std::fabs(point.y) - 0.12); //dist.data = std::fabs(point.y); //nearest_distance_pub.publish(dist); //最近距离 std_msgs::Float64 dist_msg; if (ref_name == "left_front") { dist_msg.data = distance; left_front_nearest_distance_pub.publish(dist_msg); } else if (ref_name == "left_back") { dist_msg.data = distance; left_back_nearest_distance_pub.publish(dist_msg); } else if (ref_name == "right_front") { dist_msg.data = distance; right_front_nearest_distance_pub.publish(dist_msg); } else { dist_msg.data = distance; right_back_nearest_distance_pub.publish(dist_msg); } // } /***************************************************************/ void publishFootPointMarker(const geometry_msgs::Point& left_front_foot, //垂足标记 const geometry_msgs::Point& left_back_foot, //垂足标记 const geometry_msgs::Point& right_front_foot, const geometry_msgs::Point& right_back_foot, const std::string& frame_id) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "foot_points"; marker.id = 0; marker.type = visualization_msgs::Marker::POINTS; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.1; // 点的大小 marker.scale.y = 0.1; marker.color.a = 1.0; // 不透明度 // 添加左侧垂足点 - 蓝色 marker.points.push_back(left_front_foot); std_msgs::ColorRGBA color; color.r = 0.0; color.g = 0.0; color.b = 1.0; color.a = 1.0; marker.colors.push_back(color); // 添加右侧垂足点 - 绿色 marker.points.push_back(right_front_foot); color.r = 0.0; color.g = 1.0; color.b = 0.0; color.a = 1.0; marker.colors.push_back(color); // 添加左侧垂足点 - 蓝色 marker.points.push_back(left_back_foot); color.r = 0.0; color.g = 1.0; color.b = 1.0; color.a = 1.0; marker.colors.push_back(color); // 添加右侧垂足点 - 绿色 marker.points.push_back(right_back_foot); color.r = 1.0; color.g = 1.0; color.b = 0.0; color.a = 1.0; marker.colors.push_back(color); foot_points_pub.publish(marker); } /*******************************************************************/ void publishTFPoints(const geometry_msgs::Point& left_front_point, const geometry_msgs::Point& left_back_point, const geometry_msgs::Point& right_front_point, const geometry_msgs::Point& right_back_point, const std::string& frame_id) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "tf_point"; marker.id = 0; marker.type = visualization_msgs::Marker::POINTS; marker.action = visualization_msgs::Marker::ADD; marker.scale.x = 0.15; marker.scale.y = 0.15; marker.color.a = 1.0; // 左侧点 - 蓝色 geometry_msgs::Point p; p = left_front_point; marker.points.push_back(p); std_msgs::ColorRGBA c; c.r = 0.0; c.g = 0.0; c.b = 1.0; c.a = 1.0; marker.colors.push_back(c); // 右侧点 - 绿色 p = right_front_point; marker.points.push_back(p); c.r = 0.0; c.g = 1.0; c.b = 0.0; c.a = 1.0; marker.colors.push_back(c); // 左侧点 - 蓝色 p = left_back_point; marker.points.push_back(p); c.r = 0.0; c.g = 1.0; c.b = 1.0; c.a = 1.0; marker.colors.push_back(c); // 右侧点 - 绿色 p = right_back_point; marker.points.push_back(p); c.r = 1.0; c.g = 1.0; c.b = 0.0; c.a = 1.0; marker.colors.push_back(c); tf_points_pub.publish(marker); } void publishDistanceInfo(const std::string& frame_id, double left_front_dist, double left_back_dist, double right_front_dist, double right_back_dist, const geometry_msgs::Point& wall_point) { visualization_msgs::Marker marker; marker.header.frame_id = frame_id; marker.header.stamp = ros::Time::now(); marker.ns = "distance_info"; marker.id = 0; marker.type = visualization_msgs::Marker::TEXT_VIEW_FACING; marker.action = visualization_msgs::Marker::ADD; marker.pose.position = wall_point; marker.pose.position.z += 0.5; // 在墙上点上方显示 marker.pose.orientation.w = 1.0; marker.scale.z = 0.2; // 文本大小 marker.color.r = 1.0; marker.color.g = 1.0; marker.color.b = 0.0; marker.color.a = 1.0; std::stringstream ss; ss << std::fixed << std::setprecision(2); ss << "Left_front_tf: " << left_front_dist << "m\n"; ss << "Left_back_tf: " << left_back_dist << "m\n"; ss << "Right_front_tf: " << right_front_dist << "m\n"; ss << "Right_back_tf: " << right_back_dist << "m\n"; if (left_front_dist < right_front_dist && left_back_dist < right_back_dist ) { ss << "Left is closer"; } else if (right_front_dist < left_front_dist && right_back_dist < left_back_dist ) { ss << "Right is closer"; } else { ss << "Equal distance"; } marker.text = ss.str(); min_distance_pub.publish(marker); } float min_four(float a ,float b , float c , float d ) { return std::min({a, b, c, d}); } void LidarCallback(const sensor_msgs::LaserScan::ConstPtr& msg) { PointCloudT::Ptr original_cloud(new PointCloudT); std::unordered_map<int, PointData> point_data_map; PointCloudT::Ptr cloud(new PointCloudT); cloud->header.frame_id = msg->header.frame_id; cloud->height = 1; cloud->is_dense = false; float min_distance = std::numeric_limits<float>::max(); int min_index = -1; int point_count = 0; const float min_angle1 = 30 * M_PI/180.0; const float max_angle1 = 150* M_PI/180.0; const float min_angle2 = -150 * M_PI/180.0; const float max_angle2 = -30 * M_PI/180.0; // 为参考点初始化最小距离 float left_front_min_distance = std::numeric_limits<float>::max(); int left_front_min_index = -1; geometry_msgs::Point left_front_nearest_point; float left_back_min_distance = std::numeric_limits<float>::max(); int left_back_min_index = -1; geometry_msgs::Point left_back_nearest_point; // 为参考点初始化最小距离 float right_front_min_distance = std::numeric_limits<float>::max(); int right_front_min_index = -1; geometry_msgs::Point right_front_nearest_point; float right_back_min_distance = std::numeric_limits<float>::max(); int right_back_min_index = -1; geometry_msgs::Point right_back_nearest_point; // 寻找最近点 for (size_t i = 0; i < msg->ranges.size(); ++i) { const float range = msg->ranges[i]; if (std::isnan(range)) continue; if (range < msg->range_min || range > msg->range_max) continue; const float angle = msg -> angle_min + i * msg -> angle_increment; //创建屏蔽条件检测 bool in_blocked_zone = true; float normalized_angle = angle; const float x = range * cos(angle); const float y = range * sin(angle); if (choose_wall == "left") { // 左侧 if (angle >= min_angle1 && angle <= max_angle1) { if ( x > left_back_ref.x && x < left_front_ref.x) { in_blocked_zone= false; } } } else if (choose_wall == "right") { // 右侧 if (angle >= min_angle2 && angle <= max_angle2) { if ( x > right_back_ref.x && x < right_front_ref.x) { in_blocked_zone= false; } } } else { if (angle >= min_angle1 && angle <= max_angle1) { in_blocked_zone= false; } if (angle >= min_angle2 && angle <= max_angle2) { in_blocked_zone= false; } } if (in_blocked_zone) continue; if (range < min_distance) { min_distance = range; min_index = i; } PointT point; point.x = range * cos(angle); point.y = range * sin(angle); point.z = 0.0; point.r = 0; point.g = 255; point.b = 0; PointData data; data.original_index = i; data.is_line_point = false; data.is_nearest = (i == min_index); point_data_map[point_count] = data; cloud->points.push_back(point); original_cloud->points.push_back(point); point_count++; } cloud->width = point_count; // 如果点云为空,直接返回 if (cloud->empty()) { ROS_WARN_THROTTLE(1.0, "No valid points found"); return; } std::vector<DetectedLine> detected_lines; pcl::search::KdTree::Ptr tree(new pcl::search::KdTree); tree->setInputCloud(cloud); // 2. 执行欧几里得聚类 - 确保连续性 std::vector cluster_indices; pcl::EuclideanClusterExtraction ec; ec.setClusterTolerance(0.3); // 点间最大距离阈值(米) ec.setMinClusterSize(15); // 最小聚类点数 ec.setMaxClusterSize(10000); // 最大聚类点数 ec.setSearchMethod(tree); ec.setInputCloud(cloud); ec.extract(cluster_indices); ROS_INFO_THROTTLE(1.0, "Detected %zu point cloud clusters", cluster_indices.size()); std::vector<std::vector<uint8_t>> colors = { {255, 0, 0}, // 红 {0, 255, 0}, // 绿 {0, 0, 255}, // 蓝 {255, 255, 0}, // 黄 {0, 255, 255}, // 青 {255, 0, 255} // 紫 }; // 初始化点云颜色 for (auto& point : cloud->points) { point.r = 0; point.g = 255; point.b = 0; } // 3. 对每个聚类进行直线检测 for (size_t i = 0; i < cluster_indices.size(); i++) { const auto& cluster = cluster_indices[i]; // 创建当前聚类的点云 PointCloudT::Ptr cluster_cloud(new PointCloudT); for (const auto& idx : cluster.indices) { cluster_cloud->push_back((*cloud)[idx]); } // 为当前聚类的点着色 const auto& color = colors[i % colors.size()]; for (const auto& idx : cluster.indices) { cloud->points[idx].r = color[0]; cloud->points[idx].g = color[1]; cloud->points[idx].b = color[2]; } // 跳过点数过少的聚类 if (cluster_cloud->size() < 10) continue; pcl::SACSegmentation seg; pcl::PointIndices::Ptr inliers(new pcl::PointIndices); pcl::ModelCoefficients::Ptr coefficients(new pcl::ModelCoefficients); seg.setOptimizeCoefficients(true); seg.setModelType(pcl::SACMODEL_LINE); seg.setMethodType(pcl::SAC_RANSAC); seg.setMaxIterations(1000); seg.setDistanceThreshold(0.1); // 点到直线的最大距离阈值 seg.setInputCloud(cluster_cloud); seg.segment(*inliers, *coefficients); if (inliers->indices.size() < 10) continue; DetectedLine line; float min_proj = std::numeric_limits<float>::max(); float max_proj = std::numeric_limits<float>::lowest(); Eigen::Vector3f direction(coefficients->values[3], coefficients->values[4], 0.0f); direction.normalize(); // 计算直线的起点和终点 for (const auto& idx : inliers->indices) { Eigen::Vector3f pt( cluster_cloud->points[idx].x, cluster_cloud->points[idx].y, 0.0f ); float proj = pt.dot(direction); if (proj < min_proj) { min_proj = proj; line.start.x = pt.x(); line.start.y = pt.y(); line.start.z = 0.0f; } if (proj > max_proj) { max_proj = proj; line.end.x = pt.x(); line.end.y = pt.y(); line.end.z = 0.0f; } } // 计算法线方向 line.normal.normal_x = -direction.y(); line.normal.normal_y = direction.x(); line.normal.normal_z = 0.0f; line.direction = direction; detected_lines.push_back(line); // 标记直线点 for (const auto& inlier_idx : inliers->indices) { if (inlier_idx >= 0 && inlier_idx < cluster.indices.size()) { int original_idx = cluster.indices[inlier_idx]; if (original_idx >= 0 && original_idx < cloud->size()) { point_data_map[original_idx].is_line_point = true; } } } } // 更新点云颜色 for (int i = 0; i < cloud->size(); i++) { if (point_data_map[i].is_line_point) { cloud->points[i].r = 255; cloud->points[i].g = 255; cloud->points[i].b = 0; } } sensor_msgs::PointCloud2 cloud_msg; pcl::toROSMsg(*cloud, cloud_msg); cloud_msg.header = msg->header; std::vector<DetectedLine> transformed_lines; geometry_msgs::Point left_front_tf_point_map ,left_back_tf_point_map ; geometry_msgs::Point right_front_tf_point_map , right_back_tf_point_map ; bool left_front_tf_valid = false , left_back_tf_valid = false; bool right_front_tf_valid = false , right_back_tf_valid = false; if (tfBuffer) { try { if (!tfBuffer->canTransform("base_footprint", cloud_msg.header.frame_id, cloud_msg.header.stamp, ros::Duration(0.1))) { ROS_WARN_THROTTLE(1.0, "TF transform not available"); return; } /* // 转换点云 sensor_msgs::PointCloud2 transformed_cloud; tfBuffer->transform(cloud_msg, transformed_cloud, "base_footprint"); transformed_cloud.header.stamp = ros::Time::now(); transformed_cloud.header.frame_id = "base_footprint"; pointcloud_pub.publish(transformed_cloud); */ // 转换检测到的直线 if (!detected_lines.empty()) { for (auto& line : detected_lines) { geometry_msgs::PointStamped laser_start, map_start; laser_start.header = msg->header; laser_start.point.x = line.start.x; laser_start.point.y = line.start.y; laser_start.point.z = 0.0f; tfBuffer->transform(laser_start, map_start, "base_footprint"); geometry_msgs::PointStamped laser_end, map_end; laser_end.header = msg->header; laser_end.point.x = line.end.x; laser_end.point.y = line.end.y; laser_end.point.z = 0.0f; tfBuffer->transform(laser_end, map_end, "base_footprint"); geometry_msgs::Vector3Stamped laser_normal, map_normal; laser_normal.header = msg->header; laser_normal.vector.x = line.normal.normal_x; laser_normal.vector.y = line.normal.normal_y; laser_normal.vector.z = 0.0f; tfBuffer->transform(laser_normal, map_normal, "base_footprint"); DetectedLine transformed_line; transformed_line.start.x = map_start.point.x; transformed_line.start.y = map_start.point.y; transformed_line.start.z = map_start.point.z; transformed_line.end.x = map_end.point.x; transformed_line.end.y = map_end.point.y; transformed_line.end.z = map_end.point.z; transformed_line.normal.normal_x = map_normal.vector.x; transformed_line.normal.normal_y = map_normal.vector.y; transformed_line.normal.normal_z = map_normal.vector.z; transformed_lines.push_back(transformed_line); } publishDetectedLines(transformed_lines, "base_footprint"); } // 获取TF参考点在base_footprint中的位置 try { geometry_msgs::PointStamped left_front_tf_laser, left_front_tf_map; left_front_tf_laser.header.frame_id = "laser_left_front"; left_front_tf_laser.header.stamp = ros::Time(0); left_front_tf_laser.point.x = 0; left_front_tf_laser.point.y = 0; left_front_tf_laser.point.z = 0; tfBuffer->transform(left_front_tf_laser, left_front_tf_map, "base_footprint"); left_front_tf_point_map = left_front_tf_map.point; left_front_tf_valid = true; geometry_msgs::PointStamped left_back_tf_laser, left_back_tf_map; left_back_tf_laser.header.frame_id = "laser_left_back"; left_back_tf_laser.header.stamp = ros::Time(0); left_back_tf_laser.point.x = 0; left_back_tf_laser.point.y = 0; left_back_tf_laser.point.z = 0; tfBuffer->transform(left_back_tf_laser, left_back_tf_map, "base_footprint"); left_back_tf_point_map = left_back_tf_map.point; left_back_tf_valid = true; geometry_msgs::PointStamped right_front_tf_laser, right_front_tf_map; right_front_tf_laser.header.frame_id = "laser_right_front"; right_front_tf_laser.header.stamp = ros::Time(0); right_front_tf_laser.point.x = 0; right_front_tf_laser.point.y = 0; right_front_tf_laser.point.z = 0; tfBuffer->transform(right_front_tf_laser, right_front_tf_map, "base_footprint"); right_front_tf_point_map = right_front_tf_map.point; right_front_tf_valid = true; geometry_msgs::PointStamped right_back_tf_laser, right_back_tf_map; right_back_tf_laser.header.frame_id = "laser_right_back"; right_back_tf_laser.header.stamp = ros::Time(0); right_back_tf_laser.point.x = 0; right_back_tf_laser.point.y = 0; right_back_tf_laser.point.z = 0; tfBuffer->transform(right_back_tf_laser, right_back_tf_map, "base_footprint"); right_back_tf_point_map = right_back_tf_map.point; right_back_tf_valid = true; publishTFPoints(left_front_tf_point_map, left_back_tf_point_map, right_front_tf_point_map, right_back_tf_point_map, "base_footprint"); } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what()); } } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what()); } } if ( ! transformed_lines.empty() && left_front_tf_valid && left_back_tf_valid && right_front_tf_valid && right_back_tf_valid) { std::vector<DetectedLine>left_walls; std::vector<DetectedLine>right_walls; for (const auto& wall : transformed_lines) { if (isLeftWall(wall)) { left_walls.push_back(wall); } else { right_walls.push_back(wall); } } double left_front_min_dist = std::numeric_limits<double>::max(); double left_back_min_dist = std::numeric_limits<double>::max(); geometry_msgs::Point left_front_foot , left_back_foot; geometry_msgs::Point left_front_wall_point, left_back_wall_point; for (const auto& wall : left_walls) { geometry_msgs::Point start_point, end_point; start_point.x = wall.start.x; start_point.y = wall.start.y; start_point.z = wall.start.z; end_point.x = wall.end.x; end_point.y = wall.end.y; end_point.z = wall.end.z; double dist_front = pointToLineDistance( left_front_tf_point_map, start_point, end_point); // 计算垂足点 geometry_msgs::Point foot_front = pointToLineProjection( left_front_tf_point_map, start_point, end_point ); if (dist_front < left_front_min_dist) { left_front_min_dist = dist_front; left_front_foot = foot_front; left_front_wall_point = foot_front; } double dist_back = pointToLineDistance( left_back_tf_point_map, start_point, end_point); // 计算垂足点 geometry_msgs::Point foot_back = pointToLineProjection( left_back_tf_point_map, start_point, end_point ); if (dist_back < left_back_min_dist) { left_back_min_dist = dist_back; left_back_foot = foot_back; left_back_wall_point = foot_back; } } //右墙 double right_front_min_dist = std::numeric_limits<double>::max(); double right_back_min_dist = std::numeric_limits<double>::max(); geometry_msgs::Point right_front_foot, right_back_foot; geometry_msgs::Point right_front_wall_point, right_back_wall_point; for (const auto& wall : right_walls) { geometry_msgs::Point start_point, end_point; start_point.x = wall.start.x; start_point.y = wall.start.y; start_point.z = wall.start.z; end_point.x = wall.end.x; end_point.y = wall.end.y; end_point.z = wall.end.z; // 计算右前参考点 double dist_front = pointToLineDistance( right_front_tf_point_map, start_point, end_point ); // 计算垂足点 geometry_msgs::Point foot_front = pointToLineProjection( right_front_tf_point_map, start_point, end_point ); if (dist_front < right_front_min_dist) { right_front_min_dist = dist_front; right_front_foot = foot_front; right_front_wall_point = foot_front; } // 计算右后参考点 double dist_back = pointToLineDistance( right_back_tf_point_map, start_point, end_point ); // 计算垂足点 geometry_msgs::Point foot_back = pointToLineProjection( right_back_tf_point_map, start_point, end_point ); if (dist_back < right_back_min_dist) { right_back_min_dist = dist_back; right_back_foot = foot_back; right_back_wall_point = foot_back; } } try { PointCloudT::Ptr display_cloud(new PointCloudT); Eigen::Vector2f left_vec(left_front_foot.x - left_back_foot.x, left_front_foot.y - left_back_foot.y); float left_length = left_vec.norm(); if (left_length > 0) left_vec.normalize(); // 计算右侧向量 Eigen::Vector2f right_vec(right_front_foot.x - right_back_foot.x, right_front_foot.y - right_back_foot.y); float right_length = right_vec.norm(); if (right_length > 0) right_vec.normalize(); // 处理原始点云 for (size_t i = 0; i < original_cloud->size(); ++i) { const auto& point = original_cloud->points[i]; // 转换到base_footprint坐标系 geometry_msgs::PointStamped point_laser, point_base; point_laser.header.frame_id = msg->header.frame_id; point_laser.header.stamp = ros::Time(0); point_laser.point.x = point.x; point_laser.point.y = point.y; point_laser.point.z = point.z; try { tfBuffer->transform(point_laser, point_base, "base_footprint"); Eigen::Vector2f pt(point_base.point.x, point_base.point.y); bool keep_point = false; // 检查是否在左侧垂足范围内 if (left_length > 0) { Eigen::Vector2f to_left_back( pt.x() - left_back_foot.x , pt.y() - left_back_foot.y ); float proj = to_left_back.dot(left_vec); if (proj >= 0 && proj <= left_length) { keep_point = true; } } // 检查是否在右侧垂足范围内 if (!keep_point && right_length > 0) { Eigen::Vector2f to_right_back(pt.x() - right_back_foot.x , pt.y() - right_back_foot.y ); float proj = to_right_back.dot(right_vec); if (proj >= 0 && proj <= right_length) { keep_point = true; } } if (keep_point) { // 保留原始激光坐标系的点 display_cloud->push_back(point); } } catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "TF transform error: %s", ex.what()); // 转换失败时保留点 display_cloud->push_back(point); } } // 发布点云 sensor_msgs::PointCloud2 cloud_msg; pcl::toROSMsg(*display_cloud, cloud_msg); cloud_msg.header.stamp = ros::Time::now(); cloud_msg.header.frame_id = msg->header.frame_id; pointcloud_pub.publish(cloud_msg); }catch (tf2::TransformException& ex) { ROS_WARN_THROTTLE(1.0, "TF point transform error: %s", ex.what());} std_msgs::Float64 left_front_distance_msg, left_back_distance_msg , right_front_distance_msg , right_back_distance_msg; // 左前距离 double left_front_vertical_dist = (left_front_min_dist == std::numeric_limits<double>::max()) ? 10.0 : left_front_min_dist; left_front_distance_msg.data = left_front_vertical_dist; //left_front_nearest_distance_pub.publish(left_front_distance_msg); // 左后距离 double left_back_vertical_dist = (left_back_min_dist == std::numeric_limits<double>::max()) ? 10.0 : left_back_min_dist; left_back_distance_msg.data = left_back_vertical_dist; //left_back_nearest_distance_pub.publish(left_back_distance_msg); // 右前距离 double right_front_vertical_dist = (right_front_min_dist == std::numeric_limits<double>::max()) ? 10.0 : right_front_min_dist; right_front_distance_msg.data = right_front_vertical_dist; //right_front_nearest_distance_pub.publish(right_front_distance_msg); // 右后距离 double right_back_vertical_dist = (right_back_min_dist == std::numeric_limits<double>::max()) ? 10.0 : right_back_min_dist; right_back_distance_msg.data = right_back_vertical_dist; //right_back_nearest_distance_pub.publish(right_back_distance_msg); // 发布垂足点 publishFootPointMarker(left_front_foot, left_back_foot, right_front_foot, right_back_foot, "base_footprint"); // 发布距离信息 geometry_msgs::Point wall_mid_point; wall_mid_point.x = (left_front_wall_point.x + right_front_wall_point.x) / 2; wall_mid_point.y = (left_front_wall_point.y + right_front_wall_point.y) / 2; wall_mid_point.z = 0; float dist_min = min_four(left_front_vertical_dist,left_back_vertical_dist,right_front_vertical_dist,right_back_vertical_dist); if ( dist_min == left_front_vertical_dist ) { nearest_distance_pub.publish(left_front_distance_msg); } else if ( dist_min == left_back_vertical_dist ) { nearest_distance_pub.publish(left_back_distance_msg); } else if ( dist_min == right_front_vertical_dist ) { nearest_distance_pub.publish(right_front_distance_msg); } else { nearest_distance_pub.publish(right_back_distance_msg); } publishDistanceInfo("base_footprint", left_front_min_distance, left_back_min_distance, right_front_min_distance, right_back_min_distance, wall_mid_point); ROS_INFO_THROTTLE(1.0, "Left Front TF to wall: %.3f m, Left Back TF to wall: %.3f m , Right Front TF to wall: %.3f m, Right Bcak TF to wall: %.3f m", left_front_vertical_dist , left_back_vertical_dist ,right_front_vertical_dist ,right_back_vertical_dist ); } } ”这个代码运行起来,rviz上观察不到较远距离的点云数据但是tf坐标对墙面的垂直距离计算正确

解释一下这段代码if (RB_CA != null && RBTackInfo.Count >= 4 && RBTackInfo[3].TransformStatus) { RB_CA.transform.localPosition = new Vector3(RBTackInfo[3].Ty, -RBTackInfo[3].Tx, -RBTackInfo[3].Tz); RB_CA.transform.localRotation = new Quaternion(-RBTackInfo[3].Qy, RBTackInfo[3].Qx, RBTackInfo[3].Qz, RBTackInfo[3].Q0); // qx,qy,qz,q0 if (RBTackInfo[3].markers[0].State) GameObject.Find("CA-m01").transform.localPosition = new Vector3(RBTackInfo[3].markers[0].Ty, -RBTackInfo[3].markers[0].Tx, -RBTackInfo[3].markers[0].Tz); if (RBTackInfo[3].markers[1].State) GameObject.Find("CA-m02").transform.localPosition = new Vector3(RBTackInfo[3].markers[1].Ty, -RBTackInfo[3].markers[1].Tx, -RBTackInfo[3].markers[1].Tz); if (RBTackInfo[3].markers[2].State) GameObject.Find("CA-m03").transform.localPosition = new Vector3(RBTackInfo[3].markers[2].Ty, -RBTackInfo[3].markers[2].Tx, -RBTackInfo[3].markers[2].Tz); if (RBTackInfo[3].markers[3].State) GameObject.Find("CA-m04").transform.localPosition = new Vector3(RBTackInfo[3].markers[3].Ty, -RBTackInfo[3].markers[3].Tx, -RBTackInfo[3].markers[3].Tz); Debug.Log("RB_CA_locPosition" + RB_CA.transform.localPosition.ToString("f5")); Debug.Log("RB_CA-m01" + GameObject.Find("CA-m01").transform.localPosition.ToString("f5")); Debug.Log("RB_CA-m02" + GameObject.Find("CA-m02").transform.localPosition.ToString("f5")); Debug.Log("RB_CA-m03" + GameObject.Find("CA-m03").transform.localPosition.ToString("f5")); Debug.Log("RB_CA-m04" + GameObject.Find("CA-m04").transform.localPosition.ToString("f5")); }

if (RB_CC != null && RBTackInfo.Count >= 6 && RBTackInfo[5].TransformStatus) { RB_CC.transform.localPosition = new Vector3(RBTackInfo[5].Ty, -RBTackInfo[5].Tx, -RBTackInfo[5].Tz); RB_CC.transform.localRotation = new Quaternion(-RBTackInfo[5].Qy, RBTackInfo[5].Qx, RBTackInfo[5].Qz, RBTackInfo[5].Q0); // qx,qy,qz,q0 if (RBTackInfo[5].markers[0].State) GameObject.Find("CC-m01").transform.localPosition = new Vector3(RBTackInfo[5].markers[0].Ty, -RBTackInfo[5].markers[0].Tx, -RBTackInfo[5].markers[0].Tz); if (RBTackInfo[5].markers[1].State) GameObject.Find("CC-m02").transform.localPosition = new Vector3(RBTackInfo[5].markers[1].Ty, -RBTackInfo[5].markers[1].Tx, -RBTackInfo[5].markers[1].Tz); if (RBTackInfo[5].markers[2].State) GameObject.Find("CC-m03").transform.localPosition = new Vector3(RBTackInfo[5].markers[2].Ty, -RBTackInfo[5].markers[2].Tx, -RBTackInfo[5].markers[2].Tz); if (RBTackInfo[5].markers[3].State) GameObject.Find("CC-m04").transform.localPosition = new Vector3(RBTackInfo[5].markers[3].Ty, -RBTackInfo[5].markers[3].Tx, -RBTackInfo[5].markers[3].Tz); Debug.Log("RB_CC_locPosition" + RB_CC.transform.localPosition.ToString("f5")); Debug.Log("RB_CC-m01" + GameObject.Find("CC-m01").transform.localPosition.ToString("f5")); Debug.Log("RB_CC-m02" + GameObject.Find("CC-m02").transform.localPosition.ToString("f5")); Debug.Log("RB_CC-m03" + GameObject.Find("CC-m03").transform.localPosition.ToString("f5")); Debug.Log("RB_CC-m04" + GameObject.Find("CC-m04").transform.localPosition.ToString("f5")); } } else UnityEngine.Debug.Log("Input Not fulfills the correct format!"); }

#!/usr/bin/env python3 # -*- coding: utf-8 -*- import argparse import tf import threading import rospy import sys import math from sensor_msgs.msg import LaserScan import actionlib from actionlib_msgs.msg import * from geometry_msgs.msg import Pose, PoseWithCovarianceStamped, Point, Quaternion, Twist from std_msgs.msg import Int32 from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal import roslib import tf2_ros from geometry_msgs.msg import TransformStamped import numpy as np from std_msgs.msg import String, Float32MultiArray import ctypes from ctypes.util import find_library import os from playsound import playsound from ar_track_alvar_msgs.msg import AlvarMarkers #from robot_voice.msg import Targettt # 自定义消息类型 key = 0 F_list = [] global xy global task_xy global qtn_list_xy global qtn_list_task_xy global pose_num_xy global pose_num_task_xy global yaw global now_pose_X global now_pose_Y #xy =[[0.16,-0.30,3.14], [3.0,-0.32,0], [3.03,-3.02,-0], [0.01, -2.95, 3.14],[0.5, -2.55, 3.14]] xy = [[0.21,-0.81,3.12],[0.82,-0.28,1.54], [0.821,-0.80,1.491], [2.95,-2.44,0.02],[2.37,-3.15,-1.574]] #task_xy =[[0.17,-1.22,3.14],[1.08,-0.25,3.14],[2.17,-0.27,0], [2.93,-1.14,0], [2.96,-2.15,0],[2.10,-3.04,0],[1.09,-3.0,3.14],[0.23,-2.10,3.14]] task_xy=[[0.821,-0.80,1.491],[2.55,-0.17,1.54], [2.99,-0.85,0.05], [2.95,-2.44,0.02],[2.37,-3.15,-1.574]] pid_stop_xy =[[0.395,-1.06,0],[0.416,-0.856,-90],[-0.99,-0.826,90], [0.52,-0.49,180], [0.52,0.50,180],[-0.54,-0.49,180],[-0.50,0.49,0],[0.51,-0.49,0]] qtn_list_xy = [] #四元数列表 qtn_list_task_xy = [] #任务点四元数列表 pose_num_xy= len(xy) pose_num_task_xy=len(task_xy) yaw = 0 global move_base now_pose_X=0 now_pose_Y=0 # -----------pid函数及其参数------------ # 注: # 1. global w_kp global w_ki global w_kd global w_target global w_e_all global w_last_e w_kp = 2 #2 w_ki = 0.001 w_kd = 0.005 #0 w_e_all = 0 w_last_e = 0 global x_f,x_b,y_l,y_r x_f=0.0 x_b=0.0 y_l=0.0 y_r=0.0 def w_pid_cal(pid_target,dis): global w_kp global w_ki global w_kd global w_e_all global w_last_e e = dis -pid_target #if e>-0.1 and e<0.1: #e = 0 w_e_all = w_e_all+e pid = w_kp*e+w_ki*w_e_all+w_kd*(e-w_last_e) w_last_e = e return pid global p_kp global p_ki global p_kd global p_e_all global p_last_e global p_pid p_kp = -7 p_ki = 0 p_kd = -3 p_e_all = 0 p_last_e = 0 p_pid = 0 def p_pid_cal(pid_target,pose): global p_kp global p_ki global p_kd global p_e_all global p_last_e ture_pose = (pose/3.14159265359*180.0+180.0)%360 if pid_target==0: if ture_pose>0 and ture_pose<180: pid_target=0 if ture_pose>180 and ture_pose<360: pid_target=360 e = ture_pose -pid_target # print(e) p_e_all = p_e_all+e pid = p_kp*e+p_ki*p_e_all+p_kd*(e-p_last_e) #rospy.loginfo("e %f",e) p_last_e = e return pid global point_kp global point_ki global point_kd global point_e_all global point_last_e global point_pid point_kp = -3 point_ki = 0 point_kd = 0 point_e_all = 0 point_last_e = 0 point_pid = 0 def point_pid(pid_target_x,ture): global point_kp global point_ki global point_kd global point_e_all global point_last_e e = ture -pid_target_x point_e_all = point_e_all+e pid = point_kp*e+point_ki*point_e_all+point_kd*(e-point_last_e) point_last_e = e return pid def limt(limt,target): if limt>target: limt=target if limt<-target: limt=-target return limt # ----------pid停车------- def pid_stop2(target_x,target_y,target_yaw): global w_kp,w_ki,w_kd,w_e_all w_kp = 1.5 #1.5 w_ki = 0.005 #0.005 w_kd = 0.006 #0.01 w_e_all=0 count =0 pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7) rate_loop_pid=rospy.Rate(7) speed = Twist() wich_x = 0 wich_y = 0 time = 0 while not rospy.is_shutdown(): rate_loop_pid.sleep() time+=1 if target_x>0: pid_x = w_pid_cal(target_x,x_f) wich_x=x_f if target_x<0: pid_x = w_pid_cal(target_x,-x_b) wich_x=-x_b if target_y>0: pid_y = w_pid_cal(target_y,y_l) wich_y = y_l if target_y<0: pid_y = w_pid_cal(target_y,-y_r) wich_y = -y_r p_pid = p_pid_cal(target_yaw,yaw) #if abs(wich_x)<0.8: #speed.linear.y = 0 #else: speed.linear.y = pid_y speed.linear.x = pid_x speed.angular.z = p_pid/180.0*3.14159265359 w_e_all=limt(w_e_all,5) print("w_e_all:",w_e_all) print("wich_x:",wich_x,"wich_y:",wich_y) if time>=150: w_e_all=0 break #if abs(wich_x-target_x)<=0.05 and abs(wich_y-target_y)<=0.07 and abs(target_yaw-(yaw/3.1415926*180+180))<=5: #w_e_all=5 #w_ki = 0.001 if abs(wich_x-target_x)<=0.03 and abs(wich_y-target_y)<=0.03 and abs(target_yaw-(yaw/3.1415926*180+180))<=3: w_e_all=0 count+=1 if count>=6: speed.linear.x = 0 speed.linear.y = 0 speed.linear.z = 0 pid_vel_pub.publish(speed) #rospy.sleep(0.5) w_e_all=0 break pid_vel_pub.publish(speed) def pid_stop(target_x,target_y,target_yaw): global w_kp,w_ki,w_kd,w_e_all w_kp = 1.5 #1.5 w_ki = 0.005 #0.005 w_kd = 0.006 #0.01 w_e_all=0 count =0 pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7) rate_loop_pid=rospy.Rate(7) speed = Twist() wich_x = 0 wich_y = 0 time = 0 while not rospy.is_shutdown(): rate_loop_pid.sleep() time+=1 if target_x>0: pid_x = w_pid_cal(target_x,x_f) wich_x=x_f if target_x<0: pid_x = w_pid_cal(target_x,-x_b) wich_x=-x_b if target_y>0: pid_y = w_pid_cal(target_y,y_l) wich_y = y_l if target_y<0: pid_y = w_pid_cal(target_y,-y_r) wich_y = -y_r p_pid = p_pid_cal(target_yaw,yaw) #if abs(wich_x)<0.8: #speed.linear.y = 0 #else: speed.linear.y = pid_y speed.linear.x = pid_x speed.angular.z = p_pid/180.0*3.14159265359 w_e_all=limt(w_e_all,5) print("w_e_all:",w_e_all) print("wich_x:",wich_x,"wich_y:",wich_y) if time>=150: w_e_all=0 break #if abs(wich_x-target_x)<=0.05 and abs(wich_y-target_y)<=0.07 and abs(target_yaw-(yaw/3.1415926*180+180))<=5: #w_e_all=5 #w_ki = 0.001 if abs(wich_x-target_x)<=0.08 and abs(wich_y-target_y)<=0.08 and abs(target_yaw-(yaw/3.1415926*180+180))<=5: w_e_all=0 count+=1 if count>=3: speed.linear.x = 0 speed.linear.y = 0 speed.linear.z = 0 pid_vel_pub.publish(speed) #rospy.sleep(0.5) w_e_all=0 break pid_vel_pub.publish(speed) def pid_go(target_x,target_y,target_yaw): global point_kp, vision_result, dis_trun_off,point_ki global w_kp,w_ki,w_kd w_kp = 2 #2 w_ki = 0 w_kd = 0.008#0 count =0 pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7) rate_loop_pid=rospy.Rate(7) speed = Twist() wich_x = 0 wich_y = 0 while not rospy.is_shutdown(): if target_x>0: pid_x = w_pid_cal(target_x,x_f) wich_x=x_f if target_x<0: pid_x = w_pid_cal(target_x,-x_b) wich_x=-x_b if target_y>0: pid_y = w_pid_cal(target_y,y_l) wich_y = y_l if target_y<0: pid_y = w_pid_cal(target_y,-y_r) wich_y = -y_r p_pid = p_pid_cal(target_yaw,yaw) if abs(target_x-wich_x)<0.2 and abs(target_y-wich_y)<0.2: speed.linear.x = 0 speed.linear.y = 0 else: speed.linear.y = pid_y speed.linear.x = pid_x speed.angular.z = p_pid/180.0*3.14159265359 pid_vel_pub.publish(speed) rate_loop_pid.sleep() print("x_f:",x_f,"y_l:",y_l) #print(abs(target_yaw-yaw/3.1415926*180)) if vision_result != 0: # rospy.sleep(0.3) # thread_dis.join() break def pid_go2(target_x,target_y,target_yaw): global vision_result global w_kp,w_ki,w_kd,w_e_all print("--------开始pid_go2--------") w_kp = 2 #2 w_ki = 0 w_kd = 0.01 #0 count =0 w_e_all=0 pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7) rate_loop_pid=rospy.Rate(7) speed = Twist() wich_x = 0 wich_y = 0 while not rospy.is_shutdown(): if target_x>0: pid_x = w_pid_cal(target_x,x_f) wich_x=x_f if target_x<0: pid_x = w_pid_cal(target_x,-x_b) wich_x=-x_b if target_y>0: pid_y = w_pid_cal(target_y,y_l) wich_y = y_l if target_y<0: pid_y = w_pid_cal(target_y,-y_r) wich_y = -y_r p_pid = p_pid_cal(target_yaw,yaw) if abs(target_x-wich_x)<0.2 and abs(target_y-wich_y)<0.2: speed.linear.x = 0 speed.linear.y = 0 else: if abs(wich_x)>0.55: speed.linear.y = 0.03*pid_y else: speed.linear.y = pid_y speed.linear.x = pid_x speed.angular.z = p_pid/180.0*3.14159265359 pid_vel_pub.publish(speed) rate_loop_pid.sleep() if vision_result != 0: w_e_all=0 # rospy.sleep(0.3) # thread_dis.join() break print("--------结束pid_go2--------") def pid_turn(target_x,target_y,target_yaw): global point_kp pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7) rate_loop_pid=rospy.Rate(7.2) speed = Twist() while not rospy.is_shutdown(): map_pid_x = point_pid(target_x,now_pose_X) map_pid_y = point_pid(target_y,now_pose_Y) p_pid = p_pid_cal(target_yaw,yaw) if target_yaw!=180: speed.linear.x = 0 speed.linear.y = 0 else: speed.linear.x = 0 speed.linear.y = 0 speed.angular.z = 1.5*p_pid/180.0*3.14159265359 pid_vel_pub.publish(speed) rate_loop_pid.sleep() # print(abs(target_yaw-yaw/3.1415926*180)) if abs(target_yaw-(yaw/3.1415926*180+180))<=50: #rospy.sleep(0.3) break # -----------pid函数及其参数------------ # -----------获取视觉消息--------------- global object_vision object_vision = 0 def vision_callback(msg): global object_vision if msg.data: print(msg.data) object_vision = msg.data def vision_listen(): rospy.Subscriber('/detected_label',Int32,vision_callback,queue_size=10) #订阅视觉话题 rospy.spin() global ar_sign global task_vision ar_sign = 0 def ar_callback(msg): global object_vision , task_vision ar_sign = 0 if len(msg.markers): print("----------------------------------ar") print(msg.markers[0].id) task_vision = msg.markers[0].id ar_sign = 1 def vision_ar(): rospy.Subscriber('/ar_pose_marker',AlvarMarkers,ar_callback,queue_size=10) #订阅视觉话题 rospy.spin() global ai_vision ai_vision = 0 global ai_sign ai_sign = 0 def ai_callback(msg): global object_vision , task_vision if (msg.data): print("++++++++++++++++++++++++++++++++++++ai") print("ai回复",msg.data) task_vision = msg.data ai_sign = 1 def vision_ai(): rospy.Subscriber('/ai_result',Int32,ai_callback,queue_size=10) #订阅视觉话题 rospy.spin() # -----------获取视觉消息--------------- #------------------- 雷达话题订阅线程 ---------------- global scan_data scan_data = [] def get_valid_distance(scan, start_index, direction, angle_resolution): """ 从指定的起始角度开始,以给定的方向和角度分辨率寻找有效的激光数据, 并计算修正后的距离。 参数: - scan: 激光雷达扫描数据 - start_index: 起始角度索引 - direction: 搜索方向(1 表示顺时针,-1 表示逆时针) - angle_resolution: 角度分辨率(每个索引代表的角度) 返回值: - 修正后的有效距离 """ max_angle = len(scan.ranges) for i in range(max_angle): index = (start_index + i * direction) % max_angle if scan.ranges[index] != float('inf'): distance = scan.ranges[index] angle = np.radians((index - start_index) * angle_resolution) distance_corrected = distance * np.cos(angle) return distance_corrected return float('inf') def get_laserscan(scan): """ 激光雷达回调函数,计算前后左右方向的有效激光距离并进行修正。 参数: - scan: 激光雷达扫描数据 """ global x_f, x_b, y_l, y_r, yaw, scan_data scan_data = scan.ranges front_index = 360 # 前方角度索引 angle_resolution = 0.5 # 每个索引代表的角度(假设为0.5度) # 找到有效距离并进行修正 x_f = get_valid_distance(scan, front_index, 1, angle_resolution) # 从前方开始向右搜索 x_b = get_valid_distance(scan, 0, 1, angle_resolution) # 从后方开始向右搜索 y_l = get_valid_distance(scan, 540, -1, angle_resolution) # 从左侧开始向左搜索 y_r = get_valid_distance(scan, 180, 1, angle_resolution) # 从右侧开始向右搜索 #print("x_f:", x_f, "x_b:", x_b, "y_l:", y_l, "y_r:", y_r) def laser_listen(): rospy.Subscriber('/scan_filtered', LaserScan,get_laserscan,queue_size=7) rospy.spin() #------------------- 雷达话题订阅线程 ---------------- # -----------------------pid回家-------------------------- def pid_loop(): print("---------启动回家--------") global yaw global w_kp,w_ki,w_kd,w_e_all w_e_all=0 w_kp = 0.8 #2 w_ki = 0 w_kd = 0.00 #0 pid_vel_pub = rospy.Publisher('/cmd_vel', Twist, queue_size=7) rate_loop_pid=rospy.Rate(7) speed = Twist() while not rospy.is_shutdown(): pid_x = w_pid_cal(0.155,x_f) #pid_y = w_pid_cal(-0.35,-y_r) pid_y = w_pid_cal(0.155,y_l) p_pid = p_pid_cal(0,yaw) print("x_f",x_f) print("y_l",y_l) print("yaw",yaw) speed.linear.x = pid_x speed.linear.y = pid_y speed.angular.z = p_pid/180.0*3.14159265359 pid_vel_pub.publish(speed) rate_loop_pid.sleep() if abs(x_f-0.1)<0.18 and abs(y_l-0.1)<0.18 and abs(0-(yaw/3.1415926*180+180))<=5: speed.linear.x = 0 speed.linear.y = 0 pid_vel_pub.publish(speed) break print("---------结束回家--------") # -----------------------pid回家-------------------------- # ---------------vioce 语音播报----------------- def play_voice(number): playsound(str(number)+".mp3") def play_voice_begin(numbers): print("获取的任务目标为:"+str(target_point_arr)) numbers=sorted(numbers) playsound(str(numbers[0])+"_"+str(numbers[1])+"_"+str(numbers[2])+".mp3") print("播放文件为:"+str(numbers[0])+"_"+str(numbers[1])+"_"+str(numbers[2])+".mp3") # ---------------vioce 语音播报----------------- # -----------------导航点---------------------- # 1.本区域共三个函数导航实时距离获取,发布导航点,控制导航点发布 # 2.控制导航点发布部分默认是逐一发布点列表中的点,需要请注释并自行更改 # 发布目标点 # 控制导航点发布 global count_dis_times count_dis_times=0 def dis_func(x, y,min_dis): global dis_trun_off,now_pose_X,now_pose_Y,distance, vision_result,target_point_arr,times_voice_play,count_dis_times print("dis_func函数已启动:"+str(count_dis_times)+"次") count_dis_times+=1 # lock = threading.RLock() dis_fun_rate=rospy.Rate(10) while not rospy.is_shutdown(): dis_fun_rate.sleep() car_to_map_x=now_pose_X car_to_map_y=now_pose_Y # 计算小车到目标点的距离 distance = pow(pow( car_to_map_x - x, 2) + pow(car_to_map_y - y, 2), 0.5) #print("distance:"+str(distance)) if distance<min_dis: print("reach_goal") rospy.sleep(1) break def goals(x, y, i): min_dis=0.07 global qtn_list_xy,move_base,dis_trun_off,now_pose_X,pid_stop_vision_stop_flag,goal_vision_weight_flag target_point = Pose(Point(x, y, 0), Quaternion(qtn_list_xy[i][0], qtn_list_xy[i][1], qtn_list_xy[i][2], qtn_list_xy[i][3])) goal = MoveBaseGoal() goal.target_pose.pose = target_point goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() rospy.loginfo("Going to: " + str(target_point)) print("goal") move_base.send_goal(goal) def goals_task(x, y, i): min_dis=0.07 global qtn_list_task_xy,move_base,dis_trun_off,now_pose_X target_point = Pose(Point(x, y, 0), Quaternion(qtn_list_task_xy[i][0], qtn_list_task_xy[i][1], qtn_list_task_xy[i][2], qtn_list_task_xy[i][3])) goal = MoveBaseGoal() goal.target_pose.pose = target_point goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() rospy.loginfo("Going to: " + str(target_point)) print("goal") move_base.send_goal(goal) def send_None_goal(): global move_base goal = MoveBaseGoal() move_base.send_goal(goal) # ---------------vioce 语音播报----------------- global times_voice_play times_voice_play=0 def play_voice(number): global times_voice_play playsound(str(number)+".mp3") # ---------------vioce 语音播报----------------- # ----------------- init --------------------- # 1.处理点列表的四元数并放在新的列表 # 2.连接move_base # -------------- def now_pose_xy(): global now_pose_X,now_pose_Y,yaw now_pose=rospy.Rate(10) listener = tf.TransformListener() while not rospy.is_shutdown(): now_pose.sleep() try: (trans, rot) = listener.lookupTransform("map", "base_link", rospy.Time(0)) # 小车坐标 now_pose_X=trans[0] now_pose_Y=trans[1] euler = tf.transformations.euler_from_quaternion(rot) yaw = euler[2] # 第三个元素是yaw角 # print("x",now_pose_X,"y",now_pose_Y,"yaw",yaw) except Exception as e: print(".........") # -------------- def init_fun(): #转换点 global qtn_list_xy,qtn_list_task_xy,pose_num_xy,pose_num_task_xy,move_base for i in range(pose_num_xy): qtn = tf.transformations.quaternion_from_euler(0,0,xy[i][2]) qtn_list_xy.append(qtn) i=0 for i in range(pose_num_task_xy): qtn = tf.transformations.quaternion_from_euler(0,0,task_xy[i][2]) qtn_list_task_xy.append(qtn) #连接move_base—actition move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) rospy.loginfo("Waiting for move_base action server...") while move_base.wait_for_server(rospy.Duration(5.0)) == 0: rospy.loginfo("Request to connect to move_base server") rospy.loginfo("Be connected successfully") thread_lidar = threading.Thread(target=laser_listen) thread_lidar.start() thread_vision = threading.Thread(target=vision_listen) thread_vision.start() thread_ar = threading.Thread(target=vision_ar) thread_ar.start() thread_ai = threading.Thread(target=vision_ai) thread_ai.start() thread_now_pose = threading.Thread(target=now_pose_xy) thread_now_pose.start() # 初始化 history 变量 history = {"current_value": None, "stable_count": 0, "values": []} required_stable_count = 20 def get_stable_value(new_value, required_stable_count, history, max_history_size=50): # 更新历史记录列表,保留最近的 max_history_size 个值 history["values"].append(new_value) if len(history["values"]) > max_history_size: history["values"].pop(0) # 超过最大限制时移除最早的值 # 检查稳定计数 if new_value == history["current_value"]: history["stable_count"] += 1 else: history["current_value"] = new_value history["stable_count"] = 1 # 如果达到所需的稳定次数,返回稳定的值 if history["stable_count"] >= required_stable_count: return history["current_value"] else: return None def vision_to_get_task(t_long): global object_vision , ar_sign , task_vision,ai_sign task_vision = 0 object_vision = 0 ar_sign=0 ai_sign = 0 t = 0 sure_required = 0 rate_loop_pid=rospy.Rate(10) rospy.set_param('/top_view_shot_node/im_flag', 1) rospy.sleep(3) while not rospy.is_shutdown(): print("find.............",object_vision) if object_vision and not task_vision: sure_required = get_stable_value(object_vision, 50, history) if sure_required is not None: print("sure_required",sure_required) task_vision = sure_required t+=1 if t>=t_long: print("超时") task_vision=999 break if task_vision: print("========",task_vision) oject_vision = 0 break rate_loop_pid.sleep() return task_vision def get_voice(voice): global target_2 global target_3 if (target_2==0 or target_3==0): target_2 = voice.er target_3 = voice.san rospy.loginfo("收到/target话题的反馈: %d %d",target_2, target_3) # 访问自定义消息的字段 def robot_voice(): rospy.Subscriber('/target',Targettt,get_voice,queue_size=1) rospy.spin() def voice_wakeup_publisher(): pub = rospy.Publisher('/voiceWakeup', String, queue_size=10) user_input = input("请输入1开始启动: ") if user_input == "1": msg = String() msg.data = "1" pub.publish(msg) rospy.loginfo("已发布消息: %s", msg.data) # ----------------- init --------------------- if __name__ == '__main__': # 初始化节点 rospy.init_node('move_test', anonymous=True) init_fun() a = input() rospy.loginfo("节点初始化完成,开始执行任务") # 发布唤醒信号 #voice_wakeup_publisher() goals(xy[0][0], xy[0][1], 0) dis_func(xy[0][0], xy[0][1], 0.05) send_None_goal() pid_stop2(pid_stop_xy[0][0], pid_stop_xy[0][1], pid_stop_xy[0][2]) rospy.sleep(5) '''goals(xy[1][0], xy[1][1], 1) dis_func(xy[0][0], xy[0][1], 0.05) send_None_goal() pid_stop2(pid_stop_xy[1][0], pid_stop_xy[1][1], pid_stop_xy[1][2]) #rospy.loginfo("任务2完成") rospy.sleep(5)''' '''goals_task(task_xy[0][0], task_xy[0][1], 0) dis_func(task_xy[0][0], task_xy[0][1], 0.05) # 等待到达目标点附近 send_None_goal() pid_stop2(pid_stop_xy[2][0], pid_stop_xy[2][1], pid_stop_xy[2][2]) rospy.sleep(4)''' rospy.loginfo("任务序列执行完成") 我完整代码是这个应该怎么修改

#!/usr/bin/env python #coding: utf-8 import rospy from geometry_msgs.msg import Point import threading import actionlib import time from actionlib_msgs.msg import GoalStatus from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal from geometry_msgs.msg import PoseWithCovarianceStamped from tf_conversions import transformations from ar_track_alvar_msgs.msg import AlvarMarkers from math import pi from std_msgs.msg import String import sys reload(sys) sys.setdefaultencoding('utf-8') class Navigation: def __init__(self): self.set_pose_pub = rospy.Publisher('/initialpose', PoseWithCovarianceStamped, queue_size=5) self.move_base = actionlib.SimpleActionClient("move_base", MoveBaseAction) self.move_base.wait_for_server(rospy.Duration(60)) def set_pose(self, p): if self.move_base is None: return False x, y, th = p pose = PoseWithCovarianceStamped() pose.header.stamp = rospy.Time.now() pose.header.frame_id = 'map' pose.pose.pose.position.x = x pose.pose.pose.position.y = y q = transformations.quaternion_from_euler(0.0, 0.0, th/180.0*pi) pose.pose.pose.orientation.x = q[0] pose.pose.pose.orientation.y = q[1] pose.pose.pose.orientation.z = q[2] pose.pose.pose.orientation.w = q[3] self.set_pose_pub.publish(pose) return True def _feedback_cb(self, feedback): msg = feedback #rospy.loginfo("[Navi] navigation feedback\r\n%s"%feedback) def goto(self, p): rospy.loginfo("[Navigation] goto %s"%p) goal = MoveBaseGoal() goal.target_pose.header.frame_id = 'map' goal.target_pose.header.stamp = rospy.Time.now() goal.target_pose.pose.position.x = p[0] goal.target_pose.pose.position.y = p[1] q = transformations.quaternion_from_euler(0.0, 0.0, p[2]/180.0*pi) goal.target_pose.pose.orientation.x = q[0] goal.target_pose.pose.orientation.y = q[1] goal.target_pose.pose.orientation.z = q[2] goal.target_pose.pose.orientation.w = q[3] self.move_base.send_goal(goal, self._done_cb, self._active_cb, self._feedback_cb) result = self.move_base.wait_for_result(rospy.Duration(60)) if not result: self.move_base.cancel_goal() rospy.loginfo("Timed out achieving goal") else: state = self.move_base.get_state() if state == GoalStatus.SUCCEEDED: rospy.loginfo("reach goal %s succeeded!"%p) return True def _done_cb(self, status, result): rospy.loginfo("navigation done! status:%d result:%s"%(status, result)) def _active_cb(self): rospy.loginfo("[Navi] navigation has be actived") def cancel(self): self.move_base.cancel_all_goals() return True class ARTracker: def __init__(self): self.ar_sub = rospy.Subscriber('/ar_pose_marker', AlvarMarkers, self.ar_cb) def ar_cb(self,data): global tag_id for marker in data.markers: tag_id = marker.id class Object_position: def __init__(self): self.ar_sub = rospy.Subscriber('/object_position', Point, self.find_cb) self.tts_pub = rospy.Publisher('/voiceWords', String, queue_size=10) def find_cb(self,data): global find_id point_msg = data if(point_msg.z>=1 and point_msg.z<=5): find_id = 1 self.tts_pub.publish(str(find_id)) elif(point_msg.z>=9 and point_msg.z<=15): find_id = 2 self.tts_pub.publish(str(find_id)) elif(point_msg.z>=16 and point_msg.z<=23): find_id = 3 self.tts_pub.publish(str(find_id)) elif(point_msg.z>=25 and point_msg.z<=26): find_id = 4 self.tts_pub.publish(str(find_id)) elif(point_msg.z>=36 and point_msg.z<=40): find_id = 5 self.tts_pub.publish(str(find_id)) elif(point_msg.z>=41 and point_msg.z<=43): find_id = 6 self.tts_pub.publish(str(find_id)) elif(point_msg.z>=70 and point_msg.z<=71): find_id = 7 self.tts_pub.publish(str(find_id)) elif(point_msg.z>=80 and point_msg.z<=81): find_id = 8 self.tts_pub.publish(str(find_id)) else: find_id = 0 #print("id为0,没有识别到!") def process(): rospy.spin() find_id = 0 tag_id = 0 both_id =0 if __name__ == '__main__': rospy.init_node('navigation_demo',anonymous=True) goalListX = rospy.get_param('~goalListX', '2.0, 2.0,2.0') goalListY = rospy.get_param('~goalListY', '2.0, 4.0,2.0') goalListYaw = rospy.get_param('~goalListYaw', '0, 90.0,2.0') goals = [[float(x), float(y), float(yaw)] for (x, y, yaw) in zip(goalListX.split(","),goalListY.split(","),goalListYaw.split(","))] ##为了方便记忆,goals中[0]是终点,[1]到[8]分别对应场上的8个点,9是第一个框的识别点,10是第二个框的识别点,11是第三个框的识别点,12是第四个框的识别点 object_position = Object_position() ar_acker = ARTracker() executor_thread = threading.Thread(target=process).start() navi = Navigation() find_point_flag=[0,0,0,0] have_nav_flag=[0,0,0,0] time.sleep(10) navi.goto(goals[9]) find_point_flag[0]=1 while True: if find_id==1 or tag_id==1: both_id=1 elif find_id==2 or tag_id==2: both_id=2 elif find_id==3 or tag_id==3: both_id=3 elif find_id==4 or tag_id==4: both_id=4 elif find_id==5 or tag_id==5: both_id=5 elif find_id==6 or tag_id==6: both_id=6 elif find_id==7 or tag_id==7: both_id=7 elif find_id==8 or tag_id==8: both_id=8 else: both_id=0 if both_id==0: if have_nav_flag[0]==1 and find_point_flag[1]==0: navi.goto(goals[10]) find_point_flag[1]=1 if have_nav_flag[1]==1 and find_point_flag[2]==0: navi.goto(goals[11]) find_point_flag[2]=1 if have_nav_flag[2]==1 and find_point_flag[3]==0: navi.goto(goals[12]) find_point_flag[3]=1 if have_nav_flag[3]==1: navi.goto(goals[0]) break else: if both_id==1 and have_nav_flag[0]==0: navi.goto(goals[1]) have_nav_flag[0]=1 if both_id==2 and have_nav_flag[0]==0: navi.goto(goals[2]) have_nav_flag[0]=1 if both_id==3 and have_nav_flag[0]==0: navi.goto(goals[3]) have_nav_flag[1]=1 if both_id==4 and have_nav_flag[0]==0: navi.goto(goals[4]) have_nav_flag[1]=1 if both_id==5 and have_nav_flag[0]==0: navi.goto(goals[5]) have_nav_flag[3]=1 if both_id==6 and have_nav_flag[0]==0: navi.goto(goals[6]) have_nav_flag[3]=1 if both_id==7 and have_nav_flag[0]==0: navi.goto(goals[7]) have_nav_flag[4]=1 if both_id==8 and have_nav_flag[0]==0: navi.goto(goals[8]) have_nav_flag[4]=1

最新推荐

recommend-type

commons-compress-1.10.jar中文-英文对照文档.zip

1、压缩文件中包含: 中文-英文对照文档、jar包下载地址、Maven依赖、Gradle依赖、源代码下载地址。 2、使用方法: 解压最外层zip,再解压其中的zip包,双击 【index.html】 文件,即可用浏览器打开、进行查看。 3、特殊说明: (1)本文档为人性化翻译,精心制作,请放心使用; (2)只翻译了该翻译的内容,如:注释、说明、描述、用法讲解 等; (3)不该翻译的内容保持原样,如:类名、方法名、包名、类型、关键字、代码 等。 4、温馨提示: (1)为了防止解压后路径太长导致浏览器无法打开,推荐在解压时选择“解压到当前文件夹”(放心,自带文件夹,文件不会散落一地); (2)有时,一套Java组件会有多个jar,所以在下载前,请仔细阅读本篇描述,以确保这就是你需要的文件。 5、本文件关键字: jar中文-英文对照文档.zip,java,jar包,Maven,第三方jar包,组件,开源组件,第三方组件,Gradle,中文API文档,手册,开发手册,使用手册,参考手册。
recommend-type

sqlite-jdbc-3.36.0.jar中文-英文对照文档.zip

1、压缩文件中包含: 中文-英文对照文档、jar包下载地址、Maven依赖、Gradle依赖、源代码下载地址。 2、使用方法: 解压最外层zip,再解压其中的zip包,双击 【index.html】 文件,即可用浏览器打开、进行查看。 3、特殊说明: (1)本文档为人性化翻译,精心制作,请放心使用; (2)只翻译了该翻译的内容,如:注释、说明、描述、用法讲解 等; (3)不该翻译的内容保持原样,如:类名、方法名、包名、类型、关键字、代码 等。 4、温馨提示: (1)为了防止解压后路径太长导致浏览器无法打开,推荐在解压时选择“解压到当前文件夹”(放心,自带文件夹,文件不会散落一地); (2)有时,一套Java组件会有多个jar,所以在下载前,请仔细阅读本篇描述,以确保这就是你需要的文件。 5、本文件关键字: jar中文-英文对照文档.zip,java,jar包,Maven,第三方jar包,组件,开源组件,第三方组件,Gradle,中文API文档,手册,开发手册,使用手册,参考手册。
recommend-type

[最新代码+论文+PPT]企业合同管理系统设计与实现-5c062cu7.zip

[最新代码+论文+PPT]企业合同管理系统设计与实现-5c062cu7.zip
recommend-type

tps92682-q1.pdf

tps92682-q1.pdf
recommend-type

sqlite-jdbc-3.7.2.jar中文文档.zip

1、压缩文件中包含: 中文文档、jar包下载地址、Maven依赖、Gradle依赖、源代码下载地址。 2、使用方法: 解压最外层zip,再解压其中的zip包,双击 【index.html】 文件,即可用浏览器打开、进行查看。 3、特殊说明: (1)本文档为人性化翻译,精心制作,请放心使用; (2)只翻译了该翻译的内容,如:注释、说明、描述、用法讲解 等; (3)不该翻译的内容保持原样,如:类名、方法名、包名、类型、关键字、代码 等。 4、温馨提示: (1)为了防止解压后路径太长导致浏览器无法打开,推荐在解压时选择“解压到当前文件夹”(放心,自带文件夹,文件不会散落一地); (2)有时,一套Java组件会有多个jar,所以在下载前,请仔细阅读本篇描述,以确保这就是你需要的文件。 5、本文件关键字: jar中文文档.zip,java,jar包,Maven,第三方jar包,组件,开源组件,第三方组件,Gradle,中文API文档,手册,开发手册,使用手册,参考手册。
recommend-type

响应式绿色简洁风格网络借贷网页模板分享

标题中提到的“绿色简洁风格响应式网络借贷网页模板.zip”暗示着该模板采用了绿色作为主要色彩,并且界面设计风格简洁。响应式设计则意味着网页模板能够在不同尺寸的屏幕上展示适宜的布局和内容,无论是电脑、平板还是手机等移动设备。这种设计符合现代网页设计的趋势,确保用户无论使用何种设备访问网络借贷平台,都能获得良好的浏览体验。同时,“网络借贷”表明这个网页模板可能专门适用于P2P借贷公司或金融技术服务公司,它们需要一个能够体现专业、可靠、易用界面的在线平台。 在描述部分,“html网站模版分享”表明该文件是一个分享性质的资源,用户可以通过这个模板快速搭建一个HTML网站。静态化H5网站模版源码意味着该模板可能不包含后端交互逻辑,即不会涉及数据库和服务器端编程。这里提及的H5指的是HTML5,它是HTML的最新版本,提供了更多增强的标签和功能,比如更好的多媒体和图形支持、离线存储等。PC+wap表明该模板支持传统的个人电脑浏览以及移动设备的wap(无线应用协议)浏览,平面广告设计网页模版代码则说明模板中可能包含了广告位或者特定的视觉元素来强化广告效果。 标签“html5 H5模版 HTML模版”进一步细化了文件的内容,强调了HTML5技术的应用。HTML5模版通常包含最新的HTML标记和语义化标签,能够支持现代浏览器的各种新特性,从而提升网站的交互性和用户体验。标签的使用也说明了这个模板可能适用于多种不同类型的网站,但特别适用于需要在移动设备上也能良好展示的网站。 文件名列表中的“24809”可能指的是该模板的版本号、编号或者文件在压缩包中的唯一标识。由于没有具体的文件扩展名,我们无法直接了解具体的文件内容,但是通常情况下,一个网页模板压缩包中应包含HTML、CSS、JavaScript文件以及其他可能的媒体资源,如图片、字体等。 综上所述,该模板文件包含的知识点包括: 1. 响应式网页设计:能够适应不同屏幕尺寸和分辨率的设备。 2. 绿色简洁风格:采用绿色作为视觉主色调,设计简洁明了。 3. 网络借贷领域适用性:模板设计符合金融技术服务公司或P2P借贷平台的需要。 4. 静态HTML5网页模板:适用于快速搭建静态网站,不涉及后端逻辑。 5. PC与移动设备适配:模板设计同时考虑了电脑和平板及手机等移动设备的浏览体验。 6. 广告设计元素:模板中包含设计用于展示广告内容的版块。 7. HTML5技术应用:利用HTML5的新特性和语义化标签来构建网页。 8. 跨平台的网页模板:模板适用于多种设备和浏览器,确保良好的兼容性。 这个网页模板特别适合希望快速上线的网络借贷平台、金融服务网站等,它可以帮助开发者节省设计和编码的时间,同时也确保了一个专业且用户友好的前端展示。
recommend-type

图割技术深度剖析:掌握从理论到实践的15个关键步骤

# 1. 图割技术概述 图割技术是一种应用广泛的数学算法,它来源于图论,主要应用于图像处理、网络优化、数据分析等领域。图割技术的核心在于将复杂的问题转化为图的最小割问题,并求解。这不仅涉及到了图论和网络流理论,还需要具备一定的优化算法知识。通过对图割技术的基本概念、理论基础和实际应用的深入理解,可以有
recommend-type

zmdl

<think>好的,我现在需要处理用户关于“zmdl IT相关内容”的查询。首先,“zmdl”可能是一个缩写或拼写错误,所以我要考虑常见的IT相关术语中可能的匹配。 首先,我会想到常见的缩写,比如ZMDL可能代表某个技术框架、工具、协议或者公司名称。例如,在软件开发中,可能有类似缩写的库或框架。但常见的IT缩写如XML、JSON、YAML等,似乎不太匹配。然后,检查是否有拼写错误,比如用户可能想输入的是“ZMQ”(ZeroMQ)或者“ZFS”等,但不确定。 接下来,考虑中文拼音首字母缩写。例如,“zmdl”可能对应“自动化代码”之类的词组,比如“自动化代码流程”(Zi Dong Hua D
recommend-type

紫色大气PC+wap网页模板代码包

根据给定的文件信息,我们可以提炼出以下知识点: ### 网页模板与设计 #### 1. 网页模板概述 网页模板是一种预先设计好的网页结构框架,它包含HTML、CSS以及可能的JavaScript代码,可以快速帮助开发者构建出一致风格和布局的网页。使用模板可以节省设计和编码的时间,使得开发者可以专注于网页内容的更新和功能的实现。 #### 2. PC与WAP的区别 PC端指的是使用个人电脑访问的网页版本,通常会提供更加丰富的布局和功能,因为屏幕尺寸较大,可以展示更多的内容和元素。WAP则是针对移动设备(如手机和平板电脑)设计的网页版本,它必须考虑到移动设备屏幕小、网络带宽较低等特点,因此在设计上更倾向于简洁、高效。 #### 3. 静态网页与动态网页 静态网页是一种简单的网页格式,其内容是固定的,不会因为用户的交互而改变。动态网页则允许内容根据用户的不同操作发生变化,通常包含服务器端脚本或数据库交互,可以提供更加个性化的浏览体验。静态化H5网站模板意味着这个模板是静态的,但专为H5设计,即兼容移动设备的HTML5标准。 #### 4. HTML5网页模板 HTML5是最新版本的HTML标准,它引入了诸多新特性,例如支持多媒体内容、图形和动画等,而无需依赖插件。HTML5模板专为HTML5标准设计,能够提供更好的兼容性和更丰富的用户体验。 ### 开发工具与技术 #### 1. HTML和CSS HTML(HyperText Markup Language)是构建网页的标准标记语言,它定义了网页的内容和结构。CSS(Cascading Style Sheets)用于描述HTML文档的呈现样式,包括布局、设计、颜色和字体等。两者结合使用,可以创建既美观又功能强大的网页。 #### 2. JavaScript JavaScript是一种运行在浏览器端的脚本语言,它能够让网页变得动态和交互性更强。通过使用JavaScript,开发者可以添加复杂的动画效果、表单验证、数据操作以及与用户的实时互动。 #### 3. 响应式设计 响应式网页设计是一种设计方法论,旨在让网页在不同设备和屏幕尺寸上均能提供优秀的浏览体验。这通常是通过媒体查询(Media Queries)来实现,可以根据设备的屏幕尺寸来应用不同的CSS样式。 ### 文件管理和解压缩 #### 1. 压缩文件格式 "紫色大气形式pc+wap专业维修服务网页模板代码.zip"文件意味着该文件是一个ZIP压缩包,它通过压缩算法减少了文件大小,便于传输和存储。解压缩此文件后,可以得到一系列的文件,这些文件包含了网页模板的所有资源。 #### 2. 文件命名规范 给定的压缩包中只有一个文件,即"22695"。从文件名称中,我们无法直接获取关于文件内容的具体信息。通常来说,文件命名应该反映出文件内容或者用途,以便于管理和检索。 ### 具体应用场景 #### 1. 专业维修服务网站 该网页模板被描述为面向专业维修服务的。这表明模板会包含相应的行业元素和布局设计,比如服务介绍、价格信息、联系方式、在线预约等。此类模板适合维修公司、汽车服务中心、电子产品维修点等使用。 #### 2. 平面广告设计 网页模板中还提到了平面广告设计。这意味着模板可能融入了平面设计的元素,如视觉焦点、色彩搭配和图形设计等,帮助企业在网络上展示其品牌和产品。 ### 结论 综上所述,"紫色大气形式pc+wap专业维修服务网页模板代码.zip"文件提供了一个静态化H5网页模板,可用于创建兼容PC和移动端的维修服务网站。模板代码基于HTML5、CSS和可能的JavaScript编写,具有响应式设计以适应不同设备。通过解压缩操作,开发者可以获取模板文件,然后根据需要进行修改和扩展以构建出一个功能完整、视觉吸引的网站。
recommend-type

【微信小程序CI_CD流程优化】:掌握这些技巧,部署效率提升不止一倍!

# 1. 微信小程序CI/CD的基本概念 微信小程序CI/CD(持续集成和持续部署)是一种软件开发实践,旨在使开发人员能够更快地交付新版本的小程序,同时保持高质量的标准。它强调在开发过程中持续进行构建、测试和发布,确保代码改动能够被快速发现并部署到生产环境中。通过自动化测试和部署流程,CI/CD减少了手动错误,加速