吹毛求疵的计较下变量的位置

本文通过对比两种不同的变量交换方式,探讨了在Java中进行大量循环运算时的效率问题,并发现将部分操作移出循环可以显著提高程序运行速度。此外还提供了一个关于变量交换技巧的外部链接。
long i = 0;
  long j = 0;
  long flag3 = System.currentTimeMillis();
  i = 4l;
  j = 6l;
  for (long j2 = 0; j2 < 200000001L; j2++) {
   long temp = i;
   i = j;
   j = temp;
  }
  System.out.println(i + " " + j);
  long flag4 = System.currentTimeMillis();
  
  i = 4l;
  j = 6l;
  long temp = 0;
  for (long j2 = 0; j2 < 200000001L; j2++) {
   temp = i;
   i = j;
   j = temp;
  }
  System.out.println(i + " " + j);
  long flag5 = System.currentTimeMillis();

 经过比较,放在For循环外面的赋值更有效率.

 两个变量交换的扩展思考:http://blog.youkuaiyun.com/do2jiang/archive/2009/09/13/4549679.aspx

 得出一个结论,在编译器魔术影响下,我们用了^等方式来优化代码.结果反而不能被编译器优化...性能更低.

 

#include <ros/ros.h> #include <geometry_msgs/PoseStamped.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> #include <cmath> // 无人机状态和位置变量 mavros_msgs::State current_state_typhoon0, current_state_typhoon1; mavros_msgs::State current_state_iris0, current_state_iris1; geometry_msgs::PoseStamped pos_typhoon0, pos_typhoon1; geometry_msgs::PoseStamped pos_iris0, pos_iris1; // 状态回调函数 void state_cb_typhoon0(const mavros_msgs::State::ConstPtr& msg) { current_state_typhoon0 = *msg; } void state_cb_typhoon1(const mavros_msgs::State::ConstPtr& msg) { current_state_typhoon1 = *msg; } void state_cb_iris0(const mavros_msgs::State::ConstPtr& msg) { current_state_iris0 = *msg; } void state_cb_iris1(const mavros_msgs::State::ConstPtr& msg) { current_state_iris1 = *msg; } // 位置回调函数 void pos_cb_typhoon0(const geometry_msgs::PoseStamped::ConstPtr& msg) { pos_typhoon0 = *msg; } void pos_cb_typhoon1(const geometry_msgs::PoseStamped::ConstPtr& msg) { pos_typhoon1 = *msg; } void pos_cb_iris0(const geometry_msgs::PoseStamped::ConstPtr& msg) { pos_iris0 = *msg; } void pos_cb_iris1(const geometry_msgs::PoseStamped::ConstPtr& msg) { pos_iris1 = *msg; } // 检查是否到达目标位置 bool checkPositionReached(const geometry_msgs::PoseStamped& current_pose, float target_x, float target_y, float target_z, float tolerance = 2.0) {     float dx = current_pose.pose.position.x - target_x;     float dy = current_pose.pose.position.y - target_y;     float dz = current_pose.pose.position.z - target_z;     float distance = sqrt(dx*dx + dy*dy + dz*dz);     return distance < tolerance; } int main(int argc, char **argv) {     ros::init(argc, argv, "multi_drone_offboard");          // typhoon_h480_0     ros::NodeHandle nh_typhoon0;     ros::Subscriber state_sub_typhoon0 = nh_typhoon0.subscribe<mavros_msgs::State>             ("typhoon_h480_0/mavros/state", 10, state_cb_typhoon0);     ros::Subscriber local_pos_sub_typhoon0 = nh_typhoon0.subscribe<geometry_msgs::PoseStamped>             ("/typhoon_h480_0/mavros/local_position/pose", 10, pos_cb_typhoon0);     ros::Publisher local_pos_pub_typhoon0 = nh_typhoon0.advertise<geometry_msgs::PoseStamped>             ("/typhoon_h480_0/mavros/setpoint_position/local", 10);     ros::ServiceClient arming_client_typhoon0 = nh_typhoon0.serviceClient<mavros_msgs::CommandBool>             ("/typhoon_h480_0/mavros/cmd/arming");     ros::ServiceClient set_mode_client_typhoon0 = nh_typhoon0.serviceClient<mavros_msgs::SetMode>             ("/typhoon_h480_0/mavros/set_mode");     // typhoon_h480_1     ros::NodeHandle nh_typhoon1;     ros::Subscriber state_sub_typhoon1 = nh_typhoon1.subscribe<mavros_msgs::State>             ("typhoon_h480_1/mavros/state", 10, state_cb_typhoon1);     ros::Subscriber local_pos_sub_typhoon1 = nh_typhoon1.subscribe<geometry_msgs::PoseStamped>             ("/typhoon_h480_1/mavros/local_position/pose", 10, pos_cb_typhoon1);     ros::Publisher local_pos_pub_typhoon1 = nh_typhoon1.advertise<geometry_msgs::PoseStamped>             ("/typhoon_h480_1/mavros/setpoint_position/local", 10);     ros::ServiceClient arming_client_typhoon1 = nh_typhoon1.serviceClient<mavros_msgs::CommandBool>             ("/typhoon_h480_1/mavros/cmd/arming");     ros::ServiceClient set_mode_client_typhoon1 = nh_typhoon1.serviceClient<mavros_msgs::SetMode>             ("/typhoon_h480_1/mavros/set_mode");     // iris_0     ros::NodeHandle nh_iris0;     ros::Subscriber state_sub_iris0 = nh_iris0.subscribe<mavros_msgs::State>             ("iris_0/mavros/state", 10, state_cb_iris0);     ros::Subscriber local_pos_sub_iris0 = nh_iris0.subscribe<geometry_msgs::PoseStamped>             ("/iris_0/mavros/local_position/pose", 10, pos_cb_iris0);     ros::Publisher local_pos_pub_iris0 = nh_iris0.advertise<geometry_msgs::PoseStamped>             ("/iris_0/mavros/setpoint_position/local", 10);     ros::ServiceClient arming_client_iris0 = nh_iris0.serviceClient<mavros_msgs::CommandBool>             ("/iris_0/mavros/cmd/arming");     ros::ServiceClient set_mode_client_iris0 = nh_iris0.serviceClient<mavros_msgs::SetMode>             ("/iris_0/mavros/set_mode");     // iris_1     ros::NodeHandle nh_iris1;     ros::Subscriber state_sub_iris1 = nh_iris1.subscribe<mavros_msgs::State>             ("iris_1/mavros/state", 10, state_cb_iris1);     ros::Subscriber local_pos_sub_iris1 = nh_iris1.subscribe<geometry_msgs::PoseStamped>             ("/iris_1/mavros/local_position/pose", 10, pos_cb_iris1);     ros::Publisher local_pos_pub_iris1 = nh_iris1.advertise<geometry_msgs::PoseStamped>             ("/iris_1/mavros/setpoint_position/local", 10);     ros::ServiceClient arming_client_iris1 = nh_iris1.serviceClient<mavros_msgs::CommandBool>             ("/iris_1/mavros/cmd/arming");     ros::ServiceClient set_mode_client_iris1 = nh_iris1.serviceClient<mavros_msgs::SetMode>             ("/iris_1/mavros/set_mode");     ROS_INFO("Establishing Connection with all drones........");     ros::Rate rate(20.0);     // 等待所有无人机连接     while(ros::ok() &&           (!current_state_typhoon0.connected || !current_state_typhoon1.connected ||            !current_state_iris0.connected || !current_state_iris1.connected)) {         ros::spinOnce();         rate.sleep();     }     ROS_INFO("All drones connected!");     // 目标点设置     float final_target_x = 120.0;     float final_target_y = -150.0;     float final_target_z = 10.0;     // iris0 提前降落点     float early_landing_iris0_x = 80.0;     float early_landing_iris0_y = -100.0;     float final_target_x_iris = 100.0;     float final_target_y_iris = -170.0;          // 创建位置消息     geometry_msgs::PoseStamped pose_typhoon0, pose_typhoon1;     geometry_msgs::PoseStamped pose_iris0, pose_iris1;     // 初始化位置设定点(起飞高度)     float takeoff_height = 10.0;          pose_typhoon0.pose.position.x = pos_typhoon0.pose.position.x;     pose_typhoon0.pose.position.y = pos_typhoon0.pose.position.y;     pose_typhoon0.pose.position.z = takeoff_height;     pose_typhoon1.pose.position.x = pos_typhoon1.pose.position.x;     pose_typhoon1.pose.position.y = pos_typhoon1.pose.position.y;     pose_typhoon1.pose.position.z = takeoff_height;     pose_iris0.pose.position.x = pos_iris0.pose.position.x;     pose_iris0.pose.position.y = pos_iris0.pose.position.y;     pose_iris0.pose.position.z = takeoff_height;     pose_iris1.pose.position.x = pos_iris1.pose.position.x;     pose_iris1.pose.position.y = pos_iris1.pose.position.y;     pose_iris1.pose.position.z = takeoff_height;     ROS_INFO("Sending initial setpoints...");     // 发送初始设定点     for(int i = 100; ros::ok() && i > 0; --i) {         local_pos_pub_typhoon0.publish(pose_typhoon0);         local_pos_pub_typhoon1.publish(pose_typhoon1);         local_pos_pub_iris0.publish(pose_iris0);         local_pos_pub_iris1.publish(pose_iris1);                  ros::spinOnce();         rate.sleep();     }     // 设置offboard模式和arming服务     mavros_msgs::SetMode offb_set_mode;     offb_set_mode.request.custom_mode = "OFFBOARD";     mavros_msgs::CommandBool arm_cmd;     arm_cmd.request.value = true;     ros::Time last_request = ros::Time::now();     // 任务状态标志     bool typhoon0_takeoff_complete = false;     bool typhoon1_takeoff_complete = false;     bool iris0_takeoff_complete = false;     bool iris1_takeoff_complete = false;     bool typhoon0_mission_complete = false;     bool typhoon1_mission_complete = false;     bool iris0_mission_complete = false;     bool iris1_mission_complete = false;     ROS_INFO("Starting mission...");     while(ros::ok()) {         // 设置offboard模式         if((current_state_typhoon0.mode != "OFFBOARD" || current_state_typhoon1.mode != "OFFBOARD" ||             current_state_iris0.mode != "OFFBOARD" || current_state_iris1.mode != "OFFBOARD") &&             (ros::Time::now() - last_request > ros::Duration(5.0))) {             if(set_mode_client_typhoon0.call(offb_set_mode) && set_mode_client_typhoon1.call(offb_set_mode) &&                set_mode_client_iris0.call(offb_set_mode) && set_mode_client_iris1.call(offb_set_mode) &&                offb_set_mode.response.mode_sent) {                 ROS_INFO("Offboard enabled for all drones");             }             last_request = ros::Time::now();         } else {             // 解锁无人机             if((!current_state_typhoon0.armed || !current_state_typhoon1.armed ||                 !current_state_iris0.armed || !current_state_iris1.armed) &&                 (ros::Time::now() - last_request > ros::Duration(5.0))) {                 if(arming_client_typhoon0.call(arm_cmd) && arming_client_typhoon1.call(arm_cmd) &&                    arming_client_iris0.call(arm_cmd) && arming_client_iris1.call(arm_cmd) &&                    arm_cmd.response.success) {                     ROS_INFO("All vehicles armed");                 }                 last_request = ros::Time::now();             }         }         // 第一阶段:起飞到安全高度         if(!typhoon0_takeoff_complete) {             if(checkPositionReached(pos_typhoon0, pose_typhoon0.pose.position.x, pose_typhoon0.pose.position.y, takeoff_height, 1.0)) {                 typhoon0_takeoff_complete = true;                 ROS_INFO("typhoon_h480_0 takeoff complete");             }         }         if(!typhoon1_takeoff_complete) {             if(checkPositionReached(pos_typhoon1, pose_typhoon1.pose.position.x, pose_typhoon1.pose.position.y, takeoff_height, 1.0)) {                 typhoon1_takeoff_complete = true;                 ROS_INFO("typhoon_h480_1 takeoff complete");             }         }         if(!iris0_takeoff_complete) {             if(checkPositionReached(pos_iris0, pose_iris0.pose.position.x, pose_iris0.pose.position.y, takeoff_height, 1.0)) {                 iris0_takeoff_complete = true;                 ROS_INFO("iris_0 takeoff complete");             }         }         if(!iris1_takeoff_complete) {             if(checkPositionReached(pos_iris1, pose_iris1.pose.position.x, pose_iris1.pose.position.y, takeoff_height, 1.0)) {                 iris1_takeoff_complete = true;                 ROS_INFO("iris_1 takeoff complete");             }         }         // 第二阶段:飞往目标点         if(typhoon0_takeoff_complete && !typhoon0_mission_complete) {             pose_typhoon0.pose.position.x = final_target_x;             pose_typhoon0.pose.position.y = final_target_y;             pose_typhoon0.pose.position.z = final_target_z;                          if(checkPositionReached(pos_typhoon0, final_target_x, final_target_y, final_target_z, 2.0)) {                 ROS_INFO("typhoon_h480_0 reached final target, starting landing");                 typhoon0_mission_complete = true;             }         }         if(typhoon1_takeoff_complete && !typhoon1_mission_complete) {             pose_typhoon1.pose.position.x = final_target_x;             pose_typhoon1.pose.position.y = final_target_y;             pose_typhoon1.pose.position.z = final_target_z;                          if(checkPositionReached(pos_typhoon1, final_target_x, final_target_y, final_target_z, 2.0)) {                 ROS_INFO("typhoon_h480_1 reached final target, starting landing");                 typhoon1_mission_complete = true;             }         }         // iris_0 提前在x=50,y=-85处降落         if(iris0_takeoff_complete && !iris0_mission_complete) {             pose_iris0.pose.position.x = early_landing_iris0_x;             pose_iris0.pose.position.y = early_landing_iris0_y;             pose_iris0.pose.position.z = final_target_z;                          if(checkPositionReached(pos_iris0, early_landing_iris0_x, early_landing_iris0_y, final_target_z, 2.0)) {                 ROS_INFO("iris_0 reached early landing point, starting descent");                 // 开始下降                 pose_iris0.pose.position.z = 0.5; // 下降到地面                 if(checkPositionReached(pos_iris0, early_landing_iris0_x, early_landing_iris0_y, 0.5, 0.5)) {                     iris0_mission_complete = true;                     ROS_INFO("iris_0 landed successfully");                 }             }         }         // iris_1 飞往最终目标         if(iris1_takeoff_complete && !iris1_mission_complete) {             pose_iris1.pose.position.x = final_target_x_iris;             pose_iris1.pose.position.y = final_target_y_iris;             pose_iris1.pose.position.z = final_target_z;                          if(checkPositionReached(pos_iris1, final_target_x_iris, final_target_y_iris, final_target_z, 2.0)) {                 ROS_INFO("iris_1 reached final target, starting landing");                 iris1_mission_complete = true;             }         }         // 第三阶段:降落         if(typhoon0_mission_complete) {             pose_typhoon0.pose.position.z = 0.5; // 下降到地面         }         if(typhoon1_mission_complete) {             pose_typhoon1.pose.position.z = 0.5; // 下降到地面         }         if(iris1_mission_complete) {             pose_iris1.pose.position.z = 0.5; // 下降到地面         }         // 发布所有位置设定点         local_pos_pub_typhoon0.publish(pose_typhoon0);         local_pos_pub_typhoon1.publish(pose_typhoon1);         local_pos_pub_iris0.publish(pose_iris0);         local_pos_pub_iris1.publish(pose_iris1);         ros::spinOnce();         rate.sleep();     }     return 0; }帮我检查是否有问题,这里typhoon无人机比较特殊,希望你仔细检查,但不要吹毛求疵。因为我在运行cpp文件时遇到这种问题“dell@node01:~/catkin_ws$ rosrun offboard offboard_sin_node [ INFO] [1762609701.415953640]: Establishing Connection with all drones........ [ INFO] [1762609701.697483242, 226.728000000]: All drones connected! [ INFO] [1762609701.697523195, 226.728000000]: Sending initial setpoints... [ INFO] [1762609706.699555750, 231.728000000]: Starting mission... [ INFO] [1762609709.563074280, 234.578000000]: typhoon_h480_1 takeoff complete [ INFO] [1762609709.963499081, 234.978000000]: typhoon_h480_0 takeoff complete [ INFO] [1762609711.769315981, 236.782000000]: Offboard enabled for all drones [ INFO] [1762609716.822062578, 241.832000000]: Offboard enabled for all drones [ INFO] [1762609721.872602370, 246.880000000]: Offboard enabled for all drones [ INFO] [1762609726.928338924, 251.934000000]: Offboard enabled for all drones”一直是Offboard enabled for all drones
11-09
要检查 ROS C++ 代码中 typhoon 无人机相关部分是否有问题,以及解决一直显示 `Offboard enabled for all drones` 的问题,可按以下方法进行。 ### 代码检查 #### 1. 检查通信部分 确保与 typhoon 无人机的通信正常,如 MAVROS 话题订阅和发布是否正确。以下是一个简单的 MAVROS 话题发布示例: ```cpp #include <ros/ros.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; } int main(int argc, char **argv) { ros::init(argc, argv, "offboard_node"); ros::NodeHandle nh; ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/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"); //the setpoint publishing rate MUST be faster than 2Hz ros::Rate rate(20.0); // wait for FCU connection while(ros::ok() && !current_state.connected){ ros::spinOnce(); rate.sleep(); } geometry_msgs::PoseStamped pose; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 2; //send a few setpoints before starting for(int i = 100; ros::ok() && i > 0; --i){ local_pos_pub.publish(pose); 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; ros::Time last_request = ros::Time::now(); while(ros::ok()){ if( current_state.mode != "OFFBOARD" && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ ROS_INFO("Offboard enabled"); } last_request = ros::Time::now(); } else { if( !current_state.armed && (ros::Time::now() - last_request > ros::Duration(5.0))){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){ ROS_INFO("Vehicle armed"); } last_request = ros::Time::now(); } } local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } return 0; } ``` #### 2. 检查逻辑部分 查看代码中是否存在逻辑错误,例如在不恰当的时机多次触发 Offboard 模式启用。确保只有在满足特定条件时才请求切换到 Offboard 模式。 #### 3. 检查参数设置 检查与 Offboard 模式相关的参数设置,如控制频率、超时时间等,确保参数设置合理。 ### 解决一直显示 `Offboard enabled for all drones` 的问题 - **检查 Offboard 模式切换逻辑**:确保代码中不会在不必要的情况下重复请求切换到 Offboard 模式。例如,避免在每次循环中都请求切换模式。 - **检查通信稳定性**:不稳定的通信可能导致无人机频繁丢失 Offboard 模式,从而使代码不断尝试重新启用。检查网络连接、MAVROS 配置等。 - **检查无人机状态反馈**:确保代码正确处理无人机的状态反馈,避免因错误的状态判断而重复启用 Offboard 模式。 ### 示例代码优化 以下是优化后的代码,避免不必要的 Offboard 模式切换: ```cpp #include <ros/ros.h> #include <mavros_msgs/CommandBool.h> #include <mavros_msgs/SetMode.h> #include <mavros_msgs/State.h> mavros_msgs::State current_state; void state_cb(const mavros_msgs::State::ConstPtr& msg){ current_state = *msg; } int main(int argc, char **argv) { ros::init(argc, argv, "offboard_node"); ros::NodeHandle nh; ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State> ("mavros/state", 10, state_cb); ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped> ("mavros/setpoint_position/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"); //the setpoint publishing rate MUST be faster than 2Hz ros::Rate rate(20.0); // wait for FCU connection while(ros::ok() && !current_state.connected){ ros::spinOnce(); rate.sleep(); } geometry_msgs::PoseStamped pose; pose.pose.position.x = 0; pose.pose.position.y = 0; pose.pose.position.z = 2; //send a few setpoints before starting for(int i = 100; ros::ok() && i > 0; --i){ local_pos_pub.publish(pose); 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; bool offboard_enabled = false; bool armed = false; while(ros::ok()){ if(!offboard_enabled && current_state.mode != "OFFBOARD"){ if( set_mode_client.call(offb_set_mode) && offb_set_mode.response.mode_sent){ ROS_INFO("Offboard enabled"); offboard_enabled = true; } } if(offboard_enabled && !armed && !current_state.armed){ if( arming_client.call(arm_cmd) && arm_cmd.response.success){ ROS_INFO("Vehicle armed"); armed = true; } } local_pos_pub.publish(pose); ros::spinOnce(); rate.sleep(); } return 0; } ```
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
红包 添加红包
表情包 插入表情
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

红包个数最小为10个

红包金额最低5元

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

抵扣说明:

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

余额充值