hongshaorou/YOPO
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

YOPO

Browse Source
This commit is contained in:
TJU_Lu
2025-06-16 23:18:04 +08:00
commit 94ceefc057
509 changed files with 105903 additions and 0 deletions
Download Patch File Download Diff File Expand all files Collapse all files

13
Controller/src/utils/cmake_utils/CMakeLists.txt Normal file
View File

@@ -0,0 +1,13 @@
cmake_minimum_required(VERSION 2.8.3)
project(cmake_utils)
find_package(catkin REQUIRED COMPONENTS roscpp)
set(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake)
file(GLOB CMAKE_UILTS_FILES cmake/*.cmake)
catkin_package(
CFG_EXTRAS ${CMAKE_UILTS_FILES}
)

126
Controller/src/utils/cmake_utils/cmake/arch.cmake Normal file
View File

@@ -0,0 +1,126 @@
set(archdetect_c_code "
#if defined(__arm__) || defined(__TARGET_ARCH_ARM)
#if defined(__ARM_ARCH_7__) \\
|| defined(__ARM_ARCH_7A__) \\
|| defined(__ARM_ARCH_7R__) \\
|| defined(__ARM_ARCH_7M__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 7)
#error cmake_ARCH armv7
#elif defined(__ARM_ARCH_6__) \\
|| defined(__ARM_ARCH_6J__) \\
|| defined(__ARM_ARCH_6T2__) \\
|| defined(__ARM_ARCH_6Z__) \\
|| defined(__ARM_ARCH_6K__) \\
|| defined(__ARM_ARCH_6ZK__) \\
|| defined(__ARM_ARCH_6M__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 6)
#error cmake_ARCH armv6
#elif defined(__ARM_ARCH_5TEJ__) \\
|| (defined(__TARGET_ARCH_ARM) && __TARGET_ARCH_ARM-0 >= 5)
#error cmake_ARCH armv5
#else
#error cmake_ARCH arm
#endif
#elif defined(__i386) || defined(__i386__) || defined(_M_IX86)
#error cmake_ARCH i386
#elif defined(__x86_64) || defined(__x86_64__) || defined(__amd64) || defined(_M_X64)
#error cmake_ARCH x86_64
#elif defined(__ia64) || defined(__ia64__) || defined(_M_IA64)
#error cmake_ARCH ia64
#elif defined(__ppc__) || defined(__ppc) || defined(__powerpc__) \\
|| defined(_ARCH_COM) || defined(_ARCH_PWR) || defined(_ARCH_PPC) \\
|| defined(_M_MPPC) || defined(_M_PPC)
#if defined(__ppc64__) || defined(__powerpc64__) || defined(__64BIT__)
#error cmake_ARCH ppc64
#else
#error cmake_ARCH ppc
#endif
#endif
#error cmake_ARCH unknown
")
# Set ppc_support to TRUE before including this file or ppc and ppc64
# will be treated as invalid architectures since they are no longer supported by Apple
function(target_architecture output_var)
if(APPLE AND CMAKE_OSX_ARCHITECTURES)
# On OS X we use CMAKE_OSX_ARCHITECTURES *if* it was set
# First let's normalize the order of the values
# Note that it's not possible to compile PowerPC applications if you are using
# the OS X SDK version 10.6 or later - you'll need 10.4/10.5 for that, so we
# disable it by default
# See this page for more information:
# http://stackoverflow.com/questions/5333490/how-can-we-restore-ppc-ppc64-as-well-as-full-10-4-10-5-sdk-support-to-xcode-4
# Architecture defaults to i386 or ppc on OS X 10.5 and earlier, depending on the CPU type detected at runtime.
# On OS X 10.6+ the default is x86_64 if the CPU supports it, i386 otherwise.
foreach(osx_arch ${CMAKE_OSX_ARCHITECTURES})
if("${osx_arch}" STREQUAL "ppc" AND ppc_support)
set(osx_arch_ppc TRUE)
elseif("${osx_arch}" STREQUAL "i386")
set(osx_arch_i386 TRUE)
elseif("${osx_arch}" STREQUAL "x86_64")
set(osx_arch_x86_64 TRUE)
elseif("${osx_arch}" STREQUAL "ppc64" AND ppc_support)
set(osx_arch_ppc64 TRUE)
else()
message(FATAL_ERROR "Invalid OS X arch name: ${osx_arch}")
endif()
endforeach()
# Now add all the architectures in our normalized order
if(osx_arch_ppc)
list(APPEND ARCH ppc)
endif()
if(osx_arch_i386)
list(APPEND ARCH i386)
endif()
if(osx_arch_x86_64)
list(APPEND ARCH x86_64)
endif()
if(osx_arch_ppc64)
list(APPEND ARCH ppc64)
endif()
else()
file(WRITE "${CMAKE_BINARY_DIR}/arch.c" "${archdetect_c_code}")
enable_language(C)
# Detect the architecture in a rather creative way...
# This compiles a small C program which is a series of ifdefs that selects a
# particular #error preprocessor directive whose message string contains the
# target architecture. The program will always fail to compile (both because
# file is not a valid C program, and obviously because of the presence of the
# #error preprocessor directives... but by exploiting the preprocessor in this
# way, we can detect the correct target architecture even when cross-compiling,
# since the program itself never needs to be run (only the compiler/preprocessor)
try_run(
run_result_unused
compile_result_unused
"${CMAKE_BINARY_DIR}"
"${CMAKE_BINARY_DIR}/arch.c"
COMPILE_OUTPUT_VARIABLE ARCH
CMAKE_FLAGS CMAKE_OSX_ARCHITECTURES=${CMAKE_OSX_ARCHITECTURES}
)
# Parse the architecture name from the compiler output
string(REGEX MATCH "cmake_ARCH ([a-zA-Z0-9_]+)" ARCH "${ARCH}")
# Get rid of the value marker leaving just the architecture name
string(REPLACE "cmake_ARCH " "" ARCH "${ARCH}")
# If we are compiling with an unknown architecture this variable should
# already be set to "unknown" but in the case that it's empty (i.e. due
# to a typo in the code), then set it to unknown
if (NOT ARCH)
set(ARCH unknown)
endif()
endif()
set(${output_var} "${ARCH}" PARENT_SCOPE)
endfunction()

2
Controller/src/utils/cmake_utils/cmake/cmake_modules.cmake Normal file
View File

@@ -0,0 +1,2 @@
list(APPEND CMAKE_MODULE_PATH "${cmake_utils_SOURCE_PREFIX}/cmake_modules")
link_directories( ${cmake_utils_SOURCE_PREFIX}/lib/mosek8 )

17
Controller/src/utils/cmake_utils/cmake/color.cmake Normal file
View File

@@ -0,0 +1,17 @@
string(ASCII 27 Esc)
set(ColourReset "${Esc}[m")
set(ColourBold "${Esc}[1m")
set(Red "${Esc}[31m")
set(Green "${Esc}[32m")
set(Yellow "${Esc}[33m")
set(Blue "${Esc}[34m")
set(Magenta "${Esc}[35m")
set(Cyan "${Esc}[36m")
set(White "${Esc}[37m")
set(BoldRed "${Esc}[1;31m")
set(BoldGreen "${Esc}[1;32m")
set(BoldYellow "${Esc}[1;33m")
set(BoldBlue "${Esc}[1;34m")
set(BoldMagenta "${Esc}[1;35m")
set(BoldCyan "${Esc}[1;36m")
set(BoldWhite "${Esc}[1;37m")

173
Controller/src/utils/cmake_utils/cmake_modules/FindEigen.cmake Normal file
View File

@@ -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: alexs.mac@gmail.com (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)

135
Controller/src/utils/cmake_utils/cmake_modules/FindGSL.cmake Normal file
View File

@@ -0,0 +1,135 @@
# Try to find gnu scientific library GSL
# See
# http://www.gnu.org/software/gsl/ and
# http://gnuwin32.sourceforge.net/packages/gsl.htm
#
# Based on a script of Felix Woelk and Jan Woetzel
# (www.mip.informatik.uni-kiel.de)
#
# It defines the following variables:
# GSL_FOUND - system has GSL lib
# GSL_INCLUDE_DIRS - where to find headers
# GSL_LIBRARIES - full path to the libraries
# GSL_LIBRARY_DIRS, the directory where the PLplot library is found.
# CMAKE_GSL_CXX_FLAGS = Unix compiler flags for GSL, essentially "`gsl-config --cxxflags`"
# GSL_LINK_DIRECTORIES = link directories, useful for rpath on Unix
# GSL_EXE_LINKER_FLAGS = rpath on Unix
set( GSL_FOUND OFF )
set( GSL_CBLAS_FOUND OFF )
# Windows, but not for Cygwin and MSys where gsl-config is available
if( WIN32 AND NOT CYGWIN AND NOT MSYS )
# look for headers
find_path( GSL_INCLUDE_DIR
NAMES gsl/gsl_cdf.h gsl/gsl_randist.h
)
if( GSL_INCLUDE_DIR )
# look for gsl library
find_library( GSL_LIBRARY
NAMES gsl
)
if( GSL_LIBRARY )
set( GSL_INCLUDE_DIRS ${GSL_INCLUDE_DIR} )
get_filename_component( GSL_LIBRARY_DIRS ${GSL_LIBRARY} PATH )
set( GSL_FOUND ON )
endif( GSL_LIBRARY )
# look for gsl cblas library
find_library( GSL_CBLAS_LIBRARY
NAMES gslcblas
)
if( GSL_CBLAS_LIBRARY )
set( GSL_CBLAS_FOUND ON )
endif( GSL_CBLAS_LIBRARY )
set( GSL_LIBRARIES ${GSL_LIBRARY} ${GSL_CBLAS_LIBRARY} )
endif( GSL_INCLUDE_DIR )
mark_as_advanced(
GSL_INCLUDE_DIR
GSL_LIBRARY
GSL_CBLAS_LIBRARY
)
else( WIN32 AND NOT CYGWIN AND NOT MSYS )
if( UNIX OR MSYS )
find_program( GSL_CONFIG_EXECUTABLE gsl-config
/usr/bin/
/usr/local/bin
)
if( GSL_CONFIG_EXECUTABLE )
set( GSL_FOUND ON )
# run the gsl-config program to get cxxflags
execute_process(
COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --cflags
OUTPUT_VARIABLE GSL_CFLAGS
RESULT_VARIABLE RET
ERROR_QUIET
)
if( RET EQUAL 0 )
string( STRIP "${GSL_CFLAGS}" GSL_CFLAGS )
separate_arguments( GSL_CFLAGS )
# parse definitions from cflags; drop -D* from CFLAGS
string( REGEX MATCHALL "-D[^;]+"
GSL_DEFINITIONS "${GSL_CFLAGS}" )
string( REGEX REPLACE "-D[^;]+;" ""
GSL_CFLAGS "${GSL_CFLAGS}" )
# parse include dirs from cflags; drop -I prefix
string( REGEX MATCHALL "-I[^;]+"
GSL_INCLUDE_DIRS "${GSL_CFLAGS}" )
string( REPLACE "-I" ""
GSL_INCLUDE_DIRS "${GSL_INCLUDE_DIRS}")
string( REGEX REPLACE "-I[^;]+;" ""
GSL_CFLAGS "${GSL_CFLAGS}")
message("GSL_DEFINITIONS=${GSL_DEFINITIONS}")
message("GSL_INCLUDE_DIRS=${GSL_INCLUDE_DIRS}")
message("GSL_CFLAGS=${GSL_CFLAGS}")
else( RET EQUAL 0 )
set( GSL_FOUND FALSE )
endif( RET EQUAL 0 )
# run the gsl-config program to get the libs
execute_process(
COMMAND sh "${GSL_CONFIG_EXECUTABLE}" --libs
OUTPUT_VARIABLE GSL_LIBRARIES
RESULT_VARIABLE RET
ERROR_QUIET
)
if( RET EQUAL 0 )
string(STRIP "${GSL_LIBRARIES}" GSL_LIBRARIES )
separate_arguments( GSL_LIBRARIES )
# extract linkdirs (-L) for rpath (i.e., LINK_DIRECTORIES)
string( REGEX MATCHALL "-L[^;]+"
GSL_LIBRARY_DIRS "${GSL_LIBRARIES}" )
string( REPLACE "-L" ""
GSL_LIBRARY_DIRS "${GSL_LIBRARY_DIRS}" )
else( RET EQUAL 0 )
set( GSL_FOUND FALSE )
endif( RET EQUAL 0 )
MARK_AS_ADVANCED(
GSL_CFLAGS
)
message( STATUS "Using GSL from ${GSL_PREFIX}" )
else( GSL_CONFIG_EXECUTABLE )
message( STATUS "FindGSL: gsl-config not found.")
endif( GSL_CONFIG_EXECUTABLE )
endif( UNIX OR MSYS )
endif( WIN32 AND NOT CYGWIN AND NOT MSYS )
if( GSL_FOUND )
if( NOT GSL_FIND_QUIETLY )
message( STATUS "FindGSL: Found both GSL headers and library" )
endif( NOT GSL_FIND_QUIETLY )
else( GSL_FOUND )
if( GSL_FIND_REQUIRED )
message( FATAL_ERROR "FindGSL: Could not find GSL headers or library" )
endif( GSL_FIND_REQUIRED )
endif( GSL_FOUND )

124
Controller/src/utils/cmake_utils/cmake_modules/FindmvIMPACT.cmake Normal file
View File

@@ -0,0 +1,124 @@
macro(mvIMPACT_REPORT_NOT_FOUND REASON_MSG)
unset(mvIMPACT_FOUND)
unset(mvIMPACT_INCLUDE_DIRS)
unset(mvIMPACT_LIBRARIES)
unset(mvIMPACT_WORLD_VERSION)
unset(mvIMPACT_MAJOR_VERSION)
unset(mvIMPACT_MINOR_VERSION)
# Make results of search visible in the CMake GUI if mvimpact has not
# been found so that user does not have to toggle to advanced view.
mark_as_advanced(CLEAR mvIMPACT_INCLUDE_DIR)
# Note <package>_FIND_[REQUIRED/QUIETLY] variables defined by FindPackage()
# use the camelcase library name, not uppercase.
if(Mvimpact_FIND_QUIETLY)
message(STATUS "Failed to find mvimpact - " ${REASON_MSG} ${ARGN})
elseif(Mvimpact_FIND_REQUIRED)
message(FATAL_ERROR "Failed to find mvimpact - " ${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 mvimpact - " ${REASON_MSG} ${ARGN})
endif()
endmacro(mvIMPACT_REPORT_NOT_FOUND)
# Search user-installed locations first, so that we prefer user installs
# to system installs where both exist.
get_filename_component(BLUEFOX2_DIR ${CMAKE_CURRENT_SOURCE_DIR} REALPATH)
list(APPEND mvIMPACT_CHECK_INCLUDE_DIRS
/opt/mvIMPACT_acquire
/opt/mvIMPACT_Acquire
)
execute_process(COMMAND uname -m COMMAND tr -d '\n' OUTPUT_VARIABLE ARCH)
if(NOT ARCH)
set(ARCH "arm64")
elseif(${ARCH} STREQUAL "aarch64")
set(ARCH "arm64")
endif()
list(APPEND mvIMPACT_CHECK_LIBRARY_DIRS
/opt/mvIMPACT_acquire/lib/${ARCH}
/opt/mvIMPACT_Acquire/lib/${ARCH}
)
# Check general hints
if(mvIMPACT_HINTS AND EXISTS ${mvIMPACT_HINTS})
set(mvIMPACT_INCLUDE_DIR_HINTS ${mvIMPACT_HINTS}/include)
set(mvIMPACT_LIBRARY_DIR_HINTS ${mvIMPACT_HINTS}/lib)
endif()
# Search supplied hint directories first if supplied.
# Find include directory for mvimpact
find_path(mvIMPACT_INCLUDE_DIR
NAMES mvIMPACT_CPP/mvIMPACT_acquire.h
PATHS ${mvIMPACT_INCLUDE_DIR_HINTS}
${mvIMPACT_CHECK_INCLUDE_DIRS}
NO_DEFAULT_PATH)
if(NOT mvIMPACT_INCLUDE_DIR OR NOT EXISTS ${mvIMPACT_INCLUDE_DIR})
mvIMPACT_REPORT_NOT_FOUND(
"Could not find mvimpact include directory, set mvIMPACT_INCLUDE_DIR to "
"path to mvimpact include directory,"
"e.g. /opt/mvIMPACT_acquire.")
else()
message(STATUS "mvimpact include dir found: " ${mvIMPACT_INCLUDE_DIR})
endif()
# Find library directory for mvimpact
find_library(mvIMPACT_LIBRARY
NAMES libmvBlueFOX.so
PATHS ${mvIMPACT_LIBRARY_DIR_HINTS}
${mvIMPACT_CHECK_LIBRARY_DIRS}
NO_DEFAULT_PATH)
if(NOT mvIMPACT_LIBRARY OR NOT EXISTS ${mvIMPACT_LIBRARY})
mvIMPACT_REPORT_NOT_FOUND(
"Could not find mvimpact library, set mvIMPACT_LIBRARY "
"to full path to mvimpact library direcotory.${mvIMPACT_CHECK_LIBRARY_DIRS}~~")
else()
# TODO: need to fix this hacky solution for getting mvIMPACT_LIBRARY_DIR
string(REGEX MATCH ".*/" mvIMPACT_LIBRARY_DIR ${mvIMPACT_LIBRARY})
message(STATUS "mvimpact library dir found: " ${mvIMPACT_LIBRARY_DIR})
endif()
# Mark internally as found, then verify. mvIMPACT_REPORT_NOT_FOUND() unsets if
# called.
set(mvIMPACT_FOUND TRUE)
# Extract mvimpact version
if(mvIMPACT_LIBRARY_DIR)
file(GLOB mvIMPACT_LIBS
RELATIVE ${mvIMPACT_LIBRARY_DIR}
${mvIMPACT_LIBRARY_DIR}/libmvBlueFOX.so.[0-9].[0-9].[0-9])
# TODO: add version support
# string(REGEX MATCH ""
# mvIMPACT_WORLD_VERSION ${mvIMPACT_PVBASE})
# message(STATUS "mvimpact world version: " ${mvIMPACT_WORLD_VERSION})
endif()
# Catch case when caller has set mvIMPACT_INCLUDE_DIR in the cache / GUI and
# thus FIND_[PATH/LIBRARY] are not called, but specified locations are
# invalid, otherwise we would report the library as found.
if(NOT mvIMPACT_INCLUDE_DIR AND NOT EXISTS ${mvIMPACT_INCLUDE_DIR}/mvIMPACT_CPP/mvIMPACT_acquire.h)
mvIMPACT_REPORT_NOT_FOUND("Caller defined mvIMPACT_INCLUDE_DIR: "
${mvIMPACT_INCLUDE_DIR}
" does not contain mvIMPACT_CPP/mvIMPACT_acquire.h header.")
endif()
# Set standard CMake FindPackage variables if found.
if(mvIMPACT_FOUND)
set(mvIMPACT_INCLUDE_DIRS ${mvIMPACT_INCLUDE_DIR})
file(GLOB mvIMPACT_LIBRARIES ${mvIMPACT_LIBRARY_DIR}lib*.so)
endif()
# Handle REQUIRED / QUIET optional arguments.
include(FindPackageHandleStandardArgs)
if(mvIMPACT_FOUND)
FIND_PACKAGE_HANDLE_STANDARD_ARGS(Mvimpact DEFAULT_MSG
mvIMPACT_INCLUDE_DIRS mvIMPACT_LIBRARIES)
endif()
# Only mark internal variables as advanced if we found mvimpact, otherwise
# leave it visible in the standard GUI for the user to set manually.
if(mvIMPACT_FOUND)
mark_as_advanced(FORCE mvIMPACT_INCLUDE_DIR mvIMPACT_LIBRARY)
endif()

18
Controller/src/utils/cmake_utils/package.xml Normal file
View File

@@ -0,0 +1,18 @@
<?xml version="1.0"?>
<package format="2">
<name>cmake_utils</name>
<version>0.0.0</version>
<description>
Once you used this package,
then you do not need to copy cmake files among packages
</description>
<maintainer email="william.wu@dji.com"> William.Wu </maintainer>
<license>LGPLv3</license>
<buildtool_depend>catkin</buildtool_depend>
<depend>roscpp</depend>
<depend>cmake_modules</depend>
</package>

595
Controller/src/utils/mavros_msgs/CHANGELOG.rst Normal file
View File

@@ -0,0 +1,595 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package mavros_msgs
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.16.0 (2023-05-05)
-------------------
1.15.0 (2022-12-30)
-------------------
* Merge pull request `#1811 <https://github.com/mavlink/mavros/issues/1811>`_ from scoutdi/debug-float-array
Implement debug float array handler
* Implement debug float array handler
Co-authored-by: Morten Fyhn Amundsen <morten.f.amundsen@scoutdi.com>
* Contributors: Sverre Velten Rothmund, Vladimir Ermakov
1.14.0 (2022-09-24)
-------------------
* Merge pull request `#1742 <https://github.com/mavlink/mavros/issues/1742>`_ from amilcarlucas/correct_rpm_units
ESCTelemetryItem.msg: correct RPM units
* ESCTelemetryItem.msg: correct RPM units
* Merge pull request `#1727 <https://github.com/mavlink/mavros/issues/1727>`_ from BV-OpenSource/pr-cellular-status
Pr cellular status
* Add CellularStatus plugin and message
* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Rui Mendes, Vladimir Ermakov
1.13.0 (2022-01-13)
-------------------
* Merge pull request `#1690 <https://github.com/mavlink/mavros/issues/1690>`_ from mavlink/fix-enum_sensor_orientation
Fix enum sensor_orientation
* re-generate all coglets
* Merge pull request `#1680 <https://github.com/mavlink/mavros/issues/1680>`_ from AndersonRayner/new_mav_frames
Add extra MAV_FRAMES to waypoint message
* Merge pull request `#1677 <https://github.com/mavlink/mavros/issues/1677>`_ from AndersonRayner/add_terrain
Add plugin for reporting terrain height estimate from the FCU
* More explicitly state "TerrainReport" to allow for future extension of the plugin to support other terrain messages
* Add extra MAV_FRAMES to waypoint message as defined in https://mavlink.io/en/messages/common.html
* Add plugin for reporting terrain height estimate from FCU
* Contributors: Vladimir Ermakov, matt
1.12.2 (2021-12-12)
-------------------
1.12.1 (2021-11-29)
-------------------
1.12.0 (2021-11-27)
-------------------
1.11.1 (2021-11-24)
-------------------
1.11.0 (2021-11-24)
-------------------
* msgs: use pragmas to ignore unaligned pointer warnings
* msgs: fix convert const
* msgs: try to hide 'unaligned pointer' warning
* Merge pull request `#1651 <https://github.com/mavlink/mavros/issues/1651>`_ from Jaeyoung-Lim/pr-image-capture-plugin
Add camera plugin for interfacing with mavlink camera protocol
* Address review comments
* Add camera plugin for interfacing with mavlink camera protocol
Add camera image captured message for handling camera trigger information
* msgs: add yaw field to GPS_INPUT
* Contributors: Jaeyoung-Lim, Vladimir Ermakov
1.10.0 (2021-11-04)
-------------------
* msgs: update gpsraw to have yaw field
* Merge pull request `#1625 <https://github.com/mavlink/mavros/issues/1625>`_ from scoutdi/tunnel-plugin
Plugin for TUNNEL messages
* Tunnel.msg: Generate enum with cog
* mavros_msgs: Add Tunnel message
* Merge pull request `#1623 <https://github.com/mavlink/mavros/issues/1623>`_ from amilcarlucas/pr/more-typo-fixes
More typo fixes
* MountControl.msg: fix copy-paste
* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Morten Fyhn Amundsen, Vladimir Ermakov
1.9.0 (2021-09-09)
------------------
* Merge pull request `#1616 <https://github.com/mavlink/mavros/issues/1616>`_ from amilcarlucas/pr/RC_CHANNELS-mavlink2-extensions
Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels.…
* Changed OverrideRCIn to 18 channels
* Merge pull request `#1617 <https://github.com/mavlink/mavros/issues/1617>`_ from amilcarlucas/pr/NAV_CONTROLLER_OUTPUT-plugin
Added NAV_CONTROLLER_OUTPUT Plugin
* Merge pull request `#1618 <https://github.com/mavlink/mavros/issues/1618>`_ from amilcarlucas/pr/GPS_INPUT-plugin
Added GPS_INPUT plugin
* Mavlink v2.0 specs for RC_CHANNELS_OVERRIDE accepts upto 18 channels. The plugin publishes channels 9 to 18 if the FCU protocol version is 2.0
* Added NAV_CONTROLLER_OUTPUT Plugin
* Added GPS_INPUT plugin
* Merge branch 'master' into master
* Update esc_status plugin with datatype change on MAVLink.
ESC_INFO MAVLink message was updated to have negative temperates and also at a different resolution. This commit updates those changes on this side.
* Remove Mount_Status plugin. Add Status data to Mount_Control plugin. Remove Mount_Status message.
* msgs: fix types for apm's esc telemetry
* actually allocate memory for the telemetry information
* added esc_telemetry plugin
* Add Mount angles message for communications with ardupilotmega.
* Remove extra message from CMakeLists.
* Add message and service definition.
* Contributors: Abhijith Thottumadayil Jagadeesh, André Filipe, Dr.-Ing. Amilcar do Carmo Lucas, Karthik Desai, Ricardo Marques, Russell, Vladimir Ermakov
1.8.0 (2021-05-05)
------------------
1.7.1 (2021-04-05)
------------------
* re-generate all pymavlink enums
* Contributors: Vladimir Ermakov
1.7.0 (2021-04-05)
------------------
* msgs: re-generate the code
* Contributors: Vladimir Ermakov
1.6.0 (2021-02-15)
------------------
1.5.2 (2021-02-02)
------------------
1.5.1 (2021-01-04)
------------------
1.5.0 (2020-11-11)
------------------
* mavros_msgs/VehicleInfo: Add flight_custom_version field
Mirroring the field in the corresponding MAVLink message.
* mavros_msgs/State: Fix PX4 flight mode constants
Turns out ROS message string literals don't need quotes,
so adding quotes creates strings including the quotes.
* mavros_msgs/State: Add flight mode constants
* mavros_msgs: Don't move temporary objects
* Contributors: Morten Fyhn Amundsen
1.4.0 (2020-09-11)
------------------
* play_tune: Assign tune format directly
* play_tune: Write new plugin
* Contributors: Morten Fyhn Amundsen
1.3.0 (2020-08-08)
------------------
* Add esc_status plugin.
* Add gps_status plugin to publish GPS_RAW and GPS_RTK messages from FCU.
The timestamps for the gps_status topics take into account the mavlink time and uses the convienence function
* adding support for publishing rtkbaseline msgs over ROS
* Contributors: CSCE439, Dr.-Ing. Amilcar do Carmo Lucas, Ricardo Marques
1.2.0 (2020-05-22)
------------------
* add yaw to CMD_DO_SET_HOME
* Contributors: David Jablonski
1.1.0 (2020-04-04)
------------------
1.0.0 (2020-01-01)
------------------
0.33.4 (2019-12-12)
-------------------
* Splitted the message fields.
* Updated esimator status msg according to the new cog based definition of estimator status.
* Added comments to msg.
* Added new line char at end of message.
* Added a publisher for estimator status message received from mavlink in sys_status.
* Contributors: saifullah3396
0.33.3 (2019-11-13)
-------------------
0.33.2 (2019-11-13)
-------------------
0.33.1 (2019-11-11)
-------------------
* resolved merge conflict
* Contributors: David Jablonski
0.33.0 (2019-10-10)
-------------------
* Add vtol transition service
* Apply comments
* Add mount configure service message
* cog: Update all generated code
* added manual flag to mavros/state
* use header.stamp to fill mavlink msg field time_usec
* use cog for copy
* adapt message and plugin after mavlink message merge
* rename message and adjust fields
* add component id to mavros message to distinguish ROS msgs from different systems
* component_status message and plugin draft
* Contributors: David Jablonski, Jaeyoung-Lim, Vladimir Ermakov, baumanta
0.32.2 (2019-09-09)
-------------------
0.32.1 (2019-08-08)
-------------------
0.32.0 (2019-07-06)
-------------------
* add mav_cmd associated with each point in trajectory plugin
* Use MountControl Msg
* Define new MountControl.msg
* Contributors: Jaeyoung-Lim, Martina Rivizzigno
0.31.0 (2019-06-07)
-------------------
* mavros_msgs: LandingTarget: update msg description link
* extras: landing target: improve usability and flexibility
* Contributors: TSC21
0.30.0 (2019-05-20)
-------------------
0.29.2 (2019-03-06)
-------------------
0.29.1 (2019-03-03)
-------------------
* All: catkin lint files
* mavros_msgs: Fix line endings for OpticalFlowRad message
* Contributors: Pierre Kancir, sfalexrog
0.29.0 (2019-02-02)
-------------------
* Fix broken documentation URLs
* Merge branch 'master' into param-timeout
* mavros_extras: Wheel odometry plugin updated according to the final mavlink WHEEL_DISTANCE message.
* mavros_msgs: Float32ArrayStamped replaced by WheelOdomStamped.
* mavros_msgs: Float32ArrayStamped message added.
For streaming timestamped data from FCU sensors (RPM, WHEEL_DISTANCE, etc.)
* msgs: Fix message id type, mavlink v2 uses 24 bit msg ids
* mavros_msgs: add MessageInterval.srv to CMakeLists
* sys_status: add set_message_interval service
* Contributors: Dr.-Ing. Amilcar do Carmo Lucas, Pavlo Kolomiiets, Randy Mackay, Vladimir Ermakov
0.28.0 (2019-01-03)
-------------------
* plugin:param: publish new param value
* Merge pull request `#1148 <https://github.com/mavlink/mavros/issues/1148>`_ from Kiwa21/pr-param-value
param plugin : add msg and publisher to catch latest param value
* msgs: update Header
* sys_state: Small cleanup of `#1150 <https://github.com/mavlink/mavros/issues/1150>`_
* VehicleInfo : add srv into sys_status plugin to request basic info from vehicle
* mavros_msgs/msg/LogData.msg: Define "offset" field to be of type uint32
* param plugin : add msg and publisher to catch latest param value
* style clean up
* Use component_id to determine message sender
* change message name from COMPANION_STATUS to COMPANION_PROCESS_STATUS
* change message to include pid
* Change from specific avoidance status message to a more generic companion status message
* Add message for avoidance status
* Contributors: Gregoire Linard, Vladimir Ermakov, baumanta, mlvov
0.27.0 (2018-11-12)
-------------------
* Add service to send mavlink TRIGG_INTERVAL commands
Adapt trigger_control service to current mavlink cmd spec. Add a new service to change trigger interval and integration time
* Contributors: Moritz Zimmermann
0.26.3 (2018-08-21)
-------------------
* fixup! 5a4344a2dcedc157f93b620cebd2e0b273ec24be
* mavros_msgs: Add msg and srv files related to log transfer
* Contributors: mlvov
0.26.2 (2018-08-08)
-------------------
* Updating the gps_rtk plugin to fit mavros guidelines:
- Updating max_frag_len to allow changes in size in MAVLink seamlessly
- Using std::copy instead of memset
- Zero fill with std::fill
- Preapply the sequence flags
- Use of std iterators
- Add the maximal data size in the mavros_msgs
* Renaming the GPS RTK module, Adding fragmentation, Changing the RTCM message
* RTK Plugin; to forward RTCM messages
Signed-off-by: Alexis Paques <alexis.paques@gmail.com>
* Contributors: Alexis Paques
0.26.1 (2018-07-19)
-------------------
0.26.0 (2018-06-06)
-------------------
* mavros_msgs : add timesync status message
* Contributors: Mohammed Kabir
0.25.1 (2018-05-14)
-------------------
0.25.0 (2018-05-11)
-------------------
* trajectory: add time_horizon field
* change message name from ObstacleAvoidance to Trajectory since it is
general enough to support any type of trajectory
* CMakeLists: add ObstacleAvoidance message
* add ObstacleAvoidance message
* msgs: Update message doc link
* CommandCode: update list of available commands on MAV_CMD enum (`#995 <https://github.com/mavlink/mavros/issues/995>`_)
* Contributors: Martina, Nuno Marques, Vladimir Ermakov
0.24.0 (2018-04-05)
-------------------
* Add ability to send STATUSTEXT messages
* Contributors: Anass Al
0.23.3 (2018-03-09)
-------------------
0.23.2 (2018-03-07)
-------------------
0.23.1 (2018-02-27)
-------------------
0.23.0 (2018-02-03)
-------------------
0.22.0 (2017-12-11)
-------------------
* SetMavFrame.srv: add FRAME\_ prefix
* Add cog for SetMavFrame.srv
* Setpoints: add service to specify frame
* Contributors: Pierre Kancir, khancyr
0.21.5 (2017-11-16)
-------------------
0.21.4 (2017-11-01)
-------------------
0.21.3 (2017-10-28)
-------------------
* plugin waypoints: Use stamped message
* add debug plugin
* Contributors: TSC21, Vladimir Ermakov
0.21.2 (2017-09-25)
-------------------
0.21.1 (2017-09-22)
-------------------
0.21.0 (2017-09-14)
-------------------
* plugin waypoint: Rename current seq in wp list message
* waypoint: Publish current waypoint seq
* waypoint partial: code style cleanup
* waypoint partial: extend existing service
* Partial waypoint: added wp_transfered to push partial service response
* Partial waypoint: added partial updating to mavwp
* Contributors: James Mare, James Stewart, Vladimir Ermakov
0.20.1 (2017-08-28)
-------------------
0.20.0 (2017-08-23)
-------------------
* HIL Plugin
* add HilSensor.msg, HilStateQuaternion.msg, and add them in CMakeLists.txt
* Add hil_sensor.cpp plugin to send HIL_SENSOR mavlink message to FCU.
* fix HilSensor.msg. Make it more compact.
* Fix HilStateQuaternion.msg. Make it more compact.
* Add hil_state_quaternion plugin
* fix files: some variable names were wrong+some syntax problems
* fix syntax error in plugin .cpp files, make msg files match corresponding mavlink definitions
* fix plugin source files
* fix syntax
* fix function name. It was wrong.
* add HIL_GPS plugin
* add HilGPS.msg to CMakeList
* fix missing semicolon
* fix call of class name
* Add ACTUATOR_CONTROL_TARGET MAVLink message
* fix code
* increase number of fake satellites
* control sensor and control rates
* change control rate
* change control rate
* fix fake gps rate
* fix
* fix plugin_list
* fix
* remove unnecessary hil_sensor_mixin
* update HilSensor.msg and usage
* update HilStateQuaterion.msg and usage
* redo some changes; update HilGPS.msg and usage
* update hil_controls msg - use array of floats for aux channels
* merge actuator_control with actuator_control_target
* remove hil_sensor_mixin.h
* update actuator_control logic
* merge all plugins into a single one
* delete the remaining plugin files
* update description
* redo some changes; reduce LOC
* fix type cast on gps coord
* add HIL_OPTICAL_FLOW send based on OpticalFlowRad sub
* update authors list
* update subscribers names
* refactor gps coord convention
* add HIL_RC_INPUTS_RAW sender; cog protec msg structure and content
* apply correct rc_in translation; redo cog
* apply proper rotations and frame transforms
* remote throttle
* fix typo and msg api
* small changes
* refactor rcin_raw_cb
* new refactor to rcin_raw_cb arrays
* update velocity to meters
* readjust all the units so to match mavlink msg def
* update cog
* correct cog conversion
* refefine msg definitions to remove overhead
* hil: apply frame transform to body frame
* msgs fix `#625 <https://github.com/mavlink/mavros/issues/625>`_: Rename SetMode.Response.success to mode_sent
* [WIP] Plugins: setpoint_attitude: add sync between thrust and attitude (`#700 <https://github.com/mavlink/mavros/issues/700>`_)
* plugins: setpoint_attitude: add sync between throttle and attitude topics to be sent together
* plugins: typo correction: replace throttle with thrust
* plugins: msgs: setpoint_attitude: replaces Float32Stamped for Thrust msg
* plugins: setpoint_attitude: add sync between twist and thrust (RPY+Thrust)
* setpoint_attitude: update the logic of thrust normalization verification
* setpoint_attitude: implement sync between tf listener and thrust subscriber
* TF sync listener: generalize topic type that can be syncronized with TF2
* TF2ListenerMixin: keep class template, use template for tf sync method only
* TF2ListenerMixin: fix and improve sync tf2_start method
* general update to yaml config files and parameters
* setpoint_attitude: add note on Thrust sub name
* setpoint_attitude: TF sync: pass subscriber pointer instead of binding it
* Use GeographicLib tools to guarantee ROS msg def and enhance features (`#693 <https://github.com/mavlink/mavros/issues/693>`_)
* first commit
* Check for GeographicLib first without having to install it from the beginning each compile time
* add necessary cmake files
* remove gps_conversions.h and use GeographicLib to obtain the UTM coordinates
* move conversion functions to utils.h
* geographic conversions: update CMakeLists and package.xml
* geographic conversions: force download of the datasets
* geographic conversions: remove unneeded cmake module
* dependencies: use SHARED libs of geographiclib
* dependencies: correct FindGeographicLib.cmake so it can work for common Debian platforms
* CMakeList: do not be so restrict about GeographicLib dependency
* global position: odometry-use ECEF instead of UTM; update other fields
* global position: make travis happy
* global position: fix ident
* global_position: apply correct frames and frame transforms given each coordinate frame
* global_position: convert rcvd global origin to ECEF
* global_position: be more explicit about the ecef-enu transform
* global position: use home position as origin of map frame
* global position: minor refactoring
* global position: shield code with exception catch
* fix identation
* move dataset install to script; update README with new functionalities
* update README with warning
* global_position: fix identation
* update HomePosition to be consistent with the conversions in global_position to ensure the correct transformation of height
* home|global_position: fix compile errors, logic and dependencies
* home position: add height conversion
* travis: update to get datasets
* install geo dataset: update to verify alternative dataset folders
* travis: remove dataset install to allow clean build
* hp and gp: initialize geoid dataset once and make it thread safe
* README: update description relative to GeographicLib; fix typos
* global position: improve doxygen references
* README: update with some tips on rosdep install
* update ExtendedState with new MAV_LANDED_STATE enum
* Contributors: Nicklas Stockton, Nuno Marques, Vladimir Ermakov
0.19.0 (2017-05-05)
-------------------
* msgs: Add cog script to finish ADSBVehicle.msg
* extras: Add ADSB plugin
* plugin `#695 <https://github.com/mavlink/mavros/issues/695>`_: Fix plugin
* plugin: Add home_position
* Contributors: Nuno Marques, Vladimir Ermakov
0.18.7 (2017-02-24)
-------------------
* trigger interface : rename to cycle_time to be consistent with PX4
* Contributors: Kabir Mohammed
0.18.6 (2017-02-07)
-------------------
* Plugins: system_status change status field to system_status
Add comment to State.msg for system_status enum
* Plugins: add system_status to state message
* Contributors: Pierre Kancir
0.18.5 (2016-12-12)
-------------------
0.18.4 (2016-11-11)
-------------------
* msgs: Fix `#609 <https://github.com/mavlink/mavros/issues/609>`_
* add hil_actuator_controls mavlink message
* Contributors: Beat Kung, Vladimir Ermakov
0.18.3 (2016-07-07)
-------------------
0.18.2 (2016-06-30)
-------------------
0.18.1 (2016-06-24)
-------------------
0.18.0 (2016-06-23)
-------------------
* Adding anchor to the HIL_CONTROLS message reference link
* Utilizing synchronise_stamp and adding reference to MAVLINK msg documentation
* Added a plugin that publishes HIL_CONTROLS as ROS messages
* node: Rename plugib base class - API incompatible to old class
* msgs `#543 <https://github.com/mavlink/mavros/issues/543>`_: Update for MAVLink 2.0
* Contributors: Pavel, Vladimir Ermakov
0.17.3 (2016-05-20)
-------------------
0.17.2 (2016-04-29)
-------------------
0.17.1 (2016-03-28)
-------------------
0.17.0 (2016-02-09)
-------------------
* rebased with master
* Contributors: francois
0.16.6 (2016-02-04)
-------------------
0.16.5 (2016-01-11)
-------------------
0.16.4 (2015-12-14)
-------------------
* Update mavlink message documentation links
* remove "altitude\_" prefix from members
* implemented altitude plugin
* Contributors: Andreas Antener, Vladimir Ermakov
0.16.3 (2015-11-19)
-------------------
0.16.2 (2015-11-17)
-------------------
0.16.1 (2015-11-13)
-------------------
0.16.0 (2015-11-09)
-------------------
* msgs `#418 <https://github.com/mavlink/mavros/issues/418>`_: add message for attitude setpoints
* plugin: waypoint fix `#414 <https://github.com/mavlink/mavros/issues/414>`_: remove GOTO service.
It is replaced with more standard global setpoint messages.
* msgs `#415 <https://github.com/mavlink/mavros/issues/415>`_: Add message for raw global setpoint
* msgs `#402 <https://github.com/mavlink/mavros/issues/402>`_: PositionTarget message type
* setting constant values and reference docs
* pass new extended state to ros
* msgs `#371 <https://github.com/mavlink/mavros/issues/371>`_: add missing message
* msgs `#371 <https://github.com/mavlink/mavros/issues/371>`_: add HomePosition message
* Contributors: Andreas Antener, Vladimir Ermakov
0.15.0 (2015-09-17)
-------------------
* msgs `#286 <https://github.com/mavlink/mavros/issues/286>`_: fix bug with packet header.
* msgs `#286 <https://github.com/mavlink/mavros/issues/286>`_: Add valid flag and checksum to Mavlink.msg
* plugin: manual_control: Use shared pointer message
Fix alphabetic order of msgs.
* removed old commend in .msg file
* Add MANUAL_CONTROL handling with new plugin
* Contributors: Vladimir Ermakov, v01d
0.14.2 (2015-08-20)
-------------------
0.14.1 (2015-08-19)
-------------------
0.14.0 (2015-08-17)
-------------------
* msgs: Add mixer group constants ActuatorControl
* msgs: Add notes to message headers.
* msgs: sort msgs in alphabetical order
* msgs: use std::move for mavlink->ros convert
* msgs: add note about convert function
* msgs: change description, make catkin lint happy
* msgs: move convert functions to msgs package.
* msgs: fix message generator and runtime depent tags
* msgs: remove never used Mavlink.fromlcm field.
* msgs: add package name for all non basic types
* msgs: fix msgs build
* msgs `#354 <https://github.com/mavlink/mavros/issues/354>`_: move all messages to mavros_msgs package.
* Contributors: Vladimir Ermakov

26
Controller/src/utils/mavros_msgs/CMakeLists.txt Normal file
View File

@@ -0,0 +1,26 @@
cmake_minimum_required(VERSION 2.8.3)
project(mavros_msgs)
find_package(catkin REQUIRED COMPONENTS geometry_msgs message_generation std_msgs)
include_directories(include)
add_message_files(
DIRECTORY msg
FILES
AttitudeTarget.msg
State.msg
)
add_service_files(
DIRECTORY srv
FILES
CommandBool.srv
SetMode.srv
)
generate_messages(DEPENDENCIES geometry_msgs std_msgs)
catkin_package(
INCLUDE_DIRS include
CATKIN_DEPENDS geometry_msgs message_runtime std_msgs)

129
Controller/src/utils/mavros_msgs/include/mavros_msgs/mavlink_convert.h Normal file
View File

@@ -0,0 +1,129 @@
/**
* @brief Mavlink convert utils
* @file
* @author Vladimir Ermakov <vooon341@gmail.com>
*/
/*
* Copyright 2015,2016 Vladimir Ermakov.
*
* This file is part of the mavros package and subject to the license terms
* in the top-level LICENSE file of the mavros repository.
* https://github.com/mavlink/mavros/tree/master/LICENSE.md
*/
#pragma once
#include <algorithm>
#include <mavros_msgs/Mavlink.h>
#include <mavconn/mavlink_dialect.h>
namespace mavros_msgs {
namespace mavlink {
using ::mavlink::mavlink_message_t;
// [[[cog:
// FIELD_NAMES = [
// "magic",
// "len",
// "incompat_flags",
// "compat_flags",
// "seq",
// "sysid",
// "compid",
// "msgid",
// "checksum",
// ]
// ]]]
// [[[end]]] (checksum: d41d8cd98f00b204e9800998ecf8427e)
// NOTE(vooon): Ignore impossible warning as
// memcpy() should work with unaligned pointers without any trouble.
//
// warning: taking address of packed member of ‘mavlink::__mavlink_message’ may result in an unaligned pointer value
#pragma GCC diagnostic push
#pragma GCC diagnostic ignored "-Waddress-of-packed-member"
/**
* @brief Convert mavros_msgs/Mavlink message to mavlink_message_t
*
* @note signature vector should be empty for unsigned OR
* MAVLINK_SIGNATURE_BLOCK size for signed messages
*
* @param[in] rmsg mavros_msgs/Mavlink message
* @param[out] mmsg mavlink_message_t struct
* @return true if success
*/
inline bool convert(const mavros_msgs::Mavlink &rmsg, mavlink_message_t &mmsg)
{
if (rmsg.payload64.size() > sizeof(mmsg.payload64) / sizeof(mmsg.payload64[0])) {
return false;
}
if (!rmsg.signature.empty() && rmsg.signature.size() != sizeof(mmsg.signature)) {
return false;
}
// [[[cog:
// for f in FIELD_NAMES:
// cog.outl("mmsg.%s = rmsg.%s;" % (f, f))
// ]]]
mmsg.magic = rmsg.magic;
mmsg.len = rmsg.len;
mmsg.incompat_flags = rmsg.incompat_flags;
mmsg.compat_flags = rmsg.compat_flags;
mmsg.seq = rmsg.seq;
mmsg.sysid = rmsg.sysid;
mmsg.compid = rmsg.compid;
mmsg.msgid = rmsg.msgid;
mmsg.checksum = rmsg.checksum;
// [[[end]]] (checksum: 2ef42a7798f261bfd367bf4157b11ec0)
std::copy(rmsg.payload64.begin(), rmsg.payload64.end(), mmsg.payload64);
std::copy(rmsg.signature.begin(), rmsg.signature.end(), mmsg.signature);
return true;
}
/**
* @brief Convert mavlink_message_t to mavros/Mavlink
*
* @param[in] mmsg mavlink_message_t struct
* @param[out] rmsg mavros_msgs/Mavlink message
* @param[in] framing_status framing parse result (OK, BAD_CRC or BAD_SIGNATURE)
* @return true, this convertion can't fail
*/
inline bool convert(const mavlink_message_t &mmsg, mavros_msgs::Mavlink &rmsg, uint8_t framing_status = mavros_msgs::Mavlink::FRAMING_OK)
{
const size_t payload64_len = (mmsg.len + 7) / 8;
rmsg.framing_status = framing_status;
// [[[cog:
// for f in FIELD_NAMES:
// cog.outl("rmsg.%s = mmsg.%s;" % (f, f))
// ]]]
rmsg.magic = mmsg.magic;
rmsg.len = mmsg.len;
rmsg.incompat_flags = mmsg.incompat_flags;
rmsg.compat_flags = mmsg.compat_flags;
rmsg.seq = mmsg.seq;
rmsg.sysid = mmsg.sysid;
rmsg.compid = mmsg.compid;
rmsg.msgid = mmsg.msgid;
rmsg.checksum = mmsg.checksum;
// [[[end]]] (checksum: 4f0a50d2fcd7eb8823aea3e0806cd698)
rmsg.payload64 = mavros_msgs::Mavlink::_payload64_type(mmsg.payload64, mmsg.payload64 + payload64_len);
// copy signature block only if message is signed
if (mmsg.incompat_flags & MAVLINK_IFLAG_SIGNED)
rmsg.signature = mavros_msgs::Mavlink::_signature_type(mmsg.signature, mmsg.signature + sizeof(mmsg.signature));
else
rmsg.signature.clear();
return true;
}
#pragma GCC diagnostic pop
} // namespace mavlink
} // namespace mavros_msgs

17
Controller/src/utils/mavros_msgs/msg/AttitudeTarget.msg Normal file
View File

@@ -0,0 +1,17 @@
# Message for SET_ATTITUDE_TARGET
#
# Some complex system requires all feautures that mavlink
# message provide. See issue #402, #418.
std_msgs/Header header
uint8 type_mask
uint8 IGNORE_ROLL_RATE = 1 # body_rate.x
uint8 IGNORE_PITCH_RATE = 2 # body_rate.y
uint8 IGNORE_YAW_RATE = 4 # body_rate.z
uint8 IGNORE_THRUST = 64
uint8 IGNORE_ATTITUDE = 128 # orientation field
geometry_msgs/Quaternion orientation
geometry_msgs/Vector3 body_rate
float32 thrust

82
Controller/src/utils/mavros_msgs/msg/State.msg Normal file
View File

@@ -0,0 +1,82 @@
# Current autopilot state
#
# Known modes listed here:
# http://wiki.ros.org/mavros/CustomModes
#
# For system_status values
# see https://mavlink.io/en/messages/common.html#MAV_STATE
#
std_msgs/Header header
bool connected
bool armed
bool guided
bool manual_input
string mode
uint8 system_status
string MODE_APM_PLANE_MANUAL = MANUAL
string MODE_APM_PLANE_CIRCLE = CIRCLE
string MODE_APM_PLANE_STABILIZE = STABILIZE
string MODE_APM_PLANE_TRAINING = TRAINING
string MODE_APM_PLANE_ACRO = ACRO
string MODE_APM_PLANE_FBWA = FBWA
string MODE_APM_PLANE_FBWB = FBWB
string MODE_APM_PLANE_CRUISE = CRUISE
string MODE_APM_PLANE_AUTOTUNE = AUTOTUNE
string MODE_APM_PLANE_AUTO = AUTO
string MODE_APM_PLANE_RTL = RTL
string MODE_APM_PLANE_LOITER = LOITER
string MODE_APM_PLANE_LAND = LAND
string MODE_APM_PLANE_GUIDED = GUIDED
string MODE_APM_PLANE_INITIALISING = INITIALISING
string MODE_APM_PLANE_QSTABILIZE = QSTABILIZE
string MODE_APM_PLANE_QHOVER = QHOVER
string MODE_APM_PLANE_QLOITER = QLOITER
string MODE_APM_PLANE_QLAND = QLAND
string MODE_APM_PLANE_QRTL = QRTL
string MODE_APM_COPTER_STABILIZE = STABILIZE
string MODE_APM_COPTER_ACRO = ACRO
string MODE_APM_COPTER_ALT_HOLD = ALT_HOLD
string MODE_APM_COPTER_AUTO = AUTO
string MODE_APM_COPTER_GUIDED = GUIDED
string MODE_APM_COPTER_LOITER = LOITER
string MODE_APM_COPTER_RTL = RTL
string MODE_APM_COPTER_CIRCLE = CIRCLE
string MODE_APM_COPTER_POSITION = POSITION
string MODE_APM_COPTER_LAND = LAND
string MODE_APM_COPTER_OF_LOITER = OF_LOITER
string MODE_APM_COPTER_DRIFT = DRIFT
string MODE_APM_COPTER_SPORT = SPORT
string MODE_APM_COPTER_FLIP = FLIP
string MODE_APM_COPTER_AUTOTUNE = AUTOTUNE
string MODE_APM_COPTER_POSHOLD = POSHOLD
string MODE_APM_COPTER_BRAKE = BRAKE
string MODE_APM_COPTER_THROW = THROW
string MODE_APM_COPTER_AVOID_ADSB = AVOID_ADSB
string MODE_APM_COPTER_GUIDED_NOGPS = GUIDED_NOGPS
string MODE_APM_ROVER_MANUAL = MANUAL
string MODE_APM_ROVER_LEARNING = LEARNING
string MODE_APM_ROVER_STEERING = STEERING
string MODE_APM_ROVER_HOLD = HOLD
string MODE_APM_ROVER_AUTO = AUTO
string MODE_APM_ROVER_RTL = RTL
string MODE_APM_ROVER_GUIDED = GUIDED
string MODE_APM_ROVER_INITIALISING = INITIALISING
string MODE_PX4_MANUAL = MANUAL
string MODE_PX4_ACRO = ACRO
string MODE_PX4_ALTITUDE = ALTCTL
string MODE_PX4_POSITION = POSCTL
string MODE_PX4_OFFBOARD = OFFBOARD
string MODE_PX4_STABILIZED = STABILIZED
string MODE_PX4_RATTITUDE = RATTITUDE
string MODE_PX4_MISSION = AUTO.MISSION
string MODE_PX4_LOITER = AUTO.LOITER
string MODE_PX4_RTL = AUTO.RTL
string MODE_PX4_LAND = AUTO.LAND
string MODE_PX4_RTGS = AUTO.RTGS
string MODE_PX4_READY = AUTO.READY
string MODE_PX4_TAKEOFF = AUTO.TAKEOFF

32
Controller/src/utils/mavros_msgs/package.xml Normal file
View File

@@ -0,0 +1,32 @@
<?xml version="1.0"?>
<package format="2">
<name>mavros_msgs</name>
<version>1.16.0</version>
<description>
mavros_msgs defines messages for <a href="http://wiki.ros.org/mavros">MAVROS</a>.
</description>
<maintainer email="vooon341@gmail.com">Vladimir Ermakov</maintainer>
<author email="vooon341@gmail.com">Vladimir Ermakov</author>
<license>GPLv3</license>
<license>LGPLv3</license>
<license>BSD</license>
<url type="website">http://wiki.ros.org/mavros_msgs</url>
<url type="repository">https://github.com/mavlink/mavros.git</url>
<url type="bugtracker">https://github.com/mavlink/mavros/issues</url>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>message_generation</build_depend>
<exec_depend>message_runtime</exec_depend>
<depend>std_msgs</depend>
<depend>geometry_msgs</depend>
<!-- XXX needed for users of mavlink_convert.h
<build_export_depend>libmavconn</build_export_depend>
-->
<export>
<architecture_independent/>
</export>
</package>

6
Controller/src/utils/mavros_msgs/srv/CommandBool.srv Normal file
View File

@@ -0,0 +1,6 @@
# Common type for switch commands
bool value
---
bool success
uint8 result

22
Controller/src/utils/mavros_msgs/srv/SetMode.srv Normal file
View File

@@ -0,0 +1,22 @@
# set FCU mode
#
# Known custom modes listed here:
# http://wiki.ros.org/mavros/CustomModes
# basic modes from MAV_MODE
uint8 MAV_MODE_PREFLIGHT = 0
uint8 MAV_MODE_STABILIZE_DISARMED = 80
uint8 MAV_MODE_STABILIZE_ARMED = 208
uint8 MAV_MODE_MANUAL_DISARMED = 64
uint8 MAV_MODE_MANUAL_ARMED = 192
uint8 MAV_MODE_GUIDED_DISARMED = 88
uint8 MAV_MODE_GUIDED_ARMED = 216
uint8 MAV_MODE_AUTO_DISARMED = 92
uint8 MAV_MODE_AUTO_ARMED = 220
uint8 MAV_MODE_TEST_DISARMED = 66
uint8 MAV_MODE_TEST_ARMED = 194
uint8 base_mode # filled by MAV_MODE enum value or 0 if custom_mode != ''
string custom_mode # string mode representation or integer
---
bool mode_sent # Mode known/parsed correctly and SET_MODE are sent

116
Controller/src/utils/quadrotor_msgs/CMakeLists.txt Executable file
View File

@@ -0,0 +1,116 @@
#-cmake_minimum_required(VERSION 2.4.6)
#-include($ENV{ROS_ROOT}/core/rosbuild/rosbuild.cmake)
# Set the build type. Options are:
# Coverage : w/ debug symbols, w/o optimization, w/ code-coverage
# Debug : w/ debug symbols, w/o optimization
# Release : w/o debug symbols, w/ optimization
# RelWithDebInfo : w/ debug symbols, w/ optimization
# MinSizeRel : w/o debug symbols, w/ optimization, stripped binaries
#set(ROS_BUILD_TYPE RelWithDebInfo)
#-rosbuild_init()
#set the default path for built executables to the "bin" directory
#-set(EXECUTABLE_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/bin)
#set the default path for built libraries to the "lib" directory
#-set(LIBRARY_OUTPUT_PATH ${PROJECT_SOURCE_DIR}/lib)
#uncomment if you have defined messages
#rosbuild_genmsg()
#uncomment if you have defined services
#rosbuild_gensrv()
#common commands for building c++ executables and libraries
#-\rosbuild_add_library(${PROJECT_NAME} src/example.cpp)
#target_link_libraries(${PROJECT_NAME} another_library)
#rosbuild_add_boost_directories()
#rosbuild_link_boost(${PROJECT_NAME} thread)
#rosbuild_add_executable(example examples/example.cpp)
#target_link_libraries(example ${PROJECT_NAME})
#-rosbuild_add_library(encode_msgs src/encode_msgs.cpp)
#-rosbuild_add_link_flags(encode_msgs -Wl,--as-needed)
#list(APPEND CMAKE_MODULE_PATH ${CMAKE_CURRENT_SOURCE_DIR}/cmake)
#find_package(Eigen3 REQUIRED)
#include_directories(${EIGEN3_INCLUDE_DIR})
#-rosbuild_add_library(decode_msgs src/decode_msgs.cpp)
#-rosbuild_add_link_flags(decode_msgs -Wl,--as-needed)
#------------------------------------------------------------------
cmake_minimum_required(VERSION 2.8.3)
project(quadrotor_msgs)
find_package(catkin REQUIRED COMPONENTS
#armadillo
roscpp
nav_msgs
geometry_msgs
message_generation
message_runtime
)
set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake)
include_directories(
${catkin_INCLUDE_DIRS}
include
)
add_message_files(
FILES
AuxCommand.msg
Corrections.msg
Gains.msg
OutputData.msg
PositionCommand.msg
PPROutputData.msg
Serial.msg
SO3Command.msg
StatusData.msg
TRPYCommand.msg
Odometry.msg
PolynomialTrajectory.msg
LQRTrajectory.msg
)
add_service_files(
DIRECTORY srv
FILES
SetTakeoffLand.srv
)
generate_messages(
DEPENDENCIES
geometry_msgs
nav_msgs
)
catkin_package(
#INCLUDE_DIRS include
LIBRARIES encode_msgs decode_msgs
#CATKIN_DEPENDS geometry_msgs nav_msgs
#DEPENDS system_lib
CATKIN_DEPENDS
)
# add_executable(test_exe src/test_exe.cpp)
add_library(decode_msgs src/decode_msgs.cpp)
add_library(encode_msgs src/encode_msgs.cpp)
# add_dependencies(test_exe quadrotor_msgs_generate_messages_cpp)
add_dependencies(encode_msgs quadrotor_msgs_generate_messages_cpp)
add_dependencies(decode_msgs quadrotor_msgs_generate_messages_cpp)
find_package(Eigen3 REQUIRED)
include_directories(${EIGEN3_INCLUDE_DIR})
# target_link_libraries(test_exe
# decode_msgs
# encode_msgs
# )

81
Controller/src/utils/quadrotor_msgs/cmake/FindEigen3.cmake Normal file
View File

@@ -0,0 +1,81 @@
# - Try to find Eigen3 lib
#
# This module supports requiring a minimum version, e.g. you can do
# find_package(Eigen3 3.1.2)
# to require version 3.1.2 or newer of Eigen3.
#
# Once done this will define
#
# EIGEN3_FOUND - system has eigen lib with correct version
# EIGEN3_INCLUDE_DIR - the eigen include directory
# EIGEN3_VERSION - eigen version
# Copyright (c) 2006, 2007 Montel Laurent, <montel@kde.org>
# Copyright (c) 2008, 2009 Gael Guennebaud, <g.gael@free.fr>
# Copyright (c) 2009 Benoit Jacob <jacob.benoit.1@gmail.com>
# Redistribution and use is allowed according to the terms of the 2-clause BSD license.
if(NOT Eigen3_FIND_VERSION)
if(NOT Eigen3_FIND_VERSION_MAJOR)
set(Eigen3_FIND_VERSION_MAJOR 2)
endif(NOT Eigen3_FIND_VERSION_MAJOR)
if(NOT Eigen3_FIND_VERSION_MINOR)
set(Eigen3_FIND_VERSION_MINOR 91)
endif(NOT Eigen3_FIND_VERSION_MINOR)
if(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION_PATCH 0)
endif(NOT Eigen3_FIND_VERSION_PATCH)
set(Eigen3_FIND_VERSION "${Eigen3_FIND_VERSION_MAJOR}.${Eigen3_FIND_VERSION_MINOR}.${Eigen3_FIND_VERSION_PATCH}")
endif(NOT Eigen3_FIND_VERSION)
macro(_eigen3_check_version)
file(READ "${EIGEN3_INCLUDE_DIR}/Eigen/src/Core/util/Macros.h" _eigen3_version_header)
string(REGEX MATCH "define[ \t]+EIGEN_WORLD_VERSION[ \t]+([0-9]+)" _eigen3_world_version_match "${_eigen3_version_header}")
set(EIGEN3_WORLD_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MAJOR_VERSION[ \t]+([0-9]+)" _eigen3_major_version_match "${_eigen3_version_header}")
set(EIGEN3_MAJOR_VERSION "${CMAKE_MATCH_1}")
string(REGEX MATCH "define[ \t]+EIGEN_MINOR_VERSION[ \t]+([0-9]+)" _eigen3_minor_version_match "${_eigen3_version_header}")
set(EIGEN3_MINOR_VERSION "${CMAKE_MATCH_1}")
set(EIGEN3_VERSION ${EIGEN3_WORLD_VERSION}.${EIGEN3_MAJOR_VERSION}.${EIGEN3_MINOR_VERSION})
if(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK FALSE)
else(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
set(EIGEN3_VERSION_OK TRUE)
endif(${EIGEN3_VERSION} VERSION_LESS ${Eigen3_FIND_VERSION})
if(NOT EIGEN3_VERSION_OK)
message(STATUS "Eigen3 version ${EIGEN3_VERSION} found in ${EIGEN3_INCLUDE_DIR}, "
"but at least version ${Eigen3_FIND_VERSION} is required")
endif(NOT EIGEN3_VERSION_OK)
endmacro(_eigen3_check_version)
if (EIGEN3_INCLUDE_DIR)
# in cache already
_eigen3_check_version()
set(EIGEN3_FOUND ${EIGEN3_VERSION_OK})
else (EIGEN3_INCLUDE_DIR)
find_path(EIGEN3_INCLUDE_DIR NAMES signature_of_eigen3_matrix_library
PATHS
${CMAKE_INSTALL_PREFIX}/include
${KDE4_INCLUDE_DIR}
PATH_SUFFIXES eigen3 eigen
)
if(EIGEN3_INCLUDE_DIR)
_eigen3_check_version()
endif(EIGEN3_INCLUDE_DIR)
include(FindPackageHandleStandardArgs)
find_package_handle_standard_args(Eigen3 DEFAULT_MSG EIGEN3_INCLUDE_DIR EIGEN3_VERSION_OK)
mark_as_advanced(EIGEN3_INCLUDE_DIR)
endif(EIGEN3_INCLUDE_DIR)

87
Controller/src/utils/quadrotor_msgs/include/quadrotor_msgs/comm_types.h Executable file
View File

@@ -0,0 +1,87 @@
#ifndef __QUADROTOR_MSGS_COMM_TYPES_H__
#define __QUADROTOR_MSGS_COMM_TYPES_H__
#define TYPE_SO3_CMD 's'
struct SO3_CMD_INPUT
{
// Scaling factors when decoding
int16_t force[3]; // /500
int8_t des_qx, des_qy, des_qz, des_qw; // /125
uint8_t kR[3]; // /50
uint8_t kOm[3]; // /100
int16_t cur_yaw; // /1e4
int16_t kf_correction; // /1e11;
uint8_t angle_corrections[2]; // roll,pitch /2500
uint8_t enable_motors:1;
uint8_t use_external_yaw:1;
uint8_t seq;
};
#define TYPE_STATUS_DATA 'c'
struct STATUS_DATA
{
uint16_t loop_rate;
uint16_t voltage;
uint8_t seq;
};
#define TYPE_OUTPUT_DATA 'd'
struct OUTPUT_DATA
{
uint16_t loop_rate;
uint16_t voltage;
int16_t roll, pitch, yaw;
int16_t ang_vel[3];
int16_t acc[3];
int16_t dheight;
int32_t height;
int16_t mag[3];
uint8_t radio[8];
//uint8_t rpm[4];
uint8_t seq;
};
#define TYPE_TRPY_CMD 'p'
struct TRPY_CMD
{
int16_t thrust;
int16_t roll;
int16_t pitch;
int16_t yaw;
int16_t current_yaw;
uint8_t enable_motors:1;
uint8_t use_external_yaw:1;
};
#define TYPE_PPR_OUTPUT_DATA 't'
struct PPR_OUTPUT_DATA
{
uint16_t time;
int16_t des_thrust;
int16_t des_roll;
int16_t des_pitch;
int16_t des_yaw;
int16_t est_roll;
int16_t est_pitch;
int16_t est_yaw;
int16_t est_angvel_x;
int16_t est_angvel_y;
int16_t est_angvel_z;
int16_t est_acc_x;
int16_t est_acc_y;
int16_t est_acc_z;
uint16_t pwm1;
uint16_t pwm2;
uint16_t pwm3;
uint16_t pwm4;
};
#define TYPE_PPR_GAINS 'g'
struct PPR_GAINS
{
int16_t Kp;
int16_t Kd;
int16_t Kp_yaw;
int16_t Kd_yaw;
};
#endif

23
Controller/src/utils/quadrotor_msgs/include/quadrotor_msgs/decode_msgs.h Executable file
View File

@@ -0,0 +1,23 @@
#ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__
#define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__
#include <stdint.h>
#include <vector>
#include <quadrotor_msgs/OutputData.h>
#include <quadrotor_msgs/StatusData.h>
#include <quadrotor_msgs/PPROutputData.h>
namespace quadrotor_msgs
{
bool decodeOutputData(const std::vector<uint8_t> &data,
quadrotor_msgs::OutputData &output);
bool decodeStatusData(const std::vector<uint8_t> &data,
quadrotor_msgs::StatusData &status);
bool decodePPROutputData(const std::vector<uint8_t> &data,
quadrotor_msgs::PPROutputData &output);
}
#endif

22
Controller/src/utils/quadrotor_msgs/include/quadrotor_msgs/encode_msgs.h Executable file
View File

@@ -0,0 +1,22 @@
#ifndef __QUADROTOR_MSGS_QUADROTOR_MSGS_H__
#define __QUADROTOR_MSGS_QUADROTOR_MSGS_H__
#include <stdint.h>
#include <vector>
#include <quadrotor_msgs/SO3Command.h>
#include <quadrotor_msgs/TRPYCommand.h>
#include <quadrotor_msgs/Gains.h>
namespace quadrotor_msgs
{
void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command,
std::vector<uint8_t> &output);
void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command,
std::vector<uint8_t> &output);
void encodePPRGains(const quadrotor_msgs::Gains &gains,
std::vector<uint8_t> &output);
}
#endif

5
Controller/src/utils/quadrotor_msgs/msg/AuxCommand.msg Executable file
View File

@@ -0,0 +1,5 @@
float64 current_yaw
float64 kf_correction
float64[2] angle_corrections# Trims for roll, pitch
bool enable_motors
bool use_external_yaw

2
Controller/src/utils/quadrotor_msgs/msg/Corrections.msg Executable file
View File

@@ -0,0 +1,2 @@
float64 kf_correction
float64[2] angle_corrections

4
Controller/src/utils/quadrotor_msgs/msg/Gains.msg Executable file
View File

@@ -0,0 +1,4 @@
float64 Kp
float64 Kd
float64 Kp_yaw
float64 Kd_yaw

30
Controller/src/utils/quadrotor_msgs/msg/LQRTrajectory.msg Normal file
View File

@@ -0,0 +1,30 @@
Header header
# the trajectory id, starts from "1".
uint32 trajectory_id
# the action command for trajectory server.
uint32 ACTION_ADD = 1
uint32 ACTION_ABORT = 2
uint32 ACTION_WARN_START = 3
uint32 ACTION_WARN_FINAL = 4
uint32 ACTION_WARN_IMPOSSIBLE = 5
uint32 action
# the weight coefficient of the control effort
float64 r
# the yaw command
float64 start_yaw
float64 final_yaw
# the initial and final state
float64[6] s0
float64[3] ut
float64[6] sf
# the optimal arrival time
float64 t_f
string debug_info

8
Controller/src/utils/quadrotor_msgs/msg/Odometry.msg Executable file
View File

@@ -0,0 +1,8 @@
uint8 STATUS_ODOM_VALID=0
uint8 STATUS_ODOM_INVALID=1
uint8 STATUS_ODOM_LOOPCLOSURE=2
nav_msgs/Odometry curodom
nav_msgs/Odometry kfodom
uint32 kfid
uint8 status

12
Controller/src/utils/quadrotor_msgs/msg/OutputData.msg Executable file
View File

@@ -0,0 +1,12 @@
Header header
uint16 loop_rate
float64 voltage
geometry_msgs/Quaternion orientation
geometry_msgs/Vector3 angular_velocity
geometry_msgs/Vector3 linear_acceleration
float64 pressure_dheight
float64 pressure_height
geometry_msgs/Vector3 magnetic_field
uint8[8] radio_channel
#uint8[4] motor_rpm
uint8 seq

16
Controller/src/utils/quadrotor_msgs/msg/PPROutputData.msg Executable file
View File

@@ -0,0 +1,16 @@
Header header
uint16 quad_time
float64 des_thrust
float64 des_roll
float64 des_pitch
float64 des_yaw
float64 est_roll
float64 est_pitch
float64 est_yaw
float64 est_angvel_x
float64 est_angvel_y
float64 est_angvel_z
float64 est_acc_x
float64 est_acc_y
float64 est_acc_z
uint16[4] pwm

28
Controller/src/utils/quadrotor_msgs/msg/PolynomialTrajectory.msg Executable file
View File

@@ -0,0 +1,28 @@
Header header
# the trajectory id, starts from "1".
uint32 trajectory_id
# the action command for trajectory server.
uint32 ACTION_ADD = 1
uint32 ACTION_ABORT = 2
uint32 ACTION_WARN_START = 3
uint32 ACTION_WARN_FINAL = 4
uint32 ACTION_WARN_IMPOSSIBLE = 5
uint32 action
# the order of trajectory.
uint32 num_order
uint32 num_segment
# the polynomial coecfficients of the trajectory.
float64 start_yaw
float64 final_yaw
float64[] coef_x
float64[] coef_y
float64[] coef_z
float64[] time
float64 mag_coeff
uint32[] order
string debug_info

21
Controller/src/utils/quadrotor_msgs/msg/PositionCommand.msg Executable file
View File

@@ -0,0 +1,21 @@
Header header
geometry_msgs/Point position
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 acceleration
float64 yaw
float64 yaw_dot
float64[3] kx
float64[3] kv
uint32 trajectory_id
uint8 TRAJECTORY_STATUS_EMPTY = 0
uint8 TRAJECTORY_STATUS_READY = 1
uint8 TRAJECTORY_STATUS_COMPLETED = 3
uint8 TRAJECTROY_STATUS_ABORT = 4
uint8 TRAJECTORY_STATUS_ILLEGAL_START = 5
uint8 TRAJECTORY_STATUS_ILLEGAL_FINAL = 6
uint8 TRAJECTORY_STATUS_IMPOSSIBLE = 7
# Its ID number will start from 1, allowing you comparing it with 0.
uint8 trajectory_flag

6
Controller/src/utils/quadrotor_msgs/msg/SO3Command.msg Executable file
View File

@@ -0,0 +1,6 @@
Header header
geometry_msgs/Vector3 force
geometry_msgs/Quaternion orientation
float64[3] kR
float64[3] kOm
quadrotor_msgs/AuxCommand aux

13
Controller/src/utils/quadrotor_msgs/msg/Serial.msg Executable file
View File

@@ -0,0 +1,13 @@
# Note: These constants need to be kept in sync with the types
# defined in include/quadrotor_msgs/comm_types.h
uint8 SO3_CMD = 115 # 's' in base 10
uint8 TRPY_CMD = 112 # 'p' in base 10
uint8 STATUS_DATA = 99 # 'c' in base 10
uint8 OUTPUT_DATA = 100 # 'd' in base 10
uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10
uint8 PPR_GAINS = 103 # 'g'
Header header
uint8 channel
uint8 type # One of the types listed above
uint8[] data

4
Controller/src/utils/quadrotor_msgs/msg/StatusData.msg Executable file
View File

@@ -0,0 +1,4 @@
Header header
uint16 loop_rate
float64 voltage
uint8 seq

6
Controller/src/utils/quadrotor_msgs/msg/TRPYCommand.msg Executable file
View File

@@ -0,0 +1,6 @@
Header header
float32 thrust
float32 roll
float32 pitch
float32 yaw
quadrotor_msgs/AuxCommand aux

20
Controller/src/utils/quadrotor_msgs/package.xml Executable file
View File

@@ -0,0 +1,20 @@
<package>
<name>quadrotor_msgs</name>
<version>0.0.0</version>
<description>quadrotor_msgs</description>
<maintainer email="todo@todo.todo">Kartik Mohta</maintainer>
<url>http://ros.org/wiki/quadrotor_msgs</url>
<license>BSD</license>
<buildtool_depend>catkin</buildtool_depend>
<build_depend>geometry_msgs</build_depend>
<build_depend>nav_msgs</build_depend>
<build_depend>message_generation</build_depend>
<run_depend>geometry_msgs</run_depend>
<run_depend>nav_msgs</run_depend>
<run_depend>message_runtime</run_depend>
<export>
<cpp cflags="-I${prefix}/include"
lflags="-Wl,-rpath,${prefix}/lib -L${prefix}/lib"/>
</export>
</package>

103
Controller/src/utils/quadrotor_msgs/src/decode_msgs.cpp Executable file
View File

@@ -0,0 +1,103 @@
#include "quadrotor_msgs/decode_msgs.h"
#include <quadrotor_msgs/comm_types.h>
#include <Eigen/Geometry>
namespace quadrotor_msgs
{
bool decodeOutputData(const std::vector<uint8_t> &data,
quadrotor_msgs::OutputData &output)
{
struct OUTPUT_DATA output_data;
if(data.size() != sizeof(output_data))
return false;
memcpy(&output_data, &data[0], sizeof(output_data));
output.loop_rate = output_data.loop_rate;
output.voltage = output_data.voltage/1e3;
const double roll = output_data.roll/1e2 * M_PI/180;
const double pitch = output_data.pitch/1e2 * M_PI/180;
const double yaw = output_data.yaw/1e2 * M_PI/180;
// Asctec (2012 firmware) uses Z-Y-X convention
Eigen::Quaterniond q = Eigen::AngleAxisd(yaw, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd(pitch, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd(roll, Eigen::Vector3d::UnitX());
output.orientation.w = q.w();
output.orientation.x = q.x();
output.orientation.y = q.y();
output.orientation.z = q.z();
output.angular_velocity.x = output_data.ang_vel[0]*0.0154*M_PI/180;
output.angular_velocity.y = output_data.ang_vel[1]*0.0154*M_PI/180;
output.angular_velocity.z = output_data.ang_vel[2]*0.0154*M_PI/180;
output.linear_acceleration.x = output_data.acc[0]/1e3 * 9.81;
output.linear_acceleration.y = output_data.acc[1]/1e3 * 9.81;
output.linear_acceleration.z = output_data.acc[2]/1e3 * 9.81;
output.pressure_dheight = output_data.dheight/1e3;
output.pressure_height = output_data.height/1e3;
output.magnetic_field.x = output_data.mag[0]/2500.0;
output.magnetic_field.y = output_data.mag[1]/2500.0;
output.magnetic_field.z = output_data.mag[2]/2500.0;
for(int i = 0; i < 8; i++)
{
output.radio_channel[i] = output_data.radio[i];
}
//for(int i = 0; i < 4; i++)
// output.motor_rpm[i] = output_data.rpm[i];
output.seq = output_data.seq;
return true;
}
bool decodeStatusData(const std::vector<uint8_t> &data,
quadrotor_msgs::StatusData &status)
{
struct STATUS_DATA status_data;
if(data.size() != sizeof(status_data))
return false;
memcpy(&status_data, &data[0], sizeof(status_data));
status.loop_rate = status_data.loop_rate;
status.voltage = status_data.voltage/1e3;
status.seq = status_data.seq;
return true;
}
bool decodePPROutputData(const std::vector<uint8_t> &data,
quadrotor_msgs::PPROutputData &output)
{
struct PPR_OUTPUT_DATA output_data;
if(data.size() != sizeof(output_data))
return false;
memcpy(&output_data, &data[0], sizeof(output_data));
output.quad_time = output_data.time;
output.des_thrust = output_data.des_thrust*1e-4;
output.des_roll = output_data.des_roll*1e-4;
output.des_pitch = output_data.des_pitch*1e-4;
output.des_yaw = output_data.des_yaw*1e-4;
output.est_roll = output_data.est_roll*1e-4;
output.est_pitch = output_data.est_pitch*1e-4;
output.est_yaw = output_data.est_yaw*1e-4;
output.est_angvel_x = output_data.est_angvel_x*1e-3;
output.est_angvel_y = output_data.est_angvel_y*1e-3;
output.est_angvel_z = output_data.est_angvel_z*1e-3;
output.est_acc_x = output_data.est_acc_x*1e-4;
output.est_acc_y = output_data.est_acc_y*1e-4;
output.est_acc_z = output_data.est_acc_z*1e-4;
output.pwm[0] = output_data.pwm1;
output.pwm[1] = output_data.pwm2;
output.pwm[2] = output_data.pwm3;
output.pwm[3] = output_data.pwm4;
return true;
}
}

73
Controller/src/utils/quadrotor_msgs/src/encode_msgs.cpp Executable file
View File

@@ -0,0 +1,73 @@
#include "quadrotor_msgs/encode_msgs.h"
#include <quadrotor_msgs/comm_types.h>
namespace quadrotor_msgs
{
void encodeSO3Command(const quadrotor_msgs::SO3Command &so3_command,
std::vector<uint8_t> &output)
{
struct SO3_CMD_INPUT so3_cmd_input;
so3_cmd_input.force[0] = so3_command.force.x*500;
so3_cmd_input.force[1] = so3_command.force.y*500;
so3_cmd_input.force[2] = so3_command.force.z*500;
so3_cmd_input.des_qx = so3_command.orientation.x*125;
so3_cmd_input.des_qy = so3_command.orientation.y*125;
so3_cmd_input.des_qz = so3_command.orientation.z*125;
so3_cmd_input.des_qw = so3_command.orientation.w*125;
so3_cmd_input.kR[0] = so3_command.kR[0]*50;
so3_cmd_input.kR[1] = so3_command.kR[1]*50;
so3_cmd_input.kR[2] = so3_command.kR[2]*50;
so3_cmd_input.kOm[0] = so3_command.kOm[0]*100;
so3_cmd_input.kOm[1] = so3_command.kOm[1]*100;
so3_cmd_input.kOm[2] = so3_command.kOm[2]*100;
so3_cmd_input.cur_yaw = so3_command.aux.current_yaw*1e4;
so3_cmd_input.kf_correction = so3_command.aux.kf_correction*1e11;
so3_cmd_input.angle_corrections[0] = so3_command.aux.angle_corrections[0]*2500;
so3_cmd_input.angle_corrections[1] = so3_command.aux.angle_corrections[1]*2500;
so3_cmd_input.enable_motors = so3_command.aux.enable_motors;
so3_cmd_input.use_external_yaw = so3_command.aux.use_external_yaw;
so3_cmd_input.seq = so3_command.header.seq % 255;
output.resize(sizeof(so3_cmd_input));
memcpy(&output[0], &so3_cmd_input, sizeof(so3_cmd_input));
}
void encodeTRPYCommand(const quadrotor_msgs::TRPYCommand &trpy_command,
std::vector<uint8_t> &output)
{
struct TRPY_CMD trpy_cmd_input;
trpy_cmd_input.thrust = trpy_command.thrust*1e4;
trpy_cmd_input.roll = trpy_command.roll*1e4;
trpy_cmd_input.pitch = trpy_command.pitch*1e4;
trpy_cmd_input.yaw = trpy_command.yaw*1e4;
trpy_cmd_input.current_yaw = trpy_command.aux.current_yaw*1e4;
trpy_cmd_input.enable_motors = trpy_command.aux.enable_motors;
trpy_cmd_input.use_external_yaw = trpy_command.aux.use_external_yaw;
output.resize(sizeof(trpy_cmd_input));
memcpy(&output[0], &trpy_cmd_input, sizeof(trpy_cmd_input));
}
void encodePPRGains(const quadrotor_msgs::Gains &gains,
std::vector<uint8_t> &output)
{
struct PPR_GAINS ppr_gains;
ppr_gains.Kp = gains.Kp;
ppr_gains.Kd = gains.Kd;
ppr_gains.Kp_yaw = gains.Kp_yaw;
ppr_gains.Kd_yaw = gains.Kd_yaw;
output.resize(sizeof(ppr_gains));
memcpy(&output[0], &ppr_gains, sizeof(ppr_gains));
}
}

0
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/__init__.py Executable file
View File

143
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_AuxCommand.py Executable file
View File

@@ -0,0 +1,143 @@
"""autogenerated by genpy from quadrotor_msgs/AuxCommand.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
class AuxCommand(genpy.Message):
_md5sum = "94f75840e4b1e03675da764692f2c839"
_type = "quadrotor_msgs/AuxCommand"
_has_header = False #flag to mark the presence of a Header object
_full_text = """float64 current_yaw
float64 kf_correction
float64[2] angle_corrections# Trims for roll, pitch
bool enable_motors
bool use_external_yaw
"""
__slots__ = ['current_yaw','kf_correction','angle_corrections','enable_motors','use_external_yaw']
_slot_types = ['float64','float64','float64[2]','bool','bool']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
current_yaw,kf_correction,angle_corrections,enable_motors,use_external_yaw
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(AuxCommand, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.current_yaw is None:
self.current_yaw = 0.
if self.kf_correction is None:
self.kf_correction = 0.
if self.angle_corrections is None:
self.angle_corrections = [0.,0.]
if self.enable_motors is None:
self.enable_motors = False
if self.use_external_yaw is None:
self.use_external_yaw = False
else:
self.current_yaw = 0.
self.kf_correction = 0.
self.angle_corrections = [0.,0.]
self.enable_motors = False
self.use_external_yaw = False
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_2d.pack(_x.current_yaw, _x.kf_correction))
buff.write(_struct_2d.pack(*self.angle_corrections))
_x = self
buff.write(_struct_2B.pack(_x.enable_motors, _x.use_external_yaw))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
end = 0
_x = self
start = end
end += 16
(_x.current_yaw, _x.kf_correction,) = _struct_2d.unpack(str[start:end])
start = end
end += 16
self.angle_corrections = _struct_2d.unpack(str[start:end])
_x = self
start = end
end += 2
(_x.enable_motors, _x.use_external_yaw,) = _struct_2B.unpack(str[start:end])
self.enable_motors = bool(self.enable_motors)
self.use_external_yaw = bool(self.use_external_yaw)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_2d.pack(_x.current_yaw, _x.kf_correction))
buff.write(self.angle_corrections.tostring())
_x = self
buff.write(_struct_2B.pack(_x.enable_motors, _x.use_external_yaw))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
end = 0
_x = self
start = end
end += 16
(_x.current_yaw, _x.kf_correction,) = _struct_2d.unpack(str[start:end])
start = end
end += 16
self.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2)
_x = self
start = end
end += 2
(_x.enable_motors, _x.use_external_yaw,) = _struct_2B.unpack(str[start:end])
self.enable_motors = bool(self.enable_motors)
self.use_external_yaw = bool(self.use_external_yaw)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_2d = struct.Struct("<2d")
_struct_2B = struct.Struct("<2B")

111
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Corrections.py Executable file
View File

@@ -0,0 +1,111 @@
"""autogenerated by genpy from quadrotor_msgs/Corrections.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
class Corrections(genpy.Message):
_md5sum = "61e86887a75fe520847d3256306360f5"
_type = "quadrotor_msgs/Corrections"
_has_header = False #flag to mark the presence of a Header object
_full_text = """float64 kf_correction
float64[2] angle_corrections
"""
__slots__ = ['kf_correction','angle_corrections']
_slot_types = ['float64','float64[2]']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
kf_correction,angle_corrections
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(Corrections, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.kf_correction is None:
self.kf_correction = 0.
if self.angle_corrections is None:
self.angle_corrections = [0.,0.]
else:
self.kf_correction = 0.
self.angle_corrections = [0.,0.]
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
buff.write(_struct_d.pack(self.kf_correction))
buff.write(_struct_2d.pack(*self.angle_corrections))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
end = 0
start = end
end += 8
(self.kf_correction,) = _struct_d.unpack(str[start:end])
start = end
end += 16
self.angle_corrections = _struct_2d.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
buff.write(_struct_d.pack(self.kf_correction))
buff.write(self.angle_corrections.tostring())
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
end = 0
start = end
end += 8
(self.kf_correction,) = _struct_d.unpack(str[start:end])
start = end
end += 16
self.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_2d = struct.Struct("<2d")
_struct_d = struct.Struct("<d")

114
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Gains.py Executable file
View File

@@ -0,0 +1,114 @@
"""autogenerated by genpy from quadrotor_msgs/Gains.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
class Gains(genpy.Message):
_md5sum = "b82763b9f24e5595e2a816aa779c9461"
_type = "quadrotor_msgs/Gains"
_has_header = False #flag to mark the presence of a Header object
_full_text = """float64 Kp
float64 Kd
float64 Kp_yaw
float64 Kd_yaw
"""
__slots__ = ['Kp','Kd','Kp_yaw','Kd_yaw']
_slot_types = ['float64','float64','float64','float64']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
Kp,Kd,Kp_yaw,Kd_yaw
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(Gains, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.Kp is None:
self.Kp = 0.
if self.Kd is None:
self.Kd = 0.
if self.Kp_yaw is None:
self.Kp_yaw = 0.
if self.Kd_yaw is None:
self.Kd_yaw = 0.
else:
self.Kp = 0.
self.Kd = 0.
self.Kp_yaw = 0.
self.Kd_yaw = 0.
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
end = 0
_x = self
start = end
end += 32
(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_4d.pack(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
end = 0
_x = self
start = end
end += 32
(_x.Kp, _x.Kd, _x.Kp_yaw, _x.Kd_yaw,) = _struct_4d.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_4d = struct.Struct("<4d")

277
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_OutputData.py Executable file
View File

@@ -0,0 +1,277 @@
"""autogenerated by genpy from quadrotor_msgs/OutputData.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import geometry_msgs.msg
import std_msgs.msg
class OutputData(genpy.Message):
_md5sum = "5759ee97fd5c090dcdccf7fc3e50d024"
_type = "quadrotor_msgs/OutputData"
_has_header = True #flag to mark the presence of a Header object
_full_text = """Header header
uint16 loop_rate
float64 voltage
geometry_msgs/Quaternion orientation
geometry_msgs/Vector3 angular_velocity
geometry_msgs/Vector3 linear_acceleration
float64 pressure_dheight
float64 pressure_height
geometry_msgs/Vector3 magnetic_field
uint8[8] radio_channel
#uint8[4] motor_rpm
uint8 seq
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: geometry_msgs/Quaternion
# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w
================================================================================
MSG: geometry_msgs/Vector3
# This represents a vector in free space.
float64 x
float64 y
float64 z
"""
__slots__ = ['header','loop_rate','voltage','orientation','angular_velocity','linear_acceleration','pressure_dheight','pressure_height','magnetic_field','radio_channel','seq']
_slot_types = ['std_msgs/Header','uint16','float64','geometry_msgs/Quaternion','geometry_msgs/Vector3','geometry_msgs/Vector3','float64','float64','geometry_msgs/Vector3','uint8[8]','uint8']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
header,loop_rate,voltage,orientation,angular_velocity,linear_acceleration,pressure_dheight,pressure_height,magnetic_field,radio_channel,seq
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(OutputData, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.header is None:
self.header = std_msgs.msg.Header()
if self.loop_rate is None:
self.loop_rate = 0
if self.voltage is None:
self.voltage = 0.
if self.orientation is None:
self.orientation = geometry_msgs.msg.Quaternion()
if self.angular_velocity is None:
self.angular_velocity = geometry_msgs.msg.Vector3()
if self.linear_acceleration is None:
self.linear_acceleration = geometry_msgs.msg.Vector3()
if self.pressure_dheight is None:
self.pressure_dheight = 0.
if self.pressure_height is None:
self.pressure_height = 0.
if self.magnetic_field is None:
self.magnetic_field = geometry_msgs.msg.Vector3()
if self.radio_channel is None:
self.radio_channel = chr(0)*8
if self.seq is None:
self.seq = 0
else:
self.header = std_msgs.msg.Header()
self.loop_rate = 0
self.voltage = 0.
self.orientation = geometry_msgs.msg.Quaternion()
self.angular_velocity = geometry_msgs.msg.Vector3()
self.linear_acceleration = geometry_msgs.msg.Vector3()
self.pressure_dheight = 0.
self.pressure_height = 0.
self.magnetic_field = geometry_msgs.msg.Vector3()
self.radio_channel = chr(0)*8
self.seq = 0
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_H16d.pack(_x.loop_rate, _x.voltage, _x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w, _x.angular_velocity.x, _x.angular_velocity.y, _x.angular_velocity.z, _x.linear_acceleration.x, _x.linear_acceleration.y, _x.linear_acceleration.z, _x.pressure_dheight, _x.pressure_height, _x.magnetic_field.x, _x.magnetic_field.y, _x.magnetic_field.z))
_x = self.radio_channel
# - if encoded as a list instead, serialize as bytes instead of string
if type(_x) in [list, tuple]:
buff.write(_struct_8B.pack(*_x))
else:
buff.write(_struct_8s.pack(_x))
buff.write(_struct_B.pack(self.seq))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.orientation is None:
self.orientation = geometry_msgs.msg.Quaternion()
if self.angular_velocity is None:
self.angular_velocity = geometry_msgs.msg.Vector3()
if self.linear_acceleration is None:
self.linear_acceleration = geometry_msgs.msg.Vector3()
if self.magnetic_field is None:
self.magnetic_field = geometry_msgs.msg.Vector3()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 130
(_x.loop_rate, _x.voltage, _x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w, _x.angular_velocity.x, _x.angular_velocity.y, _x.angular_velocity.z, _x.linear_acceleration.x, _x.linear_acceleration.y, _x.linear_acceleration.z, _x.pressure_dheight, _x.pressure_height, _x.magnetic_field.x, _x.magnetic_field.y, _x.magnetic_field.z,) = _struct_H16d.unpack(str[start:end])
start = end
end += 8
self.radio_channel = str[start:end]
start = end
end += 1
(self.seq,) = _struct_B.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_H16d.pack(_x.loop_rate, _x.voltage, _x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w, _x.angular_velocity.x, _x.angular_velocity.y, _x.angular_velocity.z, _x.linear_acceleration.x, _x.linear_acceleration.y, _x.linear_acceleration.z, _x.pressure_dheight, _x.pressure_height, _x.magnetic_field.x, _x.magnetic_field.y, _x.magnetic_field.z))
_x = self.radio_channel
# - if encoded as a list instead, serialize as bytes instead of string
if type(_x) in [list, tuple]:
buff.write(_struct_8B.pack(*_x))
else:
buff.write(_struct_8s.pack(_x))
buff.write(_struct_B.pack(self.seq))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.orientation is None:
self.orientation = geometry_msgs.msg.Quaternion()
if self.angular_velocity is None:
self.angular_velocity = geometry_msgs.msg.Vector3()
if self.linear_acceleration is None:
self.linear_acceleration = geometry_msgs.msg.Vector3()
if self.magnetic_field is None:
self.magnetic_field = geometry_msgs.msg.Vector3()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 130
(_x.loop_rate, _x.voltage, _x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w, _x.angular_velocity.x, _x.angular_velocity.y, _x.angular_velocity.z, _x.linear_acceleration.x, _x.linear_acceleration.y, _x.linear_acceleration.z, _x.pressure_dheight, _x.pressure_height, _x.magnetic_field.x, _x.magnetic_field.y, _x.magnetic_field.z,) = _struct_H16d.unpack(str[start:end])
start = end
end += 8
self.radio_channel = str[start:end]
start = end
end += 1
(self.seq,) = _struct_B.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_8B = struct.Struct("<8B")
_struct_8s = struct.Struct("<8s")
_struct_3I = struct.Struct("<3I")
_struct_B = struct.Struct("<B")
_struct_H16d = struct.Struct("<H16d")

243
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_PPROutputData.py Executable file
View File

@@ -0,0 +1,243 @@
"""autogenerated by genpy from quadrotor_msgs/PPROutputData.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import std_msgs.msg
class PPROutputData(genpy.Message):
_md5sum = "732c0e3ca36f241464f8c445e78a0d0a"
_type = "quadrotor_msgs/PPROutputData"
_has_header = True #flag to mark the presence of a Header object
_full_text = """Header header
uint16 quad_time
float64 des_thrust
float64 des_roll
float64 des_pitch
float64 des_yaw
float64 est_roll
float64 est_pitch
float64 est_yaw
float64 est_angvel_x
float64 est_angvel_y
float64 est_angvel_z
float64 est_acc_x
float64 est_acc_y
float64 est_acc_z
uint16[4] pwm
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
"""
__slots__ = ['header','quad_time','des_thrust','des_roll','des_pitch','des_yaw','est_roll','est_pitch','est_yaw','est_angvel_x','est_angvel_y','est_angvel_z','est_acc_x','est_acc_y','est_acc_z','pwm']
_slot_types = ['std_msgs/Header','uint16','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','float64','uint16[4]']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
header,quad_time,des_thrust,des_roll,des_pitch,des_yaw,est_roll,est_pitch,est_yaw,est_angvel_x,est_angvel_y,est_angvel_z,est_acc_x,est_acc_y,est_acc_z,pwm
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(PPROutputData, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.header is None:
self.header = std_msgs.msg.Header()
if self.quad_time is None:
self.quad_time = 0
if self.des_thrust is None:
self.des_thrust = 0.
if self.des_roll is None:
self.des_roll = 0.
if self.des_pitch is None:
self.des_pitch = 0.
if self.des_yaw is None:
self.des_yaw = 0.
if self.est_roll is None:
self.est_roll = 0.
if self.est_pitch is None:
self.est_pitch = 0.
if self.est_yaw is None:
self.est_yaw = 0.
if self.est_angvel_x is None:
self.est_angvel_x = 0.
if self.est_angvel_y is None:
self.est_angvel_y = 0.
if self.est_angvel_z is None:
self.est_angvel_z = 0.
if self.est_acc_x is None:
self.est_acc_x = 0.
if self.est_acc_y is None:
self.est_acc_y = 0.
if self.est_acc_z is None:
self.est_acc_z = 0.
if self.pwm is None:
self.pwm = [0,0,0,0]
else:
self.header = std_msgs.msg.Header()
self.quad_time = 0
self.des_thrust = 0.
self.des_roll = 0.
self.des_pitch = 0.
self.des_yaw = 0.
self.est_roll = 0.
self.est_pitch = 0.
self.est_yaw = 0.
self.est_angvel_x = 0.
self.est_angvel_y = 0.
self.est_angvel_z = 0.
self.est_acc_x = 0.
self.est_acc_y = 0.
self.est_acc_z = 0.
self.pwm = [0,0,0,0]
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_H13d.pack(_x.quad_time, _x.des_thrust, _x.des_roll, _x.des_pitch, _x.des_yaw, _x.est_roll, _x.est_pitch, _x.est_yaw, _x.est_angvel_x, _x.est_angvel_y, _x.est_angvel_z, _x.est_acc_x, _x.est_acc_y, _x.est_acc_z))
buff.write(_struct_4H.pack(*self.pwm))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 106
(_x.quad_time, _x.des_thrust, _x.des_roll, _x.des_pitch, _x.des_yaw, _x.est_roll, _x.est_pitch, _x.est_yaw, _x.est_angvel_x, _x.est_angvel_y, _x.est_angvel_z, _x.est_acc_x, _x.est_acc_y, _x.est_acc_z,) = _struct_H13d.unpack(str[start:end])
start = end
end += 8
self.pwm = _struct_4H.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_H13d.pack(_x.quad_time, _x.des_thrust, _x.des_roll, _x.des_pitch, _x.des_yaw, _x.est_roll, _x.est_pitch, _x.est_yaw, _x.est_angvel_x, _x.est_angvel_y, _x.est_angvel_z, _x.est_acc_x, _x.est_acc_y, _x.est_acc_z))
buff.write(self.pwm.tostring())
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 106
(_x.quad_time, _x.des_thrust, _x.des_roll, _x.des_pitch, _x.des_yaw, _x.est_roll, _x.est_pitch, _x.est_yaw, _x.est_angvel_x, _x.est_angvel_y, _x.est_angvel_z, _x.est_acc_x, _x.est_acc_y, _x.est_acc_z,) = _struct_H13d.unpack(str[start:end])
start = end
end += 8
self.pwm = numpy.frombuffer(str[start:end], dtype=numpy.uint16, count=4)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_H13d = struct.Struct("<H13d")
_struct_3I = struct.Struct("<3I")
_struct_4H = struct.Struct("<4H")

246
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_PositionCommand.py Executable file
View File

@@ -0,0 +1,246 @@
"""autogenerated by genpy from quadrotor_msgs/PositionCommand.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import geometry_msgs.msg
import std_msgs.msg
class PositionCommand(genpy.Message):
_md5sum = "835935bcd6f18632d9e26a3093237902"
_type = "quadrotor_msgs/PositionCommand"
_has_header = True #flag to mark the presence of a Header object
_full_text = """Header header
geometry_msgs/Point position
geometry_msgs/Vector3 velocity
geometry_msgs/Vector3 acceleration
float64 yaw
float64 yaw_dot
float64[3] kx
float64[3] kv
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: geometry_msgs/Point
# This contains the position of a point in free space
float64 x
float64 y
float64 z
================================================================================
MSG: geometry_msgs/Vector3
# This represents a vector in free space.
float64 x
float64 y
float64 z
"""
__slots__ = ['header','position','velocity','acceleration','yaw','yaw_dot','kx','kv']
_slot_types = ['std_msgs/Header','geometry_msgs/Point','geometry_msgs/Vector3','geometry_msgs/Vector3','float64','float64','float64[3]','float64[3]']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
header,position,velocity,acceleration,yaw,yaw_dot,kx,kv
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(PositionCommand, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.header is None:
self.header = std_msgs.msg.Header()
if self.position is None:
self.position = geometry_msgs.msg.Point()
if self.velocity is None:
self.velocity = geometry_msgs.msg.Vector3()
if self.acceleration is None:
self.acceleration = geometry_msgs.msg.Vector3()
if self.yaw is None:
self.yaw = 0.
if self.yaw_dot is None:
self.yaw_dot = 0.
if self.kx is None:
self.kx = [0.,0.,0.]
if self.kv is None:
self.kv = [0.,0.,0.]
else:
self.header = std_msgs.msg.Header()
self.position = geometry_msgs.msg.Point()
self.velocity = geometry_msgs.msg.Vector3()
self.acceleration = geometry_msgs.msg.Vector3()
self.yaw = 0.
self.yaw_dot = 0.
self.kx = [0.,0.,0.]
self.kv = [0.,0.,0.]
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_11d.pack(_x.position.x, _x.position.y, _x.position.z, _x.velocity.x, _x.velocity.y, _x.velocity.z, _x.acceleration.x, _x.acceleration.y, _x.acceleration.z, _x.yaw, _x.yaw_dot))
buff.write(_struct_3d.pack(*self.kx))
buff.write(_struct_3d.pack(*self.kv))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.position is None:
self.position = geometry_msgs.msg.Point()
if self.velocity is None:
self.velocity = geometry_msgs.msg.Vector3()
if self.acceleration is None:
self.acceleration = geometry_msgs.msg.Vector3()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 88
(_x.position.x, _x.position.y, _x.position.z, _x.velocity.x, _x.velocity.y, _x.velocity.z, _x.acceleration.x, _x.acceleration.y, _x.acceleration.z, _x.yaw, _x.yaw_dot,) = _struct_11d.unpack(str[start:end])
start = end
end += 24
self.kx = _struct_3d.unpack(str[start:end])
start = end
end += 24
self.kv = _struct_3d.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_11d.pack(_x.position.x, _x.position.y, _x.position.z, _x.velocity.x, _x.velocity.y, _x.velocity.z, _x.acceleration.x, _x.acceleration.y, _x.acceleration.z, _x.yaw, _x.yaw_dot))
buff.write(self.kx.tostring())
buff.write(self.kv.tostring())
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.position is None:
self.position = geometry_msgs.msg.Point()
if self.velocity is None:
self.velocity = geometry_msgs.msg.Vector3()
if self.acceleration is None:
self.acceleration = geometry_msgs.msg.Vector3()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 88
(_x.position.x, _x.position.y, _x.position.z, _x.velocity.x, _x.velocity.y, _x.velocity.z, _x.acceleration.x, _x.acceleration.y, _x.acceleration.z, _x.yaw, _x.yaw_dot,) = _struct_11d.unpack(str[start:end])
start = end
end += 24
self.kx = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=3)
start = end
end += 24
self.kv = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=3)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_11d = struct.Struct("<11d")
_struct_3I = struct.Struct("<3I")
_struct_3d = struct.Struct("<3d")

287
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_SO3Command.py Executable file
View File

@@ -0,0 +1,287 @@
"""autogenerated by genpy from quadrotor_msgs/SO3Command.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import geometry_msgs.msg
import quadrotor_msgs.msg
import std_msgs.msg
class SO3Command(genpy.Message):
_md5sum = "a466650b2633e768513aa3bf62383c86"
_type = "quadrotor_msgs/SO3Command"
_has_header = True #flag to mark the presence of a Header object
_full_text = """Header header
geometry_msgs/Vector3 force
geometry_msgs/Quaternion orientation
float64[3] kR
float64[3] kOm
quadrotor_msgs/AuxCommand aux
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: geometry_msgs/Vector3
# This represents a vector in free space.
float64 x
float64 y
float64 z
================================================================================
MSG: geometry_msgs/Quaternion
# This represents an orientation in free space in quaternion form.
float64 x
float64 y
float64 z
float64 w
================================================================================
MSG: quadrotor_msgs/AuxCommand
float64 current_yaw
float64 kf_correction
float64[2] angle_corrections# Trims for roll, pitch
bool enable_motors
bool use_external_yaw
"""
__slots__ = ['header','force','orientation','kR','kOm','aux']
_slot_types = ['std_msgs/Header','geometry_msgs/Vector3','geometry_msgs/Quaternion','float64[3]','float64[3]','quadrotor_msgs/AuxCommand']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
header,force,orientation,kR,kOm,aux
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(SO3Command, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.header is None:
self.header = std_msgs.msg.Header()
if self.force is None:
self.force = geometry_msgs.msg.Vector3()
if self.orientation is None:
self.orientation = geometry_msgs.msg.Quaternion()
if self.kR is None:
self.kR = [0.,0.,0.]
if self.kOm is None:
self.kOm = [0.,0.,0.]
if self.aux is None:
self.aux = quadrotor_msgs.msg.AuxCommand()
else:
self.header = std_msgs.msg.Header()
self.force = geometry_msgs.msg.Vector3()
self.orientation = geometry_msgs.msg.Quaternion()
self.kR = [0.,0.,0.]
self.kOm = [0.,0.,0.]
self.aux = quadrotor_msgs.msg.AuxCommand()
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_7d.pack(_x.force.x, _x.force.y, _x.force.z, _x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w))
buff.write(_struct_3d.pack(*self.kR))
buff.write(_struct_3d.pack(*self.kOm))
_x = self
buff.write(_struct_2d.pack(_x.aux.current_yaw, _x.aux.kf_correction))
buff.write(_struct_2d.pack(*self.aux.angle_corrections))
_x = self
buff.write(_struct_2B.pack(_x.aux.enable_motors, _x.aux.use_external_yaw))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.force is None:
self.force = geometry_msgs.msg.Vector3()
if self.orientation is None:
self.orientation = geometry_msgs.msg.Quaternion()
if self.aux is None:
self.aux = quadrotor_msgs.msg.AuxCommand()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 56
(_x.force.x, _x.force.y, _x.force.z, _x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w,) = _struct_7d.unpack(str[start:end])
start = end
end += 24
self.kR = _struct_3d.unpack(str[start:end])
start = end
end += 24
self.kOm = _struct_3d.unpack(str[start:end])
_x = self
start = end
end += 16
(_x.aux.current_yaw, _x.aux.kf_correction,) = _struct_2d.unpack(str[start:end])
start = end
end += 16
self.aux.angle_corrections = _struct_2d.unpack(str[start:end])
_x = self
start = end
end += 2
(_x.aux.enable_motors, _x.aux.use_external_yaw,) = _struct_2B.unpack(str[start:end])
self.aux.enable_motors = bool(self.aux.enable_motors)
self.aux.use_external_yaw = bool(self.aux.use_external_yaw)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_7d.pack(_x.force.x, _x.force.y, _x.force.z, _x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w))
buff.write(self.kR.tostring())
buff.write(self.kOm.tostring())
_x = self
buff.write(_struct_2d.pack(_x.aux.current_yaw, _x.aux.kf_correction))
buff.write(self.aux.angle_corrections.tostring())
_x = self
buff.write(_struct_2B.pack(_x.aux.enable_motors, _x.aux.use_external_yaw))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.force is None:
self.force = geometry_msgs.msg.Vector3()
if self.orientation is None:
self.orientation = geometry_msgs.msg.Quaternion()
if self.aux is None:
self.aux = quadrotor_msgs.msg.AuxCommand()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 56
(_x.force.x, _x.force.y, _x.force.z, _x.orientation.x, _x.orientation.y, _x.orientation.z, _x.orientation.w,) = _struct_7d.unpack(str[start:end])
start = end
end += 24
self.kR = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=3)
start = end
end += 24
self.kOm = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=3)
_x = self
start = end
end += 16
(_x.aux.current_yaw, _x.aux.kf_correction,) = _struct_2d.unpack(str[start:end])
start = end
end += 16
self.aux.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2)
_x = self
start = end
end += 2
(_x.aux.enable_motors, _x.aux.use_external_yaw,) = _struct_2B.unpack(str[start:end])
self.aux.enable_motors = bool(self.aux.enable_motors)
self.aux.use_external_yaw = bool(self.aux.use_external_yaw)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_2d = struct.Struct("<2d")
_struct_3I = struct.Struct("<3I")
_struct_7d = struct.Struct("<7d")
_struct_2B = struct.Struct("<2B")
_struct_3d = struct.Struct("<3d")

229
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_Serial.py Executable file
View File

@@ -0,0 +1,229 @@
"""autogenerated by genpy from quadrotor_msgs/Serial.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import std_msgs.msg
class Serial(genpy.Message):
_md5sum = "e448fb7595af9a8adfcab5ec241c7d4f"
_type = "quadrotor_msgs/Serial"
_has_header = True #flag to mark the presence of a Header object
_full_text = """# Note: These constants need to be kept in sync with the types
# defined in include/quadrotor_msgs/comm_types.h
uint8 SO3_CMD = 115 # 's' in base 10
uint8 TRPY_CMD = 112 # 'p' in base 10
uint8 STATUS_DATA = 99 # 'c' in base 10
uint8 OUTPUT_DATA = 100 # 'd' in base 10
uint8 PPR_OUTPUT_DATA = 116 # 't' in base 10
uint8 PPR_GAINS = 103 # 'g'
Header header
uint8 channel
uint8 type # One of the types listed above
uint8[] data
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
"""
# Pseudo-constants
SO3_CMD = 115
TRPY_CMD = 112
STATUS_DATA = 99
OUTPUT_DATA = 100
PPR_OUTPUT_DATA = 116
PPR_GAINS = 103
__slots__ = ['header','channel','type','data']
_slot_types = ['std_msgs/Header','uint8','uint8','uint8[]']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
header,channel,type,data
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(Serial, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.header is None:
self.header = std_msgs.msg.Header()
if self.channel is None:
self.channel = 0
if self.type is None:
self.type = 0
if self.data is None:
self.data = ''
else:
self.header = std_msgs.msg.Header()
self.channel = 0
self.type = 0
self.data = ''
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_2B.pack(_x.channel, _x.type))
_x = self.data
length = len(_x)
# - if encoded as a list instead, serialize as bytes instead of string
if type(_x) in [list, tuple]:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 2
(_x.channel, _x.type,) = _struct_2B.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
self.data = str[start:end]
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_2B.pack(_x.channel, _x.type))
_x = self.data
length = len(_x)
# - if encoded as a list instead, serialize as bytes instead of string
if type(_x) in [list, tuple]:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 2
(_x.channel, _x.type,) = _struct_2B.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
self.data = str[start:end]
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_3I = struct.Struct("<3I")
_struct_2B = struct.Struct("<2B")

186
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_StatusData.py Executable file
View File

@@ -0,0 +1,186 @@
"""autogenerated by genpy from quadrotor_msgs/StatusData.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import std_msgs.msg
class StatusData(genpy.Message):
_md5sum = "c70a4ec4725273263ae176ad30f89553"
_type = "quadrotor_msgs/StatusData"
_has_header = True #flag to mark the presence of a Header object
_full_text = """Header header
uint16 loop_rate
float64 voltage
uint8 seq
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
"""
__slots__ = ['header','loop_rate','voltage','seq']
_slot_types = ['std_msgs/Header','uint16','float64','uint8']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
header,loop_rate,voltage,seq
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(StatusData, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.header is None:
self.header = std_msgs.msg.Header()
if self.loop_rate is None:
self.loop_rate = 0
if self.voltage is None:
self.voltage = 0.
if self.seq is None:
self.seq = 0
else:
self.header = std_msgs.msg.Header()
self.loop_rate = 0
self.voltage = 0.
self.seq = 0
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_HdB.pack(_x.loop_rate, _x.voltage, _x.seq))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 11
(_x.loop_rate, _x.voltage, _x.seq,) = _struct_HdB.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_HdB.pack(_x.loop_rate, _x.voltage, _x.seq))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 11
(_x.loop_rate, _x.voltage, _x.seq,) = _struct_HdB.unpack(str[start:end])
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_3I = struct.Struct("<3I")
_struct_HdB = struct.Struct("<HdB")

233
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/_TRPYCommand.py Executable file
View File

@@ -0,0 +1,233 @@
"""autogenerated by genpy from quadrotor_msgs/TRPYCommand.msg. Do not edit."""
import sys
python3 = True if sys.hexversion > 0x03000000 else False
import genpy
import struct
import quadrotor_msgs.msg
import std_msgs.msg
class TRPYCommand(genpy.Message):
_md5sum = "6779ee8a86cc757aeea21725df32d00c"
_type = "quadrotor_msgs/TRPYCommand"
_has_header = True #flag to mark the presence of a Header object
_full_text = """Header header
float32 thrust
float32 roll
float32 pitch
float32 yaw
quadrotor_msgs/AuxCommand aux
================================================================================
MSG: std_msgs/Header
# Standard metadata for higher-level stamped data types.
# This is generally used to communicate timestamped data
# in a particular coordinate frame.
#
# sequence ID: consecutively increasing ID
uint32 seq
#Two-integer timestamp that is expressed as:
# * stamp.sec: seconds (stamp_secs) since epoch (in Python the variable is called 'secs')
# * stamp.nsec: nanoseconds since stamp_secs (in Python the variable is called 'nsecs')
# time-handling sugar is provided by the client library
time stamp
#Frame this data is associated with
# 0: no frame
# 1: global frame
string frame_id
================================================================================
MSG: quadrotor_msgs/AuxCommand
float64 current_yaw
float64 kf_correction
float64[2] angle_corrections# Trims for roll, pitch
bool enable_motors
bool use_external_yaw
"""
__slots__ = ['header','thrust','roll','pitch','yaw','aux']
_slot_types = ['std_msgs/Header','float32','float32','float32','float32','quadrotor_msgs/AuxCommand']
def __init__(self, *args, **kwds):
"""
Constructor. Any message fields that are implicitly/explicitly
set to None will be assigned a default value. The recommend
use is keyword arguments as this is more robust to future message
changes. You cannot mix in-order arguments and keyword arguments.
The available fields are:
header,thrust,roll,pitch,yaw,aux
:param args: complete set of field values, in .msg order
:param kwds: use keyword arguments corresponding to message field names
to set specific fields.
"""
if args or kwds:
super(TRPYCommand, self).__init__(*args, **kwds)
#message fields cannot be None, assign default values for those that are
if self.header is None:
self.header = std_msgs.msg.Header()
if self.thrust is None:
self.thrust = 0.
if self.roll is None:
self.roll = 0.
if self.pitch is None:
self.pitch = 0.
if self.yaw is None:
self.yaw = 0.
if self.aux is None:
self.aux = quadrotor_msgs.msg.AuxCommand()
else:
self.header = std_msgs.msg.Header()
self.thrust = 0.
self.roll = 0.
self.pitch = 0.
self.yaw = 0.
self.aux = quadrotor_msgs.msg.AuxCommand()
def _get_types(self):
"""
internal API method
"""
return self._slot_types
def serialize(self, buff):
"""
serialize message into buffer
:param buff: buffer, ``StringIO``
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_4f2d.pack(_x.thrust, _x.roll, _x.pitch, _x.yaw, _x.aux.current_yaw, _x.aux.kf_correction))
buff.write(_struct_2d.pack(*self.aux.angle_corrections))
_x = self
buff.write(_struct_2B.pack(_x.aux.enable_motors, _x.aux.use_external_yaw))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize(self, str):
"""
unpack serialized message in str into this message instance
:param str: byte array of serialized message, ``str``
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.aux is None:
self.aux = quadrotor_msgs.msg.AuxCommand()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 32
(_x.thrust, _x.roll, _x.pitch, _x.yaw, _x.aux.current_yaw, _x.aux.kf_correction,) = _struct_4f2d.unpack(str[start:end])
start = end
end += 16
self.aux.angle_corrections = _struct_2d.unpack(str[start:end])
_x = self
start = end
end += 2
(_x.aux.enable_motors, _x.aux.use_external_yaw,) = _struct_2B.unpack(str[start:end])
self.aux.enable_motors = bool(self.aux.enable_motors)
self.aux.use_external_yaw = bool(self.aux.use_external_yaw)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
def serialize_numpy(self, buff, numpy):
"""
serialize message with numpy array types into buffer
:param buff: buffer, ``StringIO``
:param numpy: numpy python module
"""
try:
_x = self
buff.write(_struct_3I.pack(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs))
_x = self.header.frame_id
length = len(_x)
if python3 or type(_x) == unicode:
_x = _x.encode('utf-8')
length = len(_x)
if python3:
buff.write(struct.pack('<I%sB'%length, length, *_x))
else:
buff.write(struct.pack('<I%ss'%length, length, _x))
_x = self
buff.write(_struct_4f2d.pack(_x.thrust, _x.roll, _x.pitch, _x.yaw, _x.aux.current_yaw, _x.aux.kf_correction))
buff.write(self.aux.angle_corrections.tostring())
_x = self
buff.write(_struct_2B.pack(_x.aux.enable_motors, _x.aux.use_external_yaw))
except struct.error as se: self._check_types(struct.error("%s: '%s' when writing '%s'" % (type(se), str(se), str(_x))))
except TypeError as te: self._check_types(ValueError("%s: '%s' when writing '%s'" % (type(te), str(te), str(_x))))
def deserialize_numpy(self, str, numpy):
"""
unpack serialized message in str into this message instance using numpy for array types
:param str: byte array of serialized message, ``str``
:param numpy: numpy python module
"""
try:
if self.header is None:
self.header = std_msgs.msg.Header()
if self.aux is None:
self.aux = quadrotor_msgs.msg.AuxCommand()
end = 0
_x = self
start = end
end += 12
(_x.header.seq, _x.header.stamp.secs, _x.header.stamp.nsecs,) = _struct_3I.unpack(str[start:end])
start = end
end += 4
(length,) = _struct_I.unpack(str[start:end])
start = end
end += length
if python3:
self.header.frame_id = str[start:end].decode('utf-8')
else:
self.header.frame_id = str[start:end]
_x = self
start = end
end += 32
(_x.thrust, _x.roll, _x.pitch, _x.yaw, _x.aux.current_yaw, _x.aux.kf_correction,) = _struct_4f2d.unpack(str[start:end])
start = end
end += 16
self.aux.angle_corrections = numpy.frombuffer(str[start:end], dtype=numpy.float64, count=2)
_x = self
start = end
end += 2
(_x.aux.enable_motors, _x.aux.use_external_yaw,) = _struct_2B.unpack(str[start:end])
self.aux.enable_motors = bool(self.aux.enable_motors)
self.aux.use_external_yaw = bool(self.aux.use_external_yaw)
return self
except struct.error as e:
raise genpy.DeserializationError(e) #most likely buffer underfill
_struct_I = genpy.struct_I
_struct_2d = struct.Struct("<2d")
_struct_3I = struct.Struct("<3I")
_struct_4f2d = struct.Struct("<4f2d")
_struct_2B = struct.Struct("<2B")

10
Controller/src/utils/quadrotor_msgs/src/quadrotor_msgs/msg/__init__.py Executable file
View File

@@ -0,0 +1,10 @@
from ._Gains import *
from ._SO3Command import *
from ._TRPYCommand import *
from ._PositionCommand import *
from ._PPROutputData import *
from ._OutputData import *
from ._Corrections import *
from ._Serial import *
from ._AuxCommand import *
from ._StatusData import *

4
Controller/src/utils/quadrotor_msgs/srv/SetTakeoffLand.srv Executable file
View File

@@ -0,0 +1,4 @@
bool takeoff
float32 takeoff_altitude
---
bool res

100
Controller/src/utils/uav_utils/CMakeLists.txt Normal file
View File

@@ -0,0 +1,100 @@
cmake_minimum_required(VERSION 2.8.3)
project(uav_utils)
## Turn on verbose output
set(CMAKE_VERBOSE_MAKEFILE "false")
## Enable C++11
include(CheckCXXCompilerFlag)
CHECK_CXX_COMPILER_FLAG("-std=c++11" COMPILER_SUPPORTS_CXX11)
CHECK_CXX_COMPILER_FLAG("-std=c++0x" COMPILER_SUPPORTS_CXX0X)
if(COMPILER_SUPPORTS_CXX11)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++11")
elseif(COMPILER_SUPPORTS_CXX0X)
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} -std=c++0x")
else()
message(STATUS "The compiler ${CMAKE_CXX_COMPILER} has no C++11 support. Please use a different C++ compiler.")
endif()
## Add some compile flags
set(ADDITIONAL_CXX_FLAG "-Wall -O3 -g")
set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${ADDITIONAL_CXX_FLAG}")
## Find catkin macros and libraries
find_package(catkin REQUIRED COMPONENTS
roscpp
rospy
cmake_utils
)
find_package(Eigen REQUIRED) # try to find manually installed eigen (Usually in /usr/local with provided FindEigen3.cmake)
###################################
## catkin specific configuration ##
###################################
catkin_package(
INCLUDE_DIRS include
# LIBRARIES uav_utils
CATKIN_DEPENDS roscpp rospy
# DEPENDS system_lib
)
###########
## Build ##
###########
include_directories(
include
${EIGEN_INCLUDE_DIRS}
${catkin_INCLUDE_DIRS}
)
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
# install(TARGETS geometry_utils geometry_utils_node
# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.h"
PATTERN ".svn" EXCLUDE
)
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )
#############
## Testing ##
#############
## Add gtest based cpp test target and link libraries
catkin_add_gtest(${PROJECT_NAME}-test src/${PROJECT_NAME}_test.cpp)
if(TARGET ${PROJECT_NAME}-test)
target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
endif()
## Add folders to be run by python nosetests
# catkin_add_nosetests(test)

95
Controller/src/utils/uav_utils/include/uav_utils/converters.h Normal file
View File

@@ -0,0 +1,95 @@
#ifndef __UAVUTILS_CONVERTERS_H
#define __UAVUTILS_CONVERTERS_H
#include <nav_msgs/Odometry.h>
#include <geometry_msgs/Vector3.h>
#include <geometry_msgs/Point.h>
#include <geometry_msgs/Quaternion.h>
#include <Eigen/Dense>
#include <Eigen/Geometry>
#include <cmath>
namespace uav_utils {
inline void extract_odometry(nav_msgs::OdometryConstPtr msg, Eigen::Vector3d& p,
Eigen::Vector3d& v, Eigen::Quaterniond& q)
{
p(0) = msg->pose.pose.position.x;
p(1) = msg->pose.pose.position.y;
p(2) = msg->pose.pose.position.z;
v(0) = msg->twist.twist.linear.x;
v(1) = msg->twist.twist.linear.y;
v(2) = msg->twist.twist.linear.z;
q.w() = msg->pose.pose.orientation.w;
q.x() = msg->pose.pose.orientation.x;
q.y() = msg->pose.pose.orientation.y;
q.z() = msg->pose.pose.orientation.z;
}
inline void extract_odometry(nav_msgs::OdometryConstPtr msg, Eigen::Vector3d& p,
Eigen::Vector3d& v, Eigen::Quaterniond& q, Eigen::Vector3d& w)
{
extract_odometry(msg, p, v, q);
w(0) = msg->twist.twist.angular.x;
w(1) = msg->twist.twist.angular.y;
w(2) = msg->twist.twist.angular.z;
}
template <typename Scalar_t = double>
Eigen::Matrix<Scalar_t, 3, 1> from_vector3_msg(const geometry_msgs::Vector3& msg) {
return Eigen::Matrix<Scalar_t, 3, 1>(msg.x, msg.y, msg.z);
}
template <typename Derived>
geometry_msgs::Vector3 to_vector3_msg(const Eigen::DenseBase<Derived>& v) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
geometry_msgs::Vector3 msg;
msg.x = v.x();
msg.y = v.y();
msg.z = v.z();
return msg;
}
template <typename Scalar_t = double>
Eigen::Matrix<Scalar_t, 3, 1> from_point_msg(const geometry_msgs::Point& msg) {
return Eigen::Matrix<Scalar_t, 3, 1>(msg.x, msg.y, msg.z);
}
template <typename Derived>
geometry_msgs::Point to_point_msg(const Eigen::DenseBase<Derived>& v) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
geometry_msgs::Point msg;
msg.x = v.x();
msg.y = v.y();
msg.z = v.z();
return msg;
}
template <typename Scalar_t = double>
Eigen::Quaternion<Scalar_t> from_quaternion_msg(const geometry_msgs::Quaternion& msg) {
return Eigen::Quaternion<Scalar_t>(msg.w, msg.x, msg.y, msg.z);
}
template <typename Scalar_t>
geometry_msgs::Quaternion to_quaternion_msg(const Eigen::Quaternion<Scalar_t>& q) {
geometry_msgs::Quaternion msg;
msg.x = q.x();
msg.y = q.y();
msg.z = q.z();
msg.w = q.w();
return msg;
}
}
#endif

248
Controller/src/utils/uav_utils/include/uav_utils/geometry_utils.h Normal file
View File

@@ -0,0 +1,248 @@
#ifndef __GEOMETRY_UTILS_H
#define __GEOMETRY_UTILS_H
#include <Eigen/Dense>
/* clang-format off */
namespace uav_utils {
template <typename Scalar_t>
Scalar_t toRad(const Scalar_t& x) {
return x / 180.0 * M_PI;
}
template <typename Scalar_t>
Scalar_t toDeg(const Scalar_t& x) {
return x * 180.0 / M_PI;
}
template <typename Scalar_t>
Eigen::Matrix<Scalar_t, 3, 3> rotx(Scalar_t t) {
Eigen::Matrix<Scalar_t, 3, 3> R;
R(0, 0) = 1.0;
R(0, 1) = 0.0;
R(0, 2) = 0.0;
R(1, 0) = 0.0;
R(1, 1) = std::cos(t);
R(1, 2) = -std::sin(t);
R(2, 0) = 0.0;
R(2, 1) = std::sin(t);
R(2, 2) = std::cos(t);
return R;
}
template <typename Scalar_t>
Eigen::Matrix<Scalar_t, 3, 3> roty(Scalar_t t) {
Eigen::Matrix<Scalar_t, 3, 3> R;
R(0, 0) = std::cos(t);
R(0, 1) = 0.0;
R(0, 2) = std::sin(t);
R(1, 0) = 0.0;
R(1, 1) = 1.0;
R(1, 2) = 0;
R(2, 0) = -std::sin(t);
R(2, 1) = 0.0;
R(2, 2) = std::cos(t);
return R;
}
template <typename Scalar_t>
Eigen::Matrix<Scalar_t, 3, 3> rotz(Scalar_t t) {
Eigen::Matrix<Scalar_t, 3, 3> R;
R(0, 0) = std::cos(t);
R(0, 1) = -std::sin(t);
R(0, 2) = 0.0;
R(1, 0) = std::sin(t);
R(1, 1) = std::cos(t);
R(1, 2) = 0.0;
R(2, 0) = 0.0;
R(2, 1) = 0.0;
R(2, 2) = 1.0;
return R;
}
template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 3> ypr_to_R(const Eigen::DenseBase<Derived>& ypr) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
typename Derived::Scalar c, s;
Eigen::Matrix<typename Derived::Scalar, 3, 3> Rz = Eigen::Matrix<typename Derived::Scalar, 3, 3>::Zero();
typename Derived::Scalar y = ypr(0);
c = cos(y);
s = sin(y);
Rz(0, 0) = c;
Rz(1, 0) = s;
Rz(0, 1) = -s;
Rz(1, 1) = c;
Rz(2, 2) = 1;
Eigen::Matrix<typename Derived::Scalar, 3, 3> Ry = Eigen::Matrix<typename Derived::Scalar, 3, 3>::Zero();
typename Derived::Scalar p = ypr(1);
c = cos(p);
s = sin(p);
Ry(0, 0) = c;
Ry(2, 0) = -s;
Ry(0, 2) = s;
Ry(2, 2) = c;
Ry(1, 1) = 1;
Eigen::Matrix<typename Derived::Scalar, 3, 3> Rx = Eigen::Matrix<typename Derived::Scalar, 3, 3>::Zero();
typename Derived::Scalar r = ypr(2);
c = cos(r);
s = sin(r);
Rx(1, 1) = c;
Rx(2, 1) = s;
Rx(1, 2) = -s;
Rx(2, 2) = c;
Rx(0, 0) = 1;
Eigen::Matrix<typename Derived::Scalar, 3, 3> R = Rz * Ry * Rx;
return R;
}
template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 1> R_to_ypr(const Eigen::DenseBase<Derived>& R) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
Eigen::Matrix<typename Derived::Scalar, 3, 1> n = R.col(0);
Eigen::Matrix<typename Derived::Scalar, 3, 1> o = R.col(1);
Eigen::Matrix<typename Derived::Scalar, 3, 1> a = R.col(2);
Eigen::Matrix<typename Derived::Scalar, 3, 1> ypr(3);
typename Derived::Scalar y = atan2(n(1), n(0));
typename Derived::Scalar p = atan2(-n(2), n(0) * cos(y) + n(1) * sin(y));
typename Derived::Scalar r =
atan2(a(0) * sin(y) - a(1) * cos(y), -o(0) * sin(y) + o(1) * cos(y));
ypr(0) = y;
ypr(1) = p;
ypr(2) = r;
return ypr;
}
template <typename Derived>
Eigen::Quaternion<typename Derived::Scalar> ypr_to_quaternion(const Eigen::DenseBase<Derived>& ypr) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
const typename Derived::Scalar cy = cos(ypr(0) / 2.0);
const typename Derived::Scalar sy = sin(ypr(0) / 2.0);
const typename Derived::Scalar cp = cos(ypr(1) / 2.0);
const typename Derived::Scalar sp = sin(ypr(1) / 2.0);
const typename Derived::Scalar cr = cos(ypr(2) / 2.0);
const typename Derived::Scalar sr = sin(ypr(2) / 2.0);
Eigen::Quaternion<typename Derived::Scalar> q;
q.w() = cr * cp * cy + sr * sp * sy;
q.x() = sr * cp * cy - cr * sp * sy;
q.y() = cr * sp * cy + sr * cp * sy;
q.z() = cr * cp * sy - sr * sp * cy;
return q;
}
template <typename Scalar_t>
Eigen::Matrix<Scalar_t, 3, 1> quaternion_to_ypr(const Eigen::Quaternion<Scalar_t>& q_) {
Eigen::Quaternion<Scalar_t> q = q_.normalized();
Eigen::Matrix<Scalar_t, 3, 1> ypr;
ypr(2) = atan2(2 * (q.w() * q.x() + q.y() * q.z()), 1 - 2 * (q.x() * q.x() + q.y() * q.y()));
ypr(1) = asin(2 * (q.w() * q.y() - q.z() * q.x()));
ypr(0) = atan2(2 * (q.w() * q.z() + q.x() * q.y()), 1 - 2 * (q.y() * q.y() + q.z() * q.z()));
return ypr;
}
template <typename Scalar_t>
Scalar_t get_yaw_from_quaternion(const Eigen::Quaternion<Scalar_t>& q) {
return quaternion_to_ypr(q)(0);
}
template <typename Scalar_t>
Eigen::Quaternion<Scalar_t> yaw_to_quaternion(Scalar_t yaw) {
return Eigen::Quaternion<Scalar_t>(rotz(yaw));
}
template <typename Scalar_t>
Scalar_t normalize_angle(Scalar_t a) {
int cnt = 0;
while (true) {
cnt++;
if (a < -M_PI) {
a += M_PI * 2.0;
} else if (a > M_PI) {
a -= M_PI * 2.0;
}
if (-M_PI <= a && a <= M_PI) {
break;
};
assert(cnt < 10 && "[uav_utils/geometry_msgs] INVALID INPUT ANGLE");
}
return a;
}
template <typename Scalar_t>
Scalar_t angle_add(Scalar_t a, Scalar_t b) {
Scalar_t c = a + b;
c = normalize_angle(c);
assert(-M_PI <= c && c <= M_PI);
return c;
}
template <typename Scalar_t>
Scalar_t yaw_add(Scalar_t a, Scalar_t b) {
return angle_add(a, b);
}
template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 3> get_skew_symmetric(const Eigen::DenseBase<Derived>& v) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 1, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
Eigen::Matrix<typename Derived::Scalar, 3, 3> M;
M.setZero();
M(0, 1) = -v(2);
M(0, 2) = v(1);
M(1, 0) = v(2);
M(1, 2) = -v(0);
M(2, 0) = -v(1);
M(2, 1) = v(0);
return M;
}
template <typename Derived>
Eigen::Matrix<typename Derived::Scalar, 3, 1> from_skew_symmetric(const Eigen::DenseBase<Derived>& M) {
EIGEN_STATIC_ASSERT_FIXED_SIZE(Derived);
EIGEN_STATIC_ASSERT(Derived::RowsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
EIGEN_STATIC_ASSERT(Derived::ColsAtCompileTime == 3, THIS_METHOD_IS_ONLY_FOR_MATRICES_OF_A_SPECIFIC_SIZE);
Eigen::Matrix<typename Derived::Scalar, 3, 1> v;
v(0) = M(2, 1);
v(1) = -M(2, 0);
v(2) = M(1, 0);
assert(v.isApprox(Eigen::Matrix<typename Derived::Scalar, 3, 1>(-M(1, 2), M(0, 2), -M(0, 1))));
return v;
}
} // end of namespace uav_utils
/* clang-format on */
#endif

59
Controller/src/utils/uav_utils/include/uav_utils/utils.h Normal file
View File

@@ -0,0 +1,59 @@
#ifndef __UAV_UTILS_H
#define __UAV_UTILS_H
#include <ros/ros.h>
#include <uav_utils/converters.h>
#include <uav_utils/geometry_utils.h>
namespace uav_utils
{
/* judge if value belongs to [low,high] */
template <typename T, typename T2>
bool
in_range(T value, const T2& low, const T2& high)
{
ROS_ASSERT_MSG(low < high, "%f < %f?", low, high);
return (low <= value) && (value <= high);
}
/* judge if value belongs to [-limit, limit] */
template <typename T, typename T2>
bool
in_range(T value, const T2& limit)
{
ROS_ASSERT_MSG(limit > 0, "%f > 0?", limit);
return in_range(value, -limit, limit);
}
template <typename T, typename T2>
void
limit_range(T& value, const T2& low, const T2& high)
{
ROS_ASSERT_MSG(low < high, "%f < %f?", low, high);
if (value < low)
{
value = low;
}
if (value > high)
{
value = high;
}
return;
}
template <typename T, typename T2>
void
limit_range(T& value, const T2& limit)
{
ROS_ASSERT_MSG(limit > 0, "%f > 0?", limit);
limit_range(value, -limit, limit);
}
typedef std::stringstream DebugSS_t;
} // end of namespace uav_utils
#endif

58
Controller/src/utils/uav_utils/package.xml Normal file
View File

@@ -0,0 +1,58 @@
<?xml version="1.0"?>
<package>
<name>uav_utils</name>
<version>0.0.0</version>
<description>The uav_utils package</description>
<!-- One maintainer tag required, multiple allowed, one person per tag -->
<!-- Example: -->
<!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
<maintainer email="liutianbocn@gmail.com">LIU Tianbo</maintainer>
<!-- One license tag required, multiple allowed, one license per tag -->
<!-- Commonly used license strings: -->
<!-- BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
<license>LGPLv3</license>
<!-- Url tags are optional, but mutiple are allowed, one per tag -->
<!-- Optional attribute type can be: website, bugtracker, or repository -->
<!-- Example: -->
<!-- <url type="website">http://wiki.ros.org/geometry_utils</url> -->
<!-- Author tags are optional, mutiple are allowed, one per tag -->
<!-- Authors do not have to be maintianers, but could be -->
<!-- Example: -->
<!-- <author email="jane.doe@example.com">Jane Doe</author> -->
<!-- The *_depend tags are used to specify dependencies -->
<!-- Dependencies can be catkin packages or system dependencies -->
<!-- Examples: -->
<!-- Use build_depend for packages you need at compile time: -->
<!-- <build_depend>message_generation</build_depend> -->
<!-- Use buildtool_depend for build tool packages: -->
<!-- <buildtool_depend>catkin</buildtool_depend> -->
<!-- Use run_depend for packages you need at runtime: -->
<!-- <run_depend>message_runtime</run_depend> -->
<!-- Use test_depend for packages you need only for testing: -->
<!-- <test_depend>gtest</test_depend> -->
<buildtool_depend>catkin</buildtool_depend>
<build_depend>roscpp </build_depend>
<build_depend>rospy </build_depend>
<build_depend>cmake_modules </build_depend>
<build_depend>cmake_utils </build_depend>
<run_depend>roscpp </run_depend>
<run_depend>rospy </run_depend>
<run_depend>cmake_modules </run_depend>
<run_depend>cmake_utils </run_depend>
<!-- The export tag contains other, unspecified, tags -->
<export>
<!-- Other tools can request additional information be placed here -->
</export>
</package>

69
Controller/src/utils/uav_utils/scripts/odom_to_euler.py Executable file
View File

@@ -0,0 +1,69 @@
#!/usr/bin/env python
import rospy
import numpy as np
import tf
from tf import transformations as tfs
from nav_msgs.msg import Odometry
from sensor_msgs.msg import Imu
from geometry_msgs.msg import Vector3Stamped
from sensor_msgs.msg import Joy
pub = None
pub1 = None
def callback(odom_msg):
q = np.array([odom_msg.pose.pose.orientation.x,
odom_msg.pose.pose.orientation.y,
odom_msg.pose.pose.orientation.z,
odom_msg.pose.pose.orientation.w])
e = tfs.euler_from_quaternion(q, 'rzyx')
euler_msg = Vector3Stamped()
euler_msg.header = odom_msg.header
euler_msg.vector.z = e[0]*180.0/3.14159
euler_msg.vector.y = e[1]*180.0/3.14159
euler_msg.vector.x = e[2]*180.0/3.14159
pub.publish(euler_msg)
def imu_callback(imu_msg):
q = np.array([imu_msg.orientation.x,
imu_msg.orientation.y,
imu_msg.orientation.z,
imu_msg.orientation.w])
e = tfs.euler_from_quaternion(q, 'rzyx')
euler_msg = Vector3Stamped()
euler_msg.header = imu_msg.header
euler_msg.vector.z = e[0]*180.0/3.14159
euler_msg.vector.y = e[1]*180.0/3.14159
euler_msg.vector.x = e[2]*180.0/3.14159
pub1.publish(euler_msg)
def joy_callback(joy_msg):
out_msg = Vector3Stamped()
out_msg.header = joy_msg.header
out_msg.vector.z = -joy_msg.axes[3]
out_msg.vector.y = joy_msg.axes[1]
out_msg.vector.x = joy_msg.axes[0]
pub2.publish(out_msg)
if __name__ == "__main__":
rospy.init_node("odom_to_euler")
pub = rospy.Publisher("~euler", Vector3Stamped, queue_size=10)
sub = rospy.Subscriber("~odom", Odometry, callback)
pub1 = rospy.Publisher("~imueuler", Vector3Stamped, queue_size=10)
sub1 = rospy.Subscriber("~imu", Imu, imu_callback)
pub2 = rospy.Publisher("~ctrlout", Vector3Stamped, queue_size=10)
sub2 = rospy.Subscriber("~ctrlin", Joy, joy_callback)
rospy.spin()

42
Controller/src/utils/uav_utils/scripts/send_odom.py Executable file
View File

@@ -0,0 +1,42 @@
#!/usr/bin/env python
import rospy
import numpy as np
import tf
from tf import transformations as tfs
from nav_msgs.msg import Odometry
if __name__ == "__main__":
rospy.init_node("odom_sender")
msg = Odometry()
msg.header.stamp = rospy.Time.now()-rospy.Duration(0.2)
msg.header.frame_id = "world"
q = tfs.quaternion_from_euler(0,0,0,"rzyx")
msg.pose.pose.position.x = 0
msg.pose.pose.position.y = 0
msg.pose.pose.position.z = 0
msg.twist.twist.linear.x = 0
msg.twist.twist.linear.y = 0
msg.twist.twist.linear.z = 0
msg.pose.pose.orientation.x = q[0]
msg.pose.pose.orientation.y = q[1]
msg.pose.pose.orientation.z = q[2]
msg.pose.pose.orientation.w = q[3]
print(msg)
pub = rospy.Publisher("odom", Odometry, queue_size=10)
counter = 0
r = rospy.Rate(1)
while not rospy.is_shutdown():
counter += 1
msg.header.stamp = rospy.Time.now()-rospy.Duration(0.2)
pub.publish(msg)
rospy.loginfo("Send %3d msg(s)."%counter)
r.sleep()

162
Controller/src/utils/uav_utils/scripts/tf_assist.py Executable file
View File

@@ -0,0 +1,162 @@
#!/usr/bin/env python
import rospy
import numpy as np
import tf
from tf import transformations as tfs
from math import pi
from nav_msgs.msg import Odometry
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped
from sensor_msgs.msg import Imu
from sensor_msgs.msg import Joy
imu_pub = None
odom_pub = None
br = None
class OdometryConverter(object):
def __init__(self, frame_id_in_, frame_id_out_, broadcast_tf_, body_frame_id_, intermediate_frame_id_, world_frame_id_):
self.frame_id_in = frame_id_in_
self.frame_id_out = frame_id_out_
self.broadcast_tf = broadcast_tf_
self.body_frame_id = body_frame_id_
self.intermediate_frame_id = intermediate_frame_id_
self.world_frame_id = world_frame_id_
self.in_odom_sub = None
self.out_odom_pub = None
self.out_path_pub = None
self.path_pub_timer = None
self.tf_pub_flag = True
if self.broadcast_tf:
rospy.loginfo('ROSTopic: [%s]->[%s] TF: [%s]-[%s]-[%s]',
self.frame_id_in, self.frame_id_out, self.body_frame_id, self.intermediate_frame_id, self.world_frame_id)
else:
rospy.loginfo('ROSTopic: [%s]->[%s] No TF',
self.frame_id_in, self.frame_id_out)
self.path = []
def in_odom_callback(self, in_odom_msg):
q = np.array([in_odom_msg.pose.pose.orientation.x,
in_odom_msg.pose.pose.orientation.y,
in_odom_msg.pose.pose.orientation.z,
in_odom_msg.pose.pose.orientation.w])
p = np.array([in_odom_msg.pose.pose.position.x,
in_odom_msg.pose.pose.position.y,
in_odom_msg.pose.pose.position.z])
e = tfs.euler_from_quaternion(q, 'rzyx')
wqb = tfs.quaternion_from_euler(e[0], e[1], e[2], 'rzyx')
wqc = tfs.quaternion_from_euler(e[0], 0.0, 0.0, 'rzyx')
#### odom ####
odom_msg = in_odom_msg
assert(in_odom_msg.header.frame_id == self.frame_id_in)
odom_msg.header.frame_id = self.frame_id_out
odom_msg.child_frame_id = ""
self.out_odom_pub.publish(odom_msg)
#### tf ####
if self.broadcast_tf and self.tf_pub_flag:
self.tf_pub_flag = False
if not self.frame_id_in == self.frame_id_out:
br.sendTransform((0.0, 0.0, 0.0),
tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'),
odom_msg.header.stamp,
self.frame_id_in,
self.frame_id_out)
if not self.world_frame_id == self.frame_id_out:
br.sendTransform((0.0, 0.0, 0.0),
tfs.quaternion_from_euler(0.0, 0.0, 0.0, 'rzyx'),
odom_msg.header.stamp,
self.world_frame_id,
self.frame_id_out)
br.sendTransform((p[0], p[1], p[2]),
wqb,
odom_msg.header.stamp,
self.body_frame_id,
self.world_frame_id)
br.sendTransform(((p[0], p[1], p[2])),
wqc,
odom_msg.header.stamp,
self.intermediate_frame_id,
self.world_frame_id)
#### path ####
pose = PoseStamped()
pose.header = odom_msg.header
pose.pose.position.x = p[0]
pose.pose.position.y = p[1]
pose.pose.position.z = p[2]
pose.pose.orientation.x = q[0]
pose.pose.orientation.y = q[1]
pose.pose.orientation.z = q[2]
pose.pose.orientation.w = q[3]
self.path.append(pose)
def path_pub_callback(self, event):
if self.path:
path = Path()
path.header = self.path[-1].header
path.poses = self.path[-30000::1]
self.out_path_pub.publish(path)
def tf_pub_callback(self, event):
self.tf_pub_flag = True
if __name__ == "__main__":
rospy.init_node('tf_assist')
converters = []
index = 0
while True:
prefix = "~converter%d/" % index
try:
frame_id_in = rospy.get_param('%sframe_id_in' % prefix)
frame_id_out = rospy.get_param('%sframe_id_out' % prefix)
broadcast_tf = rospy.get_param('%sbroadcast_tf' % prefix, False)
body_frame_id = rospy.get_param('%sbody_frame_id' % prefix, 'body')
intermediate_frame_id = rospy.get_param(
'%sintermediate_frame_id' % prefix, 'intermediate')
world_frame_id = rospy.get_param(
'%sworld_frame_id' % prefix, 'world')
converter = OdometryConverter(
frame_id_in, frame_id_out, broadcast_tf, body_frame_id, intermediate_frame_id, world_frame_id)
converter.in_odom_sub = rospy.Subscriber(
'%sin_odom' % prefix, Odometry, converter.in_odom_callback, tcp_nodelay=True)
converter.out_odom_pub = rospy.Publisher(
'%sout_odom' % prefix, Odometry, queue_size=10, tcp_nodelay=True)
converter.out_path_pub = rospy.Publisher(
'%sout_path' % prefix, Path, queue_size=10)
converter.tf_pub_timer = rospy.Timer(
rospy.Duration(0.1), converter.tf_pub_callback)
converter.path_pub_timer = rospy.Timer(
rospy.Duration(0.5), converter.path_pub_callback)
index += 1
except KeyError, e:
if index == 0:
raise(KeyError(e))
else:
if index == 1:
rospy.loginfo(
'prefix:"%s" not found. Generate %d converter.' % (prefix, index))
else:
rospy.loginfo(
'prefix:"%s" not found. Generate %d converters' % (prefix, index))
break
br = tf.TransformBroadcaster()
rospy.spin()

15
Controller/src/utils/uav_utils/scripts/topic_statistics.py Executable file
View File

@@ -0,0 +1,15 @@
#!/usr/bin/env python
import argparse as ap
import argcomplete
def main(**args):
pass
if __name__ == '__main__':
parser = ap.ArgumentParser()
parser.add_argument('positional', choices=['spam', 'eggs'])
parser.add_argument('--optional', choices=['foo1', 'foo2', 'bar'])
argcomplete.autocomplete(parser)
args = parser.parse_args()
main(**vars(args))

122
Controller/src/utils/uav_utils/src/uav_utils_test.cpp Normal file
View File

@@ -0,0 +1,122 @@
#include <gtest/gtest.h>
#include <uav_utils/utils.h>
using namespace uav_utils;
using namespace Eigen;
#define DOUBLE_EPS 1.0e-15
#define FLOAT_EPS 1.0e-6
TEST(GeometryUtilsDouble, Rotation) {
Vector3d v(0.1, 0.2, 0.3);
ASSERT_TRUE(v.isApprox(R_to_ypr(ypr_to_R(v)),DOUBLE_EPS));
ASSERT_TRUE(v.isApprox(quaternion_to_ypr(ypr_to_quaternion(v)),DOUBLE_EPS));
ASSERT_TRUE(v.isApprox(R_to_ypr(rotz(v(0)) * roty(v(1)) * rotx(v(2))),DOUBLE_EPS));
double yaw = 30.0;
ASSERT_DOUBLE_EQ(30.0, toDeg(get_yaw_from_quaternion(yaw_to_quaternion(toRad(yaw)))));
}
TEST(GeometryUtilsFloat, Rotation) {
Vector3f v(0.1, 0.2, 0.3);
ASSERT_TRUE(v.isApprox(R_to_ypr(ypr_to_R(v)),FLOAT_EPS));
ASSERT_TRUE(v.isApprox(quaternion_to_ypr(ypr_to_quaternion(v)),FLOAT_EPS));
ASSERT_TRUE(v.isApprox(R_to_ypr(rotz(v(0)) * roty(v(1)) * rotx(v(2))),FLOAT_EPS));
float yaw = 30.0;
ASSERT_FLOAT_EQ(30.0, toDeg(get_yaw_from_quaternion(yaw_to_quaternion(toRad(yaw)))));
}
TEST(GeometryUtilsDouble, Skew) {
double v1 = 0.1;
double v2 = 0.2;
double v3 = 0.3;
Vector3d v(v1, v2, v3);
Matrix3d M;
M << .0, -v3, v2,
v3, .0, -v1,
-v2, v1, .0;
ASSERT_TRUE(M.isApprox(get_skew_symmetric(v), DOUBLE_EPS));
ASSERT_TRUE(v.isApprox(from_skew_symmetric(M), DOUBLE_EPS));
}
TEST(GeometryUtilsFloat, Skew) {
float v1 = 0.1;
float v2 = 0.2;
float v3 = 0.3;
Vector3f v(v1, v2, v3);
Matrix3f M;
M << .0, -v3, v2,
v3, .0, -v1,
-v2, v1, .0;
ASSERT_TRUE(M.isApprox(get_skew_symmetric(v), FLOAT_EPS));
ASSERT_TRUE(v.isApprox(from_skew_symmetric(M), FLOAT_EPS));
}
TEST(GeometryUtilsDouble, Angle) {
double a = toRad(179.0);
double b = toRad(2.0);
ASSERT_DOUBLE_EQ(-179.0, toDeg(angle_add(a, b)));
ASSERT_DOUBLE_EQ(-179.0, toDeg(yaw_add(a, b)));
ASSERT_DOUBLE_EQ(179.0, toDeg(angle_add(-a, -b)));
ASSERT_DOUBLE_EQ(179.0, toDeg(yaw_add(-a, -b)));
ASSERT_DOUBLE_EQ(177.0, toDeg(angle_add(a, -b)));
ASSERT_DOUBLE_EQ(177.0, toDeg(yaw_add(a, -b)));
ASSERT_NEAR(toRad(-2.0), normalize_angle(toRad(358.0)), DOUBLE_EPS);
}
TEST(GeometryUtilsFloat, Angle) {
float a = toRad(179.0);
float b = toRad(2.0);
ASSERT_FLOAT_EQ(-179.0, toDeg(angle_add(a, b)));
ASSERT_FLOAT_EQ(-179.0, toDeg(yaw_add(a, b)));
ASSERT_FLOAT_EQ(179.0, toDeg(angle_add(-a, -b)));
ASSERT_FLOAT_EQ(179.0, toDeg(yaw_add(-a, -b)));
ASSERT_FLOAT_EQ(177.0, toDeg(angle_add(a, -b)));
ASSERT_FLOAT_EQ(177.0, toDeg(yaw_add(a, -b)));
ASSERT_NEAR(-2.0, toDeg(normalize_angle(toRad(358.0))),FLOAT_EPS);
}
TEST(ConverterDouble, Equality) {
nav_msgs::OdometryPtr pOdom(new nav_msgs::Odometry());
pOdom->pose.pose.position.x = 1.0;
pOdom->pose.pose.position.y = 2.0;
pOdom->pose.pose.position.z = 3.0;
pOdom->pose.pose.orientation.w = 0.5;
pOdom->pose.pose.orientation.x = -0.5;
pOdom->pose.pose.orientation.y = 0.5;
pOdom->pose.pose.orientation.z = -0.5;
pOdom->twist.twist.linear.x = -1.0;
pOdom->twist.twist.linear.y = -2.0;
pOdom->twist.twist.linear.z = -3.0;
pOdom->twist.twist.angular.x = -0.1;
pOdom->twist.twist.angular.y = -0.2;
pOdom->twist.twist.angular.z = -0.3;
Eigen::Vector3d p, v, w;
Eigen::Quaterniond q;
nav_msgs::Odometry odom_ = *pOdom;
extract_odometry(pOdom, p, v, q, w);
ASSERT_TRUE(v.isApprox(from_vector3_msg(pOdom->twist.twist.linear)));
ASSERT_TRUE(w.isApprox(from_vector3_msg(pOdom->twist.twist.angular)));
ASSERT_TRUE(p.isApprox(from_point_msg(pOdom->pose.pose.position)));
ASSERT_TRUE(q.isApprox(from_quaternion_msg(pOdom->pose.pose.orientation)));
ASSERT_TRUE(v.isApprox(from_vector3_msg(to_vector3_msg(v))));
ASSERT_TRUE(p.isApprox(from_point_msg(to_point_msg(p))));
ASSERT_TRUE(q.isApprox(from_quaternion_msg(to_quaternion_msg(q))));
}
int main(int argc, char* argv[]) {
testing::InitGoogleTest(&argc, argv);
return RUN_ALL_TESTS();
}