cd ~/src/flyingrover_workspace/catkinws_offboard/src/mavros
git pull origin
git checkout dev_flyingrover
cd ~/src/flyingrover_workspace/catkinws_offboard/src/mavlink
git pull origin
git checkout dev_flyingrover
cd ~/src/flyingrover_workspace/catkinws_offboard/offboard_demo
git pull origin
git checkout dev_flyingroverLast updated
mkdir ~/src/flyingrover_workspace
mkdir ~/src/flyingrover_workspace/catkinws_offboard
cd ~/src/flyingrover_workspace/catkinws_offboard
catkin init
cd src
git clone --recursive https://github.com/cloudkernel-tech/mavros
cd mavros && git checkout dev_flyingrover
cd ..
git clone --recursive https://github.com/cloudkernel-tech/mavlink-gdp-release mavlink
cd mavlink && git checkout dev_flyingrover
cd ..
git clone --recursive https://github.com/cloudkernel-tech/offboard_demo
cd offboard demo && git checkout dev_flyingrovercd ~/src/flyingrover_workspace/catkinws_offboard/
catkin build -j 1 # set j=1 only for raspberry Picd ~/src/flyingrover_workspace/catkinws_offboard/
catkin clean/*subscriptions, publications and services*/
ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
("mavros/state", 5, state_cb);
//subscription for flying rover state
ros::Subscriber extended_state_sub = nh.subscribe<mavros_msgs::ExtendedState>
("mavros/extended_state", 2, extendedstate_cb);
//subscription for local position
ros::Subscriber local_pose_sub = nh.subscribe<geometry_msgs::PoseStamped>
("mavros/local_position/pose", 5, local_pose_cb);
ros::Subscriber rc_input_sub = nh.subscribe<mavros_msgs::RCIn>
("mavros/rc/in", 5, rc_input_cb);
//publication for local position setpoint
ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
("mavros/setpoint_position/local", 5);
//service for arm/disarm
ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
("mavros/cmd/arming");
//service for main mode setting
ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
("mavros/set_mode");
//service for command send
ros::ServiceClient command_long_client = nh.serviceClient<mavros_msgs::CommandLong>
("mavros/cmd/command");/*service commands*/
mavros_msgs::SetMode offb_set_mode;
offb_set_mode.request.custom_mode = "OFFBOARD";
mavros_msgs::CommandBool arm_cmd;
arm_cmd.request.value = true;
mavros_msgs::CommandBool disarm_cmd;
disarm_cmd.request.value = false;
//flying rover mode switching commands
mavros_msgs::CommandLong switch_to_mc_cmd;//command to switch to multicopter
switch_to_mc_cmd.request.command = (uint16_t)mavlink::common::MAV_CMD::DO_FLYINGROVER_TRANSITION;
switch_to_mc_cmd.request.confirmation = 0;
switch_to_mc_cmd.request.param1 = (float)mavlink::common::MAV_FLYINGROVER_STATE::MC;
mavros_msgs::CommandLong switch_to_rover_cmd;//command to switch to rover
switch_to_rover_cmd.request.command = (uint16_t)mavlink::common::MAV_CMD::DO_FLYINGROVER_TRANSITION;
switch_to_rover_cmd.request.confirmation = 0;
switch_to_rover_cmd.request.param1 = (float)mavlink::common::MAV_FLYINGROVER_STATE::ROVER;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);/*mode switching or disarm after last waypoint*/
if (current_wpindex == waypoints.size()-1 && _flag_last_wp_reached){
if (_flyingrover_mode == FLYINGROVER_MODE::ROVER){
//request to switch to multicopter mode
if( current_extendedstate.flyingrover_state== mavros_msgs::ExtendedState::FLYINGROVER_STATE_ROVER &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( command_long_client.call(switch_to_mc_cmd) && switch_to_mc_cmd.response.success){
ROS_INFO("Flyingrover multicopter mode cmd activated");
//update mode for next round
_flyingrover_mode = FLYINGROVER_MODE::MULTICOPTER;
current_wpindex = 0;
_flag_last_wp_reached = false;
}
last_request = ros::Time::now();
}
}
else if (_flyingrover_mode == FLYINGROVER_MODE::MULTICOPTER){
//disarm when landed and the vehicle is heading for the last waypoint
if (current_extendedstate.landed_state == mavros_msgs::ExtendedState::LANDED_STATE_LANDING){
if( current_state.armed &&
(ros::Time::now() - last_request > ros::Duration(5.0))){
if( arming_client.call(disarm_cmd) && arm_cmd.response.success){
ROS_INFO("Vehicle disarmed");
is_tko_inited_flag = false;
is_tko_finished = false;
}
last_request = ros::Time::now();
}
}
}
} # 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