Пример #1
0
inline constexpr
bool
operator>=(const position<TTag, TDistance1>& x,
           const position<TTag, TDistance2>& y)
{
    return x.distance_from_origin() >= y.distance_from_origin();
}
Пример #2
0
inline constexpr
typename SIDIM_STD::common_type<TDistance1, TDistance2>::type
operator-(const position<TTag, TDistance1>& x,
          const position<TTag, TDistance2>& y)
{
    return x.distance_from_origin() - y.distance_from_origin();
}
Пример #3
0
void init(int       argc,
          char      **argv,
          uint      displayMode,
          position  initPos,
          size      initSize,
          string    windowTitle
          )
{
    glutInit(&argc, argv);
    //Simple buffer
    glutInitDisplayMode(displayMode);
    glutInitWindowPosition(initPos.getX(),initPos.getY());
    glutInitWindowSize(initSize.getWidth(),initSize.getHeight());
    glutCreateWindow(windowTitle.c_str());
    initRendering();

    glutDisplayFunc(draw);
    glutIdleFunc(animate);

    glutKeyboardFunc(keyboardListener);
    glutSpecialFunc(specialKeyListener);


    glutMouseFunc(mouseListener);

    //Call to the drawing function


}
Пример #4
0
Shell::Shell(position startPos) // Constructor
{
	pos.set(startPos.getX(), startPos.getY(), startPos.getTh());
	seeTank = false;
	seeAi = false;
	float x,y;
	x = pos.getX();
	y = pos.getY();
	updateBb();
}
Пример #5
0
	position treeSearch(object e,position P) //return a position after searching an element
	{
		if (isExternal(P))
			return P;

		if (P.element()==e)
			return P;
		else if (e<P.element())
			return treeSearch(e,leftChild(P));
		else 
			return treeSearch(e,rightChild(P));
	}
Пример #6
0
inline constexpr
position<TTag, TToDistance>
position_cast(const position<TTag, TFromDistance>& pos)
{
    return position<TTag, TToDistance>(distance_cast<TToDistance>(
                            pos.distance_from_origin()));
}
Пример #7
0
bool
RegionFilteredSensor::filterContact(const position & boundaryPos0, const position & boundaryPos1, const position & ps){
	double p0x = boundaryPos0.x();
	double p0y = boundaryPos0.y();
	double p0z = boundaryPos0.z();
	double p1x = boundaryPos1.x();
	double p1y = boundaryPos1.y();
	double p1z = boundaryPos1.z();

	if((p0x <= ps[0] && p0y <= ps[1] && p0z <= ps[2]) &&
		(p1x >= ps.x() && p1y >= ps.y() && p1z >= ps.z())){
		return true;
	}
	return false;
}
Пример #8
0
inline constexpr
position<TTag,
         typename SIDIM_STD::common_type<TDistance1, distance<TRep2, TInterval2> >::type>
operator-(const position<TTag, TDistance1>& pos,
          const distance<TRep2, TInterval2>& d)
{
    using common_distance = typename SIDIM_STD::common_type<
                                TDistance1, distance<TRep2, TInterval2> >::type;

    return position<TTag, common_distance>(pos.distance_from_origin() - d);
}
Пример #9
0
/// Returns true if the specified position is inside (inclusive)
/// the region.
///
/// @param[in] p Point to test.
/// @retval true Point is in the region or on regions boundary.
/// @retval false Point outside the region.
///
bool region::inside(const position & p) const
{
	// testing latitude is easy, there is no date line, no wrap-arround
	if (p.lat() > p0_.lat())
		return false;
	if (p.lat() < p1_.lat())
		return false;

	// testint longitude

	// shifted longitudes, now between 0..360 (date line, eastwards)
	const double plon_s = 180.0 + p.lon();
	const double p0lon_s = 180.0 + p0_.lon();
	const double p1lon_s = 180.0 + p1_.lon();

	// p is west of p0, but not west enough to reach p1
	if ((plon_s < p0lon_s) && (plon_s > p1lon_s))
		return false;

	return true;
}
Пример #10
0
bool operator<(const position & a, const position & b)
{
    if(a.x() < b.x() || a.y() < b.y() || a.z() < b.z())
        return true;
    return false;

}
Пример #11
0
/// Initializes the region by the specified corners
///
/// @code
/// a0
/// +--------------+
/// |              |
/// |              |
/// |              |
/// +--------------+ a1
/// @endcode
///
/// @note There is no sorting of coordinates, the positions a0/a1 must
///       already be top/left,bottom/right ordering. There is no automatic
///       way to sort them because of the international date line.
///
/// @param[in] a0 Position top/left
/// @param[in] a1 Position bottom/right
/// @exception std::invalid_argument Positions are not correct. The latitude
///   of the second point <tt>p1</tt> is northerly than <tt>p0</tt>. Or positions
///   are party or fully the same.
///
region::region(const position & a0, const position & a1)
	: p0_(a0)
	, p1_(a1)
{
	if ((a0.lat() == a1.lat()) || (a0.lon() == a1.lon()))
		throw std::invalid_argument{"specified region lacks a dimension"};
	if (a0.lat() < a1.lat())
		throw std::invalid_argument{"specified region is upside down"};
}
Пример #12
0
void add(){
  node *prevlast = 0;
  while (last.pos == len && last.to != root)
    last = go(last.to->p->lnk,last.to->L,last.pos);
  while (!last.can(s[len]-'a')) {
    node* lastnode = split(last.to, last.pos);
    addleaf(lastnode,len);
    if (prevlast) prevlast->lnk = lastnode;
    prevlast = lastnode;
    if (lastnode == root) {
      last = position(root,0);
      break;
    }
    assert(lastnode->p->lnk);
    last = go(lastnode->p->lnk,lastnode->L,last.pos);
  }
  if (prevlast && !prevlast->lnk){
    assert(last.to && last.pos == last.to->R);
    prevlast->lnk = last.to;
  }
  assert(last.can(s[len]-'a'));
  last.next(s[len]-'a');
  len++;
}
Пример #13
0
std::vector<int> person::choose_move (position &pos, bool p)
{
	in_out[options[0]]->draw(pos);
	in_out[options[0]]->message("You are ");
	if (p)
	{	
		in_out[options[0]]->message("red");
	}
	else
	{
		in_out[options[0]]->message("black");
	}
	in_out[options[0]]->message(".\n\nWhich piece do you want to move?\n");
	std::vector<int> o = in_out[options[0]]->choose_point();
	if (pos.board[o[0]][o[1]] * (2 *!p - 1) <= 0)
	{
		in_out[options[0]]->message("Please choose one of your pieces.\n\n");
		return choose_move(pos, p);
	}
	in_out[options[0]]->message("\nWhere do you want to move it?\n");
	std::vector<int> n = in_out[options[0]]->choose_point();
	std::vector<int> m;
	bool is_valid = false;
	std::vector<std::vector<int> > z;
	pos.valid_moves(z, p);
	for (unsigned int i = 0; i < z.size(); i ++)
	{
		if ((o[0] == z[i][0]) && (o[1] == z[i][1]) && (n[0] == z[i][2]) && (n[1] == z[i][3]))
		{
			is_valid = true;
		}
	}
	if (is_valid)
	{
		m.push_back(o[0]);
		m.push_back(o[1]);
		m.push_back(n[0]);
		m.push_back(n[1]);
		return m;
	}
	else
	{
		in_out[options[0]]->message("\nPlease choose a valid move.\n\n");
		return choose_move(pos, p);
	}
}
Пример #14
0
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;
}
Пример #15
0
//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));
}
Пример #16
0
location location::span(position b, unsigned n) {
    return location(b, position(b.row(), b.col() + n));
}
Пример #17
0
bool position::operator<(const position &other) const {
    if (row() < other.row()) return true;
    if (row() > other.row()) return false;
    return col() < other.col();
}
Пример #18
0
	nodeptr toPointer(position P) //return the pointer saved in position P
	{
		return P.pointer();
	}
Пример #19
0
bool operator<(const position& a, const position& b) {
    assert(&a.source() == &b.source());
    return a.index() < b.index();
}
Пример #20
0
//circle position
void position_reset_info_Callback(const image_process::drone_info msg)
{
    pos.pos_img(0) = msg.pose.x;
    pos.pos_img(1) = msg.pose.y;
    position_reset_stamp = ros::Time::now();
}
Пример #21
0
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;

						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;
					ctrl_msg.pos_halt[1] = 0;
					ctrl_msg.pos_halt[2] = 0;
					ctrl_pub.publish(ctrl_msg);
					ROS_INFO("State STANDBY,Height%f",pos.pos_f(2));
				}else{
					ros::Duration dur_r, dur_p;
					dur_r = ros::Time::now() - robot_stamp;
					dur_p = ros::Time::now() - position_reset_stamp;
					if(dur_r.toSec() < 0.2){
						flight.state = STATE_REVOLVING;
						arrived_height = false;
						break;
					}
					if(catch_position(0) < -900 || fabs(catch_position(1) > 1.9))
					{
						if(dur_p.toSec() < 0.2){
							ROS_INFO("LOCATING");
							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_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);

								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;
								self_located = true;
								initial_located = true;
								ROS_INFO("Self_located");
							}
						}else{
							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);
							ROS_INFO("NO POINT");
						}
					}
					else{//robot_coming()){
						flight.state = STATE_FLYING_TO_CATCH;
						arrived_height = false;
						break;
					}
				}
				if(!arrived_height){
					if(ctrl.flag_img_height_arrived){
						arrived_height = true;
						ROS_INFO("HEIGHT:1.5m");
					} 	
				}

				break;
			}
			case STATE_FLYING_TO_CATCH:{
				if(flight.last_state != flight.state){
					ROS_INFO("State FLY_TO_CATCH\n");
				}
				flight.last_state = flight.state;
				flight_strategy::ctrl ctrl_msg;
				ros::Duration dur_r, dur_p;
				dur_r = ros::Time::now() - robot_stamp;
				dur_p = ros::Time::now() - position_reset_stamp;
				if(dur_r.toSec() < 0.2){
					flight.state = STATE_REVOLVING;
					arrived_inaccurate = false;
					self_located = false;
					break;
				}

				if(catch_position(0) < -900 || fabs(catch_position(1) > 1.9)){
					if(dur_p.toSec() < 0.2){
						ROS_INFO("LOCATING");
						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_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);

							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;
							self_located = true;
							initial_located = true;
							ROS_INFO("Self_located");
						}
					}else{
						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);
						ROS_INFO("NO POINT");
					}
				}else{
					if(moving) 
					{
						arrived_inaccurate = false;
					}

					if(!arrived_inaccurate){
						ctrl_msg.mode = NORMAL_CTL;
						ctrl_msg.enable = 1;
						ctrl_msg.pos_sp[0] =  catch_position(0);
						ctrl_msg.pos_sp[1] =  catch_position(1);
						ctrl_msg.pos_halt[0] = 0;
						ctrl_msg.pos_halt[1] = 0;
						ctrl_msg.pos_halt[2] = 1;
						ctrl_pub.publish(ctrl_msg);
						ROS_INFO("POSITION:%f   %f",pos.pos_f(0),pos.pos_f(1));
						ROS_INFO("FLY TO CATCH:%f   %f",catch_position(0),catch_position(1));


						if(!arrived_inaccurate){
							if(ctrl.flag_arrived){
								arrived_inaccurate = true;
								ROS_INFO("FLY TO CATCH, arrived");
							} 
						}
					}else{
						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);
						ROS_INFO("WAITING");
					}	
				}

				break;
			}
			case STATE_REVOLVING:{
				if(flight.last_state != flight.state){
					ROS_INFO("State REVOLVING\n");
				}
				flight.last_state = flight.state;

				ros::Duration dur;
				dur = ros::Time::now() - robot_stamp;
				if(dur.toSec() > 0.2){
					flight_strategy::ctrl ctrl_msg;
					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);
				}else{
					flight_strategy::ctrl ctrl_msg;
					ctrl_msg.enable = 1;
					ctrl_msg.mode = 3;
					ctrl_msg.pos_sp[0] = robot.pos_b[0];
					ctrl_msg.pos_sp[1] = robot.pos_b[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(robot.whole){
					ROS_INFO("yaw_field:%f",robot.yaw_field);
					//robot_at_desired_angle
					if(fabs(robot.yaw_field) >= 0 && fabs(robot.yaw_field) < 45){
					//if(fabs(robot.yaw_field) > 10 && fabs(robot.yaw_field) < 45){
						flight.state = STATE_FLYING_AWAY;
						ROS_INFO("AAA");
						// Update the position of the drone
						pos.pos_f(0) = robot.pos_f[0];
						pos.pos_f(1) = robot.pos_f[1];
						pos.pos_b = pos.R_field_body.transpose() * pos.pos_f;
						pos_update(0) = robot.pos_f[0];
						pos_update(1) = robot.pos_f[1];
						ROS_INFO("State REVOLVING, Position:%f   %f",pos.pos_f(0),pos.pos_f(1));
						break;
					}
				}
				if(robot.pos_f[0] > 0.1){
					flight.state = STATE_FLYING_AWAY;
					ROS_INFO("TEST%f", robot.pos_f[0]);
					break;
				}
				break;
			}
			case STATE_FLYING_AWAY:{
				if(flight.last_state != flight.state){
					ROS_INFO("State FLYING_AWAY\n");

					flight.last_state = flight.state;
					flight_strategy::ctrl ctrl_msg;
					ctrl_msg.enable = 1;
					ctrl_msg.mode = NORMAL_CTL;
					ctrl_msg.pos_sp[0] = pos.pos_f(0)-0.8;
					ctrl_msg.pos_sp[1] = pos.pos_f(1);
					ctrl_msg.pos_halt[0] = 0;
					ctrl_msg.pos_halt[1] = 0;
					ctrl_msg.pos_halt[2] = 1;
					ctrl_pub.publish(ctrl_msg);
					// Update the current position
					pos_update(0) = pos_update(0) - 0.8;
					pos_update(1) = pos_update(1);
					
				}

				if(ctrl.flag_arrived){
					flight.state = STATE_LOCATING;
					ctrl.flag_arrived = false;
					flight_strategy::ctrl ctrl_msg;
					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);
					ROS_INFO("State FLYING_AWAY, UPDATE Position:%f   %f",pos_update(0),pos_update(1));
					ROS_INFO("State FLYING_AWAY, Position:%f   %f",pos.pos_f(0),pos.pos_f(1));
					break;
				}
				break;
			}
			case STATE_ON_GROUND:{
				if(flight.last_state != flight.state){
					ROS_INFO("State ON_GROUND\n");
				}
				flight.last_state = flight.state;
				break;
			}
		}
		ros::spinOnce();
		loop_rate.sleep();
	}

	return 0;
}
Пример #22
0
 constexpr
 position(const position<tag, TDistance2>& pos)
     : m_distance(pos.distance_from_origin())
 {
 }