bool set_new_pose(this_basic::SetPose::Request &req,
this_basic::SetPose::Response &res)
{
publish_pose(req.x,req.y,req.theta);
res.success = true;
return res.success;
}
void PosefromTarget::interpret() {
/*...... Taken from the url: https://github.com/jlblancoc/mrpt/blob/master/libs/topography/src/conversions.cpp ........*/
static const precnum_t a = 6378137L; // Semi-major axis of the Earth (meters)
static const precnum_t b = 6356752.3142L; // Semi-minor axis:
static const precnum_t ae = acos(b/a); // eccentricity:
static const precnum_t cos2_ae_earth = (cos(ae))*(cos(ae)); // The cos^2 of the angular eccentricity of the Earth: // 0.993305619995739L;
static const precnum_t sin2_ae_earth = (sin(ae))*(sin(ae)); // The sin^2 of the angular eccentricity of the Earth: // 0.006694380004261L;
precnum_t lon = ( precnum_t(current.longitude)*22/7/180 );
precnum_t lat = ( precnum_t(current.latitude)*22/7/180 );
precnum_t N = a / std::sqrt( 1 - sin2_ae_earth*( sin(lat) )*( sin(lat) ) );
pose.position.x = (N+current.altitude)*cos(lat)*cos(lon);
pose.position.y = (N+current.altitude)*cos(lat)*sin(lon);
pose.position.z = (cos2_ae_earth*N+current.altitude)*sin(lat);
lon = ( precnum_t(target.longitude)*22/7/180 );
lat = ( precnum_t(target.latitude) *22/7/180);
N = a / std::sqrt( 1 - sin2_ae_earth*( sin(lat) )*( sin(lat) ) );
temp.position.x = (N+target.altitude)*cos(lat)*cos(lon);
temp.position.y = (N+target.altitude)*cos(lat)*sin(lon);
temp.position.z = (cos2_ae_earth*N+target.altitude)*sin(lat);
const double clat = cos((current.latitude*22/7/180)), slat = sin((current.latitude*22/7/180));
const double clon = cos((current.longitude*22/7/180)), slon = sin((current.longitude*22/7/180));
temp.position.x -= pose.position.x;
temp.position.y -= pose.position.y;
temp.position.z -= pose.position.z;
pose.position.x = -slon*temp.position.x + clon*temp.position.y;
pose.position.y = -clon*slat*temp.position.x -slon*slat*temp.position.y + clat*temp.position.z;
pose.position.z = clon*clat*temp.position.x + slon*clat*temp.position.y +slat*temp.position.z;
posestamped.pose.position.x = pose.position.x;
posestamped.pose.position.y = pose.position.y;
posestamped.pose.position.z = pose.position.z;
publish_pose();
}
void pose_esti::navCallback(const ardrone_pose_estimation::NavdataConstPtr& nav_msg)
{
std_msgs::String str;
navdata = *nav_msg;
now = ros::Time::now();
if(ptam_init_commanded && vslam_states.data =="PTAM_uninitialized")
{
goal_pose.z+=HEIGHT_TICK;
str.data = "ptam_initial";
ptam_init_pub.publish(str);
}
if(take_off&& !pick_takeoff_time)
{
cout<<"Pick takeoff time"<<endl;
takeoff_start = now;
pick_takeoff_time=true;
}
if(!pick_init_yaw)
{
pick_init_yaw=true;
init_yaw = deg2rad(nav_msg->rotZ);
kalman.init();
cout<<"init_yaw = "<<init_yaw<<endl;
}
double pitch,roll,vx,vy,z,vx_medi,vy_medi,vx_esti,vy_esti;
if(init)
{
//Radian
//yaw = deg2rad(nav_msg->rotZ)-init_yaw;
cur_measured_yaw = deg2rad(nav_msg->rotZ);
pitch = deg2rad(nav_msg->rotY);
roll = deg2rad(nav_msg->rotX);
vx = nav_msg->vx;
vy = nav_msg->vy;
z = nav_msg->altd;
cur_pose.z = nav_msg->altd; //metre
measure_vel(0) = vx;
measure_vel(1) = vy;
double comps_yaw;
ros::Duration nav_ts = now-past;
ts = nav_ts.toSec();
//cout<<"ts = "<<ts<<endl;
comps_yaw=compensate_yaw(cur_measured_yaw,init_yaw);
yaw_test=comps_yaw;
median_vel=kalman.median(measure_vel,&median_flag);
cur_pose.x = prev_pose.x + ts*(cos(comps_yaw)*vx-sin(comps_yaw)*vy); //metre
cur_pose.y = prev_pose.y + ts*(sin(comps_yaw)*vx+cos(comps_yaw)*vy); //metre
if(median_flag)
{
vx_medi= median_vel(0);
vy_medi= median_vel(1);
measure << ptam_pose.pose.pose.position.x,
ptam_pose.pose.pose.position.y,
median_vel(0),
median_vel(1);
esti=kalman.process(measure,comps_yaw,&invertable);
if(invertable)
{
vx_esti=esti(2);
vy_esti=-esti(3);
fprintf(fp_vel,"%f,%f,%f,%f,%f,%f,%f\n",median_now.toSec(),vx,vy,esti(2),esti(3),median_vel(0),median_vel(1));
if(vslam_states.data=="PTAM_initialized") publish_kf_transform();
// Taking off to hovering takes roughly 4 sec after sending
// takeoff command.
if( now.toSec() - takeoff_start.toSec() >4 && pick_takeoff_time)
{
double cmd_temp_z;
cmd_temp_z=pid_z.updatePid(z-goal_pose.z,now-past);
////////////////////////////////
// Yaw control
////////////////////////////////
/////////////////////////////////////////////////////
// Horizontal velocity and position control
/////////////////////////////////////////////////////
double cmd_temp_vx=0;
double cmd_temp_x=0;
double cmd_temp_vy=0;
double cmd_temp_y=0;
pid_pos_x.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]); //p , i , d, i_max, i_min
pid_pos_y.setGains(pos_gain[P_GAIN], pos_gain[I_GAIN], pos_gain[D_GAIN], pos_gain[I_MAX], pos_gain[I_MIN]); //p , i , d, i_max, i_min
pid_vel_x.setGains(vel_gain[P_GAIN], vel_gain[I_GAIN], vel_gain[D_GAIN], vel_gain[I_MAX], vel_gain[I_MIN]); //p , i , d, i_max, i_min
pid_vel_y.setGains(vel_gain[P_GAIN], vel_gain[I_GAIN], vel_gain[D_GAIN], vel_gain[I_MAX], vel_gain[I_MIN]); //p , i , d, i_max, i_min
cmd_temp_x=pid_pos_x.updatePid(esti(0)-goal_pose.x,now-past);
cmd_temp_y=pid_pos_y.updatePid(esti(1)-goal_pose.y,now-past);
if(cmd_temp_z >= 1)
cmd_vel.linear.z=1;
else if (cmd_temp_z <=-1)
cmd_vel.linear.z=-1;
else
cmd_vel.linear.z = cmd_temp_z;
cmd_vel.angular.x = 0;
cmd_vel.angular.y = 0;
//cmd_vel.angular.z = 0;
//Temporaly disable x,y controllers.
//cmd_vel.linear.x=0;
//cmd_vel.linear.y=0;
if(vslam_states.data=="PTAM_initialized")
{
if(cmd_temp_x >= 1)
cmd_vel.linear.x=1;
else if (cmd_temp_x <=-1)
cmd_vel.linear.x=-1;
else
cmd_vel.linear.x=cmd_temp_x;
if(cmd_temp_y >= 1)
cmd_vel.linear.y=1;
else if (cmd_temp_y <=-1)
cmd_vel.linear.y=-1;
else
cmd_vel.linear.y=cmd_temp_y;
}
if(vslam_states.data=="PTAM_uninitialized")
{
ROS_INFO("HERE2");
cmd_vel.linear.x=0;
cmd_vel.linear.y=0;
cmd_vel.angular.x = 0;
cmd_vel.angular.y = 0;
cmd_vel.angular.z = 0;
}
cmd_pub.publish(cmd_vel);
}
//Before taking off, only sending null commands to the drone.
else
{
ROS_INFO("HERE1");
cmd_vel.linear.x =0;
cmd_vel.linear.y = 0;
cmd_vel.angular.x = 0;
cmd_vel.angular.y = 0;
cmd_vel.angular.z = 0;
cmd_pub.publish(cmd_vel);
}
//rotx(pi)
btMatrix3x3 wRb(1,0,0,
0,-1,0,
0,0,-1);
btQuaternion btq;
btMatrix3x3 temp = wRb.transpose();
temp.getRotation(btq);
btTransform world_to_base;
world_to_base.setIdentity();
btQuaternion q(0,0,0,0);
//Yaw,Pitch,Roll order
//q.setEulerZYX(comps_yaw,pitch,roll);
q.setEulerZYX(comps_yaw,0,0);
world_to_base.setRotation(btq*q);
btVector3 origin;
origin.setValue (cur_pose.x,
cur_pose.y,
cur_pose.z);
world_to_base.setOrigin (origin);
tf::StampedTransform world_to_base_tf(world_to_base, nav_msg->header.stamp, world_frame, imu_frame);
tf_broadcaster.sendTransform (world_to_base_tf);
tf::Vector3 t = world_to_base.getOrigin();
publish_pose(t.x(),t.y(),t.z(),roll,pitch,comps_yaw);
publish_new_pose(esti(0),esti(1),cur_pose.z,roll,pitch,PTAM_Yaw);
prev_pose = cur_pose;
past = now;
//median_past = median_now;
past_measured_yaw = cur_measured_yaw;
median_flag=0;
} //if(invertable)
} //if(median_flag)
} //if(init)
// At the very first time we don't know the samping time,
// Skip one frame.
else
{
past = now;
init=true;
past_measured_yaw = deg2rad(nav_msg->rotZ);
}
//ROS_INFO("navCallback");
}
