本文整理汇总了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;