tf.matrix_set_diag()

本文介绍如何使用TensorFlow的matrix_set_diag函数修改矩阵的对角线元素,通过示例展示其工作原理,适用于深度学习和数据处理场景。

摘要生成于 C知道 ,由 DeepSeek-R1 满血版支持, 前往体验 >

tf.matrix_set_diag(input,diagonal,name=None)
功能:将输入矩阵的对角元素置换为对角元素。
输入:input:矩阵,diagonal:对角元素。
例:

a=tf.constant([[0,1,2,3],[-1,0,1,2],[-2,-1,0,1],[-3,-2,-1,0]])
z=tf.matrix_set_diag(a,[10,11,12,13])

z==>[[10  1  2  3]
     [-1 11  1  2]
     [0  -1 12  1]
     [0   0 -1 13]]

作者:华夏意匠
链接:https://www.jianshu.com/p/336772764fef
來源:简书
著作权归作者所有。商业转载请联系作者获得授权,非商业转载请注明出处。

/** * @file uav_fsm_node.cpp * @brief UAV Takeoff and U-Shaped Path Flight for Simulation * @version 6.1 * @date 2023-10-15 * * 针对仿真环境的优化: * 1. 修复坐标系问题(使用ENU坐标系的数值表示) * 2. 简化起飞控制逻辑 * 3. 增强错误检测和日志输出 * 4. 优化参数默认值 * 5. 确保控制指令正确发送 */ #include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <geometry_msgs/TwistStamped.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> #include <mavros_msgs/CommandTOL.h> #include <mavros_msgs/PositionTarget.h> #include <mavros_msgs/TimesyncStatus.h> #include <cmath> #include <vector> #include <algorithm> #include <tf2/LinearMath/Quaternion.h> #include <tf2_geometry_msgs/tf2_geometry_msgs.h> #include <angles/angles.h> // 全局变量 mavros_msgs::State current_state; geometry_msgs::PoseStamped current_pose; geometry_msgs::TwistStamped current_vel; mavros_msgs::TimesyncStatus timesync_status; bool pose_received = false; bool vel_received = false; bool timesync_received = false; // 状态回调 void state_cb(const mavros_msgs::State::ConstPtr& msg) { current_state = *msg; } // 位置回调 void pose_cb(const geometry_msgs::PoseStamped::ConstPtr& msg) { current_pose = *msg; pose_received = true; } // 速度回调 void vel_cb(const geometry_msgs::TwistStamped::ConstPtr& msg) { current_vel = *msg; vel_received = true; } // 时间同步回调 void timesync_cb(const mavros_msgs::TimesyncStatus::ConstPtr& msg) { timesync_status = *msg; timesync_received = true; } // 计算两点之间的距离 double distance(const geometry_msgs::Point& p1, const geometry_msgs::Point& p2) { double dx = p1.x - p2.x; double dy = p1.y - p2.y; double dz = p1.z - p2.z; return std::sqrt(dx*dx + dy*dy + dz*dz); } // 计算航点与位置之间的距离 double distance_to_waypoint(const geometry_msgs::Point& position, double wx, double wy, double wz) { double dx = position.x - wx; double dy = position.y - wy; double dz = position.z - wz; return std::sqrt(dx*dx + dy*dy + dz*dz); } // 检查是否到达目标点 bool reached_target(double target_x, double target_y, double target_z, double tolerance = 0.3) { if (!pose_received) return false; return distance_to_waypoint(current_pose.pose.position, target_x, target_y, target_z) < tolerance; } // 创建偏航角四元数 geometry_msgs::Quaternion create_quaternion_from_yaw(double yaw_rad) { tf2::Quaternion q; q.setRPY(0, 0, yaw_rad); return tf2::toMsg(q); } // 获取当前偏航角 double get_current_yaw() { if (!pose_received) return 0.0; tf2::Quaternion q( current_pose.pose.orientation.x, current_pose.pose.orientation.y, current_pose.pose.orientation.z, current_pose.pose.orientation.w); tf2::Matrix3x3 m(q); double roll, pitch, yaw; m.getRPY(roll, pitch, yaw); return yaw; } int main(int argc, char **argv) { ros::init(argc, argv, "uav_fsm_node"); ros::NodeHandle nh; // 参数配置 - 仿真环境优化值 double flight_height = 2.0; // 飞行高度(米) - 降低高度 double u_width = 1.5; // U型宽度(米) - 缩小路径 double u_depth = 1.5; // U型深度(米) - 缩小路径 double takeoff_climb_rate = 0.8; // 起飞爬升速率(米/秒) - 降低爬升率 double max_vel = 1.0; // 最大速度(m/s) - 降低速度 double arming_tolerance = 1.0; // 解锁位置容差 - 增大容差 double state_timeout = 120.0; // 状态超时时间(秒) - 延长时间 nh.param("flight_height", flight_height, flight_height); nh.param("u_width", u_width, u_width); nh.param("u_depth", u_depth, u_depth); nh.param("takeoff_climb_rate", takeoff_climb_rate, takeoff_climb_rate); nh.param("max_vel", max_vel, max_vel); nh.param("arming_tolerance", arming_tolerance, arming_tolerance); nh.param("state_timeout", state_timeout, state_timeout); ROS_INFO("UAV Simulation Flight Control v6.1"); ROS_INFO(" - Takeoff Height: %.1f m", flight_height); ROS_INFO(" - Takeoff Climb Rate: %.1f m/s", takeoff_climb_rate); ROS_INFO(" - U-Width: %.1f m", u_width); ROS_INFO(" - U-Depth: %.1f m", u_depth); ROS_INFO(" - Max Velocity: %.1f m/s", max_vel); ROS_INFO(" - State Timeout: %.1f sec", state_timeout); // 订阅者 ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); ros::Subscriber pose_sub = nh.subscribe<geometry_msgs::PoseStamped> ("mavros/local_position/pose", 10, pose_cb); ros::Subscriber vel_sub = nh.subscribe<geometry_msgs::TwistStamped> ("mavros/local_position/velocity_local", 10, vel_cb); ros::Subscriber timesync_sub = nh.subscribe<mavros_msgs::TimesyncStatus> ("mavros/timesync_status", 10, timesync_cb); // 发布者 ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/local", 10); ros::Publisher setpoint_raw_pub = nh.advertise<mavros_msgs::PositionTarget> ("mavros/setpoint_raw/local", 10); // 服务客户端 ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool> ("mavros/cmd/arming"); ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode> ("mavros/set_mode"); ros::ServiceClient land_client = nh.serviceClient<mavros_msgs::CommandTOL> ("mavros/cmd/land"); // 设置发布频率 double control_rate = 20.0; ros::Rate rate(control_rate); double dt_val = 1.0 / control_rate; // 等待飞控连接 ROS_INFO("Waiting for FCU connection..."); int connection_attempts = 0; while (ros::ok() && !current_state.connected) { ros::spinOnce(); rate.sleep(); if (++connection_attempts % 50 == 0) { ROS_WARN("Still waiting for FCU connection..."); } } ROS_INFO("FCU connected"); // 定义航点类型 enum MovementType { FORWARD, // 机头朝向运动方向 SIDEWAYS // 侧向移动 }; struct Waypoint { double x, y, z; MovementType move_type; // 移动类型 double target_yaw; // 目标偏航角(用于侧向移动) }; // U形路径航点序列(起飞后执行的部分) std::vector<Waypoint> u_path = { // 起点(起飞后的悬停点) {0.0, 0.0, flight_height, FORWARD, 0.0}, // 向前飞行 {u_depth, 0.0, flight_height, FORWARD, 0.0}, // 向右移动 {u_depth, u_width, flight_height, FORWARD, M_PI/2}, // U型终点 {0.0, u_width, flight_height, FORWARD, M_PI}, // 降落准备点 {0.0, u_width, flight_height, FORWARD, M_PI} }; // 创建目标位置消息(用于位置控制) geometry_msgs::PoseStamped target_pose; target_pose.header.frame_id = "map"; // 创建速度控制消息 - 使用ENU坐标系(仿真环境标准) mavros_msgs::PositionTarget vel_cmd; // 关键修复:使用数值代替常量 // FRAME_LOCAL_ENU = 4, FRAME_LOCAL_NED = 1 vel_cmd.coordinate_frame = 4; // MAV_FRAME_LOCAL_ENU (z-up) ROS_INFO("Using ENU coordinate system (value 4) for simulation"); vel_cmd.type_mask = 0b0000011111000111 | // 忽略加速度和力 mavros_msgs::PositionTarget::IGNORE_YAW_RATE; // 预先发送设定点 - 关键修复:确保在OFFBOARD模式下持续发送设定点 ROS_INFO("Sending initial setpoints..."); ros::Time last_setpoint_time = ros::Time::now(); for (int i = 0; ros::ok() && i < 100; i++) { // 初始设定点为地面点(0,0,0) target_pose.pose.position.x = 0.0; target_pose.pose.position.y = 0.0; target_pose.pose.position.z = 0.0; target_pose.pose.orientation = create_quaternion_from_yaw(0.0); target_pose.header.stamp = ros::Time::now(); local_pos_pub.publish(target_pose); // 在初始设定点发送期间尝试进入OFFBOARD模式 if (i > 20) { mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode = "OFFBOARD"; if (set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent) { ROS_INFO("OFFBOARD mode enabled during initialization"); } } ros::spinOnce(); rate.sleep(); } // 准备服务请求 mavros_msgs::SetMode offb_set_mode; offb_set_mode.request.custom_mode = "OFFBOARD"; mavros_msgs::CommandBool arm_cmd; arm_cmd.request.value = true; // 飞行状态 enum FlightState { PREFLIGHT, OFFBOARD, ARMING, TAKEOFF, FLYING, LANDING, COMPLETED }; FlightState flight_state = PREFLIGHT; size_t current_waypoint = 0; ros::Time last_request = ros::Time::now(); ros::Time state_start_time = ros::Time::now(); bool landing_initiated = false; double target_altitude = flight_height; double current_yaw = 0.0; double target_yaw = 0.0; bool position_landing = false; int arming_attempts = 0; int convergence_count = 0; geometry_msgs::Point current_position; double takeoff_start_z = 0.0; double pos_error = 0.0; // 时间同步计数器 int timesync_warning_count = 0; const int MAX_TIMESYNC_WARNINGS = 10; ROS_INFO("Starting UAV simulation mission..."); while (ros::ok()) { // 获取当前状态 ros::spinOnce(); // 检查状态超时 if ((ros::Time::now() - state_start_time).toSec() > state_timeout) { ROS_ERROR("State timeout in state %d! Resetting to PREFLIGHT.", flight_state); flight_state = PREFLIGHT; state_start_time = ros::Time::now(); } // 记录当前位置 if (pose_received) { current_position = current_pose.pose.position; } // 状态机处理 switch (flight_state) { case PREFLIGHT: ROS_INFO_THROTTLE(1.0, "PREFLIGHT: Waiting for OFFBOARD mode"); ROS_INFO_THROTTLE(1.0, "Current mode: %s, Armed: %d", current_state.mode.c_str(), current_state.armed); if (ros::Time::now() - last_request > ros::Duration(5.0)) { if (set_mode_client.call(offb_set_mode)) { if (offb_set_mode.response.mode_sent) { ROS_INFO("OFFBOARD enabled"); flight_state = OFFBOARD; convergence_count = 0; state_start_time = ros::Time::now(); } else { ROS_WARN("OFFBOARD enable failed"); } } else { ROS_WARN("OFFBOARD service call failed"); } last_request = ros::Time::now(); } break; case OFFBOARD: ROS_INFO_THROTTLE(1.0, "OFFBOARD: Waiting for position convergence"); // 保持在地面点(0,0,0) target_pose.pose.position.x = 0.0; target_pose.pose.position.y = 0.0; target_pose.pose.position.z = 0.0; target_pose.pose.orientation = create_quaternion_from_yaw(0.0); target_pose.header.stamp = ros::Time::now(); local_pos_pub.publish(target_pose); // 计算位置误差 if (pose_received) { pos_error = distance_to_waypoint( current_position, 0.0, 0.0, 0.0 ); // 位置收敛检查 if (pos_error < 0.5) { convergence_count++; } else { convergence_count = std::max(0, convergence_count - 1); } } if (current_state.mode != "OFFBOARD") { ROS_WARN("OFFBOARD mode lost! Current mode: %s", current_state.mode.c_str()); flight_state = PREFLIGHT; state_start_time = ros::Time::now(); } else if (ros::Time::now() - last_request > ros::Duration(5.0)) { // 位置稳定检查 if (convergence_count > 10 && pos_error < arming_tolerance) { if (arming_client.call(arm_cmd)) { if (arm_cmd.response.success) { ROS_INFO("Armed successfully"); flight_state = TAKEOFF; if (pose_received) { takeoff_start_z = current_pose.pose.position.z; } arming_attempts = 0; state_start_time = ros::Time::now(); } else { arming_attempts++; ROS_WARN("Arming failed (attempt %d)!", arming_attempts); if (arming_attempts > 3) { ROS_ERROR("Too many arming failures. Aborting mission."); ros::shutdown(); } } } else { ROS_WARN("Arming service call failed"); } } else { ROS_WARN_THROTTLE(1.0, "Position not stable for arming: error=%.2fm, convergence=%d/%d", pos_error, convergence_count, 10); } last_request = ros::Time::now(); } break; case TAKEOFF: if (pose_received) { double current_height = current_pose.pose.position.z; double height_error = flight_height - current_height; ROS_INFO_THROTTLE(0.5, "TAKEOFF: Climbing to %.1fm (Current: %.2fm, Error: %.2fm)", flight_height, current_height, height_error); // 获取当前偏航角 current_yaw = get_current_yaw(); // 创建速度控制指令 vel_cmd.velocity.x = 0.0; vel_cmd.velocity.y = 0.0; vel_cmd.velocity.z = takeoff_climb_rate; // 直接使用固定爬升速率 vel_cmd.yaw = current_yaw; // 发布速度控制命令 vel_cmd.header.stamp = ros::Time::now(); setpoint_raw_pub.publish(vel_cmd); // 检查是否达到起飞高度 if (current_height > flight_height - 0.1) { ROS_INFO("Reached takeoff altitude: %.2f m", current_height); flight_state = FLYING; current_waypoint = 0; ROS_INFO("Starting U-shaped path flight"); state_start_time = ros::Time::now(); } } break; case FLYING: if (pose_received) { ROS_INFO_THROTTLE(1.0, "FLYING: Waypoint %zu/%zu (Current Z: %.2f)", current_waypoint+1, u_path.size(), current_pose.pose.position.z); // 获取当前偏航角 current_yaw = get_current_yaw(); // 根据移动类型计算目标偏航角 if (u_path[current_waypoint].move_type == FORWARD) { if (current_waypoint < u_path.size() - 1) { double dx = u_path[current_waypoint+1].x - u_path[current_waypoint].x; double dy = u_path[current_waypoint+1].y - u_path[current_waypoint].y; target_yaw = std::atan2(dy, dx); } else { target_yaw = u_path[current_waypoint].target_yaw; } } else { target_yaw = u_path[current_waypoint].target_yaw; } // 平滑偏航角过渡 double yaw_error = angles::shortest_angular_distance(current_yaw, target_yaw); double smoothed_yaw = current_yaw + 0.1 * yaw_error; // 创建位置控制指令 target_pose.pose.position.x = u_path[current_waypoint].x; target_pose.pose.position.y = u_path[current_waypoint].y; target_pose.pose.position.z = u_path[current_waypoint].z; target_pose.pose.orientation = create_quaternion_from_yaw(smoothed_yaw); // 发布位置指令 target_pose.header.stamp = ros::Time::now(); local_pos_pub.publish(target_pose); // 检查是否到达当前航点 if (reached_target( u_path[current_waypoint].x, u_path[current_waypoint].y, u_path[current_waypoint].z )) { ROS_INFO("Reached waypoint %zu: (%.1f, %.1f, %.1f)", current_waypoint, u_path[current_waypoint].x, u_path[current_waypoint].y, u_path[current_waypoint].z); if (current_waypoint < u_path.size() - 1) { current_waypoint++; ROS_INFO("Moving to waypoint %zu: (%.1f, %.1f, %.1f)", current_waypoint, u_path[current_waypoint].x, u_path[current_waypoint].y, u_path[current_waypoint].z); state_start_time = ros::Time::now(); } else { ROS_INFO("Final waypoint reached, initiating landing"); flight_state = LANDING; state_start_time = ros::Time::now(); } } } if (!current_state.armed) { ROS_ERROR("Unexpected disarm during flight!"); flight_state = OFFBOARD; state_start_time = ros::Time::now(); } break; case LANDING: ROS_INFO_THROTTLE(1.0, "LANDING: Descending... (Current Z: %.2f)", pose_received ? current_pose.pose.position.z : 0.0); if (!landing_initiated) { mavros_msgs::CommandTOL land_cmd_srv; land_cmd_srv.request.yaw = u_path.back().target_yaw; if (land_client.call(land_cmd_srv)) { if (land_cmd_srv.response.success) { ROS_INFO("Landing command accepted"); landing_initiated = true; } else { ROS_WARN("Land service failed, using position landing"); landing_initiated = true; position_landing = true; if (pose_received) { target_altitude = current_pose.pose.position.z; } else { target_altitude = flight_height; } } } else { ROS_WARN("Land service call failed, using position landing"); landing_initiated = true; position_landing = true; if (pose_received) { target_altitude = current_pose.pose.position.z; } else { target_altitude = flight_height; } } } if (position_landing && pose_received) { // 设置降落位置(XY保持不变) target_pose.pose.position.x = u_path.back().x; target_pose.pose.position.y = u_path.back().y; // 平滑降低高度 if (target_altitude > 0.1) { target_altitude -= 0.05 * dt_val; } else { target_altitude = 0.0; } target_pose.pose.position.z = target_altitude; // 设置偏航角 target_pose.pose.orientation = create_quaternion_from_yaw(u_path.back().target_yaw); // 发布位置指令 target_pose.header.stamp = ros::Time::now(); local_pos_pub.publish(target_pose); } // 检查是否着陆 if (pose_received && current_pose.pose.position.z < 0.2) { ROS_INFO("Landed successfully at (%.2f, %.2f, %.2f)", current_pose.pose.position.x, current_pose.pose.position.y, current_pose.pose.position.z); flight_state = COMPLETED; } break; case COMPLETED: ROS_INFO("Mission completed. Shutting down..."); ros::shutdown(); break; } rate.sleep(); } return 0; } 代码运行后无人机一直在地面附件徘徊,无法上升到设定的1m高度,帮我修改代码
最新发布
08-04
<think>我们有一个SQL查询,用户想要在现有的SELECT语句中加入DISTINCT关键字来去除重复行。原始查询是: selectt.med_type就医方式 ,t.BKKP_SN记账流水号,...--其他字段t.inscp_amt符合医保范围金额from mx_detail_gaoxin_2023_2024t;用户要求:在代码中输入distinct函数。注意:DISTINCT关键字用于返回唯一不同的值。它可以放在SELECT之后,或者作为DISTINCTON(在PostgreSQL中)的形式,但标准SQL中通常是在SELECT后直接跟DISTINCT。因此,我们可以在SELECT后面加上DISTINCT,使得整个结果集去重。修改后的SQL语句如下:</think>在SQL查询中,`DISTINCT`关键字用于返回唯一不同的值。若需在现有查询中去除重复行,需将`DISTINCT`置于`SELECT`之后、字段列表之前。修改后的代码如下: ```sql SELECT DISTINCT t.med_type 就医方式, t.BKKP_SN 记账流水号, t.ipt_otp_no 住院门诊号, t.mdtrt_id 就诊号, t.setl_id 结算单号, t.MEDRCDNO 病历号, t.refd_setl_flag 退费标志, t.sp_psn_type_name 特殊人员类型名称, t.LIST_TYPE_name 目录类别名称, t.PAY_LOC_NAME 支付地点类别名称, t.fixmedins_code 医院编号, t.fixmedins_name 医院名称, t.psn_no 个人编码, t.psn_name 姓名, t.age 年龄, t.gend_name 性别, t.certno 身份证, t.brdy 出生日期, t.INSUTYPE_name 参保险种名称, t.main_diag_code 主诊断编码, t.main_diag_name 主诊断名称, t.second_diag_code 副诊断编码, t.second_diag_name 副诊断名称, t.DSCG_MAINDIAG_CODE 住院主诊断代码, t.DSCG_MAINDIAG_NAME 住院主诊断名称, t.MAIN_COND_DSCR 主要病情描述, t.DISE_NO 病种编码, t.DISE_NAME 病种名称, t.CHRGITM_LV_name 支付类别名称, t.MED_CHRGITM_name 医疗收费项目类别名称, t.SETL_TIME 结算时间, t.FEE_OCUR_TIME 费用发生时间, t.hilist_code 医保项目编码, t.hilist_name 医保项目名称, t.medins_list_codg 医院项目编码, t.medins_list_name 医院项目名称, t.pric 单价, t.cnt 数量, t.det_item_fee_sumamt 金额, t.reimbursement_prop 报销比例, t.inscp_amt 符合医保范围金额 FROM mx_detail_gaoxin_2023_2024 t; ``` **关键说明**: 1. `DISTINCT`作用于所有查询字段,只有当所有字段值完全相同时才会去重 2. 若需对部分字段去重(如仅保留唯一病历号),需改用`GROUP BY`或窗口函数 3. 在大数据量下可能影响性能,建议评估必要性
评论 1
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值