if (current_wpindex == 0){
//use yaw measurement during multicopter takeoff (relative height<1) to avoid rotation, and use relative pose
if (_flyingrover_mode == FLYINGROVER_MODE::MULTICOPTER){
//initialize for the 1st waypoint when offboard is triggered
if (!is_tko_inited_flag && current_state.mode=="OFFBOARD"){
//reload waypoint from yaml
initTagetVector(wp_list);
waypoints.at(0).pose.position.x += current_local_pos.pose.position.x; //set with relative position here
waypoints.at(0).pose.position.y += current_local_pos.pose.position.y;
waypoints.at(0).pose.position.z += current_local_pos.pose.position.z;
tf::Quaternion q = tf::createQuaternionFromYaw(current_yaw);//set with current yaw measurement
tf::quaternionTFToMsg(q, waypoints.at(0).pose.orientation);
is_tko_inited_flag = true;
}
//update yaw setpoint after tko is finished
if (is_tko_inited_flag && !is_tko_finished){
if (current_local_pos.pose.position.z >2.0f){ //reset yaw to wp_list value
ROS_INFO("Takeoff finished, reset to waypoint yaw");
XmlRpc::XmlRpcValue data_list(wp_list[0]);
tf::Quaternion q = tf::createQuaternionFromYaw(data_list[3]);
tf::quaternionTFToMsg(q, waypoints.at(0).pose.orientation);
is_tko_finished = true;
}
}
}
else //rover mode, pass relative update, use loaded waypoints
{
waypoints.at(0).pose.position.x += current_local_pos.pose.position.x; //set with relative position here
waypoints.at(0).pose.position.y += current_local_pos.pose.position.y;
waypoints.at(0).pose.position.z += current_local_pos.pose.position.z;
}
pose = waypoints.at(0);
}
else
pose = waypoints.at(current_wpindex);
# launch a terminal
cd ~/src/flyingrover_workspace/catkinws_offboard
source devel/setup.bash
roslaunch mavros px4.launch fcu_url:="/dev/ttyPixhawk:921600"
# launch a new terminal
cd ~/src/flyingrover_workspace/catkinws_offboard
source devel/setup.bash
roslaunch offboard_flyingrover off_mission.launch