本文整理汇总了C++中position::pos_b方法的典型用法代码示例。如果您正苦于以下问题:C++ position::pos_b方法的具体用法?C++ position::pos_b怎么用?C++ position::pos_b使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类position
的用法示例。
在下文中一共展示了position::pos_b方法的1个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的C++代码示例。
示例1: navCallback
void navCallback(const ardrone_autonomy::Navdata &msg)
{
static bool start=true;
static float last_time = 0;
if(start){
start = false;
pos.pos_f(0) = -1.95;
pos.pos_f(1) = 0;
pos.pos_b(0) = -1.95;
pos.pos_b(1) = 0;
last_time = msg.tm;
}
geometry_msgs::PoseStamped rawpos_b;
geometry_msgs::PoseStamped rawpos_f;
Vector3f raw_v ;
raw_v(0) = msg.vx;
raw_v(1) = msg.vy;
raw_v(2) = msg.vz;
float dt = (msg.tm - last_time)/1000000.0;
last_time = msg.tm;
pos.pos_b += raw_v * dt / 1000.0;
pos.get_R_field_body(pos.yaw/57.3);
pos.pos_f = pos.R_field_body * pos.pos_b;
rawpos_b.pose.position.x = pos.pos_b(0);
rawpos_b.pose.position.y = pos.pos_b(1);
rawpos_b.pose.position.z = pos.pos_b(2);
rawpos_f.pose.position.x = pos.pos_f(0);
rawpos_f.pose.position.y = pos.pos_f(1);
rawpos_f.pose.position.z = pos.pos_f(2);
rawpos_b_pub.publish(rawpos_b);
rawpos_f_pub.publish(rawpos_f);
// 0: Unknown
// 1: Inited
// 2: Landed
// 3,7: Flying
// 4: Hovering
// 5: Test (?)
// 6: Taking off
// 8: Landing
// 9: Looping (?)
flight.drone_state = msg.state;
}