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;
}
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;
}
//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));
}