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())
{
}
