diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..0dcfcdb41ace182180679e0bf62a2c6484e6392d --- /dev/null +++ b/.gitignore @@ -0,0 +1,2 @@ +/.cproject +/.project diff --git a/.gitlab-ci.yml b/.gitlab-ci.yml new file mode 100644 index 0000000000000000000000000000000000000000..c6461731b57f053e5ddc7e56f1aa624e788c33b2 --- /dev/null +++ b/.gitlab-ci.yml @@ -0,0 +1,259 @@ +stages: + - license + - build_and_test + - demos + +############ YAML ANCHORS ############ +.preliminaries_template: &preliminaries_definition + ## Install ssh-agent if not already installed, it is required by Docker. + ## (change apt-get to yum if you use an RPM-based image) + - 'which ssh-agent || ( apt-get update -y && apt-get install openssh-client -y )' + + ## Run ssh-agent (inside the build environment) + - eval $(ssh-agent -s) + + ## Add the SSH key stored in SSH_PRIVATE_KEY variable to the agent store + ## We're using tr to fix line endings which makes ed25519 keys work + ## without extra base64 encoding. + ## https://gitlab.com/gitlab-examples/ssh-private-key/issues/1#note_48526556 + - mkdir -p ~/.ssh + - chmod 700 ~/.ssh + - echo "$SSH_PRIVATE_KEY" | tr -d '\r' | ssh-add - > /dev/null + # - echo "$SSH_KNOWN_HOSTS" > $HOME/.ssh/known_hosts + - ssh-keyscan -H -p 2202 gitlab.iri.upc.edu >> $HOME/.ssh/known_hosts + + # update apt + - apt-get update + + # create ci_deps folder (if not exists) + - mkdir -pv ci_deps + + # manually source ros setup.bash + - source /root/catkin_ws/devel/setup.bash + - roscd # check that it works + +.license_header_template: &license_header_definition + - cd $CI_PROJECT_DIR + + # configure git + - export CI_NEW_BRANCH=ci_processing$RANDOM + - echo creating new temporary branch... $CI_NEW_BRANCH + - git config --global user.email "${CI_EMAIL}" + - git config --global user.name "${CI_USERNAME}" + - git checkout -b $CI_NEW_BRANCH # temporary branch + + # license headers + - export CURRENT_YEAR=$( date +'%Y' ) + - echo "current year:" ${CURRENT_YEAR} + - if [ -f license_header_${CURRENT_YEAR}.txt ]; then + # add license headers to new files + - echo "File license_header_${CURRENT_YEAR}.txt already exists. License headers are assumed to be updated. Adding headers to new files..." + - ./ci_deps/wolf/wolf_scripts/license_manager.sh --add --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps + - else + # update license headers of all files + - export PREV_YEAR=$(( CURRENT_YEAR-1 )) + - echo "Creating new file license_header_${CURRENT_YEAR}.txt..." + - git mv license_header_${PREV_YEAR}.txt license_header_${CURRENT_YEAR}.txt + - sed -i "s/${PREV_YEAR}/${PREV_YEAR},${CURRENT_YEAR}/g" license_header_${CURRENT_YEAR}.txt + - ./ci_deps/wolf/wolf_scripts/license_manager.sh --update --path=. --license-header=license_header_${CURRENT_YEAR}.txt --exclude=ci_deps + - fi + + # push changes (if any) + - if git commit -a -m "[skip ci] license headers added or modified" ; then + - git remote set-url --push origin "ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git" + - git push origin $CI_NEW_BRANCH:${CI_COMMIT_REF_NAME} + - else + - echo "No changes, nothing to commit!" + - fi + +.install_wolf_template: &install_wolf_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d wolf ]; then + - echo "directory wolf exists" + - cd wolf + - git checkout devel + - git pull + - git checkout $WOLF_CORE_BRANCH + - git pull + - else + - git clone -b $WOLF_CORE_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/wolf.git + - cd wolf + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_DEMOS=OFF -DBUILD_TESTS=OFF .. + - make -j$(nproc) + - make install + +.install_visionutils_template: &install_visionutils_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d vision_utils ]; then + - echo "directory vision_utils exists" + - cd vision_utils + - git pull + - else + - git clone https://gitlab.iri.upc.edu/labrobotica/algorithms/vision_utils.git + - cd vision_utils + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release .. + - make -j$(nproc) + - make install + +.install_wolfvision_template: &install_wolfvision_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d vision ]; then + - echo "directory vision exists" + - cd vision + - git checkout devel + - git pull + - git checkout $WOLF_VISION_BRANCH + - git pull + - else + - git clone -b $WOLF_VISION_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/vision.git + - cd vision + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF .. + - make -j$(nproc) + - make install + +.install_apriltag_template: &install_apriltag_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d apriltaglib ]; then + - echo "directory apriltaglib exists" + - cd apriltaglib + - git pull + - else + - git clone https://github.com/AprilRobotics/apriltag apriltaglib + - cd apriltaglib + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release .. + - make -j$(nproc) + - make install + +.install_wolfapriltag_template: &install_wolfapriltag_definition + - cd ${CI_PROJECT_DIR}/ci_deps + - if [ -d apriltag ]; then + - echo "directory apriltag exists" + - cd apriltag + - git checkout devel + - git pull + - git checkout $WOLF_APRILTAG_BRANCH + - git pull + - else + - git clone -b $WOLF_APRILTAG_BRANCH ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_lib/plugins/apriltag.git + - cd apriltag + - fi + - mkdir -pv build + - cd build + - cmake -DCMAKE_BUILD_TYPE=release -DBUILD_TESTS=OFF .. + - make -j$(nproc) + - make install + +.clone_wolfrosnode_template: &clone_wolfrosnode_definition + - roscd + - cd ../src + - git clone ssh://git@gitlab.iri.upc.edu:2202/mobile_robotics/wolf_projects/wolf_ros/wolf_ros_node.git + - cd wolf_ros_node + - git checkout $WOLF_ROS_CORE_BRANCH + +.build_and_test_template: &build_and_test_definition + - roscd + - cd ../src + - git clone ssh://git@gitlab.iri.upc.edu:2202/${CI_PROJECT_PATH}.git + - cd wolf_ros_apriltag + - git checkout $CI_COMMIT_BRANCH + - cd ../.. + - catkin_make + +############ LICENSE HEADERS ############ +license_headers: + stage: license + image: labrobotica/wolf_vision_deps_ros:20.04 + cache: [] + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + script: + - *license_header_definition + +############ UBUNTU 18.04 TEST ############ +build_and_test:bionic: + stage: build_and_test + image: labrobotica/wolf_vision_deps_ros:18.04 + cache: + - key: wolf-bionic + paths: + - ci_deps/wolf/ + - key: visionutils-bionic + paths: + - ci_deps/vision_utils/ + - key: vision-bionic + paths: + - ci_deps/vision/ + - key: apriltaglib-bionic + paths: + - ci_deps/apriltaglib/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + - *install_visionutils_definition + - *install_wolfvision_definition + - *install_apriltag_definition + - *install_wolfapriltag_definition + - *clone_wolfrosnode_definition + - ldconfig + script: + - *build_and_test_definition + +############ UBUNTU 20.04 TEST ############ +build_and_test:focal: + stage: build_and_test + image: labrobotica/wolf_vision_deps_ros:20.04 + cache: + - key: wolf-focal + paths: + - ci_deps/wolf/ + - key: visionutils-focal + paths: + - ci_deps/vision_utils/ + - key: vision-focal + paths: + - ci_deps/vision/ + - key: apriltaglib-focal + paths: + - ci_deps/apriltaglib/ + except: + - master + before_script: + - *preliminaries_definition + - *install_wolf_definition + - *install_visionutils_definition + - *install_wolfvision_definition + - *install_apriltag_definition + - *install_wolfapriltag_definition + - *clone_wolfrosnode_definition + - ldconfig + script: + - *build_and_test_definition + +############ RUN DEMOS ############ +demo_apriltag: + stage: demos + variables: + WOLF_CORE_BRANCH: $WOLF_CORE_BRANCH + WOLF_VISION_BRANCH: $WOLF_VISION_BRANCH + WOLF_APRILTAG_BRANCH: $WOLF_APRILTAG_BRANCH + WOLF_ROS_CORE_BRANCH: $WOLF_ROS_CORE_BRANCH + WOLF_ROS_APRILTAG_BRANCH: $CI_COMMIT_BRANCH + trigger: + project: mobile_robotics/wolf_projects/wolf_ros/demos/wolf_demo_apriltag_imu diff --git a/.settings/.gitignore b/.settings/.gitignore new file mode 100644 index 0000000000000000000000000000000000000000..d81d4c414cc298fa21635d0b27c126c1b0a7ff3a --- /dev/null +++ b/.settings/.gitignore @@ -0,0 +1 @@ +/language.settings.xml diff --git a/CMakeLists.txt b/CMakeLists.txt index 5f211258854e81dad616cb1b163a1576ec9fcdb2..6ccd287e2f6918e6ceac1c8582aab703014b8e5e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,8 +1,10 @@ cmake_minimum_required(VERSION 3.0.2) project(wolf_ros_apriltag) -## Compile as C++11, supported in ROS Kinetic and newer -add_compile_options(-std=c++11) +## Compile as C++14 +add_compile_options(-std=c++14) + +##set(OpenCV_DIR /usr/local/share/OpenCV/) ## Find catkin macros and libraries ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) @@ -12,106 +14,27 @@ find_package(catkin REQUIRED COMPONENTS roscpp rospy std_msgs - dynamic_reconfigure +# dynamic_reconfigure tf wolf_ros_node - OpenCV +# OpenCV image_transport ) find_package(wolfcore REQUIRED) -find_package(wolfimu REQUIRED) find_package(wolfvision REQUIRED) find_package(wolfapriltag REQUIRED) -## System dependencies are found with CMake's conventions -# find_package(Boost REQUIRED COMPONENTS system) - - -## Uncomment this if the package has a setup.py. This macro ensures -## modules and global scripts declared therein get installed -## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html -# catkin_python_setup() - -################################################ -## Declare ROS messages, services and actions ## -################################################ - -## To declare and build messages, services or actions from within this -## package, follow these steps: -## * Let MSG_DEP_SET be the set of packages whose message types you use in -## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). -## * In the file package.xml: -## * add a build_depend tag for "message_generation" -## * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET -## * If MSG_DEP_SET isn't empty the following dependency has been pulled in -## but can be declared for certainty nonetheless: -## * add a exec_depend tag for "message_runtime" -## * In this file (CMakeLists.txt): -## * add "message_generation" and every package in MSG_DEP_SET to -## find_package(catkin REQUIRED COMPONENTS ...) -## * add "message_runtime" and every package in MSG_DEP_SET to -## catkin_package(CATKIN_DEPENDS ...) -## * uncomment the add_*_files sections below as needed -## and list every .msg/.srv/.action file to be processed -## * uncomment the generate_messages entry below -## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) - -## Generate messages in the 'msg' folder -add_message_files( - FILES - SolveLogs.msg -) - -## Generate services in the 'srv' folder -# add_service_files( -# FILES -# Service1.srv -# Service2.srv -# ) - -## Generate actions in the 'action' folder -# add_action_files( -# FILES -# Action1.action -# Action2.action -# ) ## Generate added messages and services with any dependencies listed here -generate_messages( - DEPENDENCIES - std_msgs - geometry_msgs -) - -################################################ -## Declare ROS dynamic reconfigure parameters ## -################################################ - -## To declare and build dynamic reconfigure parameters within this -## package, follow these steps: -## * In the file package.xml: -## * add a build_depend and a exec_depend tag for "dynamic_reconfigure" -## * In this file (CMakeLists.txt): -## * add "dynamic_reconfigure" to -## find_package(catkin REQUIRED COMPONENTS ...) -## * uncomment the "generate_dynamic_reconfigure_options" section below -## and list every .cfg file to be processed - -## Generate dynamic reconfigure parameters in the 'cfg' folder -# generate_dynamic_reconfigure_options( -# cfg/DynReconf1.cfg -# cfg/DynReconf2.cfg +# generate_messages( +# DEPENDENCIES +# sensor_msgs# std_msgs # ) ################################### ## catkin specific configuration ## ################################### -## The catkin_package macro generates cmake config files for your package -## Declare things to be passed to dependent projects -## INCLUDE_DIRS: uncomment this if your package contains header files -## LIBRARIES: libraries you create in this project that dependent projects also need -## CATKIN_DEPENDS: catkin_packages dependent projects also need -## DEPENDS: system dependencies of this project that dependent projects also need + catkin_package( # INCLUDE_DIRS include # LIBRARIES wolf_ros_apriltag @@ -140,107 +63,26 @@ include_directories( add_library(subscriber_${PROJECT_NAME} src/subscriber_camera.cpp - src/subscriber_imu.cpp - ) - -add_library(publisher_${PROJECT_NAME} - src/publisher_estimated_pose.cpp - src/publisher_current_state.cpp - src/publisher_solve_logs.cpp - # src/publisher_debug_img.cpp ) -## Add cmake target dependencies of the library -## as an example, code may need to be generated before libraries -## either from message generation or dynamic reconfigure -# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Declare a C++ executable -## With catkin_make all packages are built within a single CMake context -## The recommended prefix ensures that target names across packages don't collide -# add_executable(${PROJECT_NAME}_node src/wolf_ros_apriltag_node.cpp) - -## Rename C++ executable without prefix -## The above recommended prefix causes long target names, the following renames the -## target back to the shorter version for ease of user use -## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node" -# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "") -## Add cmake target dependencies of the executable -## same as for the library above -# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) - -## Specify libraries to link a library or executable target against -# target_link_libraries(${PROJECT_NAME}_node -# ${catkin_LIBRARIES} -# ) +#add_library(publisher_${PROJECT_NAME} +# src/publisher_debug_img.cpp +## src/publisher_estimated_pose.cpp +## src/publisher_current_state.cpp +## src/publisher_solve_logs.cpp +# ) target_link_libraries(subscriber_${PROJECT_NAME} ${wolfcore_LIBRARIES} - ${wolfimu_LIBRARIES} ${wolfapriltag_LIBRARIES} ${wolfvision_LIBRARIES} ${vision_utils_LIBRARY} ${catkin_LIBRARIES} ) -target_link_libraries(publisher_${PROJECT_NAME} - ${wolfcore_LIBRARIES} - ${wolfimu_LIBRARIES} - ${wolfapriltag_LIBRARIES} - ${wolfvision_LIBRARIES} - ${catkin_LIBRARIES} - ) -############# -## 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 -# catkin_install_python(PROGRAMS -# scripts/my_python_script -# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark executables for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html -# install(TARGETS ${PROJECT_NAME}_node -# RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -# ) - -## Mark libraries for installation -## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html -# install(TARGETS ${PROJECT_NAME} -# ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -# RUNTIME DESTINATION ${CATKIN_GLOBAL_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 test/test_wolf_ros_apriltag.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) +#target_link_libraries(publisher_${PROJECT_NAME} +# ${wolfcore_LIBRARIES} +# ${wolfapriltag_LIBRARIES} +# ${wolfvision_LIBRARIES} +# ${catkin_LIBRARIES} +# ) diff --git a/LICENSE b/LICENSE new file mode 100644 index 0000000000000000000000000000000000000000..281d399f13dfd23087acc56303dd38d68162587c --- /dev/null +++ b/LICENSE @@ -0,0 +1,619 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. diff --git a/include/publisher_current_state.h b/include/publisher_current_state.h deleted file mode 100644 index 5e3da37df3e65e60bdb612d993b4009819beb81b..0000000000000000000000000000000000000000 --- a/include/publisher_current_state.h +++ /dev/null @@ -1,26 +0,0 @@ -#ifndef __PUBLISHER_CURRENT_STATE_H_ -#define __PUBLISHER_CURRENT_STATE_H_ - -/************************** - * WOLF includes * - **************************/ -#include -#include - -#include "publisher.h" -using namespace wolf; - - -class PublisherCurrentState: public Publisher -{ -public: - PublisherCurrentState(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem); - WOLF_PUBLISHER_CREATE(PublisherCurrentState); - virtual ~PublisherCurrentState(){}; - void initialize(ros::NodeHandle &nh, const std::string& topic) override; - void publishDerived() override; -}; -WOLF_REGISTER_PUBLISHER(PublisherCurrentState) -#endif // __PUBLISHER_CURRENT_STATE_H_ diff --git a/include/publisher_debug_img.h b/include/publisher_debug_img.h index f76b6208e42bf2f4f8a3f9f325e6181849a00c63..d20e8387d36e78867c73778837fce4a22068f85b 100644 --- a/include/publisher_debug_img.h +++ b/include/publisher_debug_img.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- #ifndef __PUBLISHER_DEBUG_IMG_H_ #define __PUBLISHER_DEBUG_IMG_H_ diff --git a/include/publisher_estimated_pose.h b/include/publisher_estimated_pose.h deleted file mode 100644 index ea8d9d4e3477eafd86991177a1e8b13a6fd573d0..0000000000000000000000000000000000000000 --- a/include/publisher_estimated_pose.h +++ /dev/null @@ -1,27 +0,0 @@ -#ifndef __PUBLISHER_ESTIMATED_POSE_H_ -#define __PUBLISHER_ESTIMATED_POSE_H_ - -/************************** - * WOLF includes * - **************************/ -#include -#include - -#include "publisher.h" -using namespace wolf; - - -class PublisherEstimatedPose: public Publisher -{ -public: - PublisherEstimatedPose(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem); - WOLF_PUBLISHER_CREATE(PublisherEstimatedPose); - virtual ~PublisherEstimatedPose(){}; - void initialize(ros::NodeHandle &nh, const std::string& topic) override; - void publishDerived() override; -}; -WOLF_REGISTER_PUBLISHER(PublisherEstimatedPose); - -#endif // __PUBLISHER_ESTIMATED_POSE_H_ diff --git a/include/publisher_solve_logs.h b/include/publisher_solve_logs.h deleted file mode 100644 index 43978aecb88711fc4ce1a37ceb34e63668448b93..0000000000000000000000000000000000000000 --- a/include/publisher_solve_logs.h +++ /dev/null @@ -1,28 +0,0 @@ -#ifndef __PUBLISHER_SOLVE_LOGS_H_ -#define __PUBLISHER_SOLVE_LOGS_H_ - - -/************************** - * WOLF includes * - **************************/ -#include -#include - -#include "publisher.h" -using namespace wolf; - - -class PublisherSolveLogs: public Publisher -{ -public: - PublisherSolveLogs(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem); - WOLF_PUBLISHER_CREATE(PublisherSolveLogs); - virtual ~PublisherSolveLogs(){}; - void initialize(ros::NodeHandle &nh, const std::string& topic) override; - void publishDerived() override; -}; -WOLF_REGISTER_PUBLISHER(PublisherSolveLogs) - -#endif // __PUBLISHER_SOLVE_LOGS_H_ diff --git a/include/subscriber_camera.h b/include/subscriber_camera.h index 5bf663d5147946ff190feaf5b5adad48a09cbf55..67896dcb3849c5b8590078dae5798529faea23cf 100644 --- a/include/subscriber_camera.h +++ b/include/subscriber_camera.h @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- #ifndef SUBSCRIBER_CAMERA_H_ #define SUBSCRIBER_CAMERA_H_ /************************** @@ -42,7 +63,6 @@ class SubscriberCamera : public Subscriber bool use_debug_img_; ros::Time t0_cam_, ts_cam_; ros::NodeHandle my_nh_; - int shift_imu_minus_cam_; bool first_time; public: // Constructor diff --git a/include/subscriber_imu.h b/include/subscriber_imu.h deleted file mode 100644 index f7012d7ddd5487f9ccfd68003f396cfadde4921f..0000000000000000000000000000000000000000 --- a/include/subscriber_imu.h +++ /dev/null @@ -1,72 +0,0 @@ -#ifndef SUBSCRIBER_CAMERA_H_ -#define SUBSCRIBER_CAMERA_H_ -/************************** - * WOLF includes * - **************************/ -#include -#include -#include -#include -#include -#include - -/************************** - * CERES includes * - **************************/ -#include -//#include "glog/logging.h" - -/************************** - * ROS includes * - **************************/ -#include -#include -#include - -/************************** - * STD includes * - **************************/ -#include -#include -#include - -#include -#include -#include "manif/SE3.h" - -#include "subscriber.h" -#include "factory_subscriber.h" - -namespace wolf -{ -class SubscriberImu : public Subscriber -{ - bool finding_orientation_prior_; - // IMU orientation prior - int size_buffer_imu_init_; - int i_imu_buff_; - double std_imu_max_; - std::vector ax_buff_; - std::vector ay_buff_; - std::vector az_buff_; - Eigen::Quaterniond w_q_imu_init_; - bool imu_time_initialized_; - ros::Time t0_imu_; - ros::Time ts_imu_; - double acc_sat_; - double gyr_sat_; - public: - // Constructor - SubscriberImu(const std::string& _unique_name, - const ParamsServer& _server, - const SensorBasePtr _sensor_ptr); - WOLF_SUBSCRIBER_CREATE(SubscriberImu); - - virtual void initialize(ros::NodeHandle& nh, const std::string& topic); - - void callback(const sensor_msgs::Imu &msg); -}; - -WOLF_REGISTER_SUBSCRIBER(SubscriberImu); -} -#endif diff --git a/launch/online.rviz b/launch/online.rviz deleted file mode 100644 index 033541ae22f38e32ebd9825ff6f71ec0c449a254..0000000000000000000000000000000000000000 --- a/launch/online.rviz +++ /dev/null @@ -1,189 +0,0 @@ -Panels: - - Class: rviz/Displays - Help Height: 0 - Name: Displays - Property Tree Widget: - Expanded: - - /Global Options1 - - /Status1 - - /Grid1 - - /TF1 - - /Camera1 - - /MarkerArray1 - Splitter Ratio: 0.5 - Tree Height: 582 - - Class: rviz/Selection - Name: Selection - - Class: rviz/Tool Properties - Expanded: - - /2D Pose Estimate1 - - /2D Nav Goal1 - - /Publish Point1 - Name: Tool Properties - Splitter Ratio: 0.5886790156364441 - - Class: rviz/Views - Expanded: - - /Current View1 - Name: Views - Splitter Ratio: 0.5 - - Class: rviz/Time - Experimental: false - Name: Time - SyncMode: 0 - SyncSource: "" -Preferences: - PromptSaveOnExit: true -Toolbars: - toolButtonStyle: 2 -Visualization Manager: - Class: "" - Displays: - - Alpha: 0.5 - Cell Size: 1 - Class: rviz/Grid - Color: 160; 160; 164 - Enabled: true - Line Style: - Line Width: 0.029999999329447746 - Value: Lines - Name: Grid - Normal Cell Count: 0 - Offset: - X: 0 - Y: 0 - Z: 0 - Plane: XY - Plane Cell Count: 10 - Reference Frame: - Value: true - - Class: rviz/TF - Enabled: true - Frame Timeout: 15 - Frames: - All Enabled: false - base_footprint: - Value: true - map: - Value: true - odom: - Value: true - Marker Scale: 1 - Name: TF - Show Arrows: false - Show Axes: true - Show Names: false - Tree: - map: - odom: - base_footprint: - {} - Update Interval: 0 - Value: true - - Class: rviz/Camera - Enabled: true - Image Rendering: background and overlay - Image Topic: /camera_simu/image_raw - Name: Camera - Overlay Alpha: 0.5 - Queue Size: 2 - Transport Hint: raw - Unreliable: false - Value: true - Visibility: - Grid: true - MarkerArray: true - Pose: true - TF: true - Value: true - Zoom Factor: 1 - - Class: rviz/MarkerArray - Enabled: true - Marker Topic: /wolf_apriltag_vio/marker_array - Name: MarkerArray - Namespaces: - {} - Queue Size: 100 - Value: true - - Alpha: 1 - Axes Length: 1 - Axes Radius: 0.10000000149011612 - Class: rviz/Pose - Color: 255; 25; 0 - Enabled: true - Head Length: 0.30000001192092896 - Head Radius: 0.10000000149011612 - Name: Pose - Shaft Length: 1 - Shaft Radius: 0.05000000074505806 - Shape: Arrow - Topic: /wolf_apriltag_vio/estimated_pose - Unreliable: false - Value: true - Enabled: true - Global Options: - Background Color: 48; 48; 48 - Default Light: true - Fixed Frame: map - Frame Rate: 30 - Name: root - Tools: - - Class: rviz/Interact - Hide Inactive Objects: true - - Class: rviz/MoveCamera - - Class: rviz/Select - - Class: rviz/FocusCamera - - Class: rviz/Measure - - Class: rviz/SetInitialPose - Theta std deviation: 0.2617993950843811 - Topic: /initialpose - X std deviation: 0.5 - Y std deviation: 0.5 - - Class: rviz/SetGoal - Topic: /move_base_simple/goal - - Class: rviz/PublishPoint - Single click: true - Topic: /clicked_point - Value: true - Views: - Current: - Class: rviz/Orbit - Distance: 2.967782497406006 - Enable Stereo Rendering: - Stereo Eye Separation: 0.05999999865889549 - Stereo Focal Distance: 1 - Swap Stereo Eyes: false - Value: false - Focal Point: - X: -0.48822471499443054 - Y: -0.18343956768512726 - Z: 0.25536078214645386 - Focal Shape Fixed Size: true - Focal Shape Size: 0.05000000074505806 - Invert Z Axis: false - Name: Current View - Near Clip Distance: 0.009999999776482582 - Pitch: 0.24979741871356964 - Target Frame: - Value: Orbit (rviz) - Yaw: 5.988564491271973 - Saved: ~ -Window Geometry: - Camera: - collapsed: false - Displays: - collapsed: false - Height: 940 - Hide Left Dock: false - Hide Right Dock: false - QMainWindow State: 000000ff00000000fd00000004000000000000016a000002effc0200000009fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006300fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003f0000028b000000db00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261fb0000000c00430061006d00650072006101000002d00000005e0000001800ffffff000000010000016c00000607fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000004000000607000000b300fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000048200000058fc0100000002fb0000000800540069006d00650100000000000004820000028200fffffffb0000000800540069006d0065010000000000000450000000000000000000000312000002ef00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 - Selection: - collapsed: false - Time: - collapsed: false - Tool Properties: - collapsed: false - Views: - collapsed: false - Width: 1154 - X: 324 - Y: 55 diff --git a/launch/test.launch b/launch/test.launch deleted file mode 100644 index 1b875fb1f05afe2f3b975a5aa51b53d45901955d..0000000000000000000000000000000000000000 --- a/launch/test.launch +++ /dev/null @@ -1,38 +0,0 @@ - - - - - - -?> - - - - - -?> - - - - - - diff --git a/license_header_2022.txt b/license_header_2022.txt new file mode 100644 index 0000000000000000000000000000000000000000..0c987025f9dba3e7af993051b9bdf4b5ff400e0d --- /dev/null +++ b/license_header_2022.txt @@ -0,0 +1,17 @@ +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . diff --git a/msg/SolveLogs.msg b/msg/SolveLogs.msg deleted file mode 100644 index 1f09ccb67b82f31c07236c631ef95e554ed14c49..0000000000000000000000000000000000000000 --- a/msg/SolveLogs.msg +++ /dev/null @@ -1 +0,0 @@ -float32 duration diff --git a/package.xml b/package.xml index c2ab136ae3bf2816aba62b86d37413c622178f41..d46df9ccabdabf4b0d7dbee1a17477312d134f02 100644 --- a/package.xml +++ b/package.xml @@ -52,6 +52,7 @@ roscpp rospy std_msgs + wolf_ros_node roscpp rospy std_msgs diff --git a/src/publisher_current_state.cpp b/src/publisher_current_state.cpp deleted file mode 100644 index 741a87f922aebc1814faf132d136481c9bf485d0..0000000000000000000000000000000000000000 --- a/src/publisher_current_state.cpp +++ /dev/null @@ -1,34 +0,0 @@ -#include "publisher_current_state.h" - - -PublisherCurrentState::PublisherCurrentState(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem) : - Publisher(_unique_name, _server, _problem) -{ -} - -void PublisherCurrentState::initialize(ros::NodeHandle& nh, const std::string& topic) -{ - publisher_ = nh.advertise(topic, 10); -} - -void PublisherCurrentState::publishDerived() -{ - - // Eigen::VectorXd cstate = problem_->getCurrentState(); - // nav_msgs::Odometry odom_msg; - // odom_msg.pose.pose.position.x = cstate(0); - // odom_msg.pose.pose.position.y = cstate(1); - // odom_msg.pose.pose.position.z = cstate(2); - // odom_msg.pose.pose.orientation.x = cstate(3); - // odom_msg.pose.pose.orientation.y = cstate(4); - // odom_msg.pose.pose.orientation.z = cstate(5); - // odom_msg.pose.pose.orientation.w = cstate(6); - // if (cstate.size() > 7){ - // odom_msg.twist.twist.linear.x = cstate(7); - // odom_msg.twist.twist.linear.y = cstate(8); - // odom_msg.twist.twist.linear.z = cstate(9); - // } - // publisher_.publish(odom_msg); -} diff --git a/src/publisher_debug_img.cpp b/src/publisher_debug_img.cpp index 8e480a417d653a3f2fdbac69ac13206588f0490d..afdd19f168ca411ed501f80da52964734d8a5380 100644 --- a/src/publisher_debug_img.cpp +++ b/src/publisher_debug_img.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- #include "publisher_debug_img.h" diff --git a/src/publisher_estimated_pose.cpp b/src/publisher_estimated_pose.cpp deleted file mode 100644 index 93848d0eafe04d65cc5a2a646cf4f18f94fe3dac..0000000000000000000000000000000000000000 --- a/src/publisher_estimated_pose.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "publisher_estimated_pose.h" - -#include - -PublisherEstimatedPose::PublisherEstimatedPose(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem): - Publisher(_unique_name, _server, _problem) -{ -} - -void PublisherEstimatedPose::initialize(ros::NodeHandle& nh, const std::string& topic) -{ - publisher_ = nh.advertise(topic, 10); -} - -void PublisherEstimatedPose::publishDerived() -{ - -} diff --git a/src/publisher_solve_logs.cpp b/src/publisher_solve_logs.cpp deleted file mode 100644 index ab4066212a89c9438391005b672c756546188309..0000000000000000000000000000000000000000 --- a/src/publisher_solve_logs.cpp +++ /dev/null @@ -1,20 +0,0 @@ -#include "publisher_solve_logs.h" - -#include - -PublisherSolveLogs::PublisherSolveLogs(const std::string& _unique_name, - const ParamsServer& _server, - const ProblemPtr _problem) : - Publisher(_unique_name, _server, _problem) -{ -} - -void PublisherSolveLogs::initialize(ros::NodeHandle& nh, const std::string& topic) -{ - publisher_ = nh.advertise(topic, 10); -} - -void PublisherSolveLogs::publishDerived() -{ - -} diff --git a/src/subscriber_camera.cpp b/src/subscriber_camera.cpp index bccf2c6eb56d8eff95026bea7cc6799869eed5b5..ea59397b0a342b03af8ef29d03d3b376e3071d3d 100644 --- a/src/subscriber_camera.cpp +++ b/src/subscriber_camera.cpp @@ -1,3 +1,24 @@ +//--------LICENSE_START-------- +// +// Copyright (C) 2020,2021,2022 Institut de Robòtica i Informàtica Industrial, CSIC-UPC. +// Authors: Joan Solà Ortega (jsola@iri.upc.edu) +// All rights reserved. +// +// This file is part of WOLF +// WOLF is free software: you can redistribute it and/or modify +// it under the terms of the GNU Lesser General Public License as published by +// the Free Software Foundation, either version 3 of the License, or +// at your option) any later version. +// +// This program is distributed in the hope that it will be useful, +// but WITHOUT ANY WARRANTY; without even the implied warranty of +// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +// GNU Lesser General Public License for more details. +// +// You should have received a copy of the GNU Lesser General Public License +// along with this program. If not, see . +// +//--------LICENSE_END-------- #include "subscriber_camera.h" #include #include "apriltag/feature/feature_apriltag.h" @@ -17,7 +38,6 @@ SubscriberCamera::SubscriberCamera(const std::string& _unique_name, first_image_received_ = false; use_debug_img_ = true; t0_cam_ = ts_cam_ = ros::Time(0); - shift_imu_minus_cam_ = 34; first_time = true; } @@ -30,10 +50,11 @@ void SubscriberCamera::initialize(ros::NodeHandle& nh, const std::string& topic) void SubscriberCamera::callback(const sensor_msgs::ImageConstPtr& msg) { - // std::cout << "cam_callback ts: " << msg->header.stamp << std::endl; - ros::Time t_start = ros::Time::now(); + updateLastHeader(msg->header); - ts_cam_ = msg->header.stamp + ros::Duration(shift_imu_minus_cam_); + auto start = std::chrono::high_resolution_clock::now(); + + ts_cam_ = msg->header.stamp; if (!first_image_received_){ t0_cam_ = ts_cam_; // Relative timestamps -> first KF at timestamp 0 @@ -63,20 +84,15 @@ void SubscriberCamera::callback(const sensor_msgs::ImageConstPtr& msg) nb_without_rot--; } } - WOLF_TRACE(f_list.size(), " tags, nb without ROT: ", nb_without_rot, "/", f_list.size()); } } - sensor_ptr_->getProblem()->print(4,1,1,1); + auto stopbef = std::chrono::high_resolution_clock::now(); + auto durationbef = std::chrono::duration_cast(stopbef - start); + // std::cout << "APRILTAGBEFORE " << durationbef.count() << std::endl; publishDebugImg(msg, frame); - //////////////////////////// - // Write - // if (use_debug_img_){ - // publishDebugImg(msg, frame); - // } + auto stopaft = std::chrono::high_resolution_clock::now(); + auto durationaft = std::chrono::duration_cast(stopaft - start); - // if (apriltag_only_){ - // broadcastCurrentTf(); - // } } void SubscriberCamera::publishDebugImg(const sensor_msgs::ImageConstPtr &img_msg, cv::Mat& frame){ /////////////////////////////// @@ -103,81 +119,79 @@ void SubscriberCamera::publishDebugImg(const sensor_msgs::ImageConstPtr &img_msg cv::line(frame, feat_april->getTagCorners()[1], feat_april->getTagCorners()[2], CV_RGB(0, 255, 0), 2); cv::line(frame, feat_april->getTagCorners()[2], feat_april->getTagCorners()[3], CV_RGB(0, 0, 255), 2); cv::line(frame, feat_april->getTagCorners()[3], feat_april->getTagCorners()[0], CV_RGB(255, 0, 255), 2); + cv::putText(frame, cv::String(std::to_string(feat_april->getDetection().id)), feat_april->getTagCorners()[0], cv::FONT_HERSHEY_DUPLEX, 1.0, CV_RGB(0,220,0),1.5); } - /////////////////////////////// - // Draw tag location from estimated keyframes - // Eigen::VectorXd frame_state_bis = last_cap_cam_frame->getState(); // state estimation at the keyframe associated to the capture "last": late and jumping - VectorComposite frame_state_ts = problem_->getState(img_msg->header.stamp.toSec()); // state estimation at the ts of the captured frame: should match exactly the measurements - // Eigen::VectorXd current_state = problem_->getCurrentState(); // processor motion estimated state: in advance to the frame capture - Eigen::Isometry3d wTb = Eigen::Translation3d(frame_state_ts['P']) * Eigen::Quaternions(frame_state_ts['O'].data()); - - Eigen::Matrix6d Qb; - - - // Relative corner poses assuming an apriltag frame at the center of the tag with same oriention than the camera - // when they face each other - // bottom left to top left, direct order - Eigen::Vector3d lXP1(-1, 1,0); - Eigen::Vector3d lXP2( 1, 1,0); - Eigen::Vector3d lXP3( 1,-1,0); - Eigen::Vector3d lXP4(-1,-1,0); - std::vector lXP_vec = {lXP1, lXP2, lXP3, lXP4}; - std::vector cXp_vec; cXp_vec.resize(4); - - auto pos = sensor_ptr_->getP()->getState(); - auto ori = sensor_ptr_->getO()->getState(); - Eigen::Vector7d cam_extrinsics_; - cam_extrinsics_ << pos(0), pos(1), pos(2), ori(0), ori(1), ori(2), ori(3); - Eigen::Isometry3d bTc = Eigen::Translation3d(cam_extrinsics_.head(3)) * Eigen::Quaternions(cam_extrinsics_.segment<4>(3).data()); - - auto sensor_cam_ptr_ = std::dynamic_pointer_cast(sensor_ptr_); - auto cam_kvec_ = sensor_cam_ptr_->getIntrinsic()->getState(); // initialize pinhole camera intrinsics [cx cy fx fy] - auto cam_dvec_ = sensor_cam_ptr_->getDistortionVector(); // initialize pinhole camera intrinsics [cx cy fx fy] - - auto w_ = sensor_cam_ptr_->getImgWidth(); - auto h_ = sensor_cam_ptr_->getImgHeight(); - - for (auto lmk: problem_->getMap()->getLandmarkList()){ - LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); // IF all of the landmarks of this type - VectorComposite w_pose_lmk = lmk_april->getState(); - Eigen::Isometry3d wTl = Eigen::Translation3d(w_pose_lmk['P']) * Eigen::Quaternions(w_pose_lmk['O'].data()); - Eigen::Isometry3d cTl = (wTb * bTc).inverse() * wTl; - double s = lmk_april->getTagWidth()/2; // should not be very important -> proportional points in cam coord project at the same place... - cXp_vec[0] = cTl * (lXP1 * s); // if no parenthesis, not the same result -> why? - cXp_vec[1] = cTl * (lXP2 * s); - cXp_vec[2] = cTl * (lXP3 * s); - cXp_vec[3] = cTl * (lXP4 * s); - - - for (int i=0; i < cXp_vec.size(); i++){ - // drawCornerWithUncertainties(cXp_vec[i], cTl, (wTb * bTc).inverse(), frame); - ////////////////////////// - ////////////////////////// - // Draw corners with uncertainty - ////////////////////////// - - // Compute projected point and abort if not visible (Z>O and in the img boundaries) - if (!(cXp_vec[i][2] > 0)){ - return; - } - Eigen::Vector2d px; - Eigen::Matrix J_x_cXp; - pinhole::projectPoint(cam_kvec_, cam_dvec_, cXp_vec[i], px, J_x_cXp); - if (!pinhole::isInImage(px, w_, h_)){ - return; - } - - ///////////////////////// - // Draw projected estimated point projection - // std::cout << "--No Problem " << lmk_april->getTagId() << ": " << pcorn.transpose() << std::endl; - // visible_corners_vec.push_back() - cv::Point2d cv_corner; - cv_corner.x = px[0]; - cv_corner.y = px[1]; - cv::line(frame, cv_corner, cv_corner, CV_RGB(255,165,0), 4); - } - } + // /////////////////////////////// + // // Draw tag location from estimated keyframes + // // Eigen::VectorXd frame_state_bis = last_cap_cam_frame->getState(); // state estimation at the keyframe associated to the capture "last": late and jumping + // VectorComposite frame_state_ts = problem_->getState(img_msg->header.stamp.toSec()); // state estimation at the ts of the captured frame: should match exactly the measurements + // // Eigen::VectorXd current_state = problem_->getCurrentState(); // processor motion estimated state: in advance to the frame capture + // Eigen::Isometry3d wTb = Eigen::Translation3d(frame_state_ts['P']) * Eigen::Quaternions(frame_state_ts['O'].data()); + + // // Relative corner poses assuming an apriltag frame at the center of the tag with same oriention than the camera + // // when they face each other + // // bottom left to top left, direct order + // Eigen::Vector3d lXP1(-1, 1,0); + // Eigen::Vector3d lXP2( 1, 1,0); + // Eigen::Vector3d lXP3( 1,-1,0); + // Eigen::Vector3d lXP4(-1,-1,0); + // std::vector lXP_vec = {lXP1, lXP2, lXP3, lXP4}; + // std::vector cXp_vec; cXp_vec.resize(4); + + // auto pos = sensor_ptr_->getP()->getState(); + // auto ori = sensor_ptr_->getO()->getState(); + // Eigen::Vector7d cam_extrinsics_; + // cam_extrinsics_ << pos(0), pos(1), pos(2), ori(0), ori(1), ori(2), ori(3); + // Eigen::Isometry3d bTc = Eigen::Translation3d(cam_extrinsics_.head(3)) * Eigen::Quaternions(cam_extrinsics_.segment<4>(3).data()); + + // auto sensor_cam_ptr_ = std::dynamic_pointer_cast(sensor_ptr_); + // auto cam_kvec_ = sensor_cam_ptr_->getIntrinsic()->getState(); // initialize pinhole camera intrinsics [cx cy fx fy] + // auto cam_dvec_ = sensor_cam_ptr_->getDistortionVector(); // initialize pinhole camera intrinsics [cx cy fx fy] + + // auto w_ = sensor_cam_ptr_->getImgWidth(); + // auto h_ = sensor_cam_ptr_->getImgHeight(); + + // for (auto lmk: problem_->getMap()->getLandmarkList()){ + // LandmarkApriltagPtr lmk_april = std::static_pointer_cast(lmk); // IF all of the landmarks of this type + // VectorComposite w_pose_lmk = lmk_april->getState(); + // Eigen::Isometry3d wTl = Eigen::Translation3d(w_pose_lmk['P']) * Eigen::Quaternions(w_pose_lmk['O'].data()); + // Eigen::Isometry3d cTl = (wTb * bTc).inverse() * wTl; + // double s = lmk_april->getTagWidth()/2; // should not be very important -> proportional points in cam coord project at the same place... + // cXp_vec[0] = cTl * (lXP1 * s); // if no parenthesis, not the same result -> why? + // cXp_vec[1] = cTl * (lXP2 * s); + // cXp_vec[2] = cTl * (lXP3 * s); + // cXp_vec[3] = cTl * (lXP4 * s); + + + // for (int i=0; i < cXp_vec.size(); i++){ + // // drawCornerWithUncertainties(cXp_vec[i], cTl, (wTb * bTc).inverse(), frame); + // ////////////////////////// + // ////////////////////////// + // // Draw corners with uncertainty + // ////////////////////////// + + // // Compute projected point and abort if not visible (Z>O and in the img boundaries) + // if (!(cXp_vec[i][2] > 0)){ + // return; + // } + // Eigen::Vector2d px; + // Eigen::Matrix J_x_cXp; + // pinhole::projectPoint(cam_kvec_, cam_dvec_, cXp_vec[i], px, J_x_cXp); + // if (!pinhole::isInImage(px, w_, h_)){ + // return; + // } + + // ///////////////////////// + // // Draw projected estimated point projection + // // std::cout << "--No Problem " << lmk_april->getTagId() << ": " << pcorn.transpose() << std::endl; + // // visible_corners_vec.push_back() + // cv::Point2d cv_corner; + // cv_corner.x = px[0]; + // cv_corner.y = px[1]; + // cv::line(frame, cv_corner, cv_corner, CV_RGB(255,165,0), 4); + // } + // } cv_bridge::CvImage out_msg; out_msg.header = img_msg->header; diff --git a/src/subscriber_imu.cpp b/src/subscriber_imu.cpp deleted file mode 100644 index b406a97be536f0df6f4d3ce1058771bda4ac4f6b..0000000000000000000000000000000000000000 --- a/src/subscriber_imu.cpp +++ /dev/null @@ -1,134 +0,0 @@ -#include "subscriber_imu.h" -#include - -namespace wolf -{ -// Constructor -SubscriberImu::SubscriberImu(const std::string& _unique_name, - const ParamsServer& _server, - const SensorBasePtr _sensor_ptr) - : Subscriber(_unique_name, _server, _sensor_ptr) -{ - finding_orientation_prior_ = true; - i_imu_buff_ = 0; - size_buffer_imu_init_ = 100; - imu_time_initialized_ = false; - acc_sat_ = 3; - gyr_sat_ = 3; - ax_buff_ = std::vector(size_buffer_imu_init_); - ay_buff_ = std::vector(size_buffer_imu_init_); - az_buff_ = std::vector(size_buffer_imu_init_); -} - - -void SubscriberImu::initialize(ros::NodeHandle& nh, const std::string& topic) -{ - sub_ = nh.subscribe(topic, 100, &SubscriberImu::callback, this); -} - -void SubscriberImu::callback(const sensor_msgs::Imu &msg) -{ - auto sen_imu_ptr = std::static_pointer_cast(sensor_ptr_); - ////////////// - // INIT ORIENTATION - if (finding_orientation_prior_){ - if (i_imu_buff_ == size_buffer_imu_init_){ - double ax_mean = std::accumulate(ax_buff_.begin(), ax_buff_.end(), 0.0)/ax_buff_.size(); - double ay_mean = std::accumulate(ay_buff_.begin(), ay_buff_.end(), 0.0)/ay_buff_.size(); - double az_mean = std::accumulate(az_buff_.begin(), az_buff_.end(), 0.0)/az_buff_.size(); - - double ax_std = std::sqrt(std::inner_product(ax_buff_.begin(), ax_buff_.end(), ax_buff_.begin(), 0.0)/size_buffer_imu_init_ - ax_mean*ax_mean); - double ay_std = std::sqrt(std::inner_product(ay_buff_.begin(), ay_buff_.end(), ay_buff_.begin(), 0.0)/size_buffer_imu_init_ - ay_mean*ay_mean); - double az_std = std::sqrt(std::inner_product(az_buff_.begin(), az_buff_.end(), az_buff_.begin(), 0.0)/size_buffer_imu_init_ - az_mean*az_mean); - - i_imu_buff_ = 0; - - double min_acc_std = std::min({ax_std, ay_std, az_std}); - if (min_acc_std < std_imu_max_){ - std::cout << "min_acc_std < " << std_imu_max_ << ": " << min_acc_std << std::endl; - // Steady enough estimation - Eigen::Vector3d w_g_norm(0,0,1); - Eigen::Vector3d imu_g(ax_mean, ay_mean, az_mean); - imu_g.normalize(); - double ctheta = w_g_norm.dot(imu_g); - double theta = std::acos(ctheta); - Eigen::Vector3d v = imu_g.cross(w_g_norm); - // handle parallel vector case - if (v.norm() > 1e-6){ - Eigen::AngleAxisd aa(theta, v/v.norm()); - w_q_imu_init_ = Eigen::Quaterniond(aa); - } - else{ - if (ctheta > 0){ - w_q_imu_init_ = Eigen::Quaterniond(1,0,0,0); - } - else{ - w_q_imu_init_ = Eigen::Quaterniond(0,1,0,0); - } - } - // Could put zero rotation on Z "yaw" axis - // sol: q2e then yaw=0 then e2q, but not necessary - - finding_orientation_prior_ = false; - - std::cout << "Norm: " << w_q_imu_init_.norm() << "\nOrientation prior found: \n" << w_q_imu_init_.coeffs().conjugate() << std::endl; - } - else { - std::cout << "\nmin_acc_std not > " << std_imu_max_ << ": " << min_acc_std << std::endl; - std::cout << "Orientation prior not found" << std::endl; - } - - } - else { - ax_buff_[i_imu_buff_] = msg.linear_acceleration.x; - ay_buff_[i_imu_buff_] = msg.linear_acceleration.y; - az_buff_[i_imu_buff_] = msg.linear_acceleration.z; - i_imu_buff_++; - } - - - return; - } - ////////////// - - //Removed first image received check - if (!imu_time_initialized_){ - t0_imu_ = msg.header.stamp; //msg.header.stamp; - WOLF_TRACE("t0_imu_: ", t0_imu_); - imu_time_initialized_ = true; - } - // Removed priors set check - // WOLF_TRACE("msg.linear_acceleration: ", msg.linear_acceleration); - - // Process IMU - ts_imu_ = msg.header.stamp; - - Eigen::Vector6d acc_gyro_data; - Eigen::Vector3d acc; acc << msg.linear_acceleration.x, msg.linear_acceleration.y, msg.linear_acceleration.z; - Eigen::Vector3d ang_vel; ang_vel << msg.angular_velocity.x, msg.angular_velocity.y, msg.angular_velocity.z; - acc_gyro_data << acc, ang_vel; - - // std::cout << "\n\n --- acc_gyro_data: " << acc_gyro_data.transpose() << std::endl; - // std::cout << "ts: " << (ts_imu_ - t0_imu_).toSec() << std::endl; - - auto noise_cov = sen_imu_ptr->getNoiseCov(); - if ((fabs(acc(0)) > acc_sat_) || (fabs(acc(1)) > acc_sat_) || (fabs(acc(2)) >acc_sat_)){ - noise_cov.block(0,0,3,3) = noise_cov.block(0,0,3,3)*10000000; - } - if ((fabs(ang_vel(0)) > gyr_sat_) || (fabs(ang_vel(1)) > gyr_sat_) || (fabs(ang_vel(2)) > gyr_sat_)){ - noise_cov.block(3,3,3,3) = noise_cov.block(3,3,3,3)*10000000; - } - wolf::CaptureImuPtr capt_imu = std::make_shared((ts_imu_ - t0_imu_).toSec(), sen_imu_ptr, acc_gyro_data, - noise_cov, sen_imu_ptr->getIntrinsic()->getState(), - nullptr); - sen_imu_ptr->process(capt_imu); - - // auto fact = problem_->getTrajectory()->getLastKeyFrame()->getCaptureOf(sen_imu_ptr)->getFeatureList().front()->getFactorList().front(); - // auto fact_imu = std::static_pointer_cast(fact); - - // std::cout << "imu_callback ts: " << msg.header.stamp << ", bsize " << bsize << std::endl; - - // std::cout << "current_state_: " << current_state_.transpose() << std::endl; - -} -} diff --git a/yaml/apriltag_vio.yaml b/yaml/apriltag_vio.yaml deleted file mode 100644 index 08ba6add3fefec86a9bfa1f800e04b16cd7b2be1..0000000000000000000000000000000000000000 --- a/yaml/apriltag_vio.yaml +++ /dev/null @@ -1,110 +0,0 @@ -config: - - problem: - tree_manager: - follow: "treeconfig.yaml" - frame_structure: "POV" - # frame_structure: "PO" - dimension: 3 - prior: - mode: "fix" - $state: - P: [0,0,0] - O: [0,0,0,1] - V: [0,0,0] - $sigma: - P: [0.31, 0.31, 0.31] - O: [0.31, 0.31, 0.31] - V: [0.31, 0.31, 0.31] - time_tolerance: 0.1 - #mode: "factor" - #state: [0,0,0] - #cov: [[3,3],.1,0,0,0,.1,0,0,0,.1] - #time_tolerance: 0.1 - - solver: - max_num_iterations: 100 - verbose: 1 - period: 0.3 - update_immediately: false - - visualizer: - type: "Visualizer" - period: 5 - - sensors: - - - type: "SensorCamera" - name: "CAMERA" - plugin: "vision" - extrinsic: - # pose: [0.15,0.15,0] - # pose: [0.2,0.1, 1.57] - pose: [-0.0013,-0.092,0.02, 0,0,0,1] # compact setup (from kalibr) - # intrinsics: - follow: "camera_simu.yaml" - # follow: "camera_simu_wolf_standard.yaml" - - - type: "SensorImu" - name: "IMU" - plugin: "imu" - extrinsic: - # pose: [0.15,0.15,0] - # pose: [0.2,0.1, 1.57] - pose: [0,0,0, 0,0,0,1] - # intrinsics: - # follow: "imu_intrinsics.yaml" - follow: "imu_intrinsics.yaml" - processors: - - - type: "ProcessorTrackerLandmarkApriltag" - name: "APRILTAG PROC" - sensor_name: "CAMERA" - plugin: "apriltag" - time_tolerance: 0.04 - follow: "processor_tracker_landmark_apriltag.yaml" - - - type: "ProcessorImu" - name: "IMU PROC" - sensor_name: "IMU" - plugin: "imu" - apply_loss_function: true - follow: "processor_imu.yaml" - - ROS subscriber: - - - package: "wolf_ros_apriltag" - type: "SubscriberCamera" - topic: "/camera_simu/image_raw" - sensor_name: "CAMERA" - - - package: "wolf_ros_apriltag" - type: "SubscriberImu" - topic: "/imu/imu_raw" - sensor_name: "IMU" - - # ROS publisher: - # - - # package: "wolf_ros_apriltag" - # type: "PublisherCurrentState" - # topic: "/current_state" - # period: "5" - # - - # package: "wolf_ros_apriltag" - # type: "PublisherEstimatedPose" - # topic: "/estimated_pose" - # period: "5" - # - - # package: "wolf_ros_apriltag" - # type: "PublisherSolveLogs" - # topic: "/solve_logs" - # period: "5" - # - - # package: "wolf_ros_node" - # type: "PublisherPose" - # topic: "/wolf_pose" - # period: "5" - - follow: "params.yaml" - # follow: "config.yaml" -# raw_libs: "@raw_libs.yaml" diff --git a/yaml/camera_simu.yaml b/yaml/camera_simu.yaml deleted file mode 100644 index 4e62e69118183130d291f903d41dea4c675a270d..0000000000000000000000000000000000000000 --- a/yaml/camera_simu.yaml +++ /dev/null @@ -1,21 +0,0 @@ -width: 640 -height: 480 -camera_name: camera -camera_matrix: - rows: 3 - cols: 3 - data: [320, 0.000000, 320, 0.000000, 320, 240, 0.000000, 0.000000, 1.000000] -distortion_model: plumb_bob -distortion_coefficients: - rows: 1 - cols: 5 - data: [0.0, 0.0, 0.0, 0.0, 0.0] # tangential distortion not supported by wolf - # data: [-0.117928, 0.092265, -0.002119, -0.002615, 0.000000] -rectification_matrix: - rows: 3 - cols: 3 - data: [1.000000, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000, 0.000000, 0.000000, 1.000000] -projection_matrix: - rows: 3 - cols: 4 - data: [320, 0.000000, 320, 0.000000, 0.000000, 320, 240, 0.000000, 0.000000, 0.000000, 1.000000, 0.000000] diff --git a/yaml/camera_simu_wolf_standard.yaml b/yaml/camera_simu_wolf_standard.yaml deleted file mode 100644 index ec7d5f309cb1d7f106ee813178db315400c407cb..0000000000000000000000000000000000000000 --- a/yaml/camera_simu_wolf_standard.yaml +++ /dev/null @@ -1,5 +0,0 @@ -width: 640 -height: 480 -pinhole_model_raw: [320, 240, 320, 320] -pinhole_model_rectified: [320, 240, 320, 320] -distortion: [0, 0, 0, 0] diff --git a/yaml/imu_intrinsics.yaml b/yaml/imu_intrinsics.yaml deleted file mode 100644 index d5b1ef6ffac42cb0873ce6d99e0596dc1d5d8e71..0000000000000000000000000000000000000000 --- a/yaml/imu_intrinsics.yaml +++ /dev/null @@ -1,16 +0,0 @@ -type: "imu" # This must match the KEY used in the SensorFactory. Otherwise it is an error. -name: "IMU" # This is ignored. The name provided to the SensorFactory prevails -a_noise: 0.02 # standard deviation of Acceleration noise (same for all the axis) in m/s2 -w_noise: 0.03 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec -ab_initial_stdev: 0.00 # m/s2 - initial bias -wb_initial_stdev: 0.0 # rad/sec - initial bias -ab_rate_stdev: 0.0001 # m/s2/sqrt(s) -wb_rate_stdev: 0.0001 # rad/s/sqrt(s) - - # SIMU - # a_noise: 0.001 # standard deviation of Acceleration noise (same for all the axis) in m/s2 - # w_noise: 0.01 # standard deviation of Gyroscope noise (same for all the axis) in rad/sec - # ab_initial_stdev: 0.00 # m/s2 - initial bias - # wb_initial_stdev: 0.00 # rad/sec - initial bias - # ab_rate_stdev: 0.001 # m/s2/sqrt(s) - # wb_rate_stdev: 0.001 # rad/s/sqrt(s) diff --git a/yaml/params.yaml b/yaml/params.yaml deleted file mode 100644 index b04c2145c73387a2703e53898fec62b908510652..0000000000000000000000000000000000000000 --- a/yaml/params.yaml +++ /dev/null @@ -1,64 +0,0 @@ -apriltag_only: true # %% -# if the IMU is used, find a prior on the original orientation of the IMU based on measured gravity -finding_orientation_prior: false # %% -constrained_by_motion_factor_to_solve_activated: false - -# Map -# if true, uses a map with fixed landmarks -usemap: false -map_folder_path: /home/mfourmy/Documents/Phd_LAAS/tests/VIplots/test_apriltag_imu/ -map_name: map_111.000000_111.000000_5_2.yaml - - -# Display -use_debug_img: true -draw_detections: true -draw_estimations: false # to test again -draw_ellipses: false # not implemented -display_scaled_tf: 1 # to overcome the frame scale limitations of rviz, 1 to cancel - -# Topics -imu_raw_topic: /imu/imu_raw -current_frame: current - -# data flow parameters -solve_iter: 100 -max_active_KF: -1 # maximum number of active key frames, if -1, not taken into account -solve_every: 0.3 # run a factor graph solved every __ seconds -# solve_every: 5000 # !!deactivated -fast_solve_dur: 0 # solve the problem at a faster rate at the begging for __ seconds -solve_every_fast: 0.05 # run a factor graph solved every __ seconds at the beginning -broadcast_trajectory_every: 0.5 # broadcast the current trajectory and landmark estimation every __ -camera_ros_buffer_size: 1000 -imu_ros_buffer_size: 1000 - -# geometric parameters -imu_extrinsics: [0,0,0, 0,0,0,1] - -# cam_extrinsics: [0,0,0, 0,0,0,1] # simu $$ -cam_extrinsics: [-0.0013,-0.092,0.02, 0.702513,0.00349,0.0086,0.7116] # compact setup (from kalibr) -# cam_extrinsics: [0,0,0, 0.45,-0.55,-0.45,0.515] # New compact setup, rotation (from kalibr) -- WHAT? -# cam_extrinsics: [0.0168,-0.009,0.0277, 0,0,1,0] # microstrain + mvBlueFox3 (from Kalibr) $$ - -# sensors -shift_imu_minus_cam: 0.006 -acc_sat: 49 # m/s^2 -gyr_sat: 3.92 # rad/s - -# prior_vec: [0.0,0.0,0.0, 0,0,0,1, 0,0,0] # $$ -# prior_vec: [0.0,0.0,1.7, 1,0,0,0, 0,0,0] # $$ compact setup, lying on table -prior_vec: [0,0,0, 0.963449,-0.000121,-0.066758,0.259439, 0,0,0] # On the robot, IMU with Z down -# prior_vec: [0,0,0, 0,0.999982,0,0.00603841, 0,0,0] # $$ -# prior_vec: [0.0,0.5,1.1, 1,0,0,0, -0.189,0,0] # vx for Ax = 0.3 $$ -# prior_vec: [0.0,0,0.6, 0.5,0,0,0.8660254, 0,0,0] # vx for Ax = 0.3, tilted 181° on X $$ -# prior_vec: [0.0,0.6,1.1, 1,0,0,0, -0.18,0,0] # simu with everything $$ -# prior_vec: [0,0,0, 0.7071068,0,0,0.7071068, 0,0,0] # cam looking straight $$ -# prior_vec: [0,0,0, 0.7071068,0,0.7071068,0, 0,0,0] # cam +90° rotation around its Z axis $$ - -std_m: 0.001 -std_deg: 5 -std_vel: 0.001 - -# Orientation prior using IMU -size_buffer_imu_init: 20 -std_imu_max: 0.4 # if std (m/s^2) of the data recorded below a certain level, \ No newline at end of file diff --git a/yaml/processor_imu.yaml b/yaml/processor_imu.yaml deleted file mode 100644 index c3a83377454407186f931f635b57c9e7ce1c23a5..0000000000000000000000000000000000000000 --- a/yaml/processor_imu.yaml +++ /dev/null @@ -1,9 +0,0 @@ -time_tolerance: 0.004 # Time tolerance for joining KFs -unmeasured_perturbation_std: 0.00000001 -keyframe_vote: - voting_active: false - voting_aux_active: false - max_time_span: 1.0 # seconds - max_buff_length: 500 # motion deltas - dist_traveled: 2.0 # meters - angle_turned: 0.1 # radians (1 rad approx 57 deg, approx 60 deg) diff --git a/yaml/processor_tracker_landmark_apriltag.yaml b/yaml/processor_tracker_landmark_apriltag.yaml deleted file mode 100644 index 341c4fc6711352b891aa714b2c1d07574f6cce6c..0000000000000000000000000000000000000000 --- a/yaml/processor_tracker_landmark_apriltag.yaml +++ /dev/null @@ -1,90 +0,0 @@ -# type: "ProcessorTrackerLandmarkApriltag" -# name: "tracker landmark apriltag example" - -# detector parameters: -quad_decimate: 0.0 # doing quad detection at lower resolution to speed things up (see end of file) -quad_sigma: 0.0 # gaussian blur good for noisy images, may be recommended with quad_decimate. Kernel size adapted (see end of this file) -nthreads: 8 # how many thread during tag detection (does not change much?) -debug: false # write some debugging images -refine_edges: true # better edge detection if quad_decimate > 1 (quite inexpensive, almost no diff) -ippe_min_ratio: 0 # quite arbitrary, always > 1 (to deactive, set at something < 1) -ippe_max_rep_error: 100 # to deactivate, set at something big (100) - -$tag_widths: - -1: 0.0 - # 0: 0.055 - # 1: 0.055 - # 2: 0.055 - # 3: 0.055 - -tag_family: "tag36h11" # see in Wolf processor_tracker_landmark_apriltag.cpp for available families -tag_black_border: 1 # usually 1, 2 for Kalibr aprilgrid for ex -# tag_width_default: 0.165 # used if tag width not specified -- IRI $$ -tag_width_default: 0.2 # used if tag width not specified -- Simu $$ -# tag_width_default: 0.055 # used if tag width not specified -- Calibration $$ -# tag_width_default: 0.166 # used if tag width not specified -- LAAS (B181) $$ -# tag_width_default: 0.162 # used if tag width not specified -- LAAS, 04/07 $$ - -# noise: -std_xy : 0.1 # m -std_z : 0.1 # m -# std_rpy_degrees : 5 # degrees -std_rpy: 5 # degrees -std_pix: 4 # pixel error $$ -# std_pix: 10000000000000000 # pixel error -> "infinite" error $$ - -keyframe_vote: - voting_active: true - voting_aux_active: false -nb_vote_for_every_first: 1000000000 -min_time_vote: 0.1 # s -# Necessary conditions -max_time_vote: 0.5 # s -# One of the possibilities -enough_info_necessary: false # create kf if enough information to uniquely determine the KF position (recommended for apriltag_only slam) %% -min_features_for_keyframe: 1 # ! closely related to enough_info_necessary (see code) -max_features_diff: 4 - -reestimate_last_frame: false # for a better prior on the new keyframe: use only if no motion processor -add_3d_cstr: false # add 3D constraints between the KF so that they do not jump when using apriltag only - -max_new_features: -1 # max number of features detected in detectNewFeatures() (-1: unlimited) -apply_loss_function: true - -############### -# Apriltag settings -############### -# int nthreads; -# How many threads should be used? - -# float quad_decimate; -# detection of quads can be done on a lower-resolution image, -# improving speed at a cost of pose accuracy and a slight -# decrease in detection rate. Decoding the binary payload is -# still done at full resolution. . -# ! Special optimized value of 1.5 - -# float quad_sigma; -# What Gaussian blur should be applied to the segmented image -# (used for quad detection?) Parameter is the standard deviation -# in pixels. Very noisy images benefit from non-zero values -# (e.g. 0.8). - -# int refine_edges; -# When non-zero, the edges of the each quad are adjusted to "snap -# to" strong gradients nearby. This is useful when decimation is -# employed, as it can increase the quality of the initial quad -# estimate substantially. Generally recommended to be on (1). -# Very computationally inexpensive. Option is ignored if -# quad_decimate = 1. - -# double decode_sharpening; -# How much sharpening should be done to decoded images? This -# can help decode small tags but may or may not help in odd -# lighting conditions or low light conditions. -# The default value is 0.25. - -# int debug; -# When non-zero, write a variety of debugging images to the -# current working directory at various stages through the -# detection process. (Somewhat slow). diff --git a/yaml/simple.yaml b/yaml/simple.yaml deleted file mode 100644 index bbc45910e8e53db1d9ed5c5077434af8c0504117..0000000000000000000000000000000000000000 --- a/yaml/simple.yaml +++ /dev/null @@ -1,34 +0,0 @@ -config: - - problem: - tree_manager: - # follow: "treeconfig.yaml" - frame_structure: "PO" - dimension: 3 - prior: - mode: "fix" - state: [0,0,0] - time_tolerance: 0.1 - #mode: "factor" - #state: [0,0,0] - #cov: [[3,3],.1,0,0,0,.1,0,0,0,.1] - #time_tolerance: 0.1 - - solver: - max_num_iterations: 100 - verbose: 1 - period: 0.3 - - sensors: - processors: - - # ROS subscriber: - # - - # package: "wolf_ros_laser" - # type: "SubscriberLaser2d" - # topic: "/base_scan" - # sensor_name: "scanner_front_left" - - follow: "params.yaml" - follow: "config.yaml" -# raw_libs: "@raw_libs.yaml" diff --git a/yaml/treeconfig.yaml b/yaml/treeconfig.yaml deleted file mode 100644 index f3d9d1f6d60a70d4570f86334182315360618a48..0000000000000000000000000000000000000000 --- a/yaml/treeconfig.yaml +++ /dev/null @@ -1,4 +0,0 @@ -type: "TreeManagerSlidingWindow" -n_key_frames: -1 -fix_first_key_frame: false -viral_remove_empty_parent: true