Merge pull request #9 from NVIDIA-Jetson/sim_docker

Redtail Docker, APM rover and fixes

Author: Alexey-Kamenev

README.md

@@ -1,8 +1,14 @@
 # NVIDIA Redtail project
 
-Autonomous drone navigation using deep learning. Refer to [wiki](https://github.com/NVIDIA-Jetson/redtail/wiki) for more information on how to get started.
+Autonomous navigation for drones and ground vehicles using deep learning. Refer to [wiki](https://github.com/NVIDIA-Jetson/redtail/wiki) for more information on how to get started.
 
 # News
+* **2017-10-12**: added full simulation Docker image, experimental support for APM Rover and support for MAVROS v0.21+.
+  
+  * Redtail simulation Docker image contains all the components required to run full Redtail simulation in Docker. Refer to [wiki](../../wiki/Testing-in-Simulator) for more information.
+  * Experimental support for APM Rover. Refer to [wiki](../../wiki#platforms) for more information.
+  * Several other changes including support for MAVROS v0.21+, updated Jetson install script and few bug fixes.
+
 * **2017-09-07**: NVIDIA Redtail project is released as an open source project.
 
   Redtail's AI modules allow building autonomous drones and mobile robots based on Deep Learning and NVIDIA Jetson TX1 and TX2 embedded systems.

ros/packages/px4_controller/include/px4_controller/px4_controller.h

@@ -79,6 +79,27 @@ class PX4Controller
                             float /*linear_control_val*/, float /*angular_control_val*/, bool /*has_command*/) override;
     };
 
+    class APMRover: public Vehicle
+    {
+        std::string getName()             override { return "APMRover"; }
+        std::string getOffboardModeName() override { return "MANUAL"; }
+        bool init(ros::NodeHandle& nh) override;
+        void printArgs() override;
+        void executeCommand(const PX4Controller& ctl, const geometry_msgs::PoseStamped& goto_pose,
+                            float linear_control_val, float angular_control_val, bool has_command) override;
+    private:
+        float turn_angle_scale_ = 1;
+        ros::Publisher rc_pub_;
+        int rc_steer_trim_ = 1500;
+        int rc_steer_dz_   = 30;
+        int rc_steer_min_  = 1100;
+        int rc_steer_max_  = 1900;
+        int rc_throttle_trim_ = 1500;
+        int rc_throttle_dz_   = 30;
+        int rc_throttle_min_  = 1100;
+        int rc_throttle_max_  = 1900;
+    };
+
 private:
     const int QUEUE_SIZE = 5;
     const int DNN_FRAME_WIDTH = 320;
@@ -101,8 +122,8 @@ class PX4Controller
     float doExpSmoothing(float cur, float prev, float factor) const;
     float getPoseDistance(geometry_msgs::PoseStamped& pose1, geometry_msgs::PoseStamped& pose2) const;
     geometry_msgs::Quaternion getRotationTo(geometry_msgs::Point& position, geometry_msgs::Point& target_position) const;
-    geometry_msgs::Point computeNextWaypoint( geometry_msgs::PoseStamped& current_pose,
-        float linear_control_val, float angular_control_val, float linear_speed) const;
+    geometry_msgs::Point computeNextWaypoint(geometry_msgs::PoseStamped& current_pose,
+                                             float linear_control_val, float angular_control_val, float linear_speed) const;
 
 private:
     std::unique_ptr<Vehicle> vehicle_;

ros/packages/px4_controller/src/px4_controller.cpp

@@ -39,6 +39,78 @@ void PX4Controller::Drone::executeCommand(const PX4Controller& ctl, const geomet
     ctl.local_pose_pub_.publish(goto_pose);
 }
 
+bool PX4Controller::APMRover::init(ros::NodeHandle& nh)
+{
+    nh.param("turn_angle_scale", turn_angle_scale_, 1.0f);
+
+    const int QUEUE_SIZE = 1;
+    rc_pub_ = nh.advertise<mavros_msgs::OverrideRCIn>("/mavros/rc/override", QUEUE_SIZE);
+    if(!rc_pub_)
+    {
+        ROS_INFO("Could not advertise to /mavros/rc/override");
+        return false;
+    }
+
+    auto get_param_client = nh.serviceClient<mavros_msgs::ParamGet>("/mavros/param/get");
+    mavros_msgs::ParamGet param_get;
+    param_get.request.param_id = "RC1_TRIM";
+    if (get_param_client.call(param_get) && param_get.response.success)
+        rc_steer_trim_ = (int)param_get.response.value.integer;
+    nh.param("rc_steer_trim", rc_steer_trim_, rc_steer_trim_);
+
+    param_get.request.param_id = "RC1_DZ";
+    if (get_param_client.call(param_get) && param_get.response.success)
+        rc_steer_dz_ = (int)param_get.response.value.integer;
+    nh.param("rc_steer_dz", rc_steer_dz_, rc_steer_dz_);
+
+    param_get.request.param_id = "RC3_TRIM";
+    if (get_param_client.call(param_get) && param_get.response.success)
+        rc_throttle_trim_ = (int)param_get.response.value.integer;
+    nh.param("rc_throttle_trim", rc_throttle_trim_, rc_throttle_trim_);
+
+    param_get.request.param_id = "RC3_DZ";
+    if (get_param_client.call(param_get) && param_get.response.success)
+        rc_throttle_dz_ = (int)param_get.response.value.integer;
+    nh.param("rc_throttle_dz", rc_throttle_dz_, rc_throttle_dz_);
+
+    return true;
+}
+
+void PX4Controller::APMRover::printArgs()
+{
+    ROS_INFO("(%s) Turn angle scale : %.1f", getName().c_str(), turn_angle_scale_);
+    ROS_INFO("(%s) Steer trim       : %d",   getName().c_str(), rc_steer_trim_);
+    ROS_INFO("(%s) Steer deadzone   : %d",   getName().c_str(), rc_steer_dz_);
+    ROS_INFO("(%s) Steer min        : %d",   getName().c_str(), rc_steer_min_);
+    ROS_INFO("(%s) Steer max        : %d",   getName().c_str(), rc_steer_max_);
+    ROS_INFO("(%s) Throttle trim    : %d",   getName().c_str(), rc_throttle_trim_);
+    ROS_INFO("(%s) Throttle deadzone: %d",   getName().c_str(), rc_throttle_dz_);
+    ROS_INFO("(%s) Throttle min     : %d",   getName().c_str(), rc_throttle_min_);
+    ROS_INFO("(%s) Throttle max     : %d",   getName().c_str(), rc_throttle_max_);
+}
+
+void PX4Controller::APMRover::executeCommand(const PX4Controller& ctl, const geometry_msgs::PoseStamped& goto_pose,
+                                             float linear_control_val, float angular_control_val, bool has_command)
+{
+    ROS_ASSERT(is_initialized_);
+
+    // REVIEW alexeyk: should not use RC override.
+    mavros_msgs::OverrideRCIn rc_override;
+    for (int c = 0; c < 8; c++)
+        rc_override.channels[c] = mavros_msgs::OverrideRCIn::CHAN_NOCHANGE;
+    int steer_delta = turn_angle_scale_ * angular_control_val;
+    int steer_dz    = steer_delta != 0 ? copysign(rc_steer_dz_, steer_delta) : 0;
+    rc_override.channels[0] = rc_steer_trim_ + steer_dz + steer_delta;
+    int throttle_delta = ctl.linear_speed_ * linear_control_val;
+    int throttle_dz    = throttle_delta != 0 ? copysign(rc_throttle_dz_, throttle_delta) : 0;
+    rc_override.channels[2] = rc_throttle_trim_ + throttle_dz + throttle_delta;
+    if(has_command)
+    {
+        ROS_DEBUG("APMRover::executeCommand: %d, %d (%.2f, %.2f)", (int)rc_override.channels[0], (int)rc_override.channels[2], linear_control_val, angular_control_val);
+        rc_pub_.publish(rc_override);
+    }
+}
+
 void PX4Controller::px4StateCallback(const mavros_msgs::State::ConstPtr &msg)
 {
     fcu_state_ = *msg;
@@ -285,6 +357,8 @@ bool PX4Controller::parseArguments(const ros::NodeHandle& nh)
     nh.param<std::string>("vehicle_type", vehicle_type, "drone");
     if (vehicle_type == "drone")
         vehicle_ = std::make_unique<Drone>();
+    else if (vehicle_type == "apmrover")
+        vehicle_ = std::make_unique<APMRover>();
     else
     {
         ROS_ERROR("Unknown vehicle type: %s", vehicle_type.c_str());

ros/scripts/jetson_ros_install.sh

@@ -129,7 +129,6 @@ catkin_make -DGSTREAMER_VERSION_1_x=On
 # Installing caffe_ros ROS package and its dependencies.
 echo "${green}Starting installation of caffe_ros and px4_controller ROS packages...${reset}"
 cd $HOME
-# REVIEW alexeyk: update with GitHub URL.
 if [ ! -d "$HOME/redtail" ]; then
     echo "Cloning caffe_ros sources..."
     git clone https://github.com/NVIDIA-Jetson/redtail

tools/platforms/erle/erle-sim/Dockerfile.indigo

@@ -0,0 +1,89 @@
+FROM ubuntu:14.04
+
+# Build with:
+# docker build -t erle-sim:indigo -f Dockerfile.indigo .
+
+ENV HOME /root
+
+COPY scripts/* $HOME/scripts/
+RUN chmod +x -R $HOME/scripts/*
+RUN chown -R root:root $HOME/scripts/*
+
+WORKDIR $HOME
+# Add repos:
+#   ROS
+#   Gazebo
+#   DRC
+RUN apt-get update && apt-get install -y --no-install-recommends wget && \
+    sh -c 'echo "deb http://packages.ros.org/ros/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/ros-latest.list' && \
+    apt-key adv --keyserver hkp://keyserver.ubuntu.com:80 --recv-key 0xB01FA116   && \
+    sh -c 'echo "deb http://packages.osrfoundation.org/gazebo/ubuntu-stable $(lsb_release -sc) main" > /etc/apt/sources.list.d/gazebo-stable.list' && \
+    wget http://packages.osrfoundation.org/gazebo.key -O - | apt-key add -              && \
+    sh -c 'echo "deb http://packages.osrfoundation.org/drc/ubuntu $(lsb_release -sc) main" > /etc/apt/sources.list.d/drc-latest.list' && \
+    wget http://packages.osrfoundation.org/drc.key -O - | apt-key add -                 && \
+    apt-get update
+
+# Install required components first.
+RUN apt-get install -y --no-install-recommends  \
+    gawk make git curl cmake g++                \
+    python-dev python-pip python-matplotlib     \ 
+    python-serial python-wxgtk2.8 python-scipy  \ 
+    python-opencv python-numpy python-pyparsing \
+    python-setuptools ccache realpath           \
+    libopencv-dev libxml2-dev libxslt1-dev      \
+    lsb-release unzip libgl1-mesa-dri
+
+# Python packages and MAVProxy
+RUN pip install wheel   && \
+    pip install future  && \
+    pip install pymavlink catkin_pkg --upgrade  && \
+    pip install MAVProxy==1.5.2
+
+# ArUco
+RUN wget "https://downloads.sourceforge.net/project/aruco/OldVersions/aruco-1.3.0.tgz?ts=1494024608&use_mirror=cytranet" -O aruco-1.3.0.tgz && \
+    tar -xvzf aruco-1.3.0.tgz  && \
+    cd aruco-1.3.0/            && \
+    mkdir build && cd build    && \
+    cmake ..                   && \
+    make -j `nproc`            && \
+    make install
+
+# APM
+WORKDIR $HOME
+RUN mkdir -p simulation && \
+    cd simulation       && \
+    git clone https://github.com/erlerobot/ardupilot -b gazebo
+
+# ROS
+# Install and init ROS base and MAVROS
+RUN apt-get install -y ros-indigo-ros-base ros-indigo-mavros ros-indigo-mavros-extras && \
+    rosdep init     && \
+    rosdep update   && \
+    echo "source /opt/ros/indigo/setup.bash" >> $HOME/.bashrc
+
+# Install ROS packages
+RUN apt-get install -y --no-install-recommends python-rosinstall \
+    ros-indigo-octomap-msgs    \
+    ros-indigo-joy             \
+    ros-indigo-geodesy         \
+    ros-indigo-octomap-ros     \
+    ros-indigo-mavlink         \
+    ros-indigo-control-toolbox \
+    ros-indigo-transmission-interface \
+    ros-indigo-joint-limits-interface \
+    ros-indigo-image-transport      \
+    ros-indigo-cv-bridge            \
+    ros-indigo-angles               \
+    ros-indigo-polled-camera        \
+    ros-indigo-camera-info-manager  \
+    ros-indigo-controller-manager
+
+# Install Gazebo
+WORKDIR $HOME
+RUN apt-get install -y --no-install-recommends gazebo7 libgazebo7-dev drcsim7
+
+# Create ROS workspace
+WORKDIR $HOME
+RUN bash $HOME/scripts/init_workspace.bash
+
+WORKDIR $HOME/simulation

tools/platforms/erle/erle-sim/README.MD

@@ -0,0 +1,54 @@
+# Running simulation in Docker for Erle Robotics vehicles
+
+This repository contains scripts and instructions on building Docker container for the purpose of running simulations with [Erle Robotics](http://erlerobotics.com/) vehicles. It is essentially dockerized instructions provided on the official  [Erle Robotics website](http://docs.erlerobotics.com/simulation). Currently only ROS Indigo is supported and tested, Kinetic is in the works.
+
+## Building Docker image
+To build a Docker container, run the following command from this directory:
+```sh
+docker build -t erle-sim:indigo -f Dockerfile.indigo .
+```
+This command will create a Docker image with the tag ```erle-sim:indigo```. Once the build is finished, you can verify that the image is built by running ```docker images``` command.
+
+## Creating Docker container
+In order to be able to run Gazebo from the Docker container, you need to connect host's X server to the Docker container. A good tutorial on various methods is documented on the [ROS Wiki](http://wiki.ros.org/docker/Tutorials/GUI), here we use the most simple (and insecure!) way.
+1. Create Docker container:
+    ```sh
+    docker run -it --network=host -v /tmp/.X11-unix:/tmp/.X11-unix -e DISPLAY=unix${DISPLAY} --name=erle-sim erle-sim:indigo bash
+    ```
+    This command will create a container named ```erle-sim``` from the image with the same name (```erle-sim```) and enable executing GUI programs.
+2. Exit the container and restart/attach the container granting the permissions to xhost first:
+    ```sh
+    xhost +local:`docker ps -aqf "name=erle-sim"`
+    docker start erle-sim && docker attach erle-sim
+    ```
+    Replace ```erle-sim``` with your container name if needed.
+3. Run Gazebo to make sure everything is working:
+    ```sh
+    gazebo
+    ```
+    You should see Gazebo window.
+### Troubleshooting
+In case you see messages like ```failed to load driver``` while executing step 3 **and** no Gazebo window opens up, you might be missing drivers. The current Docker script already contains command that installs Mesa drivers (```libgl1-mesa-dri```) but that might not be enough so try googling for more information.
+## Testing
+Once the container is created, try running simulator. For example, to run Rover simulator, run the following commands from the container:
+```sh
+source ~/simulation/ros_catkin_ws/devel/setup.bash
+cd ~/simulation/ardupilot/APMrover2/
+../Tools/autotest/sim_vehicle.sh -j 4 -f Gazebo
+# once MAVProxy has launched completely, load the parameters
+param load /[full_path_to_your_home_directory]/simulation/ardupilot/Tools/Frame_params/3DR_Rover.param
+# For example: param load /root/simulation/ardupilot/Tools/Frame_params/3DR_Rover.param
+```
+Now connect another terminal to the currently running Docker container:
+```sh
+docker exec -it erle-sim bash
+```
+and run the following commands: 
+```sh
+source ~/simulation/ros_catkin_ws/devel/setup.bash
+roslaunch ardupilot_sitl_gazebo_plugin rover_spawn.launch
+```
+If everything was setup correctly, you should see Gazebo window with Rover model that you can control from the first terminal using MAVProxy commands:
+![Gazebo Erle Rover](./images/gazebo.png)
+Some of the MAVProxy commands are described [here](http://docs.erlerobotics.com/simulation/vehicles/erle_rover/tutorial_1).
+You can also use ```run_erle_sim.sh``` script to create new container or attach to a running container.

tools/platforms/erle/erle-sim/images/gazebo.png

@@ -0,0 +1,20 @@
+ERLE_SIM_NAME=erle-sim
+ERLE_SIM_ID=`docker ps -aqf "name=^/${ERLE_SIM_NAME}$"`
+ERLE_SIM_HOST_DATA_DIR=/data/
+if [ -z "${ERLE_SIM_ID}" ]; then
+    echo "Creating new Erle Sim container."
+    docker run -it --network=host -v ${ERLE_SIM_HOST_DATA_DIR}:/data/:rw -v /tmp/.X11-unix:/tmp/.X11-unix -e DISPLAY=unix${DISPLAY} --name=${ERLE_SIM_NAME}  erle-sim:indigo bash
+else
+    echo "Found Erle Sim container: ${ERLE_SIM_ID}."
+    # Check if the container is already running and start if necessary.
+    if [ -z `docker ps -qf "name=^/${ERLE_SIM_NAME}$"` ]; then
+        xhost +local:${ERLE_SIM_ID}
+        echo "Starting and attaching to Erle Sim container..."
+        docker start ${ERLE_SIM_ID}
+        docker attach ${ERLE_SIM_ID}
+    else
+        echo "Found running Erle Sim container, attaching bash to it..."
+        docker exec -it --privileged ${ERLE_SIM_ID} bash
+    fi
+fi
+

tools/platforms/erle/erle-sim/run_erle_sim.sh

@@ -0,0 +1,44 @@
+#!/bin/bash
+#
+
+# Stop in case of any error
+set -e
+
+source /opt/ros/indigo/setup.bash
+
+WDIR=`pwd`
+
+# Create catkin workspace
+mkdir -p simulation/ros_catkin_ws/src
+cd simulation/ros_catkin_ws/src
+catkin_init_workspace
+cd ..
+catkin_make
+source devel/setup.bash
+
+# Get packages
+cd $WDIR/simulation/ros_catkin_ws/src
+
+git clone https://github.com/ros-drivers/driver_common
+git clone https://github.com/erlerobot/ardupilot_sitl_gazebo_plugin
+git clone https://github.com/erlerobot/rotors_simulator -b sonar_plugin
+git clone https://github.com/PX4/mav_comm.git
+git clone https://github.com/ethz-asl/glog_catkin.git
+git clone https://github.com/catkin/catkin_simple.git
+git clone https://github.com/erlerobot/mavros.git
+git clone https://github.com/ros-simulation/gazebo_ros_pkgs.git -b indigo-devel
+git clone https://github.com/erlerobot/gazebo_python_examples
+#git clone https://github.com/erlerobot/gazebo_cpp_examples
+#git clone https://github.com/tu-darmstadt-ros-pkg/hector_gazebo/                
+
+# Make packages
+cd ..
+catkin_make --pkg mav_msgs mavros_msgs gazebo_msgs
+source devel/setup.bash
+catkin_make -j `nproc`
+
+# Gazebo models
+cd $WDIR
+mkdir -p .gazebo/models
+cd .gazebo/models
+git clone https://github.com/erlerobot/erle_gazebo_models