当前位置: 首页>>代码示例>>C++>>正文


C++ position::self_located方法代码示例

本文整理汇总了C++中position::self_located方法的典型用法代码示例。如果您正苦于以下问题:C++ position::self_located方法的具体用法?C++ position::self_located怎么用?C++ position::self_located使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在position的用法示例。


在下文中一共展示了position::self_located方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。

示例1: main


//.........这里部分代码省略.........
					if(arrived_inaccurate)
					{
						ros::Duration dur;
						dur = ros::Time::now() - position_reset_stamp;

						if(dur.toSec() > 0.2){
							loop_times++;
							if(loop_times > 100){
								loop_times = 0;	
								if(pos.pos_f(2) < 1.6){
									ctrl_msg.pos_sp[2] = pos.pos_f(2) + 0.5;
									ctrl_msg.pos_halt[0] = 1;
									ctrl_msg.pos_halt[1] = 1;
									ctrl_msg.pos_halt[2] = 0;
									ctrl_msg.enable = 1;
									ctrl_msg.mode = 0;
									ctrl_pub.publish(ctrl_msg);

									ROS_INFO("HIGHER:%f",ctrl_msg.pos_sp[2]);
								}	
							}
							ROS_INFO("HEIGHT:%f",pos.pos_f(2));

						}else{
							ctrl_msg.enable = 1;
							ctrl_msg.mode = 3;
							ctrl_msg.pos_sp[0] =  pos.pos_img(0); //blue point position???
							ctrl_msg.pos_sp[1] =  pos.pos_img(1);
							ctrl_msg.pos_halt[0] = 0;
							ctrl_msg.pos_halt[1] = 0;
							ctrl_msg.pos_halt[2] = 1;
							ctrl_pub.publish(ctrl_msg);

							if(pos.self_located()){
								ctrl.flag_arrived = false;
								arrived_inaccurate = false;
								arrived_height = false;
								loop_times = 0;
								flight.state = STATE_STANDBY;
								pos.pos_f(0) = -1.95;
								pos.pos_f(1) = 0;
								pos.pos_b = pos.R_field_body.transpose() * pos.pos_f;
								pos_update(0) = -1.95;
								pos_update(1) = 0;
								break;
							}
						}
					}
				}
								

				break;
			}
			case STATE_STANDBY:{
				flight_strategy::ctrl ctrl_msg;
				if(flight.last_state != flight.state){
					ROS_INFO("State STANDBY\n");
				}
				flight.last_state = flight.state;

				if(!arrived_height)
				{
					ctrl_msg.enable = 1;
					ctrl_msg.mode = 3;
					ctrl_msg.pos_sp[2] = 1.5;
					ctrl_msg.pos_halt[0] = 0;
开发者ID:SJTU-Multicopter,项目名称:flight-strategy,代码行数:67,代码来源:strategy.cpp


注:本文中的position::self_located方法示例由纯净天空整理自Github/MSDocs等开源代码及文档管理平台,相关代码片段筛选自各路编程大神贡献的开源项目,源码版权归原作者所有,传播和使用请参考对应项目的License;未经允许,请勿转载。