FroboMind

This guide was originally developed for a lecture for the course Introduction to UAS Technology by Jes Jepsen, a master student in drone technology at the SDU UAS Center.

cd <path>
git clone https://github.com/PX4/Firmware.git
cd Firmware
git submodule update --init --recursive

make posix_sitl_default gazebo

Error-fix for “PROTOBUF_PROTOC_EXECUTABLE-NOTFOUND”: https://gitter.im/PX4/PX4Devguide/archives/2016/06/16

sudo apt-get install protobuf-compiler 

Run the following commands in the terminal “pxh>”

commander takeoff
commander land

Optional: open the iris.world file (/Firmware/Tools/sitl_gazebo/worlds), and change the uneven_ground model to asphalt_plane. This will make the simulation run more smoothly.

Download the QGroundControl AppImage:

https://donlakeflyer.gitbooks.io/qgroundcontrol-user-guide/content/download_and_install.html

and run it:

cd <download-path>
chmod +x ./QGroundControl.AppImage
./QGroundControl.AppImage

 
 * Arm the UAV
 * Get it to Takeoff
 * Give it different positions
 * Load a coverage plan into the Firmware

sudo apt-get install ros-kinetic-mavlink ros-kinetic-mavros ros-kinetic-mavros-*

Clone into your Catkin workspace

git clone https://github.com/mavlink/mavros
git clone https://github.com/ros-controls/control_toolbox
git clone https://github.com/ros-controls/realtime_tools

and build

Remember to launch both SITL and MAVROS

make posix_sitl_default gazebo (in another terminal)

roslaunch mavros px4.launch fcu_url:="udp://:14540@192.168.1.36:14557"

Create a offboard node in your Catkin workspace, and copy-paste the following code:

/*
 *  Originally from: https://dev.px4.io/ros-mavros-offboard.html
 */

#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <mavros_msgs/CommandBool.h>
#include <mavros_msgs/SetMode.h>
#include <mavros_msgs/State.h>

mavros_msgs::State current_state;
void state_cb(const mavros_msgs::State::ConstPtr& msg){
    current_state = *msg;
}

int main(int argc, char **argv)
{
    ros::init(argc, argv, "offb_node");
    ros::NodeHandle nh;

    ros::Subscriber state_sub = nh.subscribe<mavros_msgs::State>
            ("mavros/state", 10, state_cb);
    ros::Publisher local_pos_pub = nh.advertise<geometry_msgs::PoseStamped>
            ("mavros/setpoint_position/local", 10);
    ros::ServiceClient arming_client = nh.serviceClient<mavros_msgs::CommandBool>
            ("mavros/cmd/arming");
    ros::ServiceClient set_mode_client = nh.serviceClient<mavros_msgs::SetMode>
            ("mavros/set_mode");

    //the setpoint publishing rate MUST be faster than 2Hz
    ros::Rate rate(20.0);

    // wait for FCU connection
    while(ros::ok() && current_state.connected){
        ros::spinOnce();
        rate.sleep();
    }

    geometry_msgs::PoseStamped pose;
    pose.pose.position.x = 0;
    pose.pose.position.y = 0;
    pose.pose.position.z = 2;

    //send a few setpoints before starting, or else you won't be able to switch to offboard mode
    for(int i = 100; ros::ok() && i > 0; --i){
        local_pos_pub.publish(pose);
        ros::spinOnce();
        rate.sleep();
    }

    mavros_msgs::SetMode offb_set_mode;
    offb_set_mode.request.custom_mode = "OFFBOARD";

    mavros_msgs::CommandBool arm_cmd;
    arm_cmd.request.value = true;

    ros::Time last_request = ros::Time::now();

    while(ros::ok()){
        if( current_state.mode != "OFFBOARD" &&
            (ros::Time::now() - last_request > ros::Duration(5.0))){
            if( set_mode_client.call(offb_set_mode) &&
                offb_set_mode.response.success){
                ROS_INFO("Offboard enabled");
            }
            last_request = ros::Time::now();
        } else {
            if( !current_state.armed &&
                (ros::Time::now() - last_request > ros::Duration(5.0))){
                if( arming_client.call(arm_cmd) &&
                    arm_cmd.response.success){
                    ROS_INFO("Vehicle armed");
                }
                last_request = ros::Time::now();
            }
        }

        local_pos_pub.publish(pose);

        ros::spinOnce();
        rate.sleep();
    }

    return 0;
}

This code example will switch the PX4 from MANUAL mode to OFFBOARD mode, arm the Iris and make it hover 2 meters above the ground.

Start the PX4 SITL simulation,

make posix_sitl_default gazebo

start MAVROS launch file,

roslaunch mavros px4.launch fcu_url:="udp://:14540@192.168.1.36:14557"

and run the offboard node.

Source the Firmware launch files

source Tools/setup_gazebo.bash $(pwd) $(pwd)/build_posix_sitl_default
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)
export ROS_PACKAGE_PATH=$ROS_PACKAGE_PATH:$(pwd)/Tools/sitl_gazebo

Test the already existing SITL launch file

roslaunch px4 posix_sitl.launch

Open the iris.sdf (/Firmware/Tools/sitl_gazebo/models/iris) and add camera (link + plugin) under the Iris, so that it points perpendicularly into the ground. Gazebo provides sensor plugins (with ROS), and following this tutorial can give some inspiration/help implementing the camera plugin in the SDF file: http://gazebosim.org/tutorials?tut=ros_gzplugins

Test it with the already existing SITL launch file and rqt (or image_view)

rqt

You can either choose to:

• Use a marker detection algorithm you already know (blob detection, SURF, SIFT, etc.) and implement it,
• Create and Publish the center of the marker to a ROS topic

Optional: publish the orientation of the marker to a ROS topic or Clone the MarkerLocator repo into your catkin workspace, and try to work with that

git clone https://github.com/FroboLab/MarkerLocator
# git clone https://github.com/henrikmidtiby/MarkerLocator

Optional: Try to print one of the markers and test the MarkerLocator with your webcam to get an idea of how it works and to test its limitations

OpenCV image error: Convert bgr8 to 8UC1. Make an node that re-publishes the Gazebo image to at image format OpenCV can handle:

#!/usr/bin/env python
import roslib
import sys
import rospy
import cv2
from std_msgs.msg import String
from sensor_msgs.msg import Image
from cv_bridge import CvBridge, CvBridgeError

class image_converter:

  def __init__(self):
    # Publish to the MarkerLocator's original topic
    self.image_pub = rospy.Publisher("/markerlocator/image_raw",Image)

    # Subscribe from the Iris camera topic
    self.bridge = CvBridge()
    self.image_sub = rospy.Subscriber("/iris/camera/image_raw",Image,self.callback)

  def callback(self,data):
    try:
      cv_image = self.bridge.imgmsg_to_cv2(data, "bgr8")
    except CvBridgeError as e:
      print(e)

    # Display the image
    #cv2.imshow("Image window", cv_image)
    #cv2.waitKey(3)

    frame_gray = cv2.cvtColor(cv_image, cv2.COLOR_RGB2GRAY)

    try:
      self.image_pub.publish(self.bridge.cv2_to_imgmsg(frame_gray, "8UC1"))
    except CvBridgeError as e:
      print(e)

def main(args):
  ic = image_converter()
  rospy.init_node('image_converter', anonymous=True)
  try:
    rospy.spin()
  except KeyboardInterrupt:
    print("Shutting down")
  cv2.destroyAllWindows()

if __name__ == '__main__':
    main(sys.argv)

If you choose to try the marker MarkerLocator, then you can download my Gazebo markers from:

https://drive.google.com/drive/folders/0B9r69jd8ZmzoVmJnYk1kaklhcHc?usp=sharing

Copy-paste the model folder into your .gazebo/models folder. Open Gazebo and see if the model is available under the “Insert” tab.

Use the offboard node example as a starting point, and make the Iris positioning it self above the marker so that it is in the center of its camera view. Try the /setpoint_-topics for this task.

Optional: include the orientation of the marker

  Make a presentation/desktop-record of the working system
  Describe the considerations you have made for the positioning
  Give a short explanation of the marker detection algorithm (if you decides not to use the MarkerLocator)