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


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

本文整理汇总了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;
}
开发者ID:SJTU-Multicopter,项目名称:flight-strategy,代码行数:48,代码来源:strategy.cpp

示例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;
//.........这里部分代码省略.........
开发者ID:SJTU-Multicopter,项目名称:flight-strategy,代码行数:101,代码来源:strategy.cpp

示例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));
}
开发者ID:SJTU-Multicopter,项目名称:flight-strategy,代码行数:6,代码来源:strategy.cpp


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