本文整理汇总了C++中position::pos_f方法的典型用法代码示例。如果您正苦于以下问题:C++ position::pos_f方法的具体用法?C++ position::pos_f怎么用?C++ position::pos_f使用的例子?那么恭喜您, 这里精选的方法代码示例或许可以为您提供帮助。您也可以进一步了解该方法所在类position
的用法示例。
在下文中一共展示了position::pos_f方法的3个代码示例,这些例子默认根据受欢迎程度排序。您可以为喜欢或者感觉有用的代码点赞,您的评价将有助于系统推荐出更棒的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;
}
示例2: main
int main(int argc, char **argv)
{
ros::init(argc, argv, "pingpong");
ros::NodeHandle n;
ros::Publisher cmd_pub = n.advertise<geometry_msgs::Twist>("/cmd_vel", 1);
ros::Publisher takeoff_pub = n.advertise<std_msgs::Empty>("/ardrone/takeoff", 1);
ros::Publisher land_pub = n.advertise<std_msgs::Empty>("/ardrone/land", 1);
ros::Publisher stop_pub = n.advertise<std_msgs::Empty>("/ardrone/reset", 1);
rawpos_b_pub = n.advertise<geometry_msgs::PoseStamped>("/ardrone/rawpos_b", 1);
rawpos_f_pub = n.advertise<geometry_msgs::PoseStamped>("/ardrone/rawpos_f", 1);
ros::Publisher ctrl_pub = n.advertise<flight_strategy::ctrl>("ctrl", 1);
ros::Subscriber ctrlBack_sub = n.subscribe("ctrlBack", 1, ctrlBack_Callback);
ros::Subscriber robot_info_sub = n.subscribe("/ardrone/robot_info", 1, robot_info_Callback);
ros::Subscriber position_reset_info_sub = n.subscribe("/ardrone/position_reset_info", 1, position_reset_info_Callback);
ros::Subscriber nav_sub = n.subscribe("/ardrone/navdata", 1, navCallback);
ros::Subscriber altitude_sub = n.subscribe("/ardrone/navdata_altitude", 1, altitudeCallback);
ros::Subscriber yaw_sub = n.subscribe("/ardrone/yaw", 1, yawCallback);
//ros::Subscriber catch_pos_sub = n.subscribe("", 1, catch_pos_Callback);
ros::Subscriber robot_pos_sub = n.subscribe("/ardrone/robot_position", 1, robot_pos_Callback);
ros::Rate loop_rate(LOOP_RATE);
std_msgs::Empty order;
//geometry_msgs::Twist cmd;
catch_position(0) = -1000;
catch_position(1) = -1000;
robot.pos_f[0] = -1.95;
robot.pos_f[1] = 0;
while(ros::ok())
{
switch(flight.state){
case STATE_IDLE:{
if(flight.last_state != flight.state){
ROS_INFO("State to IDLE\n");
}
flight.last_state = flight.state;
flight.state = STATE_TAKEOFF;
break;
}
case STATE_TAKEOFF:{
if(flight.last_state != flight.state){
ROS_INFO("State TAKEOFF\n");
takeoff_pub.publish(order);
}
flight.last_state = flight.state;
if(flight.drone_state != 6){
//fly to center
flight.state = STATE_LOCATING;
}
break;
}
case STATE_LOCATING:{
static unsigned int loop_times = 0;
flight_strategy::ctrl ctrl_msg;
if(flight.last_state != flight.state){
ROS_INFO("State LOCATING\n");
flight.last_state = flight.state;
ctrl_msg.enable = 1;
ctrl_msg.mode = NORMAL_CTL;
ctrl_msg.pos_halt[0] = 1;
ctrl_msg.pos_halt[1] = 1;
ctrl_msg.pos_halt[2] = 1;
ctrl_pub.publish(ctrl_msg);
ctrl.flag_arrived = false;
}else
{
if(!arrived_inaccurate){
ctrl_msg.enable = 1;
ctrl_msg.mode = NORMAL_CTL;
ctrl_msg.pos_sp[0] = -1.95;
ctrl_msg.pos_sp[1] = 0;
ROS_INFO("POSITION:%f %f",pos.pos_f(0),pos.pos_f(1));
ROS_INFO("State LOCATING:%f %f",ctrl_msg.pos_sp[0],ctrl_msg.pos_sp[1]);
ctrl_msg.pos_halt[0] = 0;
ctrl_msg.pos_halt[1] = 0;
ctrl_msg.pos_halt[2] = 1;
ctrl_pub.publish(ctrl_msg);
delay_counter ++;
if(delay_counter > 10)
{
delay_counter = 0;
if(ctrl.flag_arrived){
arrived_inaccurate = true;
ROS_INFO("POSITION RESET POSITION");
}
}
}
if(arrived_inaccurate)
{
ros::Duration dur;
dur = ros::Time::now() - position_reset_stamp;
//.........这里部分代码省略.........
示例3: altitudeCallback
//altitude of sonar
void altitudeCallback(const ardrone_autonomy::navdata_altitude &msg)
{
pos.pos_f(2) = msg.altitude_vision/1000.0;
//ROS_INFO("altitude:%f",pos.pos_f(2));
}