├── .gitignore ├── README.md ├── ReachLib ├── .vscode │ ├── c_cpp_properties.json │ └── settings.json ├── CMakeLists.txt ├── doc │ └── Doxyfile ├── include │ ├── aabb.hpp │ ├── articulated.hpp │ ├── articulated_accel.hpp │ ├── articulated_combined.hpp │ ├── articulated_pos.hpp │ ├── articulated_vel.hpp │ ├── body_part.hpp │ ├── body_part_accel.hpp │ ├── body_part_combined.hpp │ ├── body_part_vel.hpp │ ├── capsule.hpp │ ├── cylinder.hpp │ ├── cylinder_perimeter.hpp │ ├── extremity │ ├── extremity.hpp │ ├── intersections.hpp │ ├── obstacle.hpp │ ├── occupancy.hpp │ ├── occupancy_container.hpp │ ├── pedestrian.hpp │ ├── pedestrian_accel.hpp │ ├── pedestrian_vel.hpp │ ├── point.hpp │ ├── reach_lib.hpp │ ├── sphere.hpp │ └── system.hpp ├── src │ ├── articulated.cpp │ ├── articulated_accel.cpp │ ├── articulated_combined.cpp │ ├── articulated_pos.cpp │ ├── articulated_vel.cpp │ ├── body_part.cpp │ ├── body_part_accel.cpp │ ├── body_part_combined.cpp │ ├── body_part_vel.cpp │ ├── capsule.cpp │ ├── cylinder.cpp │ ├── cylinder_perimeter.cpp │ ├── extremity.cpp │ ├── intersections.cpp │ ├── occupancy.cpp │ ├── pedestrian.cpp │ ├── pedestrian_accel.cpp │ ├── pedestrian_vel.cpp │ ├── point.cpp │ ├── sphere.cpp │ └── system.cpp └── tests │ ├── include │ └── body_part_combined_fixture.h │ └── src │ └── body_part_combined_tests.cc ├── executables ├── SaRA_demo └── reachable_occupancy ├── experiments ├── exp_appraoching.bag ├── exp_box.bag ├── exp_headbutt.bag └── exp_punch.bag ├── images ├── Acc_Ex.png ├── Cyl.PNG ├── f1.PNG ├── f1o.PNG ├── human_accel_vel.PNG └── human_pos.PNG └── reachable_occupancy ├── .vscode ├── c_cpp_properties.json ├── launch.json └── settings.json ├── CMakeLists.txt ├── config └── reachable_occupancy.yaml ├── include ├── ReachLib │ ├── .vscode │ │ ├── c_cpp_properties.json │ │ └── settings.json │ ├── CMakeLists.txt │ ├── build │ │ ├── CMakeCache.txt │ │ ├── CMakeFiles │ │ │ ├── 3.10.2 │ │ │ │ ├── CMakeCCompiler.cmake │ │ │ │ ├── CMakeCXXCompiler.cmake │ │ │ │ ├── CMakeDetermineCompilerABI_C.bin │ │ │ │ ├── CMakeDetermineCompilerABI_CXX.bin │ │ │ │ ├── CMakeSystem.cmake │ │ │ │ ├── CompilerIdC │ │ │ │ │ ├── CMakeCCompilerId.c │ │ │ │ │ └── a.out │ │ │ │ └── CompilerIdCXX │ │ │ │ │ ├── CMakeCXXCompilerId.cpp │ │ │ │ │ └── a.out │ │ │ ├── CMakeDirectoryInformation.cmake │ │ │ ├── CMakeOutput.log │ │ │ ├── Makefile.cmake │ │ │ ├── Makefile2 │ │ │ ├── SaRA.dir │ │ │ │ ├── CXX.includecache │ │ │ │ ├── DependInfo.cmake │ │ │ │ ├── build.make │ │ │ │ ├── cmake_clean.cmake │ │ │ │ ├── cmake_clean_target.cmake │ │ │ │ ├── depend.internal │ │ │ │ ├── depend.make │ │ │ │ ├── flags.make │ │ │ │ ├── link.txt │ │ │ │ ├── progress.make │ │ │ │ └── src │ │ │ │ │ ├── articulated.cpp.o │ │ │ │ │ ├── articulated_accel.cpp.o │ │ │ │ │ ├── articulated_pos.cpp.o │ │ │ │ │ ├── articulated_vel.cpp.o │ │ │ │ │ ├── body_part.cpp.o │ │ │ │ │ ├── body_part_accel.cpp.o │ │ │ │ │ ├── body_part_vel.cpp.o │ │ │ │ │ ├── capsule.cpp.o │ │ │ │ │ ├── cylinder.cpp.o │ │ │ │ │ ├── cylinder_perimeter.cpp.o │ │ │ │ │ ├── extremity.cpp.o │ │ │ │ │ ├── occupancy.cpp.o │ │ │ │ │ ├── pedestrian.cpp.o │ │ │ │ │ ├── pedestrian_accel.cpp.o │ │ │ │ │ ├── pedestrian_vel.cpp.o │ │ │ │ │ ├── point.cpp.o │ │ │ │ │ ├── sphere.cpp.o │ │ │ │ │ └── system.cpp.o │ │ │ ├── TargetDirectories.txt │ │ │ ├── cmake.check_cache │ │ │ ├── feature_tests.bin │ │ │ ├── feature_tests.c │ │ │ ├── feature_tests.cxx │ │ │ └── progress.marks │ │ ├── Makefile │ │ ├── cmake_install.cmake │ │ └── libSaRA.a │ ├── doc │ │ └── Doxyfile │ ├── include │ │ ├── articulated.hpp │ │ ├── articulated_accel.hpp │ │ ├── articulated_pos.hpp │ │ ├── articulated_vel.hpp │ │ ├── body_part.hpp │ │ ├── body_part_accel.hpp │ │ ├── body_part_vel.hpp │ │ ├── capsule.hpp │ │ ├── cylinder.hpp │ │ ├── cylinder_perimeter.hpp │ │ ├── extremity │ │ ├── extremity.hpp │ │ ├── intersections.hpp │ │ ├── obstacle.hpp │ │ ├── occupancy.hpp │ │ ├── occupancy_container.hpp │ │ ├── pedestrian.hpp │ │ ├── pedestrian_accel.hpp │ │ ├── pedestrian_vel.hpp │ │ ├── point.hpp │ │ ├── reach_lib.hpp │ │ ├── sphere.hpp │ │ └── system.hpp │ └── src │ │ ├── articulated.cpp │ │ ├── articulated_accel.cpp │ │ ├── articulated_pos.cpp │ │ ├── articulated_vel.cpp │ │ ├── body_part.cpp │ │ ├── body_part_accel.cpp │ │ ├── body_part_vel.cpp │ │ ├── capsule.cpp │ │ ├── cylinder.cpp │ │ ├── cylinder_perimeter.cpp │ │ ├── extremity.cpp │ │ ├── intersections.cpp │ │ ├── occupancy.cpp │ │ ├── pedestrian.cpp │ │ ├── pedestrian_accel.cpp │ │ ├── pedestrian_vel.cpp │ │ ├── point.cpp │ │ ├── sphere.cpp │ │ └── system.cpp └── reachable_occupancy │ ├── udp_transiever.hpp │ ├── validation.hpp │ ├── visualizer.hpp │ └── volume.hpp ├── launch ├── SaRA_demo.launch ├── reachable_occupancy.launch ├── udp.launch └── udp_translation.launch ├── msg ├── Capsule.msg ├── CapsuleArray.msg ├── Cylinder.msg ├── CylinderArray.msg ├── JointLocations.msg ├── ReachableOccupancy.msg └── time.msg ├── package.xml ├── rviz ├── cmu_demo_for_images.rviz ├── meshes │ ├── cofee_table.stl │ ├── coffee_table.dae │ ├── door.stl │ ├── room.dae │ └── table.dae ├── ro_config_basic.rviz ├── ro_config_no-angle.rviz └── ro_rl_demo.rviz └── src ├── __init__.py ├── reachable_occupancy.cpp ├── reachable_occupancy_no_udp.cpp ├── tests └── simple_skeleton_mvmt.csv ├── udp_transiever.cpp ├── udp_translator.cpp ├── validation.cpp ├── visualizer.cpp └── volume.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | .vscode/ 3 | -------------------------------------------------------------------------------- /ReachLib/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "", 6 | "limitSymbolsToIncludedHeaders": true 7 | }, 8 | "includePath": [ 9 | "/home/sven/catkin_ws/devel/include/**", 10 | "/opt/ros/melodic/include/**", 11 | "/home/sven/catkin_ws/src/bot_generator/include/**", 12 | "/home/sven/catkin_ws/src/maze_generator/include/**", 13 | "/home/sven/catkin_ws/src/reach_ri/include/**", 14 | "/home/sven/catkin_ws/src/reachable_occupancy/include/**", 15 | "/usr/include/**", 16 | "/home/sven/Desktop/Reach-RI/spot-ro/reach_lib/include/**" 17 | ], 18 | "name": "ROS" 19 | } 20 | ], 21 | "version": 4 22 | } -------------------------------------------------------------------------------- /ReachLib/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/sven/catkin_ws/devel/lib/python2.7/dist-packages", 4 | "/opt/ros/melodic/lib/python2.7/dist-packages" 5 | ], 6 | "files.associations": { 7 | "cctype": "cpp", 8 | "clocale": "cpp", 9 | "cmath": "cpp", 10 | "csignal": "cpp", 11 | "cstdarg": "cpp", 12 | "cstddef": "cpp", 13 | "cstdio": "cpp", 14 | "cstdlib": "cpp", 15 | "cstring": "cpp", 16 | "ctime": "cpp", 17 | "cwchar": "cpp", 18 | "cwctype": "cpp", 19 | "array": "cpp", 20 | "atomic": "cpp", 21 | "strstream": "cpp", 22 | "*.tcc": "cpp", 23 | "bitset": "cpp", 24 | "chrono": "cpp", 25 | "cinttypes": "cpp", 26 | "complex": "cpp", 27 | "condition_variable": "cpp", 28 | "cstdint": "cpp", 29 | "deque": "cpp", 30 | "list": "cpp", 31 | "unordered_map": "cpp", 32 | "unordered_set": "cpp", 33 | "vector": "cpp", 34 | "exception": "cpp", 35 | "hash_map": "cpp", 36 | "hash_set": "cpp", 37 | "fstream": "cpp", 38 | "functional": "cpp", 39 | "future": "cpp", 40 | "initializer_list": "cpp", 41 | "iomanip": "cpp", 42 | "iosfwd": "cpp", 43 | "iostream": "cpp", 44 | "istream": "cpp", 45 | "limits": "cpp", 46 | "mutex": "cpp", 47 | "new": "cpp", 48 | "ostream": "cpp", 49 | "numeric": "cpp", 50 | "ratio": "cpp", 51 | "sstream": "cpp", 52 | "stdexcept": "cpp", 53 | "streambuf": "cpp", 54 | "system_error": "cpp", 55 | "thread": "cpp", 56 | "cfenv": "cpp", 57 | "tuple": "cpp", 58 | "type_traits": "cpp", 59 | "utility": "cpp", 60 | "typeindex": "cpp", 61 | "typeinfo": "cpp", 62 | "valarray": "cpp", 63 | "codecvt": "cpp", 64 | "algorithm": "cpp", 65 | "iterator": "cpp", 66 | "map": "cpp", 67 | "memory": "cpp", 68 | "memory_resource": "cpp", 69 | "optional": "cpp", 70 | "random": "cpp", 71 | "set": "cpp", 72 | "string": "cpp", 73 | "string_view": "cpp" 74 | } 75 | } -------------------------------------------------------------------------------- /ReachLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.9) 2 | project(SaRA) 3 | set(CMAKE_BUILD_TYPE "Release") 4 | 5 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 6 | message(STATUS "Setting build type to '${DEFAULT_BUILD_TYPE}' as none was specified.") 7 | set(CMAKE_BUILD_TYPE "${DEFAULT_BUILD_TYPE}" CACHE STRING "Choose the type of build." FORCE) 8 | # Set the possible values of build type for cmake-gui 9 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 10 | endif() 11 | 12 | include(GNUInstallDirs) 13 | 14 | #Bring the headers, such as Student.h into the project 15 | #include_directories(include) 16 | 17 | #However, the file(GLOB...) allows for wildcard additions: 18 | file(GLOB SOURCES "src/*.cpp") 19 | #set(SOURCES src/Point.cpp src/Capsule.cpp) 20 | 21 | #Generate the shared library from the sources [SHARED / STATIC?] 22 | add_library(${PROJECT_NAME} SHARED ${SOURCES}) 23 | 24 | target_include_directories(${PROJECT_NAME} PUBLIC 25 | $ 26 | $ 27 | )#PRIVATE src) 28 | 29 | set_target_properties(${PROJECT_NAME} PROPERTIES 30 | VERSION 1.0.0 31 | SOVERSION 1) 32 | 33 | install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Config 34 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 35 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 36 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 37 | 38 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}) 39 | 40 | install(EXPORT ${PROJECT_NAME}Config DESTINATION share/${PROJECT_NAME}/cmake) 41 | 42 | export(TARGETS ${PROJECT_NAME} FILE ${PROJECT_NAME}Config.cmake) 43 | 44 | ############# 45 | ## GTest ## 46 | ############# 47 | enable_testing() 48 | find_package(GTest REQUIRED) 49 | include(GoogleTest) 50 | 51 | add_executable(body_part_combined_tests tests/src/body_part_combined_tests.cc) 52 | target_include_directories(body_part_combined_tests PUBLIC tests/include) 53 | target_link_libraries(body_part_combined_tests ${PROJECT_NAME} GTest::GTest GTest::Main) 54 | gtest_discover_tests(body_part_combined_tests) 55 | 56 | #set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER include/reach_lib.hpp) 57 | 58 | #target_include_directories(${PROJECT_NAME} PRIVATE include) 59 | #target_include_directories(${PROJECT_NAME} PRIVATE src) 60 | 61 | #Set the location for library installation -- i.e., /usr/lib in this case 62 | # not really necessary in this example. Use "sudo make install" to apply 63 | #include(GNUInstallDirs) 64 | #install(TARGETS ${PROJECT_NAME} 65 | # LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 66 | # PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) -------------------------------------------------------------------------------- /ReachLib/include/aabb.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder.hpp" 19 | #include "occupancy_container.hpp" 20 | #include "point.hpp" 21 | #include "sphere.hpp" 22 | #include "capsule.hpp" 23 | 24 | #ifndef REACH_LIB_INCLUDE_AABB_HPP_ 25 | #define REACH_LIB_INCLUDE_AABB_HPP_ 26 | 27 | namespace occupancy_containers { 28 | namespace aabb { 29 | 30 | //! This class defines the axis-aligned bounding box occupancy medium 31 | //! It is given as a box defined by its limits in x, y, and z direction. 32 | class AABB : public OccupancyContainer { 33 | private: 34 | 35 | public: 36 | //! \brief Minimum in x, y, and z direction 37 | std::vector min_ = {0.0, 0.0, 0.0}; 38 | 39 | //! \brief Maximum in x, y, and z direction 40 | std::vector max_ = {0.0, 0.0, 0.0}; 41 | 42 | //! \brief Empty constructor 43 | AABB() : OccupancyContainer() {} 44 | 45 | //! \brief Constructs an AABB from given limits 46 | //! \param[in] min Minimum in x, y, and z direction 47 | //! \param[in] max Maximum in x, y, and z direction 48 | AABB(const std::vector& min, const std::vector& max) { 49 | min_ = min; 50 | max_ = max; 51 | } 52 | 53 | //! \brief Static cast constructor from AABB to OccupancyModels 54 | explicit AABB(const OccupancyContainer& o) { 55 | // TODO 56 | } 57 | 58 | //! \brief Empty destructor 59 | ~AABB() {} 60 | 61 | //! \brief Returns if a point is inside the AABB 62 | //! \param[in] target Point of interest in Cartesian (x, y, z). 63 | //! \return True if the point is inside the AABB, false otherwise. 64 | inline bool intersection(const Point& target) { 65 | return (min_[0] <= target.x && target.x <= max_[0] && 66 | min_[1] <= target.y && target.y <= max_[1] && 67 | min_[2] <= target.z && target.z <= max_[2]); 68 | } 69 | 70 | //! \brief Determines whether the AABB intersects with 71 | //! any point in points. 72 | //! \param[in] targets List of points of interest in Cartesian (x, y, z). 73 | //! \return True if any point is inside the AABB, false otherwise. 74 | inline bool intersection(const std::vector& targets) { 75 | for (const auto& target : targets) { 76 | if (intersection(target)) { 77 | return true; 78 | } 79 | } 80 | return false; 81 | } 82 | 83 | }; 84 | } // namespace aabb 85 | } // namespace occupancy_containers 86 | #endif // REACH_LIB_INCLUDE_AABB_HPP_ 87 | -------------------------------------------------------------------------------- /ReachLib/include/body_part.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "capsule.hpp" 19 | #include "occupancy.hpp" 20 | #include "point.hpp" 21 | 22 | #ifndef REACH_LIB_INCLUDE_BODY_PART_HPP_ 23 | #define REACH_LIB_INCLUDE_BODY_PART_HPP_ 24 | 25 | namespace occupancies { 26 | namespace body_parts { 27 | 28 | //! \typedef A shortcut to the Capsule type 29 | typedef occupancy_containers::capsule::Capsule Capsule; 30 | 31 | /*! This class defines the human as a construct 32 | * of body parts which describe their own occupancy. 33 | * A body part is defined by two joints and its thickness (diameter). 34 | * Body parts are used for the maximum velocity 35 | * and acceleration based approaches 36 | * and thus produce by two versions of occupancies: 37 | * 1. ArticulatedVel: Occupancy based on reachable space with constant velocity 38 | * 2. BodyPartAccel: Occuapancy based on live velocity measurements 39 | * and constant maximum acceleration 40 | */ 41 | class BodyPart : public Occupancy { 42 | public: 43 | //! \brief Empty constructor 44 | BodyPart(); 45 | 46 | //! \brief Instantiates a general body part 47 | //! \param[in] name Name of the body part 48 | //! \param[in] thickness Estimated thickness (diameter) of the body part 49 | BodyPart(std::string name, double thickness); 50 | 51 | //! \brief Empty destructor 52 | ~BodyPart() {} 53 | 54 | //! \brief Returns the current occupancy 55 | inline Capsule get_occupancy() const { 56 | return occupancy_; 57 | } 58 | 59 | protected: 60 | //! \brief Contains the current occupancy 61 | Capsule occupancy_ = Capsule(); 62 | }; 63 | } // namespace body_parts 64 | } // namespace occupancies 65 | #endif // REACH_LIB_INCLUDE_BODY_PART_HPP_ 66 | -------------------------------------------------------------------------------- /ReachLib/include/body_part_accel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "articulated.hpp" 19 | #include "body_part.hpp" 20 | #include "capsule.hpp" 21 | #include "occupancy.hpp" 22 | #include "point.hpp" 23 | 24 | #ifndef REACH_LIB_INCLUDE_BODY_PART_ACCEL_HPP_ 25 | #define REACH_LIB_INCLUDE_BODY_PART_ACCEL_HPP_ 26 | 27 | namespace occupancies{ 28 | namespace body_parts { 29 | namespace accel { 30 | 31 | class BodyPartAccel : public BodyPart { 32 | public: 33 | //! \brief Empty constructor 34 | BodyPartAccel() : BodyPart() {} 35 | 36 | //! \brief Instatiates a BodyPart that updates based on live velocities 37 | //! and maximum estimated acceleration. 38 | //! \param[in] name Name of the body part 39 | //! \param[in] thickness Estimated thickness (diameter) of the body part 40 | //! \param[in] max_a1, max_a2 Estimated maximum acceleration of this body part 41 | BodyPartAccel(std::string name, double thickness, double max_a1 = 0.0, double max_a2 = 0.0); 42 | 43 | //! \brief Empty destructor 44 | ~BodyPartAccel() {} 45 | 46 | //! \brief Calcualtes the current occupancy using the Articuated model. 47 | //! Overrides the identical blueprint method from Occupancy. 48 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) size(p) = 2. 49 | //! \param[in] v Current joint velocity size(v) = 2. 50 | //! \param[in] t_a Start of the interval of analysis. 51 | //! \param[in] t_b End of the interval of analysis. 52 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m]. 53 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s]. 54 | //! \param[in] delay Time delay within the executing system. 55 | void update(const std::vector& p, 56 | const std::vector& v = {}, 57 | double t_a = 0.0, double t_b = 0.0, 58 | double measurement_error_pos = 0.0, 59 | double measurement_error_vel = 0.0, 60 | double delay = 0.0); 61 | 62 | //! \brief Determines if any point from the list 'targets' is located inside 63 | //! the current occupancy. 64 | bool intersection(std::vector targets) const; 65 | 66 | protected: 67 | //! \brief Maximum estimated acceleration of the proximal joint (constant) 68 | double max_a1_ = 0.0; 69 | 70 | //! \brief Maximum estimated acceleration of the distal joint (constant) 71 | double max_a2_ = 0.0; 72 | 73 | //! \brief Determines the ball occupancy within [0.0, t_b] for one joint 74 | //! This needs to be performed for both joints before enclosing the occupacnies 75 | //! in one capsule during the updated (carried out in update()). 76 | //! \param[in] p Origin of the ball (Cartesian position of the joint) 77 | //! \param[in] v Velocity of the joint (Cartesian) 78 | //! \param[in] index Determines the joint of which ry is calculated; -> from [1,2]. 79 | //! \param[in] t_b End point of the time interval 80 | //! \param[in] delay Time delay within the executing system 81 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 82 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s] 83 | Capsule ry(const Point& p, const Point& v, 84 | int index, double t_b = 0.02, double delay = 0.0, 85 | double measurement_error_pos = 0.0, 86 | double measurement_error_vel = 0.0) const; 87 | }; 88 | } // namespace accel 89 | } // namespace body_parts 90 | } // namespace occupancies 91 | #endif // REACH_LIB_INCLUDE_BODY_PART_ACCEL_HPP_ 92 | -------------------------------------------------------------------------------- /ReachLib/include/body_part_vel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "body_part.hpp" 19 | #include "capsule.hpp" 20 | #include "occupancy.hpp" 21 | #include "point.hpp" 22 | 23 | #ifndef REACH_LIB_INCLUDE_BODY_PART_VEL_HPP_ 24 | #define REACH_LIB_INCLUDE_BODY_PART_VEL_HPP_ 25 | 26 | namespace occupancies { 27 | namespace body_parts { 28 | namespace vel { 29 | 30 | class BodyPartVel : public BodyPart { 31 | public: 32 | //! \brief Empty constructor 33 | BodyPartVel() : BodyPart() {} 34 | 35 | //! \brief Instatiates a BodyPart that updates based on 36 | //! maximum estimated velocities. 37 | //! \param[in] name Name of the body part 38 | //! \param[in] thickness Estimated thickness (diameter) of the body part 39 | //! \param[in] max_v Estimated maximum velocity of this body part 40 | BodyPartVel(std::string name, double thickness, double max_v1 = 0.0, double max_v2 = 0.0); 41 | 42 | //! \brief Empty destructor 43 | ~BodyPartVel() {} 44 | 45 | //! \brief Calcualtes the current occupancy using any model 46 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) 47 | //! \param[in] v Unused overriden parameter 48 | //! \param[in] t_a Start of the interval of analysis 49 | //! \param[in] t_b End of the interval of analysis 50 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 51 | //! \param[in] measurement_error_vel Unused overriden parameter 52 | //! \param[in] delay Time delay within the executing system 53 | void update(const std::vector& p, 54 | const std::vector& v = {}, 55 | double t_a = 0.0, double t_b = 0.0, 56 | double measurement_error_pos = 0.0, 57 | double measurement_error_vel = 0.0, 58 | double delay = 0.0); 59 | 60 | //! \brief Determines if any point from the list 'targets' is located inside 61 | //! the current occupancy. 62 | bool intersection(std::vector targets) const; 63 | 64 | protected: 65 | //! \brief Maximum estimated velocity of the proximal joint (constant) 66 | double max_v1_ = 0.0; 67 | 68 | //! \brief Maximum estimated velocity of the distal joint (constant) 69 | double max_v2_ = 0.0; 70 | 71 | //! \brief Determines the ball occupancy within [0.0, t_end] for one joint 72 | //! This needs to be performed for both joints before enclosing the occupacnies 73 | //! in one capsule during the updated (carried out in update()). 74 | //! \param[in] p Origin of the ball 75 | //! \param[in] index Indicates the proximal (1) or distal (2) link 76 | //! \param[in] t_end End point of the time interval 77 | //! \param[in] delay Time delay within the executing system 78 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 79 | Capsule ry(const Point& p, int index, double t_end = 0.02, 80 | double delay = 0.0, double measurement_error_pos = 0.0) const; 81 | }; 82 | } // namespace vel 83 | } // namespace body_parts 84 | } // namespace occupancies 85 | #endif // REACH_LIB_INCLUDE_BODY_PART_VEL_HPP_ 86 | -------------------------------------------------------------------------------- /ReachLib/include/cylinder.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "occupancy_container.hpp" 18 | #include "point.hpp" 19 | 20 | #ifndef REACH_LIB_INCLUDE_CYLINDER_HPP_ 21 | #define REACH_LIB_INCLUDE_CYLINDER_HPP_ 22 | 23 | namespace occupancy_containers { 24 | namespace cylinder { 25 | //! This class defines Cylinder based occupancies 26 | //! used in the pedestrian model defiend in: 27 | //! http://mediatum.ub.tum.de/doc/1379662/905746.pdf 28 | //! and static velocity based perimeters 29 | class Cylinder : public OccupancyContainer { 30 | public: 31 | //! \brief Radius of the cylinder 32 | double r_ = 0.0; 33 | 34 | //! \brief Center of the cylinders initial top circle 35 | Point p1_ = Point(); 36 | 37 | //! \brief Center of the cylinders initial bottom circle 38 | Point p2_ = Point(); 39 | 40 | //! \brief Empty constructor 41 | Cylinder() {} 42 | 43 | //! \brief Instantiates a cylinder occupancy 44 | //! \param[in] p A list of two points serving as 45 | //! top and bottom indicators for the cylinder 46 | //! \param[in] r Radius of the cylinder 47 | Cylinder(const Point& p1, const Point& p2, double r); 48 | 49 | //! \brief Empty destructor 50 | ~Cylinder() {} 51 | 52 | //! \brief Determines whether there exists an intersection between 53 | //! a cylinder and any point within targets. 54 | //! \param[in] targets List of points of interes in Cartesian (x y z) 55 | bool intersection(const std::vector& targets) const; 56 | 57 | //! \brief Determines whether this cylinder and a capsule c intersect. 58 | //! \param[in] c A Capsule object checked against this cylinder 59 | //! -> We can't allow circular inclusion 60 | // bool intersection(const Capsule& c); 61 | }; 62 | } // namespace cylinder 63 | } // namespace occupancy_containers 64 | #endif // REACH_LIB_INCLUDE_CYLINDER_HPP_ 65 | -------------------------------------------------------------------------------- /ReachLib/include/cylinder_perimeter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder.hpp" 19 | #include "occupancy.hpp" 20 | #include "point.hpp" 21 | 22 | #ifndef REACH_LIB_INCLUDE_CYLINDER_PERIMETER_HPP_ 23 | #define REACH_LIB_INCLUDE_CYLINDER_PERIMETER_HPP_ 24 | 25 | namespace occupancies { 26 | namespace cylinder_perimeters { 27 | //! \typedef Defines the Capsule datatype within this namespace 28 | typedef occupancy_containers::cylinder::Cylinder Cylinder; 29 | 30 | 31 | //! \brief Defines the occupancy model of the pedestrian approach 32 | //! from http://mediatum.ub.tum.de/doc/1379662/905746.pdf. 33 | //! Contains a Cylinder for the occupancy of the desired interval [t_start, t_end]. 34 | //! Contains a list of cylinder sub occupancies for intervals from within [0.0, t_end]. 35 | class CylinderPerimeter : public Occupancy { 36 | public: 37 | //! \brief Empty constructor 38 | CylinderPerimeter(); 39 | 40 | //! \brief Instatiates a list of cylinders that 41 | //! can be used to define occupancies of 42 | //! the pedestrian model from: 43 | //! http://mediatum.ub.tum.de/doc/1379662/905746.pdf 44 | //! \param[in] init_pos Initial position of the cylinder 45 | //! \param[in] height Estimated height of the human 46 | CylinderPerimeter(const Point& init_pos, double height, double radius, std::string name = ""); 47 | 48 | //! \brief Emtpy destructor 49 | ~CylinderPerimeter() {} 50 | 51 | //! Determines if the cylinder intersects with 52 | //! any point from within the targest list 53 | //! \param[in] Points of interest for inertsections 54 | bool intersection(std::vector targets) const; 55 | 56 | //! \brief Calcualtes the current occupancy using the Pedestrian model 57 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) 58 | //! \param[in] v Current joint velocity 59 | //! \param[in] t_a Start of the interval of analysis 60 | //! \param[in] t_b End of the interval of analysis 61 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 62 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s] 63 | //! \param[in] delay Time delay within the executing system 64 | void update(const std::vector& p, 65 | const std::vector& v = {}, 66 | double t_a = 0.0, double t_b = 0.0, 67 | double measurement_error_pos = 0.0, 68 | double measurement_error_vel = 0.0, 69 | double delay = 0.0) { 70 | throw "Function occupancies::cylinder_perimeters::CylinderPerimeter::update is not defined!"; 71 | } 72 | 73 | //! \brief Returns the current occupancy 74 | inline Cylinder get_occupancy() const { 75 | return this->occupancy_; 76 | } 77 | 78 | //! \brief Returns the list of cylindrical partial occupancies 79 | inline std::vector get_cylinder_list() const { 80 | return this->cylinder_list_; 81 | } 82 | 83 | private: 84 | //! \brief Contains the current occupancy 85 | Cylinder occupancy_ = Cylinder(); 86 | 87 | //! \brief The desired occupancy interval split up into smaller intervals 88 | //! represented by a list of cylinders 89 | std::vector cylinder_list_ = {}; 90 | }; 91 | } // namespace cylinder_perimeters 92 | } // namespace occupancies 93 | #endif // REACH_LIB_INCLUDE_CYLINDER_PERIMETER_HPP_ 94 | -------------------------------------------------------------------------------- /ReachLib/include/extremity: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/ReachLib/include/extremity -------------------------------------------------------------------------------- /ReachLib/include/extremity.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "capsule.hpp" 19 | #include "occupancy.hpp" 20 | #include "point.hpp" 21 | 22 | #ifndef REACH_LIB_INCLUDE_EXTREMITY_HPP_ 23 | #define REACH_LIB_INCLUDE_EXTREMITY_HPP_ 24 | 25 | namespace occupancies { 26 | namespace extremities { 27 | 28 | //! \typedef A shortcut to the Capsule type 29 | typedef occupancy_containers::capsule::Capsule Capsule; 30 | 31 | 32 | /*! This class defines the human as a construct 33 | * of extremities which describe their own occupancy. 34 | * An extreity is defined by a joint, its thickness (diameter), and length. 35 | * extremities are used for the maximum position based approache. 36 | * They describe a sphere of constant radius about their joint. 37 | */ 38 | class Extremity : public Occupancy { 39 | public: 40 | //! \brief Empty constructor 41 | Extremity(); 42 | 43 | //! \brief Instantiates an extremity 44 | Extremity(std::string name, double thickness, 45 | double length, double max_v); 46 | 47 | //! \brief Empty destructor 48 | ~Extremity() {} 49 | 50 | 51 | //! \brief Calcualtes the current occupancy using the ArticulatedPos model 52 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) 53 | //! \param[in] v Current joint velocity 54 | //! \param[in] t_a Start of the interval of analysis 55 | //! \param[in] t_b End of the interval of analysis 56 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 57 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s] 58 | //! \param[in] delay Time delay within the executing system 59 | void update(const std::vector& p, 60 | const std::vector& v = {}, 61 | double t_a = 0.0, double t_b = 0.0, 62 | double measurement_error_pos = 0.0, 63 | double measurement_error_vel = 0.0, 64 | double delay = 0.0); 65 | 66 | //! \brief Returns true if the current occupancy intersects with 67 | //! any given point in 'targets' 68 | //! \param[in] targets A list of points in global Cartesian coordinates (x, y, z) 69 | //! checked against the current occupancy 70 | bool intersection(std::vector targets) const; 71 | 72 | //! \brief Returns the current occupancy 73 | inline Capsule get_occupancy() const { 74 | return occupancy_; 75 | } 76 | 77 | //! \brief Returns the length of the extremity 78 | double get_length() const { 79 | return length_; 80 | } 81 | 82 | private: 83 | //! \brief Conains the current occupancy 84 | Capsule occupancy_ = Capsule(); 85 | 86 | //! \brief Length of the extremity 87 | double length_ = 0.0; 88 | 89 | //! \brief Maximum assumed velocity of the extremity 90 | double max_v_ = 0.0; 91 | }; 92 | } // namespace extremities 93 | } // namespace occupancies 94 | #endif // REACH_LIB_INCLUDE_EXTREMITY_HPP_ 95 | -------------------------------------------------------------------------------- /ReachLib/include/obstacle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "occupancy.hpp" 18 | #include "point.hpp" 19 | #include "system.hpp" 20 | 21 | #ifndef REACH_LIB_INCLUDE_SAFETY_PERIMETER_HPP_ 22 | #define REACH_LIB_INCLUDE_SAFETY_PERIMETER_HPP_ 23 | 24 | 25 | namespace obstacles { 26 | 27 | //! \typedef A shortcut to the Point class 28 | typedef point::Point Point; 29 | 30 | //! \typedef A shortcut to the System class 31 | typedef systems::System System; 32 | 33 | //! This class serves as a blueprint for all 34 | //! safety perimeter definitions. 35 | //! This presently includes Articulated and Pedestrian models. 36 | class Obstacle { 37 | public: 38 | //! \brief Empty constructor 39 | Obstacle() {} 40 | 41 | //! \brief Initializes the sensor attribute containing 42 | //! system parameters. 43 | explicit Obstacle(System sensor) : system(sensor) {} 44 | 45 | //! \brief Empty destructor 46 | ~Obstacle() {} 47 | 48 | //! \brief Returns true if the current occupancy intersects with 49 | //! any given point in 'targets' 50 | //! \param[in] targets A list of points in global Cartesian coordinates (x, y, z) 51 | //! checked against the current occupancy 52 | bool intersection(std::vector targets) const { 53 | throw "Function safety_perimeters::Obstacle::intersection is not defined!"; 54 | } 55 | 56 | //! \brief Returns Pointers to the current occupancies of the respective model 57 | inline std::vector get_occupancy_p() const { 58 | return this->occupancy_p; 59 | } 60 | 61 | //! \brief Returns the system parameters that the model was initialized with 62 | inline System get_system() const { 63 | return this->system; 64 | } 65 | 66 | protected: 67 | //! \brief Containins system information (measurement errors/delay) 68 | System system = System(); 69 | 70 | //! \brief Contains pointers to the most recent state of all occupancy segments 71 | std::vector occupancy_p = {}; 72 | }; 73 | } // namespace obstacles 74 | #endif // REACH_LIB_INCLUDE_SAFETY_PERIMETER_HPP_ 75 | -------------------------------------------------------------------------------- /ReachLib/include/occupancy_container.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "point.hpp" 18 | 19 | #ifndef REACH_LIB_INCLUDE_OCCUPANCY_CONTAINER_HPP_ 20 | #define REACH_LIB_INCLUDE_OCCUPANCY_CONTAINER_HPP_ 21 | 22 | namespace occupancy_containers { 23 | //! \typedef Point describes the Point class within the point namespace 24 | typedef point::Point Point; 25 | 26 | //! This class describes all types of media that 27 | //! can contain reachable occupancies. 28 | //! They contain subsets of the overall reachable 29 | //! occupancy and are used to build OccupancyModels. 30 | class OccupancyContainer { 31 | public: 32 | //! \brief Empty constructor 33 | OccupancyContainer() {} 34 | 35 | //! \brief Empty destructor 36 | virtual ~OccupancyContainer() {} 37 | 38 | //! \brief Determines whether there exists an intersection between 39 | //! an occupancy container and any point within targets. 40 | //! Throws an exception if the type of OccupancyContainer 41 | //! can not be determined. 42 | //! \param[in] targets List of points of interes in Cartesian (x y z) 43 | bool intersection(const std::vector& targets) const { 44 | throw "Function occupancy_containers::OccupancyContainer intersection is not defined!"; 45 | } 46 | }; 47 | } // namespace occupancy_containers 48 | #endif // REACH_LIB_INCLUDE_OCCUPANCY_CONTAINER_HPP_ 49 | -------------------------------------------------------------------------------- /ReachLib/include/pedestrian_accel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder.hpp" 19 | #include "cylinder_perimeter.hpp" 20 | #include "pedestrian.hpp" 21 | #include "point.hpp" 22 | #include "obstacle.hpp" 23 | #include "system.hpp" 24 | 25 | #ifndef REACH_LIB_INCLUDE_PEDESTRIAN_ACCEL_HPP_ 26 | #define REACH_LIB_INCLUDE_PEDESTRIAN_ACCEL_HPP_ 27 | 28 | namespace obstacles { 29 | namespace pedestrian { 30 | namespace accel { 31 | 32 | //! \brief Defines a cylindrical safety perimeter based on dynamic extensions 33 | //! to static aprroaches (ISO10218: https://www.iso.org/standard/51330.html) 34 | //! described by http://mediatum.ub.tum.de/doc/1379662/905746.pdf. 35 | //! Encorporates live velocity measurements and constant maximum acceleration 36 | //! into occupancy calculations. 37 | class PedestrianAccel : public Pedestrian { 38 | public: 39 | //! \brief Empty constructor 40 | PedestrianAccel() : Pedestrian() {} 41 | 42 | //! \brief Instantiates a cylindrical safety perimeter 43 | //! based on constant maximum acceleration 44 | //! and live velocity values. 45 | //! \param[in] max_a Maximum acceleration [m/s] (constant) 46 | PedestrianAccel(const System& sensor, double height, 47 | double arm_span, double max_a, double offset = 0.0, 48 | const Point& init_pos = Point(), 49 | int steps = 0); 50 | 51 | //! \brief Calcualtes the current occupancy using this model 52 | //! \param[in] p Current location of points of interest in Cartesian coordinartes (x, y ,z) 53 | //! \param[in] v Current velocity of points of interest [static velocity is used here] 54 | //! \param[in] t_a Start of the interval of analysis 55 | //! \param[in] t_b End of the interval of analysis 56 | std::vector update(double t_a, double t_b, 57 | std::vector p, 58 | std::vector v); 59 | 60 | //! \brief Determines if any point from the list 'targets' is located inside 61 | //! the current occupancy. 62 | //! \param[in] targets A list of points of interest to be checked against 63 | //! the current occupancy. 64 | bool intersection(std::vector targets) const; 65 | 66 | //! \brief Returns the mode of reachability analysis 67 | //! of this class as 'PEDESTRIAN-ACCEL' 68 | std::string get_mode() const { 69 | return "PEDESTRIAN-ACCEL"; 70 | } 71 | 72 | //! \brief Returns the maximum acceleration parameter 73 | double get_max_a() const { 74 | return max_a_; 75 | } 76 | 77 | //! \brief Emtpy destructor 78 | ~PedestrianAccel() {} 79 | 80 | private: 81 | //! \brief Maximum acceleration of the human (constant) 82 | double max_a_ = 0.0; 83 | 84 | //! \brief The number of cylinders formed from intermediate intervals 85 | int steps_ = 0.0; 86 | 87 | //! \brief A list of intermediary occupacies 88 | std::vector cylinder_list_ = {}; 89 | }; 90 | } // namespace accel 91 | } // namespace pedestrian 92 | } // namespace obstacles 93 | #endif // REACH_LIB_INCLUDE_PEDESTRIAN_ACCEL_HPP_ 94 | -------------------------------------------------------------------------------- /ReachLib/include/pedestrian_vel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder_perimeter.hpp" 19 | #include "occupancy.hpp" 20 | #include "pedestrian.hpp" 21 | #include "point.hpp" 22 | #include "obstacle.hpp" 23 | #include "system.hpp" 24 | 25 | #ifndef REACH_LIB_INCLUDE_PEDESTRIAN_VEL_HPP_ 26 | #define REACH_LIB_INCLUDE_PEDESTRIAN_VEL_HPP_ 27 | 28 | namespace obstacles { 29 | namespace pedestrian { 30 | namespace vel { 31 | 32 | //! \brief Defines a cylindrical safety perimeter based on 33 | //! definiions in ISO10218: https://www.iso.org/standard/51330.html and 34 | //! descriptions by http://mediatum.ub.tum.de/doc/1379662/905746.pdf. 35 | //! Uses constant maximum velocity estimates for occupancy calculations. 36 | class PedestrianVel : public Pedestrian { 37 | public: 38 | //! \brief Empty constructor 39 | PedestrianVel() : Pedestrian() {} 40 | 41 | //! \brief Instantiates a cylindrical safety perimeter 42 | //! based on a constant maximum velocity 43 | //! \param[in] max_v Maximum velocity (constant) 44 | PedestrianVel(const System& sensor, double height, 45 | double arm_span, double max_v, double offset = 0.0, 46 | const Point& init_pos = Point()); 47 | 48 | //! \brief Calcualtes the current occupancy using this model 49 | //! \param[in] p Current location of points of interest in Cartesian coordinartes (x, y ,z) 50 | //! \param[in] v Current velocity of points of interest [static velocity is used here] 51 | //! \param[in] t_a Start of the interval of analysis 52 | //! \param[in] t_b End of the interval of analysis 53 | std::vector update(double t_a, double t_b, 54 | std::vector p, 55 | std::vector v = {}); 56 | 57 | //! \brief Determines if any point from the list 'targets' is located inside 58 | //! the current occupancy. 59 | //! \param[in] targets A list of points of interest to be checked against 60 | //! the current occupancy. 61 | bool intersection(std::vector targets) const; 62 | 63 | //! \brief Returns the mode of reachability analysis 64 | //! of this class as 'PEDESTRIAN-VEL' 65 | std::string get_mode() const { 66 | return "PEDESTRIAN-VEL"; 67 | } 68 | 69 | //! \brief Returns the maximum velocity parameter 70 | double get_max_v() const { 71 | return max_v_; 72 | } 73 | 74 | //! \brief Emtpy destructor 75 | ~PedestrianVel() {} 76 | 77 | private: 78 | //! \brief Maximum velocity of the human (constant) 79 | double max_v_ = 0.0; 80 | }; 81 | } // namespace vel 82 | } // namespace pedestrian 83 | } // namespace obstacles 84 | #endif // REACH_LIB_INCLUDE_PEDESTRIAN_VEL_HPP_ 85 | -------------------------------------------------------------------------------- /ReachLib/include/sphere.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "occupancy_container.hpp" 18 | #include "point.hpp" 19 | 20 | #ifndef REACH_LIB_INCLUDE_SPHERE_HPP_ 21 | #define REACH_LIB_INCLUDE_SPHERE_HPP_ 22 | 23 | namespace occupancy_containers { 24 | namespace sphere { 25 | 26 | class Sphere : public OccupancyContainer { 27 | public: 28 | //! \brief Origin of the sphere 29 | Point p_ = Point(); 30 | 31 | //! \brief Radius of the sphere 32 | double r_ = 0.0; 33 | 34 | //! \brief Empty constructor 35 | Sphere() {} 36 | 37 | //! \brief Instatiates an object of type Sphere 38 | Sphere(Point p, double r); 39 | 40 | //! \brief Empty destructor 41 | ~Sphere() {} 42 | 43 | //! \brief Determines whether there exists an intersection between 44 | //! this Sphere and any point within targets. 45 | //! \param[in] targets List of points of interes in Cartesian (x y z) 46 | bool intersection(const std::vector& targets) const; 47 | }; 48 | } // namespace sphere 49 | } // namespace occupancy_containers 50 | #endif // REACH_LIB_INCLUDE_SPHERE_HPP_ 51 | -------------------------------------------------------------------------------- /ReachLib/include/system.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #ifndef REACH_LIB_INCLUDE_SYSTEM_HPP_ 16 | #define REACH_LIB_INCLUDE_SYSTEM_HPP_ 17 | 18 | namespace systems { 19 | 20 | //! This class contains parameters that characterize properties 21 | //! of the hardware and software system used for sensing and preparation of 22 | //! position and velocity data. 23 | class System { 24 | public: 25 | //! \brief Defines the positional measurement error of the underlying system 26 | double measurement_error_pos_ = 0.0; 27 | 28 | //! \brief Defines the measurement error of the underlying system for recorded velocities 29 | double measurement_error_vel_ = 0.0; 30 | 31 | //! \brief Defines the delay imposed by sensing and preparation of incoming signals 32 | double delay_ = 0.0; 33 | 34 | //! \brief Instantiates an object of type System 35 | //! \param[in] measurement_error_pos The positional measurement error 36 | //! of the underlying system 37 | //! \param[in] measurement_error_vel The measurement error of recorded velocities 38 | //! of the underlying system 39 | //! \param[in] delay The delay imposed by sensing and preparation of incoming signals 40 | System(double measurement_error_pos = 0.0, 41 | double measurement_error_vel = 0.0, 42 | double delay = 0.0); 43 | 44 | //! \brief Empty destructor 45 | ~System() {} 46 | }; 47 | } // namespace system 48 | #endif // REACH_LIB_INCLUDE_SYSTEM_HPP_ 49 | -------------------------------------------------------------------------------- /ReachLib/src/articulated.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "articulated.hpp" 21 | #include "capsule.hpp" 22 | #include "obstacle.hpp" 23 | #include "system.hpp" 24 | 25 | namespace obstacles { 26 | namespace articulated { 27 | Articulated::Articulated(System system, 28 | std::map body_segment_map) : 29 | body_segment_map_(body_segment_map), Obstacle(system) { 30 | // NO TODO 31 | } 32 | } // namespace articulated 33 | } // namespace obstacles 34 | -------------------------------------------------------------------------------- /ReachLib/src/articulated_accel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "articulated.hpp" 21 | #include "articulated_accel.hpp" 22 | #include "body_part_accel.hpp" 23 | #include "capsule.hpp" 24 | #include "obstacle.hpp" 25 | #include "system.hpp" 26 | 27 | 28 | namespace obstacles { 29 | namespace articulated { 30 | namespace accel { 31 | 32 | ArticulatedAccel::ArticulatedAccel(System system, std::map body_segment_map, 33 | const std::map& thickness, 34 | const std::vector& max_a) : 35 | Articulated(system, body_segment_map) { 36 | // Create a list of BodyPartAccel that is later set as occpancy_ 37 | std::vector body = {}; 38 | for (const auto& it : body_segment_map) { 39 | body.push_back(BodyPartAccel(it.first, thickness.at(it.first), max_a.at(it.second.first), 40 | max_a.at(it.second.second))); 41 | } 42 | this->occupancy_ = body; 43 | // Initialize pointers 44 | for (int i = 0; i < this->occupancy_.size(); i++) { 45 | this->occupancy_p.push_back(&(this->occupancy_[i])); 46 | } 47 | } 48 | 49 | std::vector ArticulatedAccel::update(double t_a, double t_b, 50 | std::vector p, 51 | std::vector v) { 52 | // std::cout << "Length: " << this->get_occupancy_().size() << "\n"; 53 | int count = 0; 54 | for (auto& it : this->occupancy_) { 55 | int p1_id = this->body_segment_map_.at(it.get_name()).first; 56 | int p2_id = this->body_segment_map_.at(it.get_name()).second; 57 | it.update({p[p1_id], p[p2_id]}, {v[p1_id], v[p2_id]}, t_a, t_b, 58 | this->system.measurement_error_pos_, 59 | this->system.measurement_error_vel_, 60 | this->system.delay_); 61 | this->occupancy_[count] = it; 62 | count++; 63 | } 64 | return this->occupancy_; 65 | } 66 | 67 | bool ArticulatedAccel::intersection(std::vector targets) const { 68 | for (auto& it : this->occupancy_) { 69 | if (it.intersection(targets)) { 70 | return true; 71 | } 72 | } 73 | return false; 74 | } 75 | } // namespace accel 76 | } // namespace articulated 77 | } // namespace obstacles 78 | -------------------------------------------------------------------------------- /ReachLib/src/articulated_combined.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include "articulated_combined.hpp" 16 | 17 | 18 | namespace obstacles { 19 | namespace articulated { 20 | namespace combined { 21 | 22 | ArticulatedCombined::ArticulatedCombined(System system, std::map body_segment_map, 23 | const std::map& thickness, 24 | const std::vector& max_v, 25 | const std::vector& max_a) : 26 | Articulated(system, body_segment_map) { 27 | // Create a list of BodyPartCombined that is later set as occpancy_ 28 | std::vector body = {}; 29 | for (const auto& it : body_segment_map) { 30 | body.push_back(BodyPartCombined(it.first, thickness.at(it.first), 31 | max_v.at(it.second.first), max_v.at(it.second.second), 32 | max_a.at(it.second.first), max_a.at(it.second.second))); 33 | } 34 | this->occupancy_ = body; 35 | // Initialize pointers 36 | for (int i = 0; i < this->occupancy_.size(); i++) { 37 | this->occupancy_p.push_back(&(this->occupancy_[i])); 38 | } 39 | } 40 | 41 | std::vector ArticulatedCombined::update(double t_a, double t_b, 42 | std::vector p, 43 | std::vector v) { 44 | // std::cout << "Length: " << this->get_occupancy_().size() << "\n"; 45 | int count = 0; 46 | for (auto& it : this->occupancy_) { 47 | int p1_id = this->body_segment_map_.at(it.get_name()).first; 48 | int p2_id = this->body_segment_map_.at(it.get_name()).second; 49 | it.update({p[p1_id], p[p2_id]}, {v[p1_id], v[p2_id]}, t_a, t_b, 50 | this->system.measurement_error_pos_, 51 | this->system.measurement_error_vel_, 52 | this->system.delay_); 53 | this->occupancy_[count] = it; 54 | count++; 55 | } 56 | return this->occupancy_; 57 | } 58 | 59 | bool ArticulatedCombined::intersection(std::vector targets) const { 60 | for (auto& it : this->occupancy_) { 61 | if (it.intersection(targets)) { 62 | return true; 63 | } 64 | } 65 | return false; 66 | } 67 | } // namespace combined 68 | } // namespace articulated 69 | } // namespace obstacles 70 | -------------------------------------------------------------------------------- /ReachLib/src/articulated_pos.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "articulated.hpp" 22 | #include "articulated_pos.hpp" 23 | #include "extremity.hpp" 24 | #include "capsule.hpp" 25 | #include "obstacle.hpp" 26 | #include "system.hpp" 27 | 28 | 29 | namespace obstacles { 30 | namespace articulated { 31 | namespace pos { 32 | 33 | ArticulatedPos::ArticulatedPos(System system, std::map body_segment_map, 34 | const std::vector& thickness, 35 | const std::vector& max_v, 36 | const std::vector& length) : 37 | Articulated(system, body_segment_map) { 38 | // Create a list of BodyPartsAccel that is later set as occpancy_ 39 | assert(thickness.size() == max_v.size()); 40 | assert(thickness.size() == length.size()); 41 | std::vector body = {}; 42 | int i=0; 43 | for (const auto& it : body_segment_map) { 44 | body.push_back(Extremity(it.first, thickness[i], length[i], max_v[i])); 45 | i++; 46 | } 47 | this->occupancy_ = body; 48 | // Initialize pointers 49 | for (int i = 0; i < this->occupancy_.size(); i++) { 50 | this->occupancy_p.push_back(&(this->occupancy_[i])); 51 | } 52 | } 53 | 54 | std::vector ArticulatedPos::update(double t_a, double t_b, 55 | std::vector p, 56 | std::vector v) { 57 | int count = 0; 58 | for (auto& it : this->occupancy_) { 59 | int p_id = this->body_segment_map_.at(it.get_name()).first; 60 | it.update({p[p_id]}, {}, t_a, t_b, 61 | this->system.measurement_error_pos_, 0.0, 62 | this->system.delay_); 63 | this->occupancy_[count] = it; 64 | count++; 65 | } 66 | return this->occupancy_; 67 | } 68 | 69 | bool ArticulatedPos::intersection(std::vector targets) const { 70 | for (auto& it : this->occupancy_) { 71 | if (it.intersection(targets)) { 72 | return true; 73 | } 74 | } 75 | return false; 76 | } 77 | } // namespace pos 78 | } // namespace articulated 79 | } // namespace obstacles 80 | -------------------------------------------------------------------------------- /ReachLib/src/articulated_vel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "articulated.hpp" 21 | #include "articulated_vel.hpp" 22 | #include "body_part_vel.hpp" 23 | #include "capsule.hpp" 24 | #include "obstacle.hpp" 25 | #include "system.hpp" 26 | 27 | 28 | namespace obstacles { 29 | namespace articulated { 30 | namespace vel { 31 | 32 | ArticulatedVel::ArticulatedVel(System system, std::map body_segment_map, 33 | const std::map& thickness, 34 | const std::vector& max_v) : 35 | Articulated(system, body_segment_map) { 36 | // Create a list of BodyPartsAccel that is later set as occpancy_ 37 | std::vector body = {}; 38 | for (const auto& it : body_segment_map) { 39 | body.push_back(BodyPartVel(it.first, thickness.at(it.first), max_v.at(it.second.first), 40 | max_v.at(it.second.second))); 41 | } 42 | this->occupancy_ = body; 43 | // Initialize pointers 44 | for (int i = 0; i < this->occupancy_.size(); i++) { 45 | this->occupancy_p.push_back(&(this->occupancy_[i])); 46 | } 47 | } 48 | 49 | std::vector ArticulatedVel::update(double t_a, double t_b, 50 | std::vector p, 51 | std::vector v) { 52 | int count = 0; 53 | for (auto& it : this->occupancy_) { 54 | int p1_id = this->body_segment_map_.at(it.get_name()).first; 55 | int p2_id = this->body_segment_map_.at(it.get_name()).second; 56 | it.update({p[p1_id], p[p2_id]}, {}, t_a, t_b, 57 | this->system.measurement_error_pos_, 0.0, 58 | this->system.delay_); 59 | this->occupancy_[count] = it; 60 | count++; 61 | } 62 | return this->occupancy_; 63 | } 64 | 65 | bool ArticulatedVel::intersection(std::vector targets) const { 66 | for (auto& it : this->occupancy_) { 67 | if (it.intersection(targets)) { 68 | return true; 69 | } 70 | } 71 | return false; 72 | } 73 | } // namespace vel 74 | } // namespace articulated 75 | } // namespace obstacles 76 | -------------------------------------------------------------------------------- /ReachLib/src/body_part.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "body_part.hpp" 19 | #include "capsule.hpp" 20 | #include "occupancy.hpp" 21 | #include "point.hpp" 22 | 23 | namespace occupancies { 24 | namespace body_parts { 25 | 26 | BodyPart::BodyPart() : Occupancy() { 27 | this->occupancy_p = &this->occupancy_; 28 | } 29 | 30 | BodyPart::BodyPart(std::string name, double thickness) : Occupancy(name, thickness) { 31 | // NO TODO 32 | } 33 | } // namespace body_parts 34 | } // namespace occupancies 35 | -------------------------------------------------------------------------------- /ReachLib/src/body_part_vel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "body_part.hpp" 20 | #include "body_part_vel.hpp" 21 | #include "capsule.hpp" 22 | #include "occupancy.hpp" 23 | #include "point.hpp" 24 | 25 | 26 | 27 | namespace occupancies { 28 | namespace body_parts { 29 | namespace vel { 30 | 31 | BodyPartVel::BodyPartVel(std::string name, double thickness, 32 | double max_v1, double max_v2) : 33 | BodyPart(name, thickness), 34 | max_v1_(max_v1), max_v2_(max_v2) { 35 | // NO TODO 36 | } 37 | 38 | bool BodyPartVel::intersection(std::vector targets) const { 39 | return this->occupancy_.intersection(targets); 40 | } 41 | 42 | void BodyPartVel::update(const std::vector& p, 43 | const std::vector& v, 44 | double t_a, double t_b, 45 | double measurement_error_pos, 46 | double measurement_error_vel, 47 | double delay) { 48 | // The vectors must contain exactly 2 points 49 | assert(("Vector p must have exactly two entries for BodyPartAccel", size(p) == 2)); 50 | 51 | // translate vector entries to proximal and distal joint values 52 | Point p1 = p[0]; 53 | Point p2 = p[1]; 54 | 55 | // Check whether this capsule is a ball 56 | bool b; 57 | if (p1 == p2) { 58 | b = true; 59 | } else { 60 | b = false; 61 | } 62 | 63 | // Calculate the occupancies of the proximal and distal joint up to t_a 64 | Capsule rp1_t1 = BodyPartVel::ry(p1, 1, t_a, delay, 65 | measurement_error_pos); 66 | Capsule rp1_t2 = BodyPartVel::ry(p1, 1, t_b, delay, 67 | measurement_error_pos); // was p2 68 | Capsule b1 = Capsule::ballEnclosure(rp1_t1, rp1_t2); 69 | 70 | if (b) { 71 | // Add the thickness (as radius) 72 | rp1_t2.r_ += this->thickness_/2.0; 73 | 74 | this->occupancy_ = rp1_t2; // was rp1_t1 75 | } else { 76 | Capsule rp2_t1 = BodyPartVel::ry(p2, 2, t_a, delay, 77 | measurement_error_pos); // was p1 78 | Capsule rp2_t2 = BodyPartVel::ry(p2, 2, t_b, delay, 79 | measurement_error_pos); 80 | Capsule b2 = Capsule::ballEnclosure(rp2_t1, rp2_t2); 81 | 82 | // Add the thickness (as radius) to the ball with the larger radius (determines capsule radius) 83 | if (b1.r_ >= b2.r_) { 84 | b1.r_ += this->thickness_/2.0; 85 | } else { 86 | b2.r_ += this->thickness_/2.0; 87 | } 88 | this->occupancy_ = Capsule::capsuleEnclosure(b1, b2); 89 | } 90 | } 91 | 92 | Capsule BodyPartVel::ry(const Point& p, int index, double t_b, 93 | double delay, double measurement_error_pos) const { 94 | double max_v; 95 | if (index == 1) { 96 | max_v = this->max_v1_; 97 | } else { 98 | max_v = this->max_v2_; 99 | } 100 | 101 | // RP 102 | return Capsule(p, p, measurement_error_pos + max_v * (t_b + delay)); 103 | } 104 | 105 | } // namespace vel 106 | } // namespace body_parts 107 | } // namespace occupancies 108 | -------------------------------------------------------------------------------- /ReachLib/src/cylinder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "cylinder.hpp" 18 | #include "occupancy_container.hpp" 19 | #include "point.hpp" 20 | 21 | namespace occupancy_containers { 22 | namespace cylinder { 23 | 24 | Cylinder::Cylinder(const Point& p1, const Point& p2, double r) : 25 | p1_(p1), p2_(p2), r_(r) { 26 | // NO TODO 27 | } 28 | 29 | bool Cylinder::intersection(const std::vector& targets) const { 30 | bool intersect = false; 31 | for (const auto& p : targets) { 32 | // The cylinder is defined by a line segment from 33 | // the center of the top circle to the center of the bottom circle. 34 | Point d = this->p2_ - this->p1_; 35 | Point p1d = p - this->p1_; 36 | Point p2d = p - this->p2_; 37 | double h = Point::norm(d); 38 | double hsq = pow(h, 2); 39 | double dotprod = Point::inner_dot(p1d, d); 40 | 41 | // Check whether the point lies higher or lower than the line segment 42 | // in the cylinders coordinates 43 | // If the point lies outside the cylinders domain given an infinite radius 44 | // (point lies higher or lower than the cylinders top/bottom): 45 | if ((dotprod < 0.0) || (dotprod > hsq)) { 46 | return false; 47 | // If the point lies next to the line segment in the cylinders coordinates: 48 | // check if the shortes point to line distance exceeds the cylinders radius 49 | // if yes: no intersection 50 | } else { 51 | double rsq = pow(this->r_, 2); 52 | double dsq = Point::inner_dot(p1d, p1d) - ((dotprod*dotprod)/hsq); 53 | if (dsq > rsq) { 54 | return false; 55 | } else { 56 | intersect = true; 57 | } 58 | } 59 | } 60 | return intersect; 61 | } 62 | 63 | } // namespace cylinder 64 | } // namespace occupancy_containers 65 | -------------------------------------------------------------------------------- /ReachLib/src/cylinder_perimeter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder.hpp" 19 | #include "cylinder_perimeter.hpp" 20 | #include "occupancy.hpp" 21 | #include "point.hpp" 22 | 23 | namespace occupancies { 24 | namespace cylinder_perimeters { 25 | 26 | CylinderPerimeter::CylinderPerimeter() : Occupancy() { 27 | this->occupancy_p = &this->occupancy_; 28 | } 29 | 30 | CylinderPerimeter::CylinderPerimeter(const Point& init_pos, double height, double radius, std::string name) : 31 | Occupancy(name) { 32 | Cylinder c = Cylinder(init_pos, Point(init_pos.x, init_pos.y, height), radius); 33 | this->occupancy_ = c; 34 | this->cylinder_list_.push_back(c); 35 | } 36 | 37 | bool CylinderPerimeter::intersection(std::vector targets) const { 38 | return this->occupancy_.intersection(targets); 39 | } 40 | } // namespace cylinder_perimeters 41 | } // namespace occupancies 42 | -------------------------------------------------------------------------------- /ReachLib/src/extremity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "capsule.hpp" 20 | #include "extremity.hpp" 21 | #include "occupancy.hpp" 22 | #include "point.hpp" 23 | 24 | 25 | namespace occupancies { 26 | namespace extremities { 27 | 28 | Extremity::Extremity() : Occupancy() { 29 | this->occupancy_p = &this->occupancy_; 30 | } 31 | 32 | Extremity::Extremity(std::string name, double thickness, 33 | double length, double max_v) : 34 | length_(length), max_v_(max_v), Occupancy(name, thickness) { 35 | // NO TODO 36 | } 37 | 38 | void Extremity::update(const std::vector& p, 39 | const std::vector& v, 40 | double t_a, double t_b, 41 | double measurement_error_pos, 42 | double measurement_error_vel, 43 | double delay) { 44 | // The vectors must contain exactly 2 points 45 | assert(("Vector p must have exactly two entries for BodyPartAccel", size(p) == 1)); 46 | // translate vector entries to proximal and distal joint values 47 | Point pe = p[0]; 48 | // Total time passed within the desired interval 49 | assert(t_b >= t_a); 50 | double delta_t = t_b + delay; 51 | // Distance covered at max_v over delta_t 52 | double dist = this->max_v_ * delta_t; 53 | // Radius of the ball occupancy (thickness added as radius) 54 | double radius = dist + this->length_ + this->thickness_/2.0 + measurement_error_pos; 55 | this->occupancy_ = Capsule(pe, pe, radius); 56 | } 57 | 58 | bool Extremity::intersection(std::vector targets) const { 59 | return this->occupancy_.intersection(targets); 60 | } 61 | 62 | } // namespace extremities 63 | } // namespace occupancies 64 | -------------------------------------------------------------------------------- /ReachLib/src/occupancy.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "occupancy_container.hpp" 19 | #include "occupancy.hpp" 20 | #include "point.hpp" 21 | 22 | namespace occupancies { 23 | 24 | Occupancy::Occupancy(std::string name) : name_(name) { 25 | // NO TODO 26 | } 27 | 28 | Occupancy::Occupancy(std::string name, double thickness) : name_(name), thickness_(thickness) { 29 | // NO TODO 30 | } 31 | } // namspace occupancies 32 | -------------------------------------------------------------------------------- /ReachLib/src/pedestrian.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder_perimeter.hpp" 19 | #include "pedestrian.hpp" 20 | #include "point.hpp" 21 | #include "obstacle.hpp" 22 | #include "system.hpp" 23 | 24 | namespace obstacles { 25 | namespace pedestrian { 26 | 27 | Pedestrian::Pedestrian(System sensor, double height, double arm_span, double offset, 28 | const Point& init_pos) : 29 | height_(height), arm_span_(arm_span), offset_(offset), 30 | Obstacle(sensor) { 31 | this->occupancy_ = {CylinderPerimeter(init_pos, height, 0.0)}; 32 | } 33 | 34 | } // namespace pedestrian 35 | } // namespace obstacles 36 | -------------------------------------------------------------------------------- /ReachLib/src/pedestrian_vel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder_perimeter.hpp" 19 | #include "pedestrian.hpp" 20 | #include "pedestrian_vel.hpp" 21 | #include "point.hpp" 22 | #include "obstacle.hpp" 23 | #include "system.hpp" 24 | 25 | 26 | namespace obstacles { 27 | namespace pedestrian { 28 | namespace vel { 29 | 30 | PedestrianVel::PedestrianVel(const System& sensor, double height, 31 | double arm_span, double max_v, double offset, 32 | const Point& init_pos) : 33 | Pedestrian(sensor, height, arm_span, offset, init_pos), 34 | max_v_(max_v) { 35 | // NO TODO 36 | } 37 | 38 | bool PedestrianVel::intersection(std::vector targets) const { 39 | bool intersection = false; 40 | for (const auto& it : targets) { 41 | intersection = this->occupancy_[0].intersection(targets); 42 | } 43 | return intersection; 44 | } 45 | 46 | std::vector PedestrianVel::update(double t_a, double t_b, 47 | std::vector p, 48 | std::vector v) { 49 | // Get the position of the cylinder using the equations of motion: x = x0 + max_v*t 50 | double vel_rad = 0.5*this->arm_span_ + this->system.measurement_error_pos_ + 51 | (this->max_v_ + this->system.measurement_error_vel_) * (t_b + this->system.delay_); 52 | 53 | this->occupancy_ = {CylinderPerimeter(Point(p[0].x, p[0].y, this->offset_), this->height_, vel_rad)}; 54 | return this->occupancy_; 55 | } 56 | 57 | 58 | 59 | } // namespace vel 60 | } // namespace pedestrian 61 | } // namespace obstacles 62 | -------------------------------------------------------------------------------- /ReachLib/src/point.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "point.hpp" 19 | 20 | namespace point { 21 | 22 | Point::Point(double x, double y, double z) : x(x), y(y), z(z) { 23 | // NO TODO 24 | } 25 | 26 | Point Point::origin(const Point& p1, const Point& p2) { 27 | Point p = p1 - p2; 28 | p.x = p2.x + p.x/2.0; 29 | p.y = p2.y + p.y/2.0; 30 | p.z = p2.z + p.z/2.0; 31 | return p; 32 | } 33 | 34 | double Point::norm(const Point& p) { 35 | return sqrt(pow(p.x, 2) + pow(p.y, 2) + pow(p.z, 2)); 36 | } 37 | 38 | double Point::norm(const Point& p1, const Point& p2) { 39 | return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2)); 40 | } 41 | 42 | double Point::inner_dot(const Point& p1, const Point& p2) { 43 | return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z; 44 | } 45 | 46 | Point Point::cross(const Point& p1, const Point& p2) { 47 | Point c = Point(); 48 | c.x = p1.y*p2.z - p1.z*p2.y; 49 | c.y = p1.z*p2.x - p1.x*p2.z; 50 | c.z = p1.x*p2.y - p1.y*p2.x; 51 | return c; 52 | } 53 | 54 | double Point::determinant3x3(Point X, Point Y, Point Z) { 55 | // A = [X, Y, Z] = [[a b c], [d e f], [g h i]] 56 | // det(A) = a(ei - fh) - b(di - fg) + c(dh - eg) 57 | return X.x * (Y.y * Z.z - Y.z * Z.y) - 58 | X.y * (Y.x * Z.z - Y.z * Z.x) + 59 | X.z * (Y.x * Z.y - Y.y * Z.x); 60 | } 61 | 62 | // legacy (not required) 63 | quaternion Point::orientation(const Point& p1, const Point& p2) { 64 | Point p1_loc = p1; 65 | Point p2_loc = p2; 66 | if (p2.z < p1.z) { 67 | Point temp = p1; 68 | p1_loc = p2_loc; 69 | p2_loc = temp; 70 | } 71 | 72 | double dis = norm(p1_loc, p2_loc); 73 | double f = 0.0174533; 74 | double r = 0.0; 75 | double p = 0.0; 76 | double y = 0.0; 77 | 78 | if (dis == 0) { 79 | p = 0.0; 80 | } else { 81 | p = -asin((p2_loc.y - p1_loc.y) / dis); 82 | } 83 | 84 | if (cos(p) == 0.0 ||dis == 0.0) { 85 | y = 0.0; 86 | } else { 87 | y = asin((p2_loc.x - p1_loc.x)/(cos(p) * dis)); 88 | } 89 | 90 | // capsules are roll invariant 91 | 92 | double cy = cos(y * 0.5); 93 | double sy = sin(y * 0.5); 94 | double cp = cos(p * 0.5); 95 | double sp = sin(p * 0.5); 96 | double cr = cos(r * 0.5); 97 | double sr = sin(r * 0.5); 98 | 99 | // q := (x,y,z,w) 100 | // q.w = cr * cp * cy + sr * sp * sy; 101 | // q.x = sr * cp * cy - cr * sp * sy; 102 | // q.y = cr * sp * cy + sr * cp * sy; 103 | // q.z = cr * cp * sy - sr * sp * cy; 104 | 105 | return std::make_tuple(sr * cp * cy - cr * sp * sy, 106 | cr * sp * cy + sr * cp * sy, 107 | cr * cp * sy - sr * sp * cy, 108 | cr * cp * cy + sr * sp * sy); 109 | } 110 | } // namespace point 111 | -------------------------------------------------------------------------------- /ReachLib/src/sphere.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "occupancy_container.hpp" 18 | #include "point.hpp" 19 | #include "sphere.hpp" 20 | 21 | namespace occupancy_containers { 22 | namespace sphere { 23 | 24 | Sphere::Sphere(Point p, double r) : p_(p), r_(r) { 25 | // NO TODO 26 | } 27 | 28 | bool Sphere::intersection(const std::vector& targets) const { 29 | double intersection = false; 30 | for (const auto& it : targets) { 31 | if (Point::norm(it, this->p_) < 0) { 32 | intersection = true; 33 | } 34 | } 35 | return intersection; 36 | } 37 | } // namespace sphere 38 | } // namespace occupancy_containers 39 | -------------------------------------------------------------------------------- /ReachLib/src/system.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include "system.hpp" 16 | 17 | namespace systems { 18 | 19 | System::System(double measurement_error_pos, 20 | double measurement_error_vel, 21 | double delay) : 22 | measurement_error_pos_(measurement_error_pos), 23 | measurement_error_vel_(measurement_error_vel), 24 | delay_(delay) { 25 | // NO TODO 26 | } 27 | } // namespace system 28 | -------------------------------------------------------------------------------- /ReachLib/tests/include/body_part_combined_fixture.h: -------------------------------------------------------------------------------- 1 | // -*- lsst-c++ -*/ 2 | /** 3 | * @file BODY_PART_COMBINED_fixture.h 4 | * @brief Defines the test fixture for the human reach object 5 | * @version 0.1 6 | * @copyright This file is part of SaRA-Shield. 7 | * SaRA-Shield is free software: you can redistribute it and/or modify it under 8 | * the terms of the GNU General Public License as published by the Free Software Foundation, 9 | * either version 3 of the License, or (at your option) any later version. 10 | * SaRA-Shield is distributed in the hope that it will be useful, but WITHOUT ANY WARRANTY; 11 | * without even the implied warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. 12 | * See the GNU General Public License for more details. 13 | * You should have received a copy of the GNU General Public License along with SaRA-Shield. 14 | * If not, see . 15 | */ 16 | 17 | #include 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #include "reach_lib.hpp" 25 | 26 | #ifndef BODY_PART_COMBINED_FIXTURE_H 27 | #define BODY_PART_COMBINED_FIXTURE_H 28 | 29 | namespace occupancies{ 30 | namespace body_parts { 31 | namespace combined { 32 | 33 | /** 34 | * @brief Test fixture for the combined body part 35 | */ 36 | class BodyPartCombinedTest : public ::testing::Test { 37 | protected: 38 | /** 39 | * @brief The body part combined object 40 | */ 41 | BodyPartCombined body_; 42 | 43 | double measurement_error_pos_; 44 | double measurement_error_vel_; 45 | double v_max_; 46 | double a_max_; 47 | double thickness_; 48 | 49 | /** 50 | * @brief Create the body part combined object 51 | */ 52 | void SetUp() override { 53 | measurement_error_pos_ = 0.01; 54 | measurement_error_vel_ = 0.02; 55 | v_max_ = 2.0; 56 | a_max_ = 50.0; 57 | thickness_ = 0.1; 58 | 59 | body_ = BodyPartCombined("test", thickness_, v_max_, v_max_, a_max_, a_max_); 60 | } 61 | }; 62 | } // namespace combined 63 | } // namespace body_parts 64 | } // namespace occupancies 65 | 66 | #endif // BODY_PART_COMBINED_FIXTURE_H -------------------------------------------------------------------------------- /executables/SaRA_demo: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/executables/SaRA_demo -------------------------------------------------------------------------------- /executables/reachable_occupancy: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/executables/reachable_occupancy -------------------------------------------------------------------------------- /experiments/exp_appraoching.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/experiments/exp_appraoching.bag -------------------------------------------------------------------------------- /experiments/exp_box.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/experiments/exp_box.bag -------------------------------------------------------------------------------- /experiments/exp_headbutt.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/experiments/exp_headbutt.bag -------------------------------------------------------------------------------- /experiments/exp_punch.bag: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/experiments/exp_punch.bag -------------------------------------------------------------------------------- /images/Acc_Ex.png: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/images/Acc_Ex.png -------------------------------------------------------------------------------- /images/Cyl.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/images/Cyl.PNG -------------------------------------------------------------------------------- /images/f1.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/images/f1.PNG -------------------------------------------------------------------------------- /images/f1o.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/images/f1o.PNG -------------------------------------------------------------------------------- /images/human_accel_vel.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/images/human_accel_vel.PNG -------------------------------------------------------------------------------- /images/human_pos.PNG: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/images/human_pos.PNG -------------------------------------------------------------------------------- /reachable_occupancy/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "", 6 | "limitSymbolsToIncludedHeaders": true 7 | }, 8 | "includePath": [ 9 | "/home/sven/catkin_ws/devel/include/**", 10 | "/opt/ros/melodic/include/**", 11 | "/home/sven/catkin_ws/src/bot_generator/include/**", 12 | "/home/sven/catkin_ws/src/maze_generator/include/**", 13 | "/home/sven/catkin_ws/src/reach_ri/include/**", 14 | "/home/sven/catkin_ws/src/reachable_occupancy/include/**", 15 | "/usr/include/**" 16 | ], 17 | "name": "ROS" 18 | } 19 | ], 20 | "version": 4 21 | } -------------------------------------------------------------------------------- /reachable_occupancy/.vscode/launch.json: -------------------------------------------------------------------------------- 1 | { 2 | // Use IntelliSense to learn about possible attributes. 3 | // Hover to view descriptions of existing attributes. 4 | // For more information, visit: https://go.microsoft.com/fwlink/?linkid=830387 5 | "version": "0.2.0", 6 | "configurations": [ 7 | { 8 | "name": "Python: Current File (Integrated Terminal)", 9 | "type": "python", 10 | "request": "launch", 11 | "program": "${file}", 12 | "console": "integratedTerminal" 13 | }, 14 | { 15 | "name": "Python: Remote Attach", 16 | "type": "python", 17 | "request": "attach", 18 | "port": 5678, 19 | "host": "localhost", 20 | "pathMappings": [ 21 | { 22 | "localRoot": "${workspaceFolder}", 23 | "remoteRoot": "." 24 | } 25 | ] 26 | }, 27 | { 28 | "name": "Python: Module", 29 | "type": "python", 30 | "request": "launch", 31 | "module": "enter-your-module-name-here", 32 | "console": "integratedTerminal" 33 | }, 34 | { 35 | "name": "Python: Django", 36 | "type": "python", 37 | "request": "launch", 38 | "program": "${workspaceFolder}/manage.py", 39 | "console": "integratedTerminal", 40 | "args": [ 41 | "runserver", 42 | "--noreload", 43 | "--nothreading" 44 | ], 45 | "django": true 46 | }, 47 | { 48 | "name": "Python: Flask", 49 | "type": "python", 50 | "request": "launch", 51 | "module": "flask", 52 | "env": { 53 | "FLASK_APP": "app.py" 54 | }, 55 | "args": [ 56 | "run", 57 | "--no-debugger", 58 | "--no-reload" 59 | ], 60 | "jinja": true 61 | }, 62 | { 63 | "name": "Python: Current File (External Terminal)", 64 | "type": "python", 65 | "request": "launch", 66 | "program": "${file}", 67 | "console": "externalTerminal" 68 | } 69 | ] 70 | } -------------------------------------------------------------------------------- /reachable_occupancy/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.pythonPath": "/usr/bin/python3", 3 | "python.autoComplete.extraPaths": [ 4 | "/home/sven/catkin_ws/devel/lib/python2.7/dist-packages", 5 | "/opt/ros/melodic/lib/python2.7/dist-packages" 6 | ] 7 | } -------------------------------------------------------------------------------- /reachable_occupancy/config/reachable_occupancy.yaml: -------------------------------------------------------------------------------- 1 | use_ros_params: true 2 | use_udp_transmission: false 3 | ip_receive: "192.168.7.13" 4 | ip_send: "192.168.7.1" 5 | port_human_pos: 8080 6 | port_robot_capsules: 8079 7 | joint_num: 8 8 | rob_capsule_num: 11 9 | scale_human_pos: 0.001 10 | scale_robot_capsules: 1.0 11 | robot_topic: "/SaRA_demo_robot_capsules" 12 | joint_topic: /SaRA_demo_human_capsules 13 | vis_pub_topic: "/visualization_marker_array" 14 | occupancy_topic: "/occupancies" 15 | timing_on: false 16 | scenery: true 17 | calc_articulated_pos: false 18 | calc_articulated_vel: true 19 | calc_articulated_acc: false 20 | calc_pedestrian_vel: false 21 | calc_pedestrian_acc: false 22 | measurement_uncertainties_pos: 0.004 23 | measurement_uncertainties_vel: 0.04 24 | body_parts_names: 25 | - "RightUpperArm" 26 | - "RightForeArm" 27 | - "RightHand" 28 | - "LeftUpperArm" 29 | - "LeftForeArm" 30 | - "LeftHand" 31 | - "Torso" 32 | - "Head" 33 | body_parts_joint_1: 34 | - 3 35 | - 4 36 | - 5 37 | - 0 38 | - 1 39 | - 2 40 | - 8 41 | - 6 42 | body_parts_joint_2: 43 | - 4 44 | - 5 45 | - 5 46 | - 1 47 | - 2 48 | - 2 49 | - 9 50 | - 6 51 | articulated_max_a: 52 | - 50.0 53 | - 50.0 54 | - 50.0 55 | - 50.0 56 | - 50.0 57 | - 50.0 58 | - 30.0 59 | - 2.0 60 | - 20.0 61 | - 20.0 62 | articulated_max_v: 63 | - 2.0 64 | - 2.0 65 | - 2.0 66 | - 2.0 67 | - 2.0 68 | - 2.0 69 | - 2.0 70 | - 2.0 71 | - 2.0 72 | - 2.0 73 | body_part_thickness: 74 | - 0.1 75 | - 0.1 76 | - 0.205 77 | - 0.1 78 | - 0.1 79 | - 0.205 80 | - 0.3 81 | - 0.3 82 | extremities_names: 83 | - "RightArm" 84 | - "LeftArm" 85 | extremity_joints: 86 | - 3 87 | - 0 88 | extremity_max_v: 89 | - 2.46 90 | - 0.0 91 | - 0.0 92 | - 2.46 93 | extremity_length: 94 | - 0.635 95 | - 0.0 96 | - 0.0 97 | - 0.635 98 | extremity_thickness: 99 | - 0.205 100 | - 0.0 101 | - 0.0 102 | - 0.205 103 | pedestrian_arm_span: 1.8 104 | pedestrian_height: 3.0 105 | pedestrian_max_a: 10.0 106 | pedestrian_max_v: 2.0 107 | coordinate_offset: -1.1 108 | pedestrian_center_joint: 8 109 | use_visualization: true 110 | vis_validated: false 111 | vis_joints: true 112 | color_articulated_pos: 113 | - 0.0 114 | - 0.0 115 | - 1.0 116 | - 0.6 117 | color_articulated_vel: 118 | - 0.0 119 | - 0.0 120 | - 1.0 121 | - 0.6 122 | color_articulated_acc: 123 | - 0.0 124 | - 0.0 125 | - 1.0 126 | - 0.6 127 | color_pedestrian_vel: 128 | - 0.0 129 | - 0.0 130 | - 1.0 131 | - 0.6 132 | color_pedestrian_acc: 133 | - 0.0 134 | - 0.0 135 | - 1.0 136 | - 0.6 137 | color_robot_capsule: 138 | - 1.0 139 | - 0.0 140 | - 1.0 141 | - 0.6 142 | color_joints: 143 | - 1.0 144 | - 0.0 145 | - 0.0 146 | - 1.0 147 | color_points_in: 148 | - 1.0 149 | - 0.0 150 | - 0.0 151 | - 1.0 152 | color_points_out: 153 | - 0.0 154 | - 1.0 155 | - 0.0 156 | - 1.0 157 | display_grid: 158 | - false 159 | - false 160 | - false 161 | - false 162 | - false 163 | frame_duration: 0.016666666667 164 | origin_grid: 165 | - 0.0 166 | - 0.0 167 | - 0.0 168 | grid_size: 169 | - 3.0 170 | - 3.0 171 | - 3.0 172 | grid_resolution: 173 | - 0.1 174 | use_volume: 175 | - false 176 | - false 177 | - false 178 | - false 179 | - false 180 | smart_origin: 8 181 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/.vscode/c_cpp_properties.json: -------------------------------------------------------------------------------- 1 | { 2 | "configurations": [ 3 | { 4 | "browse": { 5 | "databaseFilename": "", 6 | "limitSymbolsToIncludedHeaders": true 7 | }, 8 | "includePath": [ 9 | "/home/sven/catkin_ws/devel/include/**", 10 | "/opt/ros/melodic/include/**", 11 | "/home/sven/catkin_ws/src/bot_generator/include/**", 12 | "/home/sven/catkin_ws/src/maze_generator/include/**", 13 | "/home/sven/catkin_ws/src/reach_ri/include/**", 14 | "/home/sven/catkin_ws/src/reachable_occupancy/include/**", 15 | "/usr/include/**", 16 | "/home/sven/Desktop/Reach-RI/spot-ro/reach_lib/include/**" 17 | ], 18 | "name": "ROS" 19 | } 20 | ], 21 | "version": 4 22 | } -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/.vscode/settings.json: -------------------------------------------------------------------------------- 1 | { 2 | "python.autoComplete.extraPaths": [ 3 | "/home/sven/catkin_ws/devel/lib/python2.7/dist-packages", 4 | "/opt/ros/melodic/lib/python2.7/dist-packages" 5 | ], 6 | "files.associations": { 7 | "cctype": "cpp", 8 | "clocale": "cpp", 9 | "cmath": "cpp", 10 | "csignal": "cpp", 11 | "cstdarg": "cpp", 12 | "cstddef": "cpp", 13 | "cstdio": "cpp", 14 | "cstdlib": "cpp", 15 | "cstring": "cpp", 16 | "ctime": "cpp", 17 | "cwchar": "cpp", 18 | "cwctype": "cpp", 19 | "array": "cpp", 20 | "atomic": "cpp", 21 | "strstream": "cpp", 22 | "*.tcc": "cpp", 23 | "bitset": "cpp", 24 | "chrono": "cpp", 25 | "cinttypes": "cpp", 26 | "complex": "cpp", 27 | "condition_variable": "cpp", 28 | "cstdint": "cpp", 29 | "deque": "cpp", 30 | "list": "cpp", 31 | "unordered_map": "cpp", 32 | "unordered_set": "cpp", 33 | "vector": "cpp", 34 | "exception": "cpp", 35 | "hash_map": "cpp", 36 | "hash_set": "cpp", 37 | "fstream": "cpp", 38 | "functional": "cpp", 39 | "future": "cpp", 40 | "initializer_list": "cpp", 41 | "iomanip": "cpp", 42 | "iosfwd": "cpp", 43 | "iostream": "cpp", 44 | "istream": "cpp", 45 | "limits": "cpp", 46 | "mutex": "cpp", 47 | "new": "cpp", 48 | "ostream": "cpp", 49 | "numeric": "cpp", 50 | "ratio": "cpp", 51 | "sstream": "cpp", 52 | "stdexcept": "cpp", 53 | "streambuf": "cpp", 54 | "system_error": "cpp", 55 | "thread": "cpp", 56 | "cfenv": "cpp", 57 | "tuple": "cpp", 58 | "type_traits": "cpp", 59 | "utility": "cpp", 60 | "typeindex": "cpp", 61 | "typeinfo": "cpp", 62 | "valarray": "cpp", 63 | "codecvt": "cpp", 64 | "algorithm": "cpp", 65 | "iterator": "cpp", 66 | "map": "cpp", 67 | "memory": "cpp", 68 | "memory_resource": "cpp", 69 | "optional": "cpp", 70 | "random": "cpp", 71 | "set": "cpp", 72 | "string": "cpp", 73 | "string_view": "cpp" 74 | } 75 | } -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.9) 2 | project(SaRA) 3 | set(CMAKE_BUILD_TYPE "Release") 4 | 5 | if(NOT CMAKE_BUILD_TYPE AND NOT CMAKE_CONFIGURATION_TYPES) 6 | message(STATUS "Setting build type to '${DEFAULT_BUILD_TYPE}' as none was specified.") 7 | set(CMAKE_BUILD_TYPE "${DEFAULT_BUILD_TYPE}" CACHE STRING "Choose the type of build." FORCE) 8 | # Set the possible values of build type for cmake-gui 9 | set_property(CACHE CMAKE_BUILD_TYPE PROPERTY STRINGS "Debug" "Release" "MinSizeRel" "RelWithDebInfo") 10 | endif() 11 | 12 | include(GNUInstallDirs) 13 | 14 | #Bring the headers, such as Student.h into the project 15 | #include_directories(include) 16 | 17 | #However, the file(GLOB...) allows for wildcard additions: 18 | file(GLOB SOURCES "src/*.cpp") 19 | #set(SOURCES src/Point.cpp src/Capsule.cpp) 20 | 21 | #Generate the shared library from the sources [SHARED / STATIC?] 22 | add_library(${PROJECT_NAME} SHARED ${SOURCES}) 23 | 24 | target_include_directories(${PROJECT_NAME} PUBLIC 25 | $ 26 | $ 27 | )#PRIVATE src) 28 | 29 | set_target_properties(${PROJECT_NAME} PROPERTIES 30 | VERSION 1.0.0 31 | SOVERSION 1) 32 | 33 | install(TARGETS ${PROJECT_NAME} EXPORT ${PROJECT_NAME}Config 34 | ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} 35 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 36 | RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR}) 37 | 38 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/${PROJECT_NAME}) 39 | 40 | install(EXPORT ${PROJECT_NAME}Config DESTINATION share/${PROJECT_NAME}/cmake) 41 | 42 | export(TARGETS ${PROJECT_NAME} FILE ${PROJECT_NAME}Config.cmake) 43 | 44 | 45 | #set_target_properties(${PROJECT_NAME} PROPERTIES PUBLIC_HEADER include/reach_lib.hpp) 46 | 47 | #target_include_directories(${PROJECT_NAME} PRIVATE include) 48 | #target_include_directories(${PROJECT_NAME} PRIVATE src) 49 | 50 | #Set the location for library installation -- i.e., /usr/lib in this case 51 | # not really necessary in this example. Use "sudo make install" to apply 52 | #include(GNUInstallDirs) 53 | #install(TARGETS ${PROJECT_NAME} 54 | # LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 55 | # PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CMakeCCompiler.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_C_COMPILER "/usr/lib/ccache/cc") 2 | set(CMAKE_C_COMPILER_ARG1 "") 3 | set(CMAKE_C_COMPILER_ID "GNU") 4 | set(CMAKE_C_COMPILER_VERSION "7.5.0") 5 | set(CMAKE_C_COMPILER_VERSION_INTERNAL "") 6 | set(CMAKE_C_COMPILER_WRAPPER "") 7 | set(CMAKE_C_STANDARD_COMPUTED_DEFAULT "11") 8 | set(CMAKE_C_COMPILE_FEATURES "c_std_90;c_function_prototypes;c_std_99;c_restrict;c_variadic_macros;c_std_11;c_static_assert") 9 | set(CMAKE_C90_COMPILE_FEATURES "c_std_90;c_function_prototypes") 10 | set(CMAKE_C99_COMPILE_FEATURES "c_std_99;c_restrict;c_variadic_macros") 11 | set(CMAKE_C11_COMPILE_FEATURES "c_std_11;c_static_assert") 12 | 13 | set(CMAKE_C_PLATFORM_ID "Linux") 14 | set(CMAKE_C_SIMULATE_ID "") 15 | set(CMAKE_C_SIMULATE_VERSION "") 16 | 17 | 18 | 19 | set(CMAKE_AR "/usr/bin/ar") 20 | set(CMAKE_C_COMPILER_AR "/usr/bin/gcc-ar-7") 21 | set(CMAKE_RANLIB "/usr/bin/ranlib") 22 | set(CMAKE_C_COMPILER_RANLIB "/usr/bin/gcc-ranlib-7") 23 | set(CMAKE_LINKER "/usr/bin/ld") 24 | set(CMAKE_COMPILER_IS_GNUCC 1) 25 | set(CMAKE_C_COMPILER_LOADED 1) 26 | set(CMAKE_C_COMPILER_WORKS TRUE) 27 | set(CMAKE_C_ABI_COMPILED TRUE) 28 | set(CMAKE_COMPILER_IS_MINGW ) 29 | set(CMAKE_COMPILER_IS_CYGWIN ) 30 | if(CMAKE_COMPILER_IS_CYGWIN) 31 | set(CYGWIN 1) 32 | set(UNIX 1) 33 | endif() 34 | 35 | set(CMAKE_C_COMPILER_ENV_VAR "CC") 36 | 37 | if(CMAKE_COMPILER_IS_MINGW) 38 | set(MINGW 1) 39 | endif() 40 | set(CMAKE_C_COMPILER_ID_RUN 1) 41 | set(CMAKE_C_SOURCE_FILE_EXTENSIONS c;m) 42 | set(CMAKE_C_IGNORE_EXTENSIONS h;H;o;O;obj;OBJ;def;DEF;rc;RC) 43 | set(CMAKE_C_LINKER_PREFERENCE 10) 44 | 45 | # Save compiler ABI information. 46 | set(CMAKE_C_SIZEOF_DATA_PTR "8") 47 | set(CMAKE_C_COMPILER_ABI "ELF") 48 | set(CMAKE_C_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 49 | 50 | if(CMAKE_C_SIZEOF_DATA_PTR) 51 | set(CMAKE_SIZEOF_VOID_P "${CMAKE_C_SIZEOF_DATA_PTR}") 52 | endif() 53 | 54 | if(CMAKE_C_COMPILER_ABI) 55 | set(CMAKE_INTERNAL_PLATFORM_ABI "${CMAKE_C_COMPILER_ABI}") 56 | endif() 57 | 58 | if(CMAKE_C_LIBRARY_ARCHITECTURE) 59 | set(CMAKE_LIBRARY_ARCHITECTURE "x86_64-linux-gnu") 60 | endif() 61 | 62 | set(CMAKE_C_CL_SHOWINCLUDES_PREFIX "") 63 | if(CMAKE_C_CL_SHOWINCLUDES_PREFIX) 64 | set(CMAKE_CL_SHOWINCLUDES_PREFIX "${CMAKE_C_CL_SHOWINCLUDES_PREFIX}") 65 | endif() 66 | 67 | 68 | 69 | 70 | 71 | set(CMAKE_C_IMPLICIT_LINK_LIBRARIES "gcc;gcc_s;c;gcc;gcc_s") 72 | set(CMAKE_C_IMPLICIT_LINK_DIRECTORIES "/usr/lib/gcc/x86_64-linux-gnu/7;/usr/lib/x86_64-linux-gnu;/usr/lib;/lib/x86_64-linux-gnu;/lib") 73 | set(CMAKE_C_IMPLICIT_LINK_FRAMEWORK_DIRECTORIES "") 74 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_C.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_C.bin -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_CXX.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CMakeDetermineCompilerABI_CXX.bin -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CMakeSystem.cmake: -------------------------------------------------------------------------------- 1 | set(CMAKE_HOST_SYSTEM "Linux-5.4.0-81-generic") 2 | set(CMAKE_HOST_SYSTEM_NAME "Linux") 3 | set(CMAKE_HOST_SYSTEM_VERSION "5.4.0-81-generic") 4 | set(CMAKE_HOST_SYSTEM_PROCESSOR "x86_64") 5 | 6 | 7 | 8 | set(CMAKE_SYSTEM "Linux-5.4.0-81-generic") 9 | set(CMAKE_SYSTEM_NAME "Linux") 10 | set(CMAKE_SYSTEM_VERSION "5.4.0-81-generic") 11 | set(CMAKE_SYSTEM_PROCESSOR "x86_64") 12 | 13 | set(CMAKE_CROSSCOMPILING "FALSE") 14 | 15 | set(CMAKE_SYSTEM_LOADED 1) 16 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CompilerIdC/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CompilerIdC/a.out -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CompilerIdCXX/a.out: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/3.10.2/CompilerIdCXX/a.out -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/CMakeDirectoryInformation.cmake: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # Relative path conversion top directories. 5 | set(CMAKE_RELATIVE_PATH_TOP_SOURCE "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib") 6 | set(CMAKE_RELATIVE_PATH_TOP_BINARY "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build") 7 | 8 | # Force unix paths in dependencies. 9 | set(CMAKE_FORCE_UNIX_PATHS 1) 10 | 11 | 12 | # The C and CXX include file regular expressions for this directory. 13 | set(CMAKE_C_INCLUDE_REGEX_SCAN "^.*$") 14 | set(CMAKE_C_INCLUDE_REGEX_COMPLAIN "^$") 15 | set(CMAKE_CXX_INCLUDE_REGEX_SCAN ${CMAKE_C_INCLUDE_REGEX_SCAN}) 16 | set(CMAKE_CXX_INCLUDE_REGEX_COMPLAIN ${CMAKE_C_INCLUDE_REGEX_COMPLAIN}) 17 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/Makefile2: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # Default target executed when no arguments are given to make. 5 | default_target: all 6 | 7 | .PHONY : default_target 8 | 9 | # The main recursive all target 10 | all: 11 | 12 | .PHONY : all 13 | 14 | # The main recursive preinstall target 15 | preinstall: 16 | 17 | .PHONY : preinstall 18 | 19 | #============================================================================= 20 | # Special targets provided by cmake. 21 | 22 | # Disable implicit rules so canonical targets will work. 23 | .SUFFIXES: 24 | 25 | 26 | # Remove some rules from gmake that .SUFFIXES does not remove. 27 | SUFFIXES = 28 | 29 | .SUFFIXES: .hpux_make_needs_suffix_list 30 | 31 | 32 | # Suppress display of executed commands. 33 | $(VERBOSE).SILENT: 34 | 35 | 36 | # A target that is always out of date. 37 | cmake_force: 38 | 39 | .PHONY : cmake_force 40 | 41 | #============================================================================= 42 | # Set environment variables for the build. 43 | 44 | # The shell in which to execute make rules. 45 | SHELL = /bin/sh 46 | 47 | # The CMake executable. 48 | CMAKE_COMMAND = /usr/bin/cmake 49 | 50 | # The command to remove a file. 51 | RM = /usr/bin/cmake -E remove -f 52 | 53 | # Escaping for special characters. 54 | EQUALS = = 55 | 56 | # The top-level source directory on which CMake was run. 57 | CMAKE_SOURCE_DIR = /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib 58 | 59 | # The top-level build directory on which CMake was run. 60 | CMAKE_BINARY_DIR = /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build 61 | 62 | #============================================================================= 63 | # Target rules for target CMakeFiles/SaRA.dir 64 | 65 | # All Build rule for target. 66 | CMakeFiles/SaRA.dir/all: 67 | $(MAKE) -f CMakeFiles/SaRA.dir/build.make CMakeFiles/SaRA.dir/depend 68 | $(MAKE) -f CMakeFiles/SaRA.dir/build.make CMakeFiles/SaRA.dir/build 69 | @$(CMAKE_COMMAND) -E cmake_echo_color --switch=$(COLOR) --progress-dir=/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles --progress-num=1,2,3,4,5,6,7,8,9,10,11,12,13,14,15,16,17,18,19 "Built target SaRA" 70 | .PHONY : CMakeFiles/SaRA.dir/all 71 | 72 | # Include target in all. 73 | all: CMakeFiles/SaRA.dir/all 74 | 75 | .PHONY : all 76 | 77 | # Build rule for subdir invocation for target. 78 | CMakeFiles/SaRA.dir/rule: cmake_check_build_system 79 | $(CMAKE_COMMAND) -E cmake_progress_start /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles 19 80 | $(MAKE) -f CMakeFiles/Makefile2 CMakeFiles/SaRA.dir/all 81 | $(CMAKE_COMMAND) -E cmake_progress_start /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles 0 82 | .PHONY : CMakeFiles/SaRA.dir/rule 83 | 84 | # Convenience name for target. 85 | SaRA: CMakeFiles/SaRA.dir/rule 86 | 87 | .PHONY : SaRA 88 | 89 | # clean rule for target. 90 | CMakeFiles/SaRA.dir/clean: 91 | $(MAKE) -f CMakeFiles/SaRA.dir/build.make CMakeFiles/SaRA.dir/clean 92 | .PHONY : CMakeFiles/SaRA.dir/clean 93 | 94 | # clean rule for target. 95 | clean: CMakeFiles/SaRA.dir/clean 96 | 97 | .PHONY : clean 98 | 99 | #============================================================================= 100 | # Special targets to cleanup operation of make. 101 | 102 | # Special rule to run CMake to check the build system integrity. 103 | # No rule that depends on this can have commands that come from listfiles 104 | # because they might be regenerated. 105 | cmake_check_build_system: 106 | $(CMAKE_COMMAND) -H$(CMAKE_SOURCE_DIR) -B$(CMAKE_BINARY_DIR) --check-build-system CMakeFiles/Makefile.cmake 0 107 | .PHONY : cmake_check_build_system 108 | 109 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/DependInfo.cmake: -------------------------------------------------------------------------------- 1 | # The set of languages for which implicit dependencies are needed: 2 | set(CMAKE_DEPENDS_LANGUAGES 3 | "CXX" 4 | ) 5 | # The set of files for implicit dependencies of each language: 6 | set(CMAKE_DEPENDS_CHECK_CXX 7 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/articulated.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated.cpp.o" 8 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/articulated_accel.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_accel.cpp.o" 9 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/articulated_pos.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_pos.cpp.o" 10 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/articulated_vel.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_vel.cpp.o" 11 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/body_part.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part.cpp.o" 12 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/body_part_accel.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part_accel.cpp.o" 13 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/body_part_vel.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part_vel.cpp.o" 14 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/capsule.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/capsule.cpp.o" 15 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/cylinder.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/cylinder.cpp.o" 16 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/cylinder_perimeter.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/cylinder_perimeter.cpp.o" 17 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/extremity.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/extremity.cpp.o" 18 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/occupancy.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/occupancy.cpp.o" 19 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/pedestrian.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian.cpp.o" 20 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/pedestrian_accel.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian_accel.cpp.o" 21 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/pedestrian_vel.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian_vel.cpp.o" 22 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/point.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/point.cpp.o" 23 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/sphere.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/sphere.cpp.o" 24 | "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/src/system.cpp" "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/system.cpp.o" 25 | ) 26 | set(CMAKE_CXX_COMPILER_ID "GNU") 27 | 28 | # The include file search paths: 29 | set(CMAKE_CXX_TARGET_INCLUDE_PATH 30 | "../include" 31 | ) 32 | 33 | # Targets to which this target links. 34 | set(CMAKE_TARGET_LINKED_INFO_FILES 35 | ) 36 | 37 | # Fortran module output directory. 38 | set(CMAKE_Fortran_TARGET_MODULE_DIR "") 39 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/cmake_clean.cmake: -------------------------------------------------------------------------------- 1 | file(REMOVE_RECURSE 2 | "CMakeFiles/SaRA.dir/src/articulated.cpp.o" 3 | "CMakeFiles/SaRA.dir/src/articulated_accel.cpp.o" 4 | "CMakeFiles/SaRA.dir/src/articulated_pos.cpp.o" 5 | "CMakeFiles/SaRA.dir/src/articulated_vel.cpp.o" 6 | "CMakeFiles/SaRA.dir/src/body_part.cpp.o" 7 | "CMakeFiles/SaRA.dir/src/body_part_accel.cpp.o" 8 | "CMakeFiles/SaRA.dir/src/body_part_vel.cpp.o" 9 | "CMakeFiles/SaRA.dir/src/capsule.cpp.o" 10 | "CMakeFiles/SaRA.dir/src/cylinder.cpp.o" 11 | "CMakeFiles/SaRA.dir/src/cylinder_perimeter.cpp.o" 12 | "CMakeFiles/SaRA.dir/src/extremity.cpp.o" 13 | "CMakeFiles/SaRA.dir/src/occupancy.cpp.o" 14 | "CMakeFiles/SaRA.dir/src/pedestrian.cpp.o" 15 | "CMakeFiles/SaRA.dir/src/pedestrian_accel.cpp.o" 16 | "CMakeFiles/SaRA.dir/src/pedestrian_vel.cpp.o" 17 | "CMakeFiles/SaRA.dir/src/point.cpp.o" 18 | "CMakeFiles/SaRA.dir/src/sphere.cpp.o" 19 | "CMakeFiles/SaRA.dir/src/system.cpp.o" 20 | "libSaRA.pdb" 21 | "libSaRA.a" 22 | ) 23 | 24 | # Per-language clean rules from dependency scanning. 25 | foreach(lang CXX) 26 | include(CMakeFiles/SaRA.dir/cmake_clean_${lang}.cmake OPTIONAL) 27 | endforeach() 28 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/cmake_clean_target.cmake: -------------------------------------------------------------------------------- 1 | file(REMOVE_RECURSE 2 | "libSaRA.a" 3 | ) 4 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/flags.make: -------------------------------------------------------------------------------- 1 | # CMAKE generated file: DO NOT EDIT! 2 | # Generated by "Unix Makefiles" Generator, CMake Version 3.10 3 | 4 | # compile CXX with /usr/lib/ccache/c++ 5 | CXX_FLAGS = -O3 -DNDEBUG 6 | 7 | CXX_DEFINES = 8 | 9 | CXX_INCLUDES = -I/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/include 10 | 11 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/link.txt: -------------------------------------------------------------------------------- 1 | /usr/bin/ar qc libSaRA.a CMakeFiles/SaRA.dir/src/articulated.cpp.o CMakeFiles/SaRA.dir/src/articulated_accel.cpp.o CMakeFiles/SaRA.dir/src/articulated_pos.cpp.o CMakeFiles/SaRA.dir/src/articulated_vel.cpp.o CMakeFiles/SaRA.dir/src/body_part.cpp.o CMakeFiles/SaRA.dir/src/body_part_accel.cpp.o CMakeFiles/SaRA.dir/src/body_part_vel.cpp.o CMakeFiles/SaRA.dir/src/capsule.cpp.o CMakeFiles/SaRA.dir/src/cylinder.cpp.o CMakeFiles/SaRA.dir/src/cylinder_perimeter.cpp.o CMakeFiles/SaRA.dir/src/extremity.cpp.o CMakeFiles/SaRA.dir/src/occupancy.cpp.o CMakeFiles/SaRA.dir/src/pedestrian.cpp.o CMakeFiles/SaRA.dir/src/pedestrian_accel.cpp.o CMakeFiles/SaRA.dir/src/pedestrian_vel.cpp.o CMakeFiles/SaRA.dir/src/point.cpp.o CMakeFiles/SaRA.dir/src/sphere.cpp.o CMakeFiles/SaRA.dir/src/system.cpp.o 2 | /usr/bin/ranlib libSaRA.a 3 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/progress.make: -------------------------------------------------------------------------------- 1 | CMAKE_PROGRESS_1 = 1 2 | CMAKE_PROGRESS_2 = 2 3 | CMAKE_PROGRESS_3 = 3 4 | CMAKE_PROGRESS_4 = 4 5 | CMAKE_PROGRESS_5 = 5 6 | CMAKE_PROGRESS_6 = 6 7 | CMAKE_PROGRESS_7 = 7 8 | CMAKE_PROGRESS_8 = 8 9 | CMAKE_PROGRESS_9 = 9 10 | CMAKE_PROGRESS_10 = 10 11 | CMAKE_PROGRESS_11 = 11 12 | CMAKE_PROGRESS_12 = 12 13 | CMAKE_PROGRESS_13 = 13 14 | CMAKE_PROGRESS_14 = 14 15 | CMAKE_PROGRESS_15 = 15 16 | CMAKE_PROGRESS_16 = 16 17 | CMAKE_PROGRESS_17 = 17 18 | CMAKE_PROGRESS_18 = 18 19 | CMAKE_PROGRESS_19 = 19 20 | 21 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_accel.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_accel.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_pos.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_pos.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_vel.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/articulated_vel.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part_accel.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part_accel.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part_vel.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/body_part_vel.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/capsule.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/capsule.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/cylinder.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/cylinder.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/cylinder_perimeter.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/cylinder_perimeter.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/extremity.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/extremity.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/occupancy.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/occupancy.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian_accel.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian_accel.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian_vel.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/pedestrian_vel.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/point.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/point.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/sphere.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/sphere.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/system.cpp.o: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir/src/system.cpp.o -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/TargetDirectories.txt: -------------------------------------------------------------------------------- 1 | /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/install/strip.dir 2 | /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/edit_cache.dir 3 | /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/SaRA.dir 4 | /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/list_install_components.dir 5 | /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/install/local.dir 6 | /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/rebuild_cache.dir 7 | /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/CMakeFiles/install.dir 8 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/cmake.check_cache: -------------------------------------------------------------------------------- 1 | # This file is generated by cmake for dependency checking of the CMakeCache.txt file 2 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/feature_tests.bin: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/CMakeFiles/feature_tests.bin -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/feature_tests.c: -------------------------------------------------------------------------------- 1 | 2 | const char features[] = {"\n" 3 | "C_FEATURE:" 4 | #if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304 5 | "1" 6 | #else 7 | "0" 8 | #endif 9 | "c_function_prototypes\n" 10 | "C_FEATURE:" 11 | #if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L 12 | "1" 13 | #else 14 | "0" 15 | #endif 16 | "c_restrict\n" 17 | "C_FEATURE:" 18 | #if (__GNUC__ * 100 + __GNUC_MINOR__) >= 406 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 201000L 19 | "1" 20 | #else 21 | "0" 22 | #endif 23 | "c_static_assert\n" 24 | "C_FEATURE:" 25 | #if (__GNUC__ * 100 + __GNUC_MINOR__) >= 304 && defined(__STDC_VERSION__) && __STDC_VERSION__ >= 199901L 26 | "1" 27 | #else 28 | "0" 29 | #endif 30 | "c_variadic_macros\n" 31 | 32 | }; 33 | 34 | int main(int argc, char** argv) { (void)argv; return features[argc]; } 35 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/CMakeFiles/progress.marks: -------------------------------------------------------------------------------- 1 | 19 2 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/cmake_install.cmake: -------------------------------------------------------------------------------- 1 | # Install script for directory: /home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib 2 | 3 | # Set the install prefix 4 | if(NOT DEFINED CMAKE_INSTALL_PREFIX) 5 | set(CMAKE_INSTALL_PREFIX "/usr/local") 6 | endif() 7 | string(REGEX REPLACE "/$" "" CMAKE_INSTALL_PREFIX "${CMAKE_INSTALL_PREFIX}") 8 | 9 | # Set the install configuration name. 10 | if(NOT DEFINED CMAKE_INSTALL_CONFIG_NAME) 11 | if(BUILD_TYPE) 12 | string(REGEX REPLACE "^[^A-Za-z0-9_]+" "" 13 | CMAKE_INSTALL_CONFIG_NAME "${BUILD_TYPE}") 14 | else() 15 | set(CMAKE_INSTALL_CONFIG_NAME "Release") 16 | endif() 17 | message(STATUS "Install configuration: \"${CMAKE_INSTALL_CONFIG_NAME}\"") 18 | endif() 19 | 20 | # Set the component getting installed. 21 | if(NOT CMAKE_INSTALL_COMPONENT) 22 | if(COMPONENT) 23 | message(STATUS "Install component: \"${COMPONENT}\"") 24 | set(CMAKE_INSTALL_COMPONENT "${COMPONENT}") 25 | else() 26 | set(CMAKE_INSTALL_COMPONENT) 27 | endif() 28 | endif() 29 | 30 | # Install shared libraries without execute permission? 31 | if(NOT DEFINED CMAKE_INSTALL_SO_NO_EXE) 32 | set(CMAKE_INSTALL_SO_NO_EXE "1") 33 | endif() 34 | 35 | # Is this installation the result of a crosscompile? 36 | if(NOT DEFINED CMAKE_CROSSCOMPILING) 37 | set(CMAKE_CROSSCOMPILING "FALSE") 38 | endif() 39 | 40 | if("x${CMAKE_INSTALL_COMPONENT}x" STREQUAL "xUnspecifiedx" OR NOT CMAKE_INSTALL_COMPONENT) 41 | list(APPEND CMAKE_ABSOLUTE_DESTINATION_FILES 42 | "/usr/lib/libSaRA.a") 43 | if(CMAKE_WARN_ON_ABSOLUTE_INSTALL_DESTINATION) 44 | message(WARNING "ABSOLUTE path INSTALL DESTINATION : ${CMAKE_ABSOLUTE_DESTINATION_FILES}") 45 | endif() 46 | if(CMAKE_ERROR_ON_ABSOLUTE_INSTALL_DESTINATION) 47 | message(FATAL_ERROR "ABSOLUTE path INSTALL DESTINATION forbidden (by caller): ${CMAKE_ABSOLUTE_DESTINATION_FILES}") 48 | endif() 49 | file(INSTALL DESTINATION "/usr/lib" TYPE STATIC_LIBRARY FILES "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/libSaRA.a") 50 | endif() 51 | 52 | if(CMAKE_INSTALL_COMPONENT) 53 | set(CMAKE_INSTALL_MANIFEST "install_manifest_${CMAKE_INSTALL_COMPONENT}.txt") 54 | else() 55 | set(CMAKE_INSTALL_MANIFEST "install_manifest.txt") 56 | endif() 57 | 58 | string(REPLACE ";" "\n" CMAKE_INSTALL_MANIFEST_CONTENT 59 | "${CMAKE_INSTALL_MANIFEST_FILES}") 60 | file(WRITE "/home/sven/catkin_ws/src/reachable_occupancy/include/ReachLib/build/${CMAKE_INSTALL_MANIFEST}" 61 | "${CMAKE_INSTALL_MANIFEST_CONTENT}") 62 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/build/libSaRA.a: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/build/libSaRA.a -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/articulated_accel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "articulated.hpp" 21 | #include "body_part_accel.hpp" 22 | #include "capsule.hpp" 23 | #include "obstacle.hpp" 24 | #include "system.hpp" 25 | 26 | #ifndef REACH_LIB_INCLUDE_ARTICULATED_ACCEL_HPP_ 27 | #define REACH_LIB_INCLUDE_ARTICULATED_ACCEL_HPP_ 28 | 29 | namespace obstacles { 30 | namespace articulated { 31 | namespace accel { 32 | 33 | //! \typedef A shortcut to the body part class for the ACCEL model 34 | typedef occupancies::body_parts::accel::BodyPartAccel BodyPartAccel; 35 | 36 | /*! This class defines our maximum acceleration based 37 | * full body articulated reachable occupancy model. 38 | * It recieves current Cartesian joint positions 39 | * and velocity vectors and determines occupancy using 40 | * static maximum acceleration parameters for all joints. 41 | */ 42 | class ArticulatedAccel : public Articulated { 43 | public: 44 | //! \brief Empty constructor 45 | ArticulatedAccel() : Articulated() {} 46 | 47 | //! \brief Instantiates the maximum acceleration based model from joint pairs. 48 | //! \param[in] system System parameters such as: delay and measurement errors 49 | //! \param[in] body_segment_map_ An association between joints and body segments 50 | //! \param[in] thickness Defines the thickness (diameter) of each body part [body part name, thickness] 51 | //! \param[in] max_a Maximum acceleration of each joint 52 | ArticulatedAccel(System system, std::map body_segment_map, 53 | const std::map& thickness, 54 | const std::vector& max_a); 55 | 56 | //! \brief Empty destructor 57 | ~ArticulatedAccel() {} 58 | 59 | //! \brief Calcualtes the current occupancy using the Articuated 'ACCEL' model 60 | //! \param[in] p Current joint positions in Cartesian coordinartes (x, y ,z) 61 | //! \param[in] v Current joint velocities 62 | //! \param[in] t_a Start of the interval of analysis 63 | //! \param[in] t_b End of the interval of analysis 64 | std::vector update(double t_a, double t_b, 65 | std::vector p, 66 | std::vector v = {}); 67 | 68 | //! \brief Returns true if the current occupancy intersects with 69 | //! any given point in 'targets' 70 | //! \param[in] targets A list of points in global Cartesian coordinates (x, y, z) 71 | //! checked against the current occupancy 72 | bool intersection(std::vector targets); 73 | 74 | //! \brief Returns the mode of reachability analysis 75 | //! of this class as 'ACCEL' 76 | std::string get_mode() { 77 | return "ARICULATED-ACCEL"; 78 | } 79 | 80 | //! \brief Returns the current occupancy as a list of body parts 81 | std::vector get_occupancy() { 82 | return this->occupancy_; 83 | } 84 | 85 | private: 86 | //! \brief Contains the most recent state of all occupancy segments 87 | std::vector occupancy_ = {}; 88 | }; 89 | } // namespace accel 90 | } // namespace articulated 91 | } // namespace obstacles 92 | #endif // REACH_LIB_INCLUDE_ARTICULATED_ACCEL_HPP_ 93 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/articulated_vel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "articulated.hpp" 21 | #include "body_part_vel.hpp" 22 | #include "capsule.hpp" 23 | #include "obstacle.hpp" 24 | #include "system.hpp" 25 | 26 | #ifndef REACH_LIB_INCLUDE_ARTICULATED_VEL_HPP_ 27 | #define REACH_LIB_INCLUDE_ARTICULATED_VEL_HPP_ 28 | 29 | namespace obstacles { 30 | namespace articulated { 31 | namespace vel { 32 | 33 | //! \typedef A shortcut to the body part class for the VEL model 34 | typedef occupancies::body_parts::vel::BodyPartVel BodyPartVel; 35 | 36 | /*! This class defines our maximum velocity based 37 | * full body articulated reachable occupancy model. 38 | * It recieves current Cartesian joint positions 39 | * and determines occupancy using 40 | * static maximum velocity parameters for all joints. 41 | */ 42 | class ArticulatedVel : public Articulated { 43 | public: 44 | //! \brief Empty constructor 45 | ArticulatedVel() : Articulated() {} 46 | 47 | //! \brief Instantiates the maximum velocity based model from joint pairs. 48 | //! \param[in] system System parameters such as: delay and measurement errors 49 | //! \param[in] body_segment_map_ An association between joints and body segments 50 | //! \param[in] thickness Defines the thickness (diameter) of each body part [body part name, thickness] 51 | //! \param[in] max_v Maximum velocity of each joint 52 | ArticulatedVel(System system, std::map body_segment_map, 53 | const std::map& thickness, 54 | const std::vector& max_v); 55 | 56 | //! \brief Empty destructor 57 | ~ArticulatedVel() {} 58 | 59 | //! \brief Calcualtes the current occupancy using the Articuated 'ACCEL' model 60 | //! \param[in] p Current joint positions in Cartesian coordinartes (x, y ,z) 61 | //! \param[in] v Current joint velocities 62 | //! \param[in] t_a Start of the interval of analysis 63 | //! \param[in] t_b End of the interval of analysis 64 | std::vector update(double t_a, double t_b, 65 | std::vector p, 66 | std::vector v = {}); 67 | 68 | //! \brief Returns true if the current occupancy intersects with 69 | //! any given point in 'targets' 70 | //! \param[in] targets A list of points in global Cartesian coordinates (x, y, z) 71 | //! checked against the current occupancy 72 | const bool intersection(std::vector targets); 73 | 74 | //! \brief Returns the mode of reachability analysis 75 | //! of this class as 'VEL' 76 | std::string get_mode() { 77 | return "ARICULATED-VEL"; 78 | } 79 | 80 | //! \brief Returns the current occupancy as a list of body parts 81 | std::vector get_occupancy() { 82 | return this->occupancy_; 83 | } 84 | 85 | private: 86 | //! \brief Contains the current occupancies 87 | std::vector occupancy_; 88 | }; 89 | } // namespace vel 90 | } // namespace articulated 91 | } // namespace obstacles 92 | #endif // REACH_LIB_INCLUDE_ARTICULATED_ACCEL_HPP_ 93 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/body_part.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "capsule.hpp" 19 | #include "occupancy.hpp" 20 | #include "point.hpp" 21 | 22 | #ifndef REACH_LIB_INCLUDE_BODY_PART_HPP_ 23 | #define REACH_LIB_INCLUDE_BODY_PART_HPP_ 24 | 25 | namespace occupancies { 26 | namespace body_parts { 27 | 28 | //! \typedef A shortcut to the Capsule type 29 | typedef occupancy_containers::capsule::Capsule Capsule; 30 | 31 | /*! This class defines the human as a construct 32 | * of body parts which describe their own occupancy. 33 | * A body part is defined by two joints and its thickness (diameter). 34 | * Body parts are used for the maximum velocity 35 | * and acceleration based approaches 36 | * and thus produce by two versions of occupancies: 37 | * 1. ArticulatedVel: Occupancy based on reachable space with constant velocity 38 | * 2. BodyPartAccel: Occuapancy based on live velocity measurements 39 | * and constant maximum acceleration 40 | */ 41 | class BodyPart : public Occupancy { 42 | public: 43 | //! \brief Empty constructor 44 | BodyPart(); 45 | 46 | //! \brief Instantiates a general body part 47 | //! \param[in] name Name of the body part 48 | //! \param[in] thickness Estimated thickness (diameter) of the body part 49 | BodyPart(std::string name, double thickness); 50 | 51 | //! \brief Empty destructor 52 | ~BodyPart() {} 53 | 54 | //! \brief Returns the current occupancy 55 | inline Capsule get_occupancy() { 56 | return this->occupancy_; 57 | } 58 | 59 | //! \brief Returns the thickness of the body part 60 | double get_thickness() { 61 | return this->thickness_; 62 | } 63 | 64 | protected: 65 | //! \brief Contains the current occupancy 66 | Capsule occupancy_ = Capsule(); 67 | 68 | //! \brief Thickness of the body part 69 | double thickness_; 70 | }; 71 | } // namespace body_parts 72 | } // namespace occupancies 73 | #endif // REACH_LIB_INCLUDE_BODY_PART_HPP_ 74 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/body_part_accel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "articulated.hpp" 19 | #include "body_part.hpp" 20 | #include "capsule.hpp" 21 | #include "occupancy.hpp" 22 | #include "point.hpp" 23 | 24 | #ifndef REACH_LIB_INCLUDE_BODY_PART_ACCEL_HPP_ 25 | #define REACH_LIB_INCLUDE_BODY_PART_ACCEL_HPP_ 26 | 27 | namespace occupancies{ 28 | namespace body_parts { 29 | namespace accel { 30 | 31 | class BodyPartAccel : public BodyPart { 32 | public: 33 | //! \brief Empty constructor 34 | BodyPartAccel() : BodyPart() {} 35 | 36 | //! \brief Instatiates a BodyPart that updates based on live velocities 37 | //! and maximum estimated acceleration. 38 | //! \param[in] name Name of the body part 39 | //! \param[in] thickness Estimated thickness (diameter) of the body part 40 | //! \param[in] max_a12 Estimated maximum acceleration of this body part 41 | BodyPartAccel(std::string name, double thickness, double max_a1 = 0.0, double max_a2 = 0.0); 42 | 43 | //! \brief Empty destructor 44 | ~BodyPartAccel() {} 45 | 46 | //! \brief Calcualtes the current occupancy using the Articuated model. 47 | //! Overrides the identical blueprint method from Occupancy. 48 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) size(p) = 2. 49 | //! \param[in] v Current joint velocity size(v) = 2. 50 | //! \param[in] t_a Start of the interval of analysis. 51 | //! \param[in] t_b End of the interval of analysis. 52 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m]. 53 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s]. 54 | //! \param[in] delay Time delay within the executing system. 55 | void update(const std::vector& p, 56 | const std::vector& v = {}, 57 | double t_a = 0.0, double t_b = 0.0, 58 | double measurement_error_pos = 0.0, 59 | double measurement_error_vel = 0.0, 60 | double delay = 0.0); 61 | 62 | //! \brief Determines if any point from the list 'targets' is located inside 63 | //! the current occupancy. 64 | bool intersection(std::vector targets); 65 | 66 | private: 67 | //! \brief Maximum estimated acceleration of the proximal joint (constant) 68 | double max_a1_ = 0.0; 69 | 70 | //! \brief Maximum estimated acceleration of the distal joint (constant) 71 | double max_a2_ = 0.0; 72 | 73 | //! \brief Determines the ball occupancy within [0.0, t_b] for one joint 74 | //! This needs to be performed for both joints before enclosing the occupacnies 75 | //! in one capsule during the updated (carried out in update()). 76 | //! \param[in] p Origin of the ball (Cartesian position of the joint) 77 | //! \param[in] v Velocity of the joint (Cartesian) 78 | //! \param[in] index Determines the joint of which ry is calculated; -> from [1,2]. 79 | //! \param[in] t_b End point of the time interval 80 | //! \param[in] delay Time delay within the executing system 81 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 82 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s] 83 | Capsule ry(const Point& p, const Point& v, 84 | int index, double t_b = 0.02, double delay = 0.0, 85 | double measurement_error_pos = 0.0, 86 | double measurement_error_vel = 0.0); 87 | }; 88 | } // namespace accel 89 | } // namespace body_parts 90 | } // namespace occupancies 91 | #endif // REACH_LIB_INCLUDE_BODY_PART_ACCEL_HPP_ 92 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/body_part_vel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "body_part.hpp" 19 | #include "capsule.hpp" 20 | #include "occupancy.hpp" 21 | #include "point.hpp" 22 | 23 | #ifndef REACH_LIB_INCLUDE_BODY_PART_VEL_HPP_ 24 | #define REACH_LIB_INCLUDE_BODY_PART_VEL_HPP_ 25 | 26 | namespace occupancies { 27 | namespace body_parts { 28 | namespace vel { 29 | 30 | class BodyPartVel : public BodyPart { 31 | public: 32 | //! \brief Empty constructor 33 | BodyPartVel() : BodyPart() {} 34 | 35 | //! \brief Instatiates a BodyPart that updates based on 36 | //! maximum estimated velocities. 37 | //! \param[in] name Name of the body part 38 | //! \param[in] thickness Estimated thickness (diameter) of the body part 39 | //! \param[in] max_a Estimated maximum velocity of this body part 40 | BodyPartVel(std::string name, double thickness, double max_a1 = 0.0, double max_a2 = 0.0); 41 | 42 | //! \brief Empty destructor 43 | ~BodyPartVel() {} 44 | 45 | //! \brief Calcualtes the current occupancy using any model 46 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) 47 | //! \param[in] v Unused overriden parameter 48 | //! \param[in] t_a Start of the interval of analysis 49 | //! \param[in] t_b End of the interval of analysis 50 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 51 | //! \param[in] measurement_error_vel Unused overriden parameter 52 | //! \param[in] delay Time delay within the executing system 53 | void update(const std::vector& p, 54 | const std::vector& v = {}, 55 | double t_a = 0.0, double t_b = 0.0, 56 | double measurement_error_pos = 0.0, 57 | double measurement_error_vel = 0.0, 58 | double delay = 0.0); 59 | 60 | //! \brief Determines if any point from the list 'targets' is located inside 61 | //! the current occupancy. 62 | const bool intersection(std::vector targets); 63 | 64 | private: 65 | //! \brief Maximum estimated velocity of the proximal joint (constant) 66 | double max_v1_ = 0.0; 67 | 68 | //! \brief Maximum estimated velocity of the distal joint (constant) 69 | double max_v2_ = 0.0; 70 | 71 | //! \brief Determines the ball occupancy within [0.0, t_end] for one joint 72 | //! This needs to be performed for both joints before enclosing the occupacnies 73 | //! in one capsule during the updated (carried out in update()). 74 | //! \param[in] p Origin of the ball 75 | //! \param[in] index Indicates the proximal (1) or distal (2) link 76 | //! \param[in] t_end End point of the time interval 77 | //! \param[in] delay Time delay within the executing system 78 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 79 | Capsule ry(const Point& p, int index, double t_end = 0.02, 80 | double delay = 0.0, double measurement_error_pos = 0.0); 81 | }; 82 | } // namespace vel 83 | } // namespace body_parts 84 | } // namespace occupancies 85 | #endif // REACH_LIB_INCLUDE_BODY_PART_VEL_HPP_ 86 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/cylinder.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "occupancy_container.hpp" 18 | #include "point.hpp" 19 | 20 | #ifndef REACH_LIB_INCLUDE_CYLINDER_HPP_ 21 | #define REACH_LIB_INCLUDE_CYLINDER_HPP_ 22 | 23 | namespace occupancy_containers { 24 | namespace cylinder { 25 | //! This class defines Cylinder based occupancies 26 | //! used in the pedestrian model defiend in: 27 | //! http://mediatum.ub.tum.de/doc/1379662/905746.pdf 28 | //! and static velocity based perimeters 29 | class Cylinder : public OccupancyContainer { 30 | public: 31 | //! \brief Radius of the cylinder 32 | double r_ = 0.0; 33 | 34 | //! \brief Center of the cylinders initial top circle 35 | Point p1_ = Point(); 36 | 37 | //! \brief Center of the cylinders initial bottom circle 38 | Point p2_ = Point(); 39 | 40 | //! \brief Empty constructor 41 | Cylinder() {} 42 | 43 | //! \brief Instantiates a cylinder occupancy 44 | //! \param[in] p A list of two points serving as 45 | //! top and bottom indicators for the cylinder 46 | //! \param[in] r Radius of the cylinder 47 | Cylinder(const Point& p1, const Point& p2, double r); 48 | 49 | //! \brief Empty destructor 50 | ~Cylinder() {} 51 | 52 | //! \brief Determines whether there exists an intersection between 53 | //! a cylinder and any point within targets. 54 | //! \param[in] targets List of points of interes in Cartesian (x y z) 55 | const bool intersection(const std::vector& targets); 56 | 57 | //! \brief Determines whether this cylinder and a capsule c intersect. 58 | //! \param[in] c A Capsule object checked against this cylinder 59 | //! -> We can't allow circular inclusion 60 | // bool intersection(const Capsule& c); 61 | }; 62 | } // namespace cylinder 63 | } // namespace occupancy_containers 64 | #endif // REACH_LIB_INCLUDE_CYLINDER_HPP_ 65 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/cylinder_perimeter.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder.hpp" 19 | #include "occupancy.hpp" 20 | #include "point.hpp" 21 | 22 | #ifndef REACH_LIB_INCLUDE_CYLINDER_PERIMETER_HPP_ 23 | #define REACH_LIB_INCLUDE_CYLINDER_PERIMETER_HPP_ 24 | 25 | namespace occupancies { 26 | namespace cylinder_perimeters { 27 | //! \typedef Defines the Capsule datatype within this namespace 28 | typedef occupancy_containers::cylinder::Cylinder Cylinder; 29 | 30 | 31 | //! \brief Defines the occupancy model of the pedestrian approach 32 | //! from http://mediatum.ub.tum.de/doc/1379662/905746.pdf. 33 | //! Contains a Cylinder for the occupancy of the desired interval [t_start, t_end]. 34 | //! Contains a list of cylinder sub occupancies for intervals from within [0.0, t_end]. 35 | class CylinderPerimeter : public Occupancy { 36 | public: 37 | //! \brief Empty constructor 38 | CylinderPerimeter(); 39 | 40 | //! \brief Instatiates a list of cylinders that 41 | //! can be used to define occupancies of 42 | //! the pedestrian model from: 43 | //! http://mediatum.ub.tum.de/doc/1379662/905746.pdf 44 | //! \param[in] init_pos Initial position of the cylinder 45 | //! \param[in] height Estimated height of the human 46 | CylinderPerimeter(const Point& init_pos, double height, double radius, std::string name = ""); 47 | 48 | //! \brief Emtpy destructor 49 | ~CylinderPerimeter() {} 50 | 51 | //! Determines if the cylinder intersects with 52 | //! any point from within the targest list 53 | //! \param[in] Points of interest for inertsections 54 | const bool intersection(std::vector targets); 55 | 56 | //! \brief Calcualtes the current occupancy using the Pedestrian model 57 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) 58 | //! \param[in] v Current joint velocity 59 | //! \param[in] t_a Start of the interval of analysis 60 | //! \param[in] t_b End of the interval of analysis 61 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 62 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s] 63 | //! \param[in] delay Time delay within the executing system 64 | void update(const std::vector& p, 65 | const std::vector& v = {}, 66 | double t_a = 0.0, double t_b = 0.0, 67 | double measurement_error_pos = 0.0, 68 | double measurement_error_vel = 0.0, 69 | double delay = 0.0) { 70 | throw "Function occupancies::cylinder_perimeters::CylinderPerimeter::update is not defined!"; 71 | } 72 | 73 | //! \brief Returns the current occupancy 74 | inline Cylinder get_occupancy() { 75 | return this->occupancy_; 76 | } 77 | 78 | //! \brief Returns the list of cylindrical partial occupancies 79 | inline std::vector get_cylinder_list() { 80 | return this->cylinder_list_; 81 | } 82 | 83 | private: 84 | //! \brief Contains the current occupancy 85 | Cylinder occupancy_ = Cylinder(); 86 | 87 | //! \brief The desired occupancy interval split up into smaller intervals 88 | //! represented by a list of cylinders 89 | std::vector cylinder_list_ = {}; 90 | }; 91 | } // namespace cylinder_perimeters 92 | } // namespace occupancies 93 | #endif // REACH_LIB_INCLUDE_CYLINDER_PERIMETER_HPP_ 94 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/extremity: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/include/ReachLib/include/extremity -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/extremity.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "capsule.hpp" 19 | #include "occupancy.hpp" 20 | #include "point.hpp" 21 | 22 | #ifndef REACH_LIB_INCLUDE_EXTREMITY_HPP_ 23 | #define REACH_LIB_INCLUDE_EXTREMITY_HPP_ 24 | 25 | namespace occupancies { 26 | namespace extremities { 27 | 28 | //! \typedef A shortcut to the Capsule type 29 | typedef occupancy_containers::capsule::Capsule Capsule; 30 | 31 | 32 | /*! This class defines the human as a construct 33 | * of extremities which describe their own occupancy. 34 | * An extreity is defined by a joint, its thickness (diameter), and length. 35 | * extremities are used for the maximum position based approache. 36 | * They describe a sphere of constant radius about their joint. 37 | */ 38 | class Extremity : public Occupancy { 39 | public: 40 | //! \brief Empty constructor 41 | Extremity(); 42 | 43 | //! \brief Instantiates an extremity 44 | Extremity(std::string name, double thickness, 45 | double length, double max_v); 46 | 47 | //! \brief Empty destructor 48 | ~Extremity() {} 49 | 50 | 51 | //! \brief Calcualtes the current occupancy using the ArticulatedPos model 52 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) 53 | //! \param[in] v Current joint velocity 54 | //! \param[in] t_a Start of the interval of analysis 55 | //! \param[in] t_b End of the interval of analysis 56 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 57 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s] 58 | //! \param[in] delay Time delay within the executing system 59 | void update(const std::vector& p, 60 | const std::vector& v = {}, 61 | double t_a = 0.0, double t_b = 0.0, 62 | double measurement_error_pos = 0.0, 63 | double measurement_error_vel = 0.0, 64 | double delay = 0.0); 65 | 66 | //! \brief Returns true if the current occupancy intersects with 67 | //! any given point in 'targets' 68 | //! \param[in] targets A list of points in global Cartesian coordinates (x, y, z) 69 | //! checked against the current occupancy 70 | const bool intersection(std::vector targets); 71 | 72 | //! \brief Returns the current occupancy 73 | inline Capsule get_occupancy() { 74 | return this->occupancy_; 75 | } 76 | 77 | //! \brief Returns the thickness of the extremity 78 | double get_thickness() { 79 | return this->thickness_; 80 | } 81 | 82 | //! \brief Returns the length of the extremity 83 | double get_length() { 84 | return this->length_; 85 | } 86 | 87 | private: 88 | //! \brief Conains the current occupancy 89 | Capsule occupancy_ = Capsule(); 90 | 91 | //! \brief Thickness of the extremity 92 | double thickness_ = 0.0; 93 | 94 | //! \brief Length of the extremity 95 | double length_ = 0.0; 96 | 97 | //! \brief Maximum assumed velocity of the extremity 98 | double max_v_ = 0.0; 99 | }; 100 | } // namespace extremities 101 | } // namespace occupancies 102 | #endif // REACH_LIB_INCLUDE_EXTREMITY_HPP_ 103 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/obstacle.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "occupancy.hpp" 18 | #include "point.hpp" 19 | #include "system.hpp" 20 | 21 | #ifndef REACH_LIB_INCLUDE_SAFETY_PERIMETER_HPP_ 22 | #define REACH_LIB_INCLUDE_SAFETY_PERIMETER_HPP_ 23 | 24 | 25 | namespace obstacles { 26 | 27 | //! \typedef A shortcut to the Point class 28 | typedef point::Point Point; 29 | 30 | //! \typedef A shortcut to the System class 31 | typedef systems::System System; 32 | 33 | //! This class serves as a blueprint for all 34 | //! safety perimeter definitions. 35 | //! This presently includes Articulated and Pedestrian models. 36 | class Obstacle { 37 | public: 38 | //! \brief Empty constructor 39 | Obstacle() {} 40 | 41 | //! \brief Initializes the sensor attribute containing 42 | //! system parameters. 43 | explicit Obstacle(System sensor) : system(sensor) {} 44 | 45 | //! \brief Empty destructor 46 | ~Obstacle() {} 47 | 48 | //! \brief Returns true if the current occupancy intersects with 49 | //! any given point in 'targets' 50 | //! \param[in] targets A list of points in global Cartesian coordinates (x, y, z) 51 | //! checked against the current occupancy 52 | const bool intersection(std::vector targets) { 53 | throw "Function safety_perimeters::Obstacle::intersection is not defined!"; 54 | } 55 | 56 | //! \brief Returns Pointers to the current occupancies of the respective model 57 | inline std::vector get_occupancy_p() { 58 | return this->occupancy_p; 59 | } 60 | 61 | //! \brief Returns the system parameters that the model was initialized with 62 | inline System get_system() { 63 | return this->system; 64 | } 65 | 66 | protected: 67 | //! \brief Containins system information (measurement errors/delay) 68 | System system = System(); 69 | 70 | //! \brief Contains pointers to the most recent state of all occupancy segments 71 | std::vector occupancy_p = {}; 72 | }; 73 | } // namespace obstacles 74 | #endif // REACH_LIB_INCLUDE_SAFETY_PERIMETER_HPP_ 75 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/occupancy.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "occupancy_container.hpp" 20 | #include "point.hpp" 21 | 22 | #ifndef REACH_LIB_INCLUDE_OCCUPANCY_MODEL_HPP_ 23 | #define REACH_LIB_INCLUDE_OCCUPANCY_MODEL_HPP_ 24 | 25 | 26 | namespace occupancies { 27 | //! \typedef A shortcut to the Point type 28 | typedef point::Point Point; 29 | 30 | //! \typedef A shortcut to the OccupancyContainer type 31 | typedef occupancy_containers::OccupancyContainer OccupancyContainer; 32 | 33 | //! This class defines the structure of all accepted 34 | //! models of occupancy. This includes a container for 35 | //! the current occupancy as well as an intersection check. 36 | //! Occupancy can be defined using two models: 37 | //! 1. BodySegments: A part of the human body [body part, extremity] 38 | //! 2. SafetyField: A cylinder encompassing reachable space of a pedestrian 39 | class Occupancy { 40 | public: 41 | //! \brief Empty costructor 42 | Occupancy() {} 43 | 44 | //! \brief Initializes a named object of underterined type among Occupancys 45 | explicit Occupancy(std::string name); 46 | 47 | //! \brief Empty destructor 48 | virtual ~Occupancy() {} 49 | 50 | //! \brief Returns true if the current occupancy intersects with 51 | //! any given point in 'targets' 52 | //! \param[in] targets A list of points in global Cartesian coordinates (x, y, z) 53 | //! checked against the current occupancy 54 | const bool intersection(std::vector targets) { 55 | throw "Function occupancy_models::Occupancy::intersection is not defined!"; 56 | } 57 | 58 | //! \brief Calcualtes the current occupancy using any model 59 | //! \param[in] p Current joint position in Cartesian coordinartes (x, y ,z) 60 | //! \param[in] v Current joint velocity 61 | //! \param[in] t_a Start of the interval of analysis 62 | //! \param[in] t_b End of the interval of analysis 63 | //! \param[in] measurement_error_pos Measurement errors in cartesian position [m] 64 | //! \param[in] measurement_error_vel Measurement errors in cartesian velocity [m/s] 65 | //! \param[in] delay Time delay within the executing system 66 | void update(const std::vector& p, 67 | const std::vector& v = {}, 68 | double t_a = 0.0, double t_b = 0.0, 69 | double measurement_error_pos = 0.0, 70 | double measurement_error_vel = 0.0, 71 | double delay = 0.0) { 72 | throw "Function occupancy_models::Occupancy::intersection is not defined!"; 73 | } 74 | 75 | //! \brief Returns the occupancy according to its occupancy model 76 | //! Throws a NULL-pointer exception if the occupancy pointer is not set 77 | OccupancyContainer* get_occupancy_p() { 78 | return this->occupancy_p; 79 | } 80 | 81 | //! \brief Returns the name of the body part 82 | std::string get_name() { 83 | return this->name_; 84 | } 85 | 86 | protected: 87 | //! \brief Points to the current occupancy (for dynamic cast purposes) 88 | OccupancyContainer* occupancy_p = NULL; 89 | 90 | //! \brief A name that defines this occupancy 91 | std::string name_ = ""; 92 | }; 93 | } // namespace occupancies 94 | #endif // REACH_LIB_INCLUDE_OCCUPANCY_MODEL_HPP_ 95 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/occupancy_container.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "point.hpp" 18 | 19 | #ifndef REACH_LIB_INCLUDE_OCCUPANCY_CONTAINER_HPP_ 20 | #define REACH_LIB_INCLUDE_OCCUPANCY_CONTAINER_HPP_ 21 | 22 | namespace occupancy_containers { 23 | //! \typedef Point describes the Point class within the point namespace 24 | typedef point::Point Point; 25 | 26 | //! This class describes all types of media that 27 | //! can contain reachable occupancies. 28 | //! They contain subsets of the overall reachable 29 | //! occupancy and are used to build OccupancyModels. 30 | class OccupancyContainer { 31 | public: 32 | //! \brief Empty constructor 33 | OccupancyContainer() {} 34 | 35 | //! \brief Empty destructor 36 | virtual ~OccupancyContainer() {} 37 | 38 | //! \brief Determines whether there exists an intersection between 39 | //! an occupancy container and any point within targets. 40 | //! Throws an exception if the type of OccupancyContainer 41 | //! can not be determined. 42 | //! \param[in] targets List of points of interes in Cartesian (x y z) 43 | bool intersection(const std::vector& targets) { 44 | throw "Function occupancy_containers::OccupancyContainer intersection is not defined!"; 45 | } 46 | }; 47 | } // namespace occupancy_containers 48 | #endif // REACH_LIB_INCLUDE_OCCUPANCY_CONTAINER_HPP_ 49 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/pedestrian_accel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder.hpp" 19 | #include "cylinder_perimeter.hpp" 20 | #include "pedestrian.hpp" 21 | #include "point.hpp" 22 | #include "obstacle.hpp" 23 | #include "system.hpp" 24 | 25 | #ifndef REACH_LIB_INCLUDE_PEDESTRIAN_ACCEL_HPP_ 26 | #define REACH_LIB_INCLUDE_PEDESTRIAN_ACCEL_HPP_ 27 | 28 | namespace obstacles { 29 | namespace pedestrian { 30 | namespace accel { 31 | 32 | //! \brief Defines a cylindrical safety perimeter based on dynamic extensions 33 | //! to static aprroaches (ISO10218: https://www.iso.org/standard/51330.html) 34 | //! described by http://mediatum.ub.tum.de/doc/1379662/905746.pdf. 35 | //! Encorporates live velocity measurements and constant maximum acceleration 36 | //! into occupancy calculations. 37 | class PedestrianAccel : public Pedestrian { 38 | public: 39 | //! \brief Empty constructor 40 | PedestrianAccel() : Pedestrian() {} 41 | 42 | //! \brief Instantiates a cylindrical safety perimeter 43 | //! based on constant maximum acceleration 44 | //! and live velocity values. 45 | //! \param[in] max_a Maximum acceleration [m/s] (constant) 46 | PedestrianAccel(const System& sensor, double height, 47 | double arm_span, double max_a, double offset = 0.0, 48 | const Point& init_pos = Point(), 49 | int steps = 0); 50 | 51 | //! \brief Calcualtes the current occupancy using this model 52 | //! \param[in] p Current location of points of interest in Cartesian coordinartes (x, y ,z) 53 | //! \param[in] v Current velocity of points of interest [static velocity is used here] 54 | //! \param[in] t_a Start of the interval of analysis 55 | //! \param[in] t_b End of the interval of analysis 56 | std::vector update(double t_a, double t_b, 57 | std::vector p, 58 | std::vector v); 59 | 60 | //! \brief Determines if any point from the list 'targets' is located inside 61 | //! the current occupancy. 62 | //! \param[in] targets A list of points of interest to be checked against 63 | //! the current occupancy. 64 | const bool intersection(std::vector targets); 65 | 66 | //! \brief Returns the mode of reachability analysis 67 | //! of this class as 'PEDESTRIAN-ACCEL' 68 | std::string get_mode() { 69 | return "PEDESTRIAN-ACCEL"; 70 | } 71 | 72 | //! \brief Returns the maximum acceleration parameter 73 | double get_max_a() { 74 | return this->max_a_; 75 | } 76 | 77 | //! \brief Emtpy destructor 78 | ~PedestrianAccel() {} 79 | 80 | private: 81 | //! \brief Maximum acceleration of the human (constant) 82 | double max_a_ = 0.0; 83 | 84 | //! \brief The number of cylinders formed from intermediate intervals 85 | int steps_ = 0.0; 86 | 87 | //! \brief A list of intermediary occupacies 88 | std::vector cylinder_list_ = {}; 89 | }; 90 | } // namespace accel 91 | } // namespace pedestrian 92 | } // namespace obstacles 93 | #endif // REACH_LIB_INCLUDE_PEDESTRIAN_ACCEL_HPP_ 94 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/pedestrian_vel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder_perimeter.hpp" 19 | #include "occupancy.hpp" 20 | #include "pedestrian.hpp" 21 | #include "point.hpp" 22 | #include "obstacle.hpp" 23 | #include "system.hpp" 24 | 25 | #ifndef REACH_LIB_INCLUDE_PEDESTRIAN_VEL_HPP_ 26 | #define REACH_LIB_INCLUDE_PEDESTRIAN_VEL_HPP_ 27 | 28 | namespace obstacles { 29 | namespace pedestrian { 30 | namespace vel { 31 | 32 | //! \brief Defines a cylindrical safety perimeter based on 33 | //! definiions in ISO10218: https://www.iso.org/standard/51330.html and 34 | //! descriptions by http://mediatum.ub.tum.de/doc/1379662/905746.pdf. 35 | //! Uses constant maximum velocity estimates for occupancy calculations. 36 | class PedestrianVel : public Pedestrian { 37 | public: 38 | //! \brief Empty constructor 39 | PedestrianVel() : Pedestrian() {} 40 | 41 | //! \brief Instantiates a cylindrical safety perimeter 42 | //! based on a constant maximum velocity 43 | //! \param[in] max_v Maximum velocity (constant) 44 | PedestrianVel(const System& sensor, double height, 45 | double arm_span, double max_v, double offset = 0.0, 46 | const Point& init_pos = Point()); 47 | 48 | //! \brief Calcualtes the current occupancy using this model 49 | //! \param[in] p Current location of points of interest in Cartesian coordinartes (x, y ,z) 50 | //! \param[in] v Current velocity of points of interest [static velocity is used here] 51 | //! \param[in] t_a Start of the interval of analysis 52 | //! \param[in] t_b End of the interval of analysis 53 | std::vector update(double t_a, double t_b, 54 | std::vector p, 55 | std::vector v = {}); 56 | 57 | //! \brief Determines if any point from the list 'targets' is located inside 58 | //! the current occupancy. 59 | //! \param[in] targets A list of points of interest to be checked against 60 | //! the current occupancy. 61 | const bool intersection(std::vector targets); 62 | 63 | //! \brief Returns the mode of reachability analysis 64 | //! of this class as 'PEDESTRIAN-VEL' 65 | std::string get_mode() { 66 | return "PEDESTRIAN-VEL"; 67 | } 68 | 69 | //! \brief Returns the maximum velocity parameter 70 | double get_max_v() { 71 | return this->max_v_; 72 | } 73 | 74 | //! \brief Emtpy destructor 75 | ~PedestrianVel() {} 76 | 77 | private: 78 | //! \brief Maximum velocity of the human (constant) 79 | double max_v_ = 0.0; 80 | }; 81 | } // namespace vel 82 | } // namespace pedestrian 83 | } // namespace obstacles 84 | #endif // REACH_LIB_INCLUDE_PEDESTRIAN_VEL_HPP_ 85 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/sphere.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "occupancy_container.hpp" 18 | #include "point.hpp" 19 | 20 | #ifndef REACH_LIB_INCLUDE_SPHERE_HPP_ 21 | #define REACH_LIB_INCLUDE_SPHERE_HPP_ 22 | 23 | namespace occupancy_containers { 24 | namespace sphere { 25 | 26 | class Sphere : public OccupancyContainer { 27 | public: 28 | //! \brief Origin of the sphere 29 | Point p_ = Point(); 30 | 31 | //! \brief Radius of the sphere 32 | double r_ = 0.0; 33 | 34 | //! \brief Empty constructor 35 | Sphere() {} 36 | 37 | //! \brief Instatiates an object of type Sphere 38 | Sphere(Point p, double r); 39 | 40 | //! \brief Empty destructor 41 | ~Sphere() {} 42 | 43 | //! \brief Determines whether there exists an intersection between 44 | //! this Sphere and any point within targets. 45 | //! \param[in] targets List of points of interes in Cartesian (x y z) 46 | bool intersection(const std::vector& targets); 47 | }; 48 | } // namespace sphere 49 | } // namespace occupancy_containers 50 | #endif // REACH_LIB_INCLUDE_SPHERE_HPP_ 51 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/include/system.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #ifndef REACH_LIB_INCLUDE_SYSTEM_HPP_ 16 | #define REACH_LIB_INCLUDE_SYSTEM_HPP_ 17 | 18 | namespace systems { 19 | 20 | //! This class contains parameters that characterize properties 21 | //! of the hardware and software system used for sensing and preparation of 22 | //! position and velocity data. 23 | class System { 24 | public: 25 | //! \brief Defines the positional measurement error of the underlying system 26 | double measurement_error_pos_ = 0.0; 27 | 28 | //! \brief Defines the measurement error of the underlying system for recorded velocities 29 | double measurement_error_vel_ = 0.0; 30 | 31 | //! \brief Defines the delay imposed by sensing and preparation of incoming signals 32 | double delay_ = 0.0; 33 | 34 | //! \brief Instantiates an object of type System 35 | //! \param[in] measurement_error_pos The positional measurement error 36 | //! of the underlying system 37 | //! \param[in] measurement_error_vel The measurement error of recorded velocities 38 | //! of the underlying system 39 | //! \param[in] delay The delay imposed by sensing and preparation of incoming signals 40 | System(double measurement_error_pos = 0.0, 41 | double measurement_error_vel = 0.0, 42 | double delay = 0.0); 43 | 44 | //! \brief Empty destructor 45 | ~System() {} 46 | }; 47 | } // namespace system 48 | #endif // REACH_LIB_INCLUDE_SYSTEM_HPP_ 49 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/articulated.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "articulated.hpp" 21 | #include "capsule.hpp" 22 | #include "obstacle.hpp" 23 | #include "system.hpp" 24 | 25 | namespace obstacles { 26 | namespace articulated { 27 | Articulated::Articulated(System system, 28 | std::map body_segment_map) : 29 | body_segment_map_(body_segment_map), Obstacle(system) { 30 | // NO TODO 31 | } 32 | } // namespace articulated 33 | } // namespace obstacles 34 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/articulated_accel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "articulated.hpp" 21 | #include "articulated_accel.hpp" 22 | #include "body_part_accel.hpp" 23 | #include "capsule.hpp" 24 | #include "obstacle.hpp" 25 | #include "system.hpp" 26 | 27 | 28 | namespace obstacles { 29 | namespace articulated { 30 | namespace accel { 31 | 32 | ArticulatedAccel::ArticulatedAccel(System system, std::map body_segment_map, 33 | const std::map& thickness, 34 | const std::vector& max_a) : 35 | Articulated(system, body_segment_map) { 36 | // Create a list of BodyPartAccel that is later set as occpancy_ 37 | std::vector body = {}; 38 | for (const auto& it : body_segment_map) { 39 | body.push_back(BodyPartAccel(it.first, thickness.at(it.first), max_a.at(it.second.first), 40 | max_a.at(it.second.second))); 41 | } 42 | this->occupancy_ = body; 43 | // Initialize pointers 44 | for (int i = 0; i < this->occupancy_.size(); i++) { 45 | this->occupancy_p.push_back(&(this->occupancy_[i])); 46 | } 47 | } 48 | 49 | std::vector ArticulatedAccel::update(double t_a, double t_b, 50 | std::vector p, 51 | std::vector v) { 52 | // std::cout << "Length: " << this->get_occupancy_().size() << "\n"; 53 | int count = 0; 54 | for (auto& it : this->occupancy_) { 55 | int p1_id = this->body_segment_map_.at(it.get_name()).first; 56 | int p2_id = this->body_segment_map_.at(it.get_name()).second; 57 | it.update({p[p1_id], p[p2_id]}, {v[p1_id], v[p2_id]}, t_a, t_b, 58 | this->system.measurement_error_pos_, 59 | this->system.measurement_error_vel_, 60 | this->system.delay_); 61 | this->occupancy_[count] = it; 62 | count++; 63 | } 64 | return this->occupancy_; 65 | } 66 | 67 | bool ArticulatedAccel::intersection(std::vector targets) { 68 | for (auto& it : this->occupancy_) { 69 | if (it.intersection(targets)) { 70 | return true; 71 | } 72 | } 73 | return false; 74 | } 75 | } // namespace accel 76 | } // namespace articulated 77 | } // namespace obstacles 78 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/articulated_pos.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | #include "articulated.hpp" 22 | #include "articulated_pos.hpp" 23 | #include "extremity.hpp" 24 | #include "capsule.hpp" 25 | #include "obstacle.hpp" 26 | #include "system.hpp" 27 | 28 | 29 | namespace obstacles { 30 | namespace articulated { 31 | namespace pos { 32 | 33 | ArticulatedPos::ArticulatedPos(System system, std::map body_segment_map, 34 | const std::vector& thickness, 35 | const std::vector& max_v, 36 | const std::vector& length) : 37 | Articulated(system, body_segment_map) { 38 | // Create a list of BodyPartsAccel that is later set as occpancy_ 39 | assert(thickness.size() == max_v.size()); 40 | assert(thickness.size() == length.size()); 41 | std::vector body = {}; 42 | int i=0; 43 | for (const auto& it : body_segment_map) { 44 | body.push_back(Extremity(it.first, thickness[i], length[i], max_v[i])); 45 | i++; 46 | } 47 | this->occupancy_ = body; 48 | // Initialize pointers 49 | for (int i = 0; i < this->occupancy_.size(); i++) { 50 | this->occupancy_p.push_back(&(this->occupancy_[i])); 51 | } 52 | } 53 | 54 | std::vector ArticulatedPos::update(double t_a, double t_b, 55 | std::vector p, 56 | std::vector v) { 57 | int count = 0; 58 | for (auto& it : this->occupancy_) { 59 | int p_id = this->body_segment_map_.at(it.get_name()).first; 60 | it.update({p[p_id]}, {}, t_a, t_b, 61 | this->system.measurement_error_pos_, 0.0, 62 | this->system.delay_); 63 | this->occupancy_[count] = it; 64 | count++; 65 | } 66 | return this->occupancy_; 67 | } 68 | 69 | const bool ArticulatedPos::intersection(std::vector targets) { 70 | for (auto& it : this->occupancy_) { 71 | if (it.intersection(targets)) { 72 | return true; 73 | } 74 | } 75 | return false; 76 | } 77 | } // namespace pos 78 | } // namespace articulated 79 | } // namespace obstacles 80 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/articulated_vel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include "articulated.hpp" 21 | #include "articulated_vel.hpp" 22 | #include "body_part_vel.hpp" 23 | #include "capsule.hpp" 24 | #include "obstacle.hpp" 25 | #include "system.hpp" 26 | 27 | 28 | namespace obstacles { 29 | namespace articulated { 30 | namespace vel { 31 | 32 | ArticulatedVel::ArticulatedVel(System system, std::map body_segment_map, 33 | const std::map& thickness, 34 | const std::vector& max_v) : 35 | Articulated(system, body_segment_map) { 36 | // Create a list of BodyPartsAccel that is later set as occpancy_ 37 | std::vector body = {}; 38 | for (const auto& it : body_segment_map) { 39 | body.push_back(BodyPartVel(it.first, thickness.at(it.first), max_v.at(it.second.first), 40 | max_v.at(it.second.second))); 41 | } 42 | this->occupancy_ = body; 43 | // Initialize pointers 44 | for (int i = 0; i < this->occupancy_.size(); i++) { 45 | this->occupancy_p.push_back(&(this->occupancy_[i])); 46 | } 47 | } 48 | 49 | std::vector ArticulatedVel::update(double t_a, double t_b, 50 | std::vector p, 51 | std::vector v) { 52 | int count = 0; 53 | for (auto& it : this->occupancy_) { 54 | int p1_id = this->body_segment_map_.at(it.get_name()).first; 55 | int p2_id = this->body_segment_map_.at(it.get_name()).second; 56 | it.update({p[p1_id], p[p2_id]}, {}, t_a, t_b, 57 | this->system.measurement_error_pos_, 0.0, 58 | this->system.delay_); 59 | this->occupancy_[count] = it; 60 | count++; 61 | } 62 | return this->occupancy_; 63 | } 64 | 65 | const bool ArticulatedVel::intersection(std::vector targets) { 66 | for (auto& it : this->occupancy_) { 67 | if (it.intersection(targets)) { 68 | return true; 69 | } 70 | } 71 | return false; 72 | } 73 | } // namespace vel 74 | } // namespace articulated 75 | } // namespace obstacles 76 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/body_part.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "body_part.hpp" 19 | #include "capsule.hpp" 20 | #include "occupancy.hpp" 21 | #include "point.hpp" 22 | 23 | namespace occupancies { 24 | namespace body_parts { 25 | 26 | BodyPart::BodyPart() : Occupancy() { 27 | this->occupancy_p = &this->occupancy_; 28 | } 29 | 30 | BodyPart::BodyPart(std::string name, double thickness) : thickness_(thickness), Occupancy(name) { 31 | // NO TODO 32 | } 33 | } // namespace body_parts 34 | } // namespace occupancies 35 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/body_part_vel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "body_part.hpp" 20 | #include "body_part_vel.hpp" 21 | #include "capsule.hpp" 22 | #include "occupancy.hpp" 23 | #include "point.hpp" 24 | 25 | 26 | 27 | namespace occupancies { 28 | namespace body_parts { 29 | namespace vel { 30 | 31 | BodyPartVel::BodyPartVel(std::string name, double thickness, 32 | double max_v1, double max_v2) : 33 | BodyPart(name, thickness), 34 | max_v1_(max_v1), max_v2_(max_v2) { 35 | // NO TODO 36 | } 37 | 38 | const bool BodyPartVel::intersection(std::vector targets) { 39 | return this->occupancy_.intersection(targets); 40 | } 41 | 42 | void BodyPartVel::update(const std::vector& p, 43 | const std::vector& v, 44 | double t_a, double t_b, 45 | double measurement_error_pos, 46 | double measurement_error_vel, 47 | double delay) { 48 | // The vectors must contain exactly 2 points 49 | assert(("Vector p must have exactly two entries for BodyPartAccel", size(p) == 2)); 50 | 51 | // translate vector entries to proximal and distal joint values 52 | Point p1 = p[0]; 53 | Point p2 = p[1]; 54 | 55 | // Check whether this capsule is a ball 56 | bool b; 57 | if (p1 == p2) { 58 | b = true; 59 | } else { 60 | b = false; 61 | } 62 | 63 | // Calculate the occupancies of the proximal and distal joint up to t_a 64 | Capsule rp1_t1 = BodyPartVel::ry(p1, 1, t_a, delay, 65 | measurement_error_pos); 66 | Capsule rp1_t2 = BodyPartVel::ry(p1, 1, t_b, delay, 67 | measurement_error_pos); // was p2 68 | Capsule b1 = Capsule::ballEnclosure(rp1_t1, rp1_t2); 69 | 70 | if (b) { 71 | // Add the thickness (as radius) 72 | rp1_t2.r_ += this->thickness_/2.0; 73 | 74 | this->occupancy_ = rp1_t2; // was rp1_t1 75 | } else { 76 | Capsule rp2_t1 = BodyPartVel::ry(p2, 2, t_a, delay, 77 | measurement_error_pos); // was p1 78 | Capsule rp2_t2 = BodyPartVel::ry(p2, 2, t_b, delay, 79 | measurement_error_pos); 80 | Capsule b2 = Capsule::ballEnclosure(rp2_t1, rp2_t2); 81 | 82 | // Add the thickness (as radius) to the ball with the larger radius (determines capsule radius) 83 | if (b1.r_ >= b2.r_) { 84 | b1.r_ += this->thickness_/2.0; 85 | } else { 86 | b2.r_ += this->thickness_/2.0; 87 | } 88 | this->occupancy_ = Capsule::capsuleEnclosure(b1, b2); 89 | } 90 | } 91 | 92 | Capsule BodyPartVel::ry(const Point& p, int index, double t_b, 93 | double delay, double measurement_error_pos) { 94 | double max_v; 95 | if (index == 1) { 96 | max_v = this->max_v1_; 97 | } else { 98 | max_v = this->max_v2_; 99 | } 100 | 101 | // RP 102 | return Capsule(p, p, measurement_error_pos + max_v * (t_b + delay)); 103 | } 104 | 105 | } // namespace vel 106 | } // namespace body_parts 107 | } // namespace occupancies 108 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/cylinder.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "cylinder.hpp" 18 | #include "occupancy_container.hpp" 19 | #include "point.hpp" 20 | 21 | namespace occupancy_containers { 22 | namespace cylinder { 23 | 24 | Cylinder::Cylinder(const Point& p1, const Point& p2, double r) : 25 | p1_(p1), p2_(p2), r_(r) { 26 | // NO TODO 27 | } 28 | 29 | const bool Cylinder::intersection(const std::vector& targets) { 30 | bool intersect = false; 31 | for (const auto& p : targets) { 32 | // The cylinder is defined by a line segment from 33 | // the center of the top circle to the center of the bottom circle. 34 | Point d = this->p2_ - this->p1_; 35 | Point p1d = p - this->p1_; 36 | Point p2d = p - this->p2_; 37 | double h = Point::norm(d); 38 | double hsq = pow(h, 2); 39 | double dotprod = Point::inner_dot(p1d, d); 40 | 41 | // Check whether the point lies higher or lower than the line segment 42 | // in the cylinders coordinates 43 | // If the point lies outside the cylinders domain given an infinite radius 44 | // (point lies higher or lower than the cylinders top/bottom): 45 | if ((dotprod < 0.0) || (dotprod > hsq)) { 46 | return false; 47 | // If the point lies next to the line segment in the cylinders coordinates: 48 | // check if the shortes point to line distance exceeds the cylinders radius 49 | // if yes: no intersection 50 | } else { 51 | double rsq = pow(this->r_, 2); 52 | double dsq = Point::inner_dot(p1d, p1d) - ((dotprod*dotprod)/hsq); 53 | if (dsq > rsq) { 54 | return false; 55 | } else { 56 | intersect = true; 57 | } 58 | } 59 | } 60 | return intersect; 61 | } 62 | 63 | } // namespace cylinder 64 | } // namespace occupancy_containers 65 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/cylinder_perimeter.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder.hpp" 19 | #include "cylinder_perimeter.hpp" 20 | #include "occupancy.hpp" 21 | #include "point.hpp" 22 | 23 | namespace occupancies { 24 | namespace cylinder_perimeters { 25 | 26 | CylinderPerimeter::CylinderPerimeter() : Occupancy() { 27 | this->occupancy_p = &this->occupancy_; 28 | } 29 | 30 | CylinderPerimeter::CylinderPerimeter(const Point& init_pos, double height, double radius, std::string name) : 31 | Occupancy(name) { 32 | Cylinder c = Cylinder(init_pos, Point(init_pos.x, init_pos.y, height), radius); 33 | this->occupancy_ = c; 34 | this->cylinder_list_.push_back(c); 35 | } 36 | 37 | const bool CylinderPerimeter::intersection(std::vector targets) { 38 | return this->occupancy_.intersection(targets); 39 | } 40 | } // namespace cylinder_perimeters 41 | } // namespace occupancies 42 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/extremity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | #include 18 | 19 | #include "capsule.hpp" 20 | #include "extremity.hpp" 21 | #include "occupancy.hpp" 22 | #include "point.hpp" 23 | 24 | 25 | namespace occupancies { 26 | namespace extremities { 27 | 28 | Extremity::Extremity() : Occupancy() { 29 | this->occupancy_p = &this->occupancy_; 30 | } 31 | 32 | Extremity::Extremity(std::string name, double thickness, 33 | double length, double max_v) : 34 | thickness_(thickness), length_(length), 35 | max_v_(max_v), Occupancy(name) { 36 | // NO TODO 37 | } 38 | 39 | void Extremity::update(const std::vector& p, 40 | const std::vector& v, 41 | double t_a, double t_b, 42 | double measurement_error_pos, 43 | double measurement_error_vel, 44 | double delay) { 45 | // The vectors must contain exactly 2 points 46 | assert(("Vector p must have exactly two entries for BodyPartAccel", size(p) == 1)); 47 | // translate vector entries to proximal and distal joint values 48 | Point pe = p[0]; 49 | // Total time passed within the desired interval 50 | double delta_t = t_b - t_a + delay; 51 | // Distance covered at max_v over delta_t 52 | double dist = this->max_v_ * delta_t; 53 | // Radius of the ball occupancy (thickness added as radius) 54 | double radius = dist + this->length_ + this->thickness_/2.0 + measurement_error_pos; 55 | this->occupancy_ = Capsule(pe, pe, radius); 56 | } 57 | 58 | const bool Extremity::intersection(std::vector targets) { 59 | return this->occupancy_.intersection(targets); 60 | } 61 | 62 | } // namespace extremities 63 | } // namespace occupancies 64 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/intersections.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of Reach-RI. 3 | 4 | Reach-RI is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | Reach-RI is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include "intersections.hpp" 16 | 17 | namespace occupancy_containers { 18 | namespace intersections { 19 | double segmentsDistance(const Capsule& c1, const Capsule& c2) { 20 | Point d1 = c1.p2_ - c1.p1_; 21 | Point d2 = c2.p2_ - c2.p1_; 22 | Point r = c2.p1_ - c1.p1_; 23 | double a = Point::inner_dot(d1, d1); 24 | double e = Point::inner_dot(d2, d2); 25 | double f = Point::inner_dot(d2, r); 26 | double epsilon = 1e-6; 27 | 28 | double t = 0.0; 29 | double s = 0.0; 30 | // [p1,q1] is actually a point && [p2,q2] is actually a point 31 | if (a <= epsilon && e <= epsilon) { 32 | // distance = norm(p2-p1) 33 | return Point::inner_dot(r, r); 34 | } else { 35 | double c = Point::inner_dot(d1, r); 36 | if (a <= epsilon) { 37 | s = 0; 38 | t = clamp(f/e, 0.0, 1.0); 39 | } else if (e < epsilon) { 40 | t = 0; 41 | s = clamp(-c/a, 0.0, 1.0); 42 | } else { 43 | double b = Point::inner_dot(d1, d2); 44 | double denom = a*e - b*b; 45 | if (denom != 0) { 46 | s = clamp((b*f - c*e)/denom, 0.0, 1.0); 47 | } else { 48 | s = 0; 49 | } 50 | t = (b*s + f)/e; 51 | if (t < 0) { 52 | t = 0; 53 | s = clamp(-c/a, 0.0, 1.0); 54 | } else if (t > 1) { 55 | t = 1; 56 | s = clamp((b - c)/a, 0.0, 1.0); 57 | } 58 | } 59 | Point closest_point_1 = get_point_from_line_segment(c1, static_cast(s)); 60 | Point closest_point_2 = get_point_from_line_segment(c2, static_cast(t)); 61 | return Point::norm(closest_point_1, closest_point_2); 62 | } 63 | } 64 | } // namespace intersections 65 | } // namespace occupancy_containers -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/occupancy.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "occupancy_container.hpp" 19 | #include "occupancy.hpp" 20 | #include "point.hpp" 21 | 22 | namespace occupancies { 23 | 24 | Occupancy::Occupancy(std::string name) : name_(name) { 25 | // NO TODO 26 | } 27 | } // namspace occupancies 28 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/pedestrian.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder_perimeter.hpp" 19 | #include "pedestrian.hpp" 20 | #include "point.hpp" 21 | #include "obstacle.hpp" 22 | #include "system.hpp" 23 | 24 | namespace obstacles { 25 | namespace pedestrian { 26 | 27 | Pedestrian::Pedestrian(System sensor, double height, double arm_span, double offset, 28 | const Point& init_pos) : 29 | height_(height), arm_span_(arm_span), offset_(offset), 30 | Obstacle(sensor) { 31 | this->occupancy_ = {CylinderPerimeter(init_pos, height, 0.0)}; 32 | } 33 | 34 | } // namespace pedestrian 35 | } // namespace obstacles 36 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/pedestrian_vel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "cylinder_perimeter.hpp" 19 | #include "pedestrian.hpp" 20 | #include "pedestrian_vel.hpp" 21 | #include "point.hpp" 22 | #include "obstacle.hpp" 23 | #include "system.hpp" 24 | 25 | 26 | namespace obstacles { 27 | namespace pedestrian { 28 | namespace vel { 29 | 30 | PedestrianVel::PedestrianVel(const System& sensor, double height, 31 | double arm_span, double max_v, double offset, 32 | const Point& init_pos) : 33 | Pedestrian(sensor, height, arm_span, offset, init_pos), 34 | max_v_(max_v) { 35 | // NO TODO 36 | } 37 | 38 | const bool PedestrianVel::intersection(std::vector targets) { 39 | bool intersection = false; 40 | for (const auto& it : targets) { 41 | intersection = this->occupancy_[0].intersection(targets); 42 | } 43 | return intersection; 44 | } 45 | 46 | std::vector PedestrianVel::update(double t_a, double t_b, 47 | std::vector p, 48 | std::vector v) { 49 | // Get the position of the cylinder using the equations of motion: x = x0 + max_v*t 50 | double vel_rad = 0.5*this->arm_span_ + this->system.measurement_error_pos_ + 51 | (this->max_v_ + this->system.measurement_error_vel_) * (t_b + this->system.delay_); 52 | 53 | this->occupancy_ = {CylinderPerimeter(Point(p[0].x, p[0].y, this->offset_), this->height_, vel_rad)}; 54 | return this->occupancy_; 55 | } 56 | 57 | 58 | 59 | } // namespace vel 60 | } // namespace pedestrian 61 | } // namespace obstacles 62 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/point.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "point.hpp" 19 | 20 | namespace point { 21 | 22 | Point::Point(double x, double y, double z) : x(x), y(y), z(z) { 23 | // NO TODO 24 | } 25 | 26 | Point Point::origin(const Point& p1, const Point& p2) { 27 | Point p = p1 - p2; 28 | p.x = p2.x + p.x/2.0; 29 | p.y = p2.y + p.y/2.0; 30 | p.z = p2.z + p.z/2.0; 31 | return p; 32 | } 33 | 34 | double Point::norm(const Point& p) { 35 | return sqrt(pow(p.x, 2) + pow(p.y, 2) + pow(p.z, 2)); 36 | } 37 | 38 | double Point::norm(const Point& p1, const Point& p2) { 39 | return sqrt(pow(p1.x - p2.x, 2) + pow(p1.y - p2.y, 2) + pow(p1.z - p2.z, 2)); 40 | } 41 | 42 | double Point::inner_dot(const Point& p1, const Point& p2) { 43 | return p1.x * p2.x + p1.y * p2.y + p1.z * p2.z; 44 | } 45 | 46 | Point Point::cross(const Point& p1, const Point& p2) { 47 | Point c = Point(); 48 | c.x = p1.y*p2.z - p1.z*p2.y; 49 | c.y = p1.z*p2.x - p1.x*p2.z; 50 | c.z = p1.x*p2.y - p1.y*p2.x; 51 | return c; 52 | } 53 | 54 | double Point::determinant3x3(Point X, Point Y, Point Z) { 55 | // A = [X, Y, Z] = [[a b c], [d e f], [g h i]] 56 | // det(A) = a(ei - fh) - b(di - fg) + c(dh - eg) 57 | return X.x * (Y.y * Z.z - Y.z * Z.y) - 58 | X.y * (Y.x * Z.z - Y.z * Z.x) + 59 | X.z * (Y.x * Z.y - Y.y * Z.x); 60 | } 61 | 62 | // legacy (not required) 63 | quaternion Point::orientation(const Point& p1, const Point& p2) { 64 | Point p1_loc = p1; 65 | Point p2_loc = p2; 66 | if (p2.z < p1.z) { 67 | Point temp = p1; 68 | p1_loc = p2_loc; 69 | p2_loc = temp; 70 | } 71 | 72 | double dis = norm(p1_loc, p2_loc); 73 | double f = 0.0174533; 74 | double r = 0.0; 75 | double p = 0.0; 76 | double y = 0.0; 77 | 78 | if (dis == 0) { 79 | p = 0.0; 80 | } else { 81 | p = -asin((p2_loc.y - p1_loc.y) / dis); 82 | } 83 | 84 | if (cos(p) == 0.0 ||dis == 0.0) { 85 | y = 0.0; 86 | } else { 87 | y = asin((p2_loc.x - p1_loc.x)/(cos(p) * dis)); 88 | } 89 | 90 | // capsules are roll invariant 91 | 92 | double cy = cos(y * 0.5); 93 | double sy = sin(y * 0.5); 94 | double cp = cos(p * 0.5); 95 | double sp = sin(p * 0.5); 96 | double cr = cos(r * 0.5); 97 | double sr = sin(r * 0.5); 98 | 99 | // q := (x,y,z,w) 100 | // q.w = cr * cp * cy + sr * sp * sy; 101 | // q.x = sr * cp * cy - cr * sp * sy; 102 | // q.y = cr * sp * cy + sr * cp * sy; 103 | // q.z = cr * cp * sy - sr * sp * cy; 104 | 105 | return std::make_tuple(sr * cp * cy - cr * sp * sy, 106 | cr * sp * cy + sr * cp * sy, 107 | cr * cp * sy - sr * sp * cy, 108 | cr * cp * cy + sr * sp * sy); 109 | } 110 | } // namespace point 111 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/sphere.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | 17 | #include "occupancy_container.hpp" 18 | #include "point.hpp" 19 | #include "sphere.hpp" 20 | 21 | namespace occupancy_containers { 22 | namespace sphere { 23 | 24 | Sphere::Sphere(Point p, double r) : p_(p), r_(r) { 25 | // NO TODO 26 | } 27 | 28 | bool Sphere::intersection(const std::vector& targets) { 29 | double intersection = false; 30 | for (const auto& it : targets) { 31 | if (Point::norm(it, this->p_) < 0) { 32 | intersection = true; 33 | } 34 | } 35 | return intersection; 36 | } 37 | } // namespace sphere 38 | } // namespace occupancy_containers 39 | -------------------------------------------------------------------------------- /reachable_occupancy/include/ReachLib/src/system.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include "system.hpp" 16 | 17 | namespace systems { 18 | 19 | System::System(double measurement_error_pos, 20 | double measurement_error_vel, 21 | double delay) : 22 | measurement_error_pos_(measurement_error_pos), 23 | measurement_error_vel_(measurement_error_vel), 24 | delay_(delay) { 25 | // NO TODO 26 | } 27 | } // namespace system 28 | -------------------------------------------------------------------------------- /reachable_occupancy/launch/SaRA_demo.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /reachable_occupancy/launch/reachable_occupancy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /reachable_occupancy/launch/udp.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | -------------------------------------------------------------------------------- /reachable_occupancy/launch/udp_translation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /reachable_occupancy/msg/Capsule.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string name 3 | geometry_msgs/Point p1 4 | geometry_msgs/Point p2 5 | float64 r 6 | -------------------------------------------------------------------------------- /reachable_occupancy/msg/CapsuleArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string name 3 | reachable_occupancy/Capsule[] capsules -------------------------------------------------------------------------------- /reachable_occupancy/msg/Cylinder.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string name 3 | geometry_msgs/Point p 4 | float64 r -------------------------------------------------------------------------------- /reachable_occupancy/msg/CylinderArray.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | string name 3 | reachable_occupancy/Cylinder[] cylinders -------------------------------------------------------------------------------- /reachable_occupancy/msg/JointLocations.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | # This contains an array of Points as defined by geometry_msgs 4 | # They represent the joint positions in Cartesian (x y z) space. 5 | geometry_msgs/Point[] positions 6 | 7 | # They represent the joint velocities in Cartesian (x y z) space. 8 | geometry_msgs/Point[] velocities 9 | 10 | # The time between the last two frame transmissions 11 | float64 delta_t -------------------------------------------------------------------------------- /reachable_occupancy/msg/ReachableOccupancy.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | 3 | reachable_occupancy/CapsuleArray capsules 4 | 5 | reachable_occupancy/CylinderArray cylinders 6 | 7 | reachable_occupancy/time t -------------------------------------------------------------------------------- /reachable_occupancy/msg/time.msg: -------------------------------------------------------------------------------- 1 | Header header 2 | float64 t_a 3 | float64 t_b -------------------------------------------------------------------------------- /reachable_occupancy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | reachable_occupancy 4 | 0.0.0 5 | The reachable_occupancy package 6 | 7 | 8 | 9 | 10 | sven 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | catkin 52 | message_generation 53 | geometry_msgs 54 | roscpp 55 | rospy 56 | std_msgs 57 | tf 58 | urdf 59 | geometry_msgs 60 | roscpp 61 | rospy 62 | std_msgs 63 | tf 64 | urdf 65 | geometry_msgs 66 | message_runtime 67 | roscpp 68 | rospy 69 | std_msgs 70 | tf 71 | urdf 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /reachable_occupancy/rviz/meshes/cofee_table.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/rviz/meshes/cofee_table.stl -------------------------------------------------------------------------------- /reachable_occupancy/rviz/meshes/door.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/rviz/meshes/door.stl -------------------------------------------------------------------------------- /reachable_occupancy/src/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/Sven-Schepp/SaRA/a45d8471492e3e53b9eb5b199a326304921e6f97/reachable_occupancy/src/__init__.py -------------------------------------------------------------------------------- /reachable_occupancy/src/udp_translator.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | This file is part of SaRA. 3 | 4 | SaRA is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU General Public License as published by 6 | TUM, either version 3 of the License, or 7 | (at your option) any later version. 8 | 9 | SaRA is distributed in the hope that it will be useful, 10 | but WITHOUT ANY WARRANTY; without even the implied warranty of 11 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 12 | GNU General Public License for more details: https://www.gnu.org/licenses/. 13 | */ 14 | 15 | #include 16 | #include 17 | 18 | #include "geometry_msgs/Point.h" 19 | #include "point.hpp" 20 | #include "reach_lib.hpp" 21 | #include "reachable_occupancy/JointLocations.h" 22 | #include "reachable_occupancy/ReachableOccupancy.h" 23 | #include "reachable_occupancy/Capsule.h" 24 | #include "reachable_occupancy/CapsuleArray.h" 25 | 26 | #include "udp_com/UdpPacket.h" 27 | #include "udp_com/UdpSend.h" 28 | #include "udp_com/UdpSocket.h" 29 | 30 | #include "udp_transiever.cpp" 31 | 32 | typedef udp_transiever::UDPTransiever UDPTransiever; 33 | 34 | //! \brief Translate capsule type to capsule msg type 35 | reachable_occupancy::CapsuleArray cap_to_msg(std::vector ca) { 36 | reachable_occupancy::CapsuleArray cma; 37 | for (int i = 0; i < ca.size(); i++) { 38 | reachable_occupancy::Capsule c; 39 | 40 | geometry_msgs::Point p1; 41 | p1.x = ca[i].p1_.x; 42 | p1.y = ca[i].p1_.y; 43 | p1.z = ca[i].p1_.z; 44 | 45 | geometry_msgs::Point p2; 46 | p2.x = ca[i].p2_.x; 47 | p2.y = ca[i].p2_.y; 48 | p2.z = ca[i].p2_.z; 49 | 50 | c.p1 = p1; 51 | c.p2 = p2; 52 | c.r = ca[i].r_; 53 | 54 | cma.capsules.push_back(c); 55 | } 56 | return cma; 57 | } 58 | 59 | //! \brief Translate reach_lib::Point to geometry_msgs::Point 60 | std::vector pos_to_msg (std::vector pa) { 61 | std::vector pma; 62 | for (int i = 0; i < pa.size(); i++) { 63 | geometry_msgs::Point p; 64 | p.x = pa[i].x; 65 | p.y = pa[i].y; 66 | p.z = pa[i].z; 67 | pma.push_back(p); 68 | } 69 | return pma; 70 | } 71 | 72 | // This file is sued to build an executable ROS node that 73 | // translate data recieved using the udp_co package to messages types 74 | // defined in reachable_occupancy 75 | int main(int argc, char** argv) { 76 | // ros system 77 | ros::init(argc, argv, "udp_translator"); 78 | ros::NodeHandle nh("eno2"); 79 | ros::Rate loop_rate(60); 80 | 81 | UDPTransiever tsv = UDPTransiever(8, 1.0/1000.0, "192.168.7.13", "192.168.7.1", 8080); 82 | UDPTransiever tsv_rob = UDPTransiever(11, 1.0/1.0, "192.168.7.13", "192.168.7.1", 8079); 83 | ros::Subscriber frame_data_subscriber = nh.subscribe("udp/p8080", 1000, 84 | &udp_transiever::UDPTransiever::udp_callback, &tsv); 85 | 86 | ros::Subscriber robot_subscriber = nh.subscribe("udp/p8079", 1000, 87 | &udp_transiever::UDPTransiever::robot_callback, &tsv_rob); 88 | 89 | ros::Publisher pub_rob = nh.advertise("/SaRA_demo_robot_capsules", 1000); 90 | ros::Publisher pub_human = nh.advertise("/SaRA_demo_human_capsules", 1000); 91 | 92 | while (ros::ok()) { 93 | ros::spinOnce(); 94 | 95 | reachable_occupancy::ReachableOccupancy rob_capsules; 96 | reachable_occupancy::JointLocations human_pos; 97 | 98 | rob_capsules.capsules = cap_to_msg(tsv_rob.get_robot_capsules()); 99 | 100 | human_pos.positions = pos_to_msg(tsv.get_joint_pos()); 101 | human_pos.velocities = pos_to_msg(tsv.get_joint_vel()); 102 | human_pos.delta_t = tsv_rob.get_t_brake(); 103 | 104 | pub_rob.publish(rob_capsules); 105 | pub_human.publish(human_pos); 106 | 107 | loop_rate.sleep(); 108 | } 109 | return 0; 110 | } --------------------------------------------------------------------------------