6.14 opensource

moderncity.sh

@@ -0,0 +1,6 @@
+roslaunch plan_manage moderncity.launch     & sleep 1;
+roslaunch traj_server traj_server.launch & sleep 1;
+roslaunch Ctrl ctrl_md.launch  & sleep 1;
+roslaunch plan_manage visualize.launch;
+wait
+

src/CMakeLists.txt

@@ -0,0 +1 @@
+/opt/ros/melodic/share/catkin/cmake/toplevel.cmake

src/Ctrl/CMakeLists.txt

@@ -0,0 +1,98 @@
+cmake_minimum_required(VERSION 2.8.3)
+project(Ctrl)
+
+set(CMAKE_VERBOSE_MAKEFILE "false")
+
+set(CMAKE_BUILD_TYPE "Release")
+set(CMAKE_CXX_FLAGS "-std=c++14")
+set(CMAKE_CXX_FLAGS_RELEASE "-O3 -Wall -g")
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  quadrotor_msgs
+  geometry_msgs
+  uav_utils
+  mavros
+)
+
+list(APPEND CMAKE_MODULE_PATH "${CMAKE_CURRENT_LIST_DIR}/cmake")
+find_package(Eigen REQUIRED) # try to find manually installed eigen (Usually in /usr/local with provided FindEigen3.cmake)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+# catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+# add_message_files(
+#   FILES
+#   PositionCommand.msg
+# )
+
+# generate_messages(
+#     DEPENDENCIES
+#     geometry_msgs
+#     std_msgs
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if you package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES Ctrl
+#  CATKIN_DEPENDS roscpp rospy
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(include)
+include_directories(
+  ${catkin_INCLUDE_DIRS}
+  ${EIGEN_INCLUDE_DIRS}
+  include/${PROJECT_NAME}
+  include
+)
+
+## Declare a cpp executable
+add_executable(Ctrl_node 
+	src/Ctrl_node.cpp
+  src/CtrlFSM.cpp
+	src/CtrlParam.cpp
+  src/controller.cpp
+  src/hovthrkf.cpp
+  src/input.cpp
+)
+
+
+## Add cmake target dependencies of the executable/library
+## as an example, message headers may need to be generated before nodes
+add_dependencies(Ctrl_node quadrotor_msgs)
+
+## Specify libraries to link a library or executable target against
+target_link_libraries(Ctrl_node
+  ${catkin_LIBRARIES}
+)
+

src/Ctrl/README.md

@@ -0,0 +1,53 @@
+# N3Ctrl #
+## A high level controller compatible with DJI N3 flight controller
+
+## 1. Prerequisites
+1.1 **Ubuntu** and **ROS**
+Ubuntu  16.04.
+ROS Kinetic. [ROS Installation](http://wiki.ros.org/ROS/Installation)
+
+1.2 **Eigen**
+Download Eigen and install (http://eigen.tuxfamily.org/index.php)
+The code is tested in Eigen 3.3
+
+1.3 **djiros**
+Follow the instruction to install the specified version of djiros ()
+
+1.4 **quadrotor_msgs**
+Follow the instruction to install the specified version of quadrotor_msgs ()
+
+## 2. Build N3Ctrl on ROS
+Clone the repository and catkin_make:
+```
+    cd ~/catkin_ws/src
+    git clone https://github.com/HKUST-Aerial-Robotics/N3Ctrl.git
+    cd ../
+    catkin_make
+    source ~/catkin_ws/devel/setup.bash
+```
+
+## 3. Control parameters
+There is a sample config file ctrl_param_fpv.yaml in the config folder.
+Change the parameters according the real drone. Tune the PID parameters of the hover state and tracking state accordingly.
+
+The input roll and pitch angle and thrust to the N3 flight controller is directly related to Kp * e_p + Kv * e_v + Kv_i * ∫ e_v + Ka * des_a.
+If the input to the N3 flight controller is desired yaw rate, which means using yaw rate control, the input is equal to Kyaw * e_yaw. Otherwise, the desired yaw is directly sent to N3 flight controller.   
+
+## 4. Topics
+Subscribers:
+```
+ ~imu  : [sensor_msgs/IMU]               IMU message from djiros.
+ ~joy   : [sensor_msgs/Joy]              RC message form djiros
+ ~cmd : [quadrotor_msgs/PositionCommand] Position command message from the planner.
+```
+Publishers:
+```
+ ~traj_start_trigger  : [geometry_msgs/PoseStamped]        A trigger message is sent when enter command mode.
+ ~desire_pose : [geometry_msgs/PoseStamped]     The desired pose of the controller.
+ ~ctrl : [sensor_msgs/Joy] The output of the control signal. The axes are the roll, pitch, thrust, yaw or yaw rate, thrust mode (if thrust mode > 0, thrust = 0~100%; if mode < 0, thrust = -? m/s ~ +? m/s), yaw mode (if yaw_mode > 0, axes[3] = yaw; if yaw_mode < 0, axes[3] = yaw_rate) respectively. The roll, pitch and yaw or yaw rate are in FRD frame. 
+```
+
+There is a sample launch file ctrl_md.launch in the launch folder.
+
+## 5. Finite state machine (FSM)
+The detailed graph of the FSM is shown in **docs/fsm.pdf**

src/Ctrl/cmake/FindEigen.cmake

@@ -0,0 +1,173 @@
+# Ceres Solver - A fast non-linear least squares minimizer
+# Copyright 2015 Google Inc. All rights reserved.
+# http://ceres-solver.org/
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions are met:
+#
+# * Redistributions of source code must retain the above copyright notice,
+#   this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above copyright notice,
+#   this list of conditions and the following disclaimer in the documentation
+#   and/or other materials provided with the distribution.
+# * Neither the name of Google Inc. nor the names of its contributors may be
+#   used to endorse or promote products derived from this software without
+#   specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
+# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+# Author: [email protected] (Alex Stewart)
+#
+
+# FindEigen.cmake - Find Eigen library, version >= 3.
+#
+# This module defines the following variables:
+#
+# EIGEN_FOUND: TRUE iff Eigen is found.
+# EIGEN_INCLUDE_DIRS: Include directories for Eigen.
+#
+# EIGEN_VERSION: Extracted from Eigen/src/Core/util/Macros.h
+# EIGEN_WORLD_VERSION: Equal to 3 if EIGEN_VERSION = 3.2.0
+# EIGEN_MAJOR_VERSION: Equal to 2 if EIGEN_VERSION = 3.2.0
+# EIGEN_MINOR_VERSION: Equal to 0 if EIGEN_VERSION = 3.2.0
+#
+# The following variables control the behaviour of this module:
+#
+# EIGEN_INCLUDE_DIR_HINTS: List of additional directories in which to
+#                          search for eigen includes, e.g: /timbuktu/eigen3.
+#
+# The following variables are also defined by this module, but in line with
+# CMake recommended FindPackage() module style should NOT be referenced directly
+# by callers (use the plural variables detailed above instead).  These variables
+# do however affect the behaviour of the module via FIND_[PATH/LIBRARY]() which
+# are NOT re-called (i.e. search for library is not repeated) if these variables
+# are set with valid values _in the CMake cache_. This means that if these
+# variables are set directly in the cache, either by the user in the CMake GUI,
+# or by the user passing -DVAR=VALUE directives to CMake when called (which
+# explicitly defines a cache variable), then they will be used verbatim,
+# bypassing the HINTS variables and other hard-coded search locations.
+#
+# EIGEN_INCLUDE_DIR: Include directory for CXSparse, not including the
+#                    include directory of any dependencies.
+
+# Called if we failed to find Eigen or any of it's required dependencies,
+# unsets all public (designed to be used externally) variables and reports
+# error message at priority depending upon [REQUIRED/QUIET/<NONE>] argument.
+macro(EIGEN_REPORT_NOT_FOUND REASON_MSG)
+  unset(EIGEN_FOUND)
+  unset(EIGEN_INCLUDE_DIRS)
+  # Make results of search visible in the CMake GUI if Eigen has not
+  # been found so that user does not have to toggle to advanced view.
+  mark_as_advanced(CLEAR EIGEN_INCLUDE_DIR)
+  # Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
+  # use the camelcase library name, not uppercase.
+  if (Eigen_FIND_QUIETLY)
+    message(STATUS "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+  elseif (Eigen_FIND_REQUIRED)
+    message(FATAL_ERROR "Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+  else()
+    # Neither QUIETLY nor REQUIRED, use no priority which emits a message
+    # but continues configuration and allows generation.
+    message("-- Failed to find Eigen - " ${REASON_MSG} ${ARGN})
+  endif ()
+  return()
+endmacro(EIGEN_REPORT_NOT_FOUND)
+
+# Protect against any alternative find_package scripts for this library having
+# been called previously (in a client project) which set EIGEN_FOUND, but not
+# the other variables we require / set here which could cause the search logic
+# here to fail.
+unset(EIGEN_FOUND)
+
+# Search user-installed locations first, so that we prefer user installs
+# to system installs where both exist.
+#
+# TODO: Add standard Windows search locations for Eigen.
+list(APPEND EIGEN_CHECK_INCLUDE_DIRS
+  /usr/local/include
+  /usr/local/homebrew/include # Mac OS X
+  /opt/local/var/macports/software # Mac OS X.
+  /opt/local/include
+  /usr/include)
+# Additional suffixes to try appending to each search path.
+list(APPEND EIGEN_CHECK_PATH_SUFFIXES
+  eigen3 # Default root directory for Eigen.
+  Eigen/include/eigen3 ) # Windows (for C:/Program Files prefix).
+
+# Search supplied hint directories first if supplied.
+find_path(EIGEN_INCLUDE_DIR
+  NAMES Eigen/Core
+  PATHS ${EIGEN_INCLUDE_DIR_HINTS}
+  ${EIGEN_CHECK_INCLUDE_DIRS}
+  PATH_SUFFIXES ${EIGEN_CHECK_PATH_SUFFIXES})
+
+if (NOT EIGEN_INCLUDE_DIR OR
+    NOT EXISTS ${EIGEN_INCLUDE_DIR})
+  eigen_report_not_found(
+    "Could not find eigen3 include directory, set EIGEN_INCLUDE_DIR to "
+    "path to eigen3 include directory, e.g. /usr/local/include/eigen3.")
+endif (NOT EIGEN_INCLUDE_DIR OR
+       NOT EXISTS ${EIGEN_INCLUDE_DIR})
+
+# Mark internally as found, then verify. EIGEN_REPORT_NOT_FOUND() unsets
+# if called.
+set(EIGEN_FOUND TRUE)
+
+# Extract Eigen version from Eigen/src/Core/util/Macros.h
+if (EIGEN_INCLUDE_DIR)
+  set(EIGEN_VERSION_FILE ${EIGEN_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h)
+  if (NOT EXISTS ${EIGEN_VERSION_FILE})
+    eigen_report_not_found(
+      "Could not find file: ${EIGEN_VERSION_FILE} "
+      "containing version information in Eigen install located at: "
+      "${EIGEN_INCLUDE_DIR}.")
+  else (NOT EXISTS ${EIGEN_VERSION_FILE})
+    file(READ ${EIGEN_VERSION_FILE} EIGEN_VERSION_FILE_CONTENTS)
+
+    string(REGEX MATCH "#define EIGEN_WORLD_VERSION [0-9]+"
+      EIGEN_WORLD_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+    string(REGEX REPLACE "#define EIGEN_WORLD_VERSION ([0-9]+)" "\\1"
+      EIGEN_WORLD_VERSION "${EIGEN_WORLD_VERSION}")
+
+    string(REGEX MATCH "#define EIGEN_MAJOR_VERSION [0-9]+"
+      EIGEN_MAJOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+    string(REGEX REPLACE "#define EIGEN_MAJOR_VERSION ([0-9]+)" "\\1"
+      EIGEN_MAJOR_VERSION "${EIGEN_MAJOR_VERSION}")
+
+    string(REGEX MATCH "#define EIGEN_MINOR_VERSION [0-9]+"
+      EIGEN_MINOR_VERSION "${EIGEN_VERSION_FILE_CONTENTS}")
+    string(REGEX REPLACE "#define EIGEN_MINOR_VERSION ([0-9]+)" "\\1"
+      EIGEN_MINOR_VERSION "${EIGEN_MINOR_VERSION}")
+
+    # This is on a single line s/t CMake does not interpret it as a list of
+    # elements and insert ';' separators which would result in 3.;2.;0 nonsense.
+    set(EIGEN_VERSION "${EIGEN_WORLD_VERSION}.${EIGEN_MAJOR_VERSION}.${EIGEN_MINOR_VERSION}")
+  endif (NOT EXISTS ${EIGEN_VERSION_FILE})
+endif (EIGEN_INCLUDE_DIR)
+
+# Set standard CMake FindPackage variables if found.
+if (EIGEN_FOUND)
+  set(EIGEN_INCLUDE_DIRS ${EIGEN_INCLUDE_DIR})
+endif (EIGEN_FOUND)
+
+# Handle REQUIRED / QUIET optional arguments and version.
+include(FindPackageHandleStandardArgs)
+find_package_handle_standard_args(Eigen
+  REQUIRED_VARS EIGEN_INCLUDE_DIRS
+  VERSION_VAR EIGEN_VERSION)
+
+# Only mark internal variables as advanced if we found Eigen, otherwise
+# leave it visible in the standard GUI for the user to set manually.
+if (EIGEN_FOUND)
+  mark_as_advanced(FORCE EIGEN_INCLUDE_DIR)
+endif (EIGEN_FOUND)