Merge pull request #6 from uzh-rpg/drone_commands

added drone commands

drone_control/dronet/README.md

@@ -0,0 +1,167 @@
+# DroNet: Learning to fly by driving
+
+This folder contains code and instruction to run DroNet code on a Bebop drone.
+
+## Introduction
+
+The bridge between the DroNet Keras code and the Bebop control is implemented in ROS.
+
+## Installation and Setup
+
+### Step 1: Install ROS
+
+It is necessary for you to install [ROS](http://wiki.ros.org/ROS/Installation) to have the basic tools available. The project was tested under ROS [indigo](http://wiki.ros.org/indigo/Installation/Ubuntu), but you can use any other version without problems.
+
+### Step 2: Build your workspace
+
+The folder containing all the related code for a project is usually defined as `workspace'.
+Create your own workspace following [these instructions](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) and call it ```bebop_ws'''.
+
+### Step 3: Setup Bebop Autonomy
+
+In this step, we will bridge our ROS workspace with the bebop drone.
+To do it, just follow [these instructions](http://bebop-autonomy.readthedocs.io/en/latest/installation.html).
+
+___Be sure to read properly all the instructions about how to run the driver, how to send commands and how to read data from the drone available on the website!___
+
+### Step 4: Install DroNet perception package
+
+After we're done with the bridge between ROS and Bebop, we now need to connect the DroNet Keras code to ROS. This is again very easy to do, since ROS has both a cpp and python interface.
+
+You can find code to do this step in the folder [dronet_perception](./dronet_perception).
+Add this folder to your workspace:
+
+```
+mkdir ~/bebop_ws/dronet
+
+cp -r YOUR_PATH/dronet_perception ~/bebop_ws/dronet
+```
+
+Now build the package:
+
+```
+cd ~/bebop_ws/dronet/dronet_perception
+
+catkin build --this
+```
+
+### Step 5: Install DroNet control package
+
+It is now time to have an interface that converts the output of DroNet to control commands for the Bebop drone.
+This is implemented in the folder [dronet_control](./dronet_control).
+Again, build this folder into your workspace.
+
+```
+cp -r YOUR_PATH/dronet_control ~/bebop_ws/dronet
+
+cd ~/bebop_ws/dronet/dronet_perception
+
+catkin build --this
+```
+
+### Step 6: Some tests
+
+To make sure that everything is as expected, try to run some tests. Example:
+
+1) See if you can connect to the drone
+
+```
+cd ~/bebop_ws/dronet/dronet_perception/launch
+
+roslaunch bebop_launch.launch
+```
+
+2) See if you can receive images from it with [rqt_img_view](http://wiki.ros.org/rqt_image_view)
+
+3) See if you can publish control commands [through the terminal](http://bebop-autonomy.readthedocs.io/en/latest/piloting.html)
+
+4) Try to run the DroNet network:
+
+```
+cd ~/bebop_ws/dronet/dronet_perception/launch
+
+roslaunch dronet_launch.launch
+```
+
+
+## Connect the Perception and Control Block
+
+There are three basic step to perform a flight with DroNet:
+
+1) Launch the perception and control pipeline.
+
+2) Start the drone and feed-through computed commands.
+
+3) Land the drone.
+
+There are two options for implementing this last step:
+
+1) Publish high level commands through terminal [NOT RECOMMENDED]
+
+2) Implement your own Graphical Interface Unit [GUI](http://wiki.ros.org/rqt)
+
+
+### Disclaimer
+
+DroNet directly produces flying commands for the Bebop drone. 
+Closely supervise the robot at all times, especially when running code for the first time.
+There might be some parameters in the Bebop you have to set to have good performance. 
+
+### Publish commands through terminal (NOT RECOMMENDED)
+
+First, connect to the drone and launch the perception pipeline:
+
+```
+cd ~/bebop_ws/dronet/dronet_perception/launch
+
+roslaunch full_perception_launch.launch
+```
+
+Then launch the control pipeline:
+
+```
+cd ~/bebop_ws/dronet/dronet_control/launch
+
+roslaunch deep_navigation.launch
+```
+
+Start the Bebop:
+
+```
+rostopic pub --once /bebop/takeoff std_msgs/Empty
+```
+
+Enable Control from DroNet (__Be cautious: The drone will be autonomous from now on__)
+
+```
+rostopic pub --once /bebop/state_change std_msgs/Bool "data: true"
+```
+
+(Optional) Deactivate Control from DroNet:
+
+```
+rostopic pub --once /bebop/state_change std_msgs/Bool "data: false"
+```
+
+Land the Bebop:
+
+```
+rostopic pub --once /bebop/land std_msgs/Empty
+
+```
+
+### Publish commands through a GUI (RECOMMENDED)
+
+It is generally recommended to implement a GUI(http://wiki.ros.org/rqt) to perform all
+the aforementioned steps. In this way, it will be easier to command the drone, and perform
+quickly many experiments. The steps to do remain the same as in the previous section.
+
+
+### Troubleshooting
+
+There might be some variables you would like to tune to get the maximal performance out of your drone. A comprehensive list can be found on the official [documentation page](http://bebop-autonomy.readthedocs.io/en/latest/autogenerated/ardrone3_settings_param.html).
+
+The most important parameters are _SpeedSettingsMaxRotationSpeedCurrent_, _PilotingSettingsMaxTiltCurrent_, _SpeedSettingsOutdoorOutdoor_.
+
+Additionally, you might want to tune the control pipeline accordingly. To do this, modify the file [deep_navigation.launch](./dronet_control/launch/deep_navigation.launch).
+Here, the most important parameter to tune is _critical\_prob_. For low values, the drone will be very conservative. For high values, it will stop only very close to obstacles.

drone_control/dronet/dronet_control/CMakeLists.txt

@@ -0,0 +1,13 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(dronet_control)
+
+find_package(catkin_simple REQUIRED)
+catkin_simple()
+quad_cmake_setup()
+
+
+cs_add_executable(dronet_control src/deep_navigation.cpp)
+
+cs_export()
+
+

drone_control/dronet/dronet_control/include/dronet_control/deep_navigation.h

@@ -0,0 +1,88 @@
+#pragma once
+
+#include <ros/ros.h>
+#include <std_msgs/Bool.h>
+#include <std_msgs/Empty.h>
+#include <geometry_msgs/Twist.h>
+#include "dronet_perception/CNN_out.h"
+
+namespace deep_navigation
+{
+
+class deepNavigation final
+{
+
+public:
+  deepNavigation(const ros::NodeHandle& nh,
+                 const ros::NodeHandle& nh_private);
+  deepNavigation() : deepNavigation(ros::NodeHandle(), ros::NodeHandle("~") ) {}
+
+  void run();
+
+private:
+
+  // ROS 
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+  ros::Subscriber deep_network_sub_;
+  ros::Subscriber state_change_sub_;
+  ros::Publisher desired_velocity_pub_;
+
+  // Callback for networks outputs
+  void deepNetworkCallback(const dronet_perception::CNN_out::ConstPtr& msg);
+  void stateChangeCallback(const std_msgs::Bool& msg);
+  double probability_of_collision_;
+  double steering_angle_;
+  bool use_network_out_;  // If True, it will use the network out, else will use zero
+
+
+  // Parameters
+  void loadParameters();
+  double alpha_velocity_, alpha_yaw_; // Smoothers for CNN outs
+  double critical_prob_coll_;
+  double max_forward_index_;
+  std::string name_;
+
+  // Internal variables
+
+  double desired_forward_velocity_;
+  double desired_angular_velocity_;
+  geometry_msgs::Twist cmd_velocity_;
+
+};
+
+class HeightChange{
+
+public:
+  HeightChange(const ros::NodeHandle& nh,
+                 const ros::NodeHandle& nh_private);
+  HeightChange() : HeightChange(ros::NodeHandle(), ros::NodeHandle("~") ) {}
+
+  virtual ~HeightChange();
+
+  void run();
+
+private:
+
+  // ROS 
+  ros::NodeHandle nh_;
+  ros::NodeHandle nh_private_;
+
+  ros::Subscriber is_up_sub_;
+  ros::Subscriber move_sub_;
+  ros::Subscriber alt_c_sub_;
+
+  ros::Publisher desired_velocity_pub_;
+
+  // Callback for networks outputs
+  void is_up(const std_msgs::Empty& msg);
+  void move(const std_msgs::Empty& msg);
+  void change_altitude(const std_msgs::Empty& msg);
+
+  std::string name_;
+  bool is_up_;
+  bool change_altitude_, should_move_;
+
+};
+
+}

drone_control/dronet/dronet_control/launch/deep_navigation.launch

@@ -0,0 +1,11 @@
+<launch>
+	<node pkg="dronet_control" name="dronet_control" type="dronet_control" output="screen"> 
+        	<remap from="cnn_predictions" to="/cnn_out/predictions"/>
+        	<remap from="state_change" to="/bebop/state_change"/>
+        	<remap from="velocity" to="/bebop/cmd_vel/"/>
+        	<param name="alpha_yaw" value="0.7"/>
+        	<param name="alpha_velocity" value=".4"/>
+        	<param name="max_forward_index" value="0.4"/>
+        	<param name="critical_prob" value="1.3"/>
+	</node>
+</launch>

drone_control/dronet/dronet_control/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<package format="2">
+	<name>dronet_control</name>
+	<version>0.0.0</version>
+	<description>dronet_control</description>
+
+	<maintainer email="loquercio@ifi.uzh.ch">Antonio Loquercio</maintainer>
+	<license>MIT</license>
+
+	<buildtool_depend>catkin</buildtool_depend>
+	<buildtool_depend>catkin_simple</buildtool_depend>
+
+	<depend>quad_cmake</depend>
+	<depend>roscpp</depend>
+	<depend>eigen_catkin</depend>
+
+	<depend>geometry_msgs</depend>
+	<depend>dronet_perception</depend>
+
+	<export>
+
+	</export>
+</package>