inline constexpr bool operator>=(const position<TTag, TDistance1>& x, const position<TTag, TDistance2>& y) { return x.distance_from_origin() >= y.distance_from_origin(); }
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(); }
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 }
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(); }
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)); }
inline constexpr position<TTag, TToDistance> position_cast(const position<TTag, TFromDistance>& pos) { return position<TTag, TToDistance>(distance_cast<TToDistance>( pos.distance_from_origin())); }
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; }
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); }
/// 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; }
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; }
/// 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"}; }
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++; }
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); } }
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; }
//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)); }
location location::span(position b, unsigned n) { return location(b, position(b.row(), b.col() + n)); }
bool position::operator<(const position &other) const { if (row() < other.row()) return true; if (row() > other.row()) return false; return col() < other.col(); }
nodeptr toPointer(position P) //return the pointer saved in position P { return P.pointer(); }
bool operator<(const position& a, const position& b) { assert(&a.source() == &b.source()); return a.index() < b.index(); }
//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(); }
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; }
constexpr position(const position<tag, TDistance2>& pos) : m_distance(pos.distance_from_origin()) { }