├── src ├── marti_common │ ├── swri_yaml_util │ │ ├── include │ │ │ └── swri_yaml_util │ │ │ │ ├── .gitignore │ │ │ │ └── yaml_util.h │ │ ├── version.h.in │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ └── CHANGELOG.rst │ ├── swri_prefix_tools │ │ ├── xterm_prefix_normal │ │ ├── xterm_prefix_hold │ │ ├── mainpage.dox │ │ ├── xterm_prefix_gdb │ │ ├── xterm_prefix_callgrind │ │ ├── CMakeLists.txt │ │ ├── xterm_prefix_callgrind_wait │ │ ├── package.xml │ │ └── CHANGELOG.rst │ ├── swri_math_util │ │ ├── launch │ │ │ ├── random.test │ │ │ ├── math_util.test │ │ │ └── trig_util.test │ │ ├── mainpage.dox │ │ ├── package.xml │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── src │ │ │ ├── ransac.cpp │ │ │ ├── random.cpp │ │ │ ├── trig_util.cpp │ │ │ └── math_util.cpp │ │ ├── include │ │ │ └── swri_math_util │ │ │ │ ├── constants.h │ │ │ │ └── trig_util.h │ │ └── test │ │ │ ├── test_random.cpp │ │ │ └── test_trig_util.cpp │ ├── swri_transform_util │ │ ├── launch │ │ │ ├── utm_util.test │ │ │ ├── transform_util.test │ │ │ ├── georeference.test │ │ │ ├── local_xy_util.test │ │ │ ├── initialize_origin.launch │ │ │ └── transform_manager.test │ │ ├── nodelet_plugins.xml │ │ ├── test │ │ │ ├── data │ │ │ │ ├── test.geo │ │ │ │ └── test_extension.geo │ │ │ └── test_georeference.cpp │ │ ├── transformer_plugins.xml │ │ ├── cfg │ │ │ └── DynamicPublisher.cfg │ │ ├── mainpage.dox │ │ ├── package.xml │ │ └── include │ │ │ └── swri_transform_util │ │ │ ├── frames.h │ │ │ ├── earth_constants.h │ │ │ └── transformer.h │ ├── swri_string_util │ │ ├── test │ │ │ └── string_util.test │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── CHANGELOG.rst │ │ └── include │ │ │ └── swri_string_util │ │ │ └── string_util.h │ ├── swri_image_util │ │ ├── launch │ │ │ ├── geometry_util.test │ │ │ └── blend_images.launch │ │ ├── mainpage.dox │ │ ├── package.xml │ │ ├── nodelet_plugins.xml │ │ ├── src │ │ │ ├── nodes │ │ │ │ ├── draw_text.cpp │ │ │ │ ├── scale_image.cpp │ │ │ │ ├── rotate_image.cpp │ │ │ │ ├── contrast_stretch.cpp │ │ │ │ ├── normalize_response.cpp │ │ │ │ ├── blend_images_node.cpp │ │ │ │ └── dummy_image_publisher.cpp │ │ │ └── blend_images_util.cpp │ │ └── include │ │ │ └── swri_image_util │ │ │ ├── blend_images_util.h │ │ │ ├── rolling_normalization.h │ │ │ └── geometry_util.h │ ├── swri_roscpp │ │ ├── mainpage.dox │ │ ├── test │ │ │ ├── params.test │ │ │ └── params.cpp │ │ ├── package.xml │ │ ├── launch │ │ │ └── param_example.launch │ │ ├── src │ │ │ └── nodes │ │ │ │ ├── param_example.cpp │ │ │ │ └── timer_test.cpp │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── swri_roscpp │ │ │ │ ├── swri_roscpp.h │ │ │ │ └── publisher.h │ │ └── CHANGELOG.rst │ ├── swri_opencv_util │ │ ├── mainpage.dox │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── swri_opencv_util │ │ │ │ ├── show.h │ │ │ │ └── convert.h │ │ └── CHANGELOG.rst │ ├── swri_system_util │ │ ├── mainpage.dox │ │ ├── launch │ │ │ └── file_util.test │ │ ├── package.xml │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── test │ │ │ └── test_file_util.cpp │ ├── swri_console_util │ │ ├── mainpage.dox │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── CHANGELOG.rst │ │ └── include │ │ │ └── swri_console_util │ │ │ └── progress_bar.h │ ├── marti_data_structures │ │ ├── mainpage.dox │ │ ├── CMakeLists.txt │ │ ├── package.xml │ │ └── CHANGELOG.rst │ ├── swri_nodelet │ │ ├── test │ │ │ ├── test_node_wrapper.test │ │ │ ├── test_standalone.test │ │ │ ├── test_manager.test │ │ │ └── test_nodelet_exists.py │ │ ├── README.md │ │ ├── nodelet_plugins.xml │ │ ├── nodelet.cpp.in │ │ ├── launch │ │ │ ├── example_include.launch │ │ │ └── example.launch │ │ ├── package.xml │ │ ├── cmake │ │ │ └── swri_nodelet-extras.cmake.in │ │ ├── nodes │ │ │ └── nodelet │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── src │ │ │ └── test_nodelet.cpp │ │ └── include │ │ │ └── swri_nodelet │ │ │ └── class_list_macros.h │ ├── swri_serial_util │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ └── CHANGELOG.rst │ ├── swri_route_util │ │ ├── package.xml │ │ └── CMakeLists.txt │ ├── swri_geometry_util │ │ ├── package.xml │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── swri_geometry_util │ │ │ │ ├── intersection.h │ │ │ │ ├── cubic_spline.h │ │ │ │ ├── geometry_util.h │ │ │ │ └── polygon.h │ │ └── src │ │ │ └── geometry_util.cpp │ ├── LICENSE │ └── README.md ├── tractobots_gps │ ├── CMakeLists.txt │ └── package.xml ├── tractobots_description │ ├── meshes │ │ ├── over_chassis.stl │ │ ├── rear_left_wheel.stl │ │ ├── front_left_wheel.stl │ │ ├── front_right_wheel.stl │ │ └── rear_right_wheel.stl │ ├── CMakeLists.txt │ ├── urdf │ │ ├── tractobots_library.urdf.xacro │ │ └── includes │ │ │ ├── tractobots_materials.urdf.xacro │ │ │ └── tractobots_properties.urdf.xacro │ ├── package.xml │ └── launch │ │ ├── tractobots_tf_tree.launch │ │ └── tractobots_view_robot.launch ├── tractobots_navigation │ ├── CMakeLists.txt │ └── package.xml ├── tractobots_launchers │ ├── launch │ │ ├── pose_tf.launch │ │ ├── mapviz.launch │ │ └── bringup.launch │ └── package.xml └── tractobots_robot_localization │ ├── launch │ ├── ekf_localization_node.launch │ └── navsat_transform_node.launch │ ├── scripts │ ├── pose_transformer.py │ └── imu_publisher.py │ ├── package.xml │ └── params │ └── navsat_transform.yaml ├── .gitignore ├── Arduino-JD6330 └── try └── README.md /src/marti_common/swri_yaml_util/include/swri_yaml_util/.gitignore: -------------------------------------------------------------------------------- 1 | version.h 2 | -------------------------------------------------------------------------------- /src/tractobots_gps/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(tractobots_gps) 3 | find_package(catkin REQUIRED COMPONENTS) 4 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/xterm_prefix_normal: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # xterm_prefix subcommand to run a node directly in xterm. 4 | 5 | $@ 6 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/launch/random.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/tractobots_description/meshes/over_chassis.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylerlaird/tractobots/HEAD/src/tractobots_description/meshes/over_chassis.stl -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/launch/math_util.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/launch/trig_util.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /src/tractobots_description/meshes/rear_left_wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylerlaird/tractobots/HEAD/src/tractobots_description/meshes/rear_left_wheel.stl -------------------------------------------------------------------------------- /src/tractobots_navigation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(tractobots_navigation) 3 | find_package(catkin REQUIRED COMPONENTS) 4 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/launch/utm_util.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /src/tractobots_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(tractobots_description) 3 | find_package(catkin REQUIRED COMPONENTS) 4 | -------------------------------------------------------------------------------- /src/tractobots_description/meshes/front_left_wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylerlaird/tractobots/HEAD/src/tractobots_description/meshes/front_left_wheel.stl -------------------------------------------------------------------------------- /src/tractobots_description/meshes/front_right_wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylerlaird/tractobots/HEAD/src/tractobots_description/meshes/front_right_wheel.stl -------------------------------------------------------------------------------- /src/tractobots_description/meshes/rear_right_wheel.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/kylerlaird/tractobots/HEAD/src/tractobots_description/meshes/rear_right_wheel.stl -------------------------------------------------------------------------------- /src/marti_common/swri_string_util/test/string_util.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/launch/geometry_util.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/launch/transform_util.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | -------------------------------------------------------------------------------- /src/marti_common/swri_yaml_util/version.h.in: -------------------------------------------------------------------------------- 1 | #ifndef YAML_UTIL_VERSION_H_ 2 | #define YAML_UTIL_VERSION_H_ 3 | 4 | #cmakedefine YAMLCPP_OLD_API 5 | 6 | #endif // YAML_UTIL_VERSION_H_ 7 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # Do not track files with this endings: 2 | *~ 3 | *orig 4 | *.autosave 5 | *.workspace 6 | *.user 7 | # Ignore these Paths: 8 | .catkin_tools 9 | devel 10 | build 11 | logs 12 | src/.idea/ -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b swri_roscpp 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/xterm_prefix_hold: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Run the node directly in xterm and keep the window open after the 4 | # process finished. 5 | 6 | $@ 7 | echo "<--- Node Completed ---->" 8 | sleep infinity -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b swri_math_util 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /src/marti_common/swri_opencv_util/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b swri_opencv_util 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /src/marti_common/swri_system_util/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b swri_system_util 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /src/marti_common/swri_console_util/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b swri_console_util 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b swri_prefix_tools 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /src/marti_common/marti_data_structures/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b marti_data_structures 6 | 7 | 10 | 11 | --> 12 | 13 | 14 | */ 15 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/xterm_prefix_gdb: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Run the node in gdb. The process will start immediately. 4 | 5 | # We need to use exec so that the node is the grandchild of 6 | # xterm_prefix. 7 | exec gdb -ex run --args $@ -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/test/test_node_wrapper.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/xterm_prefix_callgrind: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Launch node in valgrind with instrumentation running at start up. 4 | 5 | # We need to use exec so that the node is the grandchild of 6 | # xterm_prefix. 7 | exec valgrind --tool=callgrind $@ -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/test/test_standalone.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/README.md: -------------------------------------------------------------------------------- 1 | # swri_nodelet 2 | 3 | This package provides a simple script that simplifies writing launch 4 | files that easily switch between running nodelets as in a shared 5 | nodelet manager or as standalone processes. 6 | 7 | See the example launch files for details on how to use this package. -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | A simple nodelet for testing swri_nodelet functionality. 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/tractobots_launchers/launch/pose_tf.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /Arduino-JD6330/try: -------------------------------------------------------------------------------- 1 | #!/bin/bash -e 2 | 3 | device=/dev/ttyACM2 4 | 5 | source /opt/ros/kinetic/setup.bash 6 | 7 | #~/arduino-1.8.1/arduino --verify Arduino-JD6330.ino 8 | ~/arduino-1.6.13/arduino --upload --port ${device} Arduino-JD6330.ino 9 | #~/arduino-*/arduino --upload Arduino-JD6330.ino 10 | 11 | rosrun rosserial_python serial_node.py ${device} _baud:=57600 12 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/launch/georeference.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/marti_common/marti_data_structures/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(marti_data_structures) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | INCLUDE_DIRS include) 8 | 9 | install(DIRECTORY include/${PROJECT_NAME}/ 10 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 11 | FILES_MATCHING PATTERN "*.h" 12 | ) 13 | -------------------------------------------------------------------------------- /src/marti_common/swri_system_util/launch/file_util.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/test/test_manager.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Nodelet to publish a dynamically reconfigurable TF transform. 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/test/data/test.geo: -------------------------------------------------------------------------------- 1 | image_path: "." 2 | image_width: 29184 3 | image_height: 15872 4 | tile_size: 512 5 | 6 | datum: "wgs84" 7 | projection: "utm" 8 | 9 | tiepoints: 10 | - point: [4799, 209, 535674.5, 3258382.5] 11 | - point: [2336, 15596, 535329.5, 3256198.5] 12 | - point: [26925, 15457, 538837.5, 3256233.5] 13 | - point: [29133, 84, 539142.5, 3258416.5] 14 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/test/data/test_extension.geo: -------------------------------------------------------------------------------- 1 | image_path: "." 2 | image_width: 29184 3 | image_height: 15872 4 | tile_size: 512 5 | extension: "png" 6 | 7 | datum: "wgs84" 8 | projection: "utm" 9 | 10 | tiepoints: 11 | - point: [4799, 209, 535674.5, 3258382.5] 12 | - point: [2336, 15596, 535329.5, 3256198.5] 13 | - point: [26925, 15457, 538837.5, 3256233.5] 14 | - point: [29133, 84, 539142.5, 3258416.5] 15 | -------------------------------------------------------------------------------- /src/marti_common/marti_data_structures/package.xml: -------------------------------------------------------------------------------- 1 | 2 | marti_data_structures 3 | 0.2.4 4 | 5 | 6 | marti_data_structures 7 | 8 | 9 | Kris Kozak 10 | Kris Kozak 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | 16 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_prefix_tools) 3 | 4 | find_package(catkin REQUIRED) 5 | catkin_package() 6 | 7 | install(PROGRAMS xterm_prefix 8 | xterm_prefix_callgrind 9 | xterm_prefix_callgrind_wait 10 | xterm_prefix_gdb 11 | xterm_prefix_hold 12 | xterm_prefix_normal 13 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 14 | ) 15 | -------------------------------------------------------------------------------- /src/marti_common/swri_serial_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_serial_util 3 | 0.2.4 4 | 5 | 6 | swri_serial_util 7 | 8 | 9 | Marc Alban 10 | Marc Alban 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | 16 | boost 17 | 18 | 19 | -------------------------------------------------------------------------------- /src/marti_common/swri_string_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_string_util 3 | 0.2.4 4 | 5 | 6 | swri_string_util 7 | 8 | 9 | Marc Alban 10 | Marc Alban 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | rostest 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/tractobots_robot_localization/launch/ekf_localization_node.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/xterm_prefix_callgrind_wait: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Launch callgrind with instrumentation disabled. This allows you 4 | # to target the section of code to profile using the 5 | # CALLGRIND_START_INSTRUMENTATION and CALLGRIND_STOP_INSTRUMENTATION 6 | # macros in your code. Alternatively, you can toggle instrumentation 7 | # using "callgrind_control -i on/off" in a shell. 8 | 9 | # We need to use exec so that the node is the grandchild of 10 | # xterm_prefix. 11 | exec valgrind --tool=callgrind --instr-atstart=no $@ -------------------------------------------------------------------------------- /src/tractobots_gps/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tractobots_gps 4 | 0.0.1 5 | 6 | The tractobots_gps package is used for controlling a gps receiver module. 7 | 8 | 9 | Kyler Laird 10 | Kyler Laird 11 | https://github.com/kylerlaird/tractobots.git 12 | GPLv3 13 | 14 | catkin 15 | 16 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/transformer_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | Support for transforming to and from UTM 4 | 5 | 6 | Support for transforming to and from WGS84 Lat/Lon 7 | 8 | -------------------------------------------------------------------------------- /src/marti_common/swri_console_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_console_util 3 | 0.2.4 4 | 5 | 6 | swri_console_util 7 | 8 | 9 | Marc Alban 10 | Marc Alban 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | 16 | roscpp 17 | swri_math_util 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_math_util 3 | 0.2.4 4 | 5 | 6 | swri_math_util 7 | 8 | 9 | Marc Alban 10 | Marc Alban 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | 16 | roscpp 17 | 18 | rostest 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/marti_common/swri_system_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_system_util 3 | 0.2.4 4 | 5 | 6 | swri_system_util 7 | 8 | 9 | Marc Alban 10 | Marc Alban 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | 16 | roscpp 17 | rostest 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/marti_common/swri_opencv_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_opencv_util 3 | 0.2.4 4 | 5 | 6 | swri_opencv_util 7 | 8 | 9 | Marc Alban 10 | Marc Alban 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | 16 | cv_bridge 17 | swri_math_util 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/tractobots_navigation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tractobots_navigation 4 | 0.0.1 5 | 6 | The tractobots_navigation package is used for providing the 7 | appropiated signals for controlling the tractobot tractor mobile 8 | platform. 9 | 10 | 11 | Kyler Laird 12 | Kyler Laird 13 | https://github.com/kylerlaird/tractobots.git 14 | GPLv3 15 | 16 | catkin 17 | 18 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_prefix_tools 3 | 0.2.4 4 | 5 | 6 | Contains scripts that are useful as prefix commands for nodes 7 | started by roslaunch. 8 | 9 | 10 | Elliot Johnson 11 | Elliot Johnson 12 | BSD 13 | https://github.com/swri-robotics/marti_common 14 | 15 | catkin 16 | python-psutil 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/tractobots_description/urdf/tractobots_library.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/nodelet.cpp.in: -------------------------------------------------------------------------------- 1 | #include 2 | #include "ros/ros.h" 3 | #include "nodelet/nodelet.h" 4 | 5 | namespace @NODELET_NS@ 6 | { 7 | boost::shared_ptr create@NODELET_CLASS@(); 8 | } // namespace @NODELET_NS@ 9 | 10 | int main(int argc, char **argv) 11 | { 12 | ros::init(argc, argv, "@NODELET_NODENAME@"); 13 | nodelet::M_string remap(ros::names::getRemappings());; 14 | nodelet::V_string my_argv; 15 | boost::shared_ptr n = @NODELET_NS@::create@NODELET_CLASS@(); 16 | n->init(ros::this_node::getName(), remap, my_argv); 17 | ros::spin(); 18 | return 0; 19 | } 20 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/test/params.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | used_int_var_1: 1 6 | used_int_var_2: 1 7 | unused_int_var: 0 8 | 9 | used_dbl_var_1: 1.0 10 | used_dbl_var_2: 2.0 11 | unused_dbl_var: 0.0 12 | 13 | used_str_var_1: "foo" 14 | used_str_var_2: "bar" 15 | unused_str_var: "baz" 16 | 17 | used_bool_var_1: true 18 | used_bool_var_2: true 19 | unused_bool_var: false 20 | 21 | 22 | 23 | unused_global: "fizz" 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/launch/blend_images.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/launch/local_xy_util.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | [{ name: swri, 7 | latitude: 29.45196669, 8 | longitude: -98.61370577, 9 | altitude: 233.719, 10 | heading: 0.0}, 11 | 12 | { name: back_40, 13 | latitude: 29.447507, 14 | longitude: -98.629367, 15 | altitude: 200.0, 16 | heading: 0.0}] 17 | 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swri_roscpp 4 | 0.2.4 5 | 6 | 7 | swri_roscpp 8 | 9 | 10 | Elliot Johnson 11 | BSD 12 | catkin 13 | diagnostic_updater 14 | gtest 15 | nav_msgs 16 | roscpp 17 | rostest 18 | std_msgs 19 | std_srvs 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/marti_common/swri_route_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swri_route_util 4 | 0.2.4 5 | 6 | 7 | This library provides functionality to simplify working with the 8 | navigation messages defined in marti_nav_msgs. 9 | 10 | 11 | Elliot Johnson 12 | BSD 13 | 14 | catkin 15 | 16 | marti_nav_msgs 17 | roscpp 18 | swri_transform_util 19 | 20 | 21 | 22 | 23 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/launch/param_example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | used_int_var_1: 1 6 | used_int_var_2: 1 7 | unused_int_var: 0 8 | 9 | used_dbl_var_1: 1.0 10 | used_dbl_var_2: 2.0 11 | unused_dbl_var: 0.0 12 | 13 | used_str_var_1: "foo" 14 | used_str_var_2: "bar" 15 | unused_str_var: "baz" 16 | 17 | used_bool_var_1: true 18 | used_bool_var_2: true 19 | unused_bool_var: false 20 | 21 | 22 | 23 | unused_global: "fizz" 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/tractobots_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tractobots_description 4 | 0.0.1 5 | 6 | The tractobots_description package contains URDF files and meshes 7 | for the tractobots project. It is used for computing the needed 8 | transforms and for visualization in RViz. 9 | 10 | 11 | Alfonso Troya 12 | Alfonso Troya 13 | https://github.com/kylerlaird/tractobots.git 14 | GPLv3 15 | 16 | catkin 17 | 18 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/launch/example_include.launch: -------------------------------------------------------------------------------- 1 | 2 | 8 | 9 | 10 | 11 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/tractobots_launchers/launch/mapviz.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | [{ name: tractofarm, 9 | latitude: 40.8882561488, 10 | longitude: -87.1996192638, 11 | altitude: 0.0, 12 | heading: 0.0}] 13 | 14 | 15 | 16 | 17 | -------------------------------------------------------------------------------- /src/marti_common/swri_geometry_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_geometry_util 3 | 0.2.4 4 | 5 | 6 | swri_geometry_util 7 | 8 | 9 | Marc Alban 10 | Marc Alban 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | pkg-config 16 | 17 | cmake_modules 18 | cv_bridge 19 | eigen 20 | libgeos++-dev 21 | roscpp 22 | tf 23 | 24 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b ranger_common is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/cfg/DynamicPublisher.cfg: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | PACKAGE = "swri_transform_util" 3 | 4 | from dynamic_reconfigure.parameter_generator_catkin import * 5 | 6 | gen = ParameterGenerator() 7 | 8 | gen.add("x", double_t, 0, "X offset (m)", 0, -100000.0, 100000.0) 9 | gen.add("y", double_t, 0, "Y offset (m)", 0, -100000.0, 100000.0) 10 | gen.add("z", double_t, 0, "Z offset (m)", 0, -100000.0, 100000.0) 11 | gen.add("yaw", double_t, 0, "Yaw offset (rad)", 0, -3.1415, 3.1415) 12 | gen.add("pitch", double_t, 0, "Pitch offset (rad)", 0, -3.1415, 3.1415) 13 | gen.add("roll", double_t, 0, "Roll offset (rad)", 0, -3.1415, 3.1415) 14 | 15 | exit(gen.generate(PACKAGE, "swri_transform_util", "DynamicPublisher")) 16 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | \mainpage 3 | \htmlinclude manifest.html 4 | 5 | \b ranger_common is ... 6 | 7 | 10 | 11 | 12 | \section codeapi Code API 13 | 14 | 24 | 25 | 26 | */ 27 | -------------------------------------------------------------------------------- /src/marti_common/swri_yaml_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_yaml_util 3 | 0.2.4 4 | 5 | Provides wrappers around the yaml-cpp library for various utility functions 6 | and to abstract out the API changes made to yaml-cpp between ubuntu:precise 7 | and ubuntu:trusty. 8 | 9 | Marc Alban 10 | Marc Alban 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | pkg-config 16 | boost 17 | yaml-cpp 18 | 19 | 20 | -------------------------------------------------------------------------------- /src/tractobots_launchers/launch/bringup.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/marti_common/swri_serial_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_serial_util) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package( 7 | INCLUDE_DIRS include 8 | LIBRARIES ${PROJECT_NAME} 9 | ) 10 | 11 | include_directories(include ${catkin_INCLUDE_DIRS}) 12 | 13 | add_library(${PROJECT_NAME} 14 | src/serial_port.cpp) 15 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 16 | 17 | install(DIRECTORY include/${PROJECT_NAME}/ 18 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 19 | FILES_MATCHING PATTERN "*.h" 20 | ) 21 | 22 | install(TARGETS ${PROJECT_NAME} 23 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 24 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 25 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 26 | ) 27 | 28 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | swri_nodelet 4 | 0.2.4 5 | 6 | This package provides a simple script to write simple launch files 7 | that can easily switch between running nodelets together or as 8 | standalone nodes. 9 | 10 | 11 | Elliot Johnson 12 | BSD 13 | 14 | catkin 15 | 16 | nodelet 17 | rosbash 18 | rosbash 19 | roscpp 20 | rostest 21 | std_msgs 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/marti_common/swri_console_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_console_util) 3 | 4 | find_package(catkin REQUIRED COMPONENTS roscpp) 5 | 6 | catkin_package( 7 | INCLUDE_DIRS include 8 | LIBRARIES ${PROJECT_NAME} 9 | CATKIN_DEPENDS roscpp 10 | ) 11 | 12 | include_directories(include ${catkin_INCLUDE_DIRS}) 13 | 14 | add_library(${PROJECT_NAME} src/progress_bar.cpp) 15 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 16 | 17 | install(DIRECTORY include/${PROJECT_NAME}/ 18 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 19 | FILES_MATCHING PATTERN "*.h" 20 | ) 21 | 22 | install(TARGETS ${PROJECT_NAME} 23 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 24 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 25 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 26 | ) 27 | 28 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/cmake/swri_nodelet-extras.cmake.in: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | set(swri_nodelet_SHARE ${swri_nodelet_PREFIX}/@CATKIN_PACKAGE_SHARE_DESTINATION@) 4 | 5 | macro(swri_nodelet_add_node NODELET_NODENAME NODELET_NS NODELET_CLASS) 6 | find_package(roscpp REQUIRED) 7 | find_package(nodelet REQUIRED) 8 | include_directories(${roscpp_INCLUDE} ${nodelet_INCLUDE}) 9 | set(NODELET_NODENAME ${NODELET_NODENAME}) 10 | set(NODELET_NS ${NODELET_NS}) 11 | set(NODELET_CLASS ${NODELET_CLASS}) 12 | configure_file(${swri_nodelet_SHARE}/nodelet.cpp.in "${NODELET_NODENAME}.cpp") 13 | add_executable(${NODELET_NODENAME} "${NODELET_NODENAME}.cpp") 14 | target_link_libraries(${NODELET_NODENAME} ${roscpp_LIBRARIES} ${nodelet_LIBRARIES}) 15 | unset(NODELET_NODENAME) 16 | unset(NODELET_NS) 17 | unset(NODELET_CLASS) 18 | endmacro() 19 | -------------------------------------------------------------------------------- /src/tractobots_description/launch/tractobots_tf_tree.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/test/test_nodelet_exists.py: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | 3 | import rospy 4 | import rostest 5 | from std_msgs.msg import Int32 6 | import sys 7 | from time import sleep 8 | import unittest 9 | 10 | PKG = 'swri_nodelet' 11 | NAME = 'test_nodelet_exists' 12 | 13 | class TestNodeletExists(unittest.TestCase): 14 | 15 | def callback(self, msg): 16 | self.msg = msg 17 | 18 | def test_init(self): 19 | ''' 20 | Test that the test nodelet exists by waiting for the message it 21 | publishes. 22 | ''' 23 | self.msg = None 24 | sub = rospy.Subscriber('numbers', Int32, self.callback) 25 | while self.msg is None: 26 | sleep(0.1) 27 | sub.unregister() 28 | self.assertEquals(self.msg.data, 1337) 29 | 30 | if __name__ == '__main__': 31 | rospy.init_node(NAME) 32 | rostest.rosrun(PKG, NAME, TestNodeletExists, sys.argv) 33 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/nodes/nodelet: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env bash 2 | 3 | # This script loads a nodelet using the standard nodelet manager. It 4 | # uses an alternative syntax to the nodelet command that makes writing 5 | # programmable launch files simpler. The command syntax is: 6 | # 7 | # nodelet_launcher [nodelet_type] [target] [ROS arguments...] 8 | # 9 | # where target is either the name of a nodelet manager or one of the 10 | # special keywords: 11 | # 12 | # standalone - Run as a standalone nodelet 13 | 14 | # Pop the nodelet type and manager off the argument list. 15 | NODELET_TYPE=$1 16 | NODELET_MANAGER=$2 17 | shift 18 | shift 19 | 20 | if [ $NODELET_MANAGER = "standalone" ] 21 | then 22 | echo Launching nodelet of type $NODELET_TYPE in standalone configuration. 23 | rosrun nodelet nodelet standalone $NODELET_TYPE $@ 24 | else 25 | echo Launching nodelet of type $NODELET_TYPE on nodelet manager $NODELET_MANAGER 26 | rosrun nodelet nodelet load $NODELET_TYPE $NODELET_MANAGER $@ 27 | fi 28 | -------------------------------------------------------------------------------- /src/marti_common/swri_string_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(swri_string_util) 4 | 5 | find_package(catkin REQUIRED) 6 | 7 | catkin_package( 8 | INCLUDE_DIRS include 9 | LIBRARIES ${PROJECT_NAME} 10 | ) 11 | 12 | include_directories(include ${catkin_INCLUDE_DIRS}) 13 | 14 | add_library(${PROJECT_NAME} 15 | src/string_util.cpp) 16 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 17 | 18 | if(CATKIN_ENABLE_TESTING) 19 | find_package(rostest REQUIRED) 20 | 21 | add_rostest_gtest(test_string_util test/string_util.test test/test_string_util.cpp) 22 | target_link_libraries(test_string_util ${PROJECT_NAME}) 23 | endif() 24 | 25 | install(DIRECTORY include/${PROJECT_NAME}/ 26 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 27 | FILES_MATCHING PATTERN "*.h" 28 | ) 29 | 30 | install(TARGETS ${PROJECT_NAME} 31 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 32 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | ) 35 | -------------------------------------------------------------------------------- /src/tractobots_description/launch/tractobots_view_robot.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/src/nodes/param_example.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | int main(int argc, char **argv) 6 | { 7 | ros::init(argc, argv, "param_test"); 8 | ros::NodeHandle pnh("~"); 9 | 10 | int int_var; 11 | swri::getParam(pnh, "used_int_var_1", int_var); 12 | swri::param(pnh, "used_int_var_2", int_var, 2); 13 | swri::param(pnh, "unset_int_var", int_var, 2); 14 | 15 | std::string str_var; 16 | swri::getParam(pnh, "used_str_var_1", str_var); 17 | swri::param(pnh, "used_str_var_2", str_var, "foo"); 18 | swri::param(pnh, "unset_str_var", str_var, "foo"); 19 | 20 | double dbl_var; 21 | swri::getParam(pnh, "used_dbl_var_1", dbl_var); 22 | swri::param(pnh, "used_dbl_var_2", dbl_var, 2.0); 23 | swri::param(pnh, "unset_dbl_var", dbl_var, 2.0); 24 | 25 | bool bool_var; 26 | swri::getParam(pnh, "used_bool_var_1", bool_var); 27 | swri::param(pnh, "used_bool_var_2", bool_var, true); 28 | swri::param(pnh, "unset_bool_var", bool_var, true); 29 | 30 | swri::warnUnusedParams(pnh); 31 | 32 | return 0; 33 | } 34 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/launch/initialize_origin.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | [{ name: swri, 8 | latitude: 29.45196669, 9 | longitude: -98.61370577, 10 | altitude: 233.719, 11 | heading: 0.0}, 12 | 13 | { name: back_40, 14 | latitude: 29.447507, 15 | longitude: -98.629367, 16 | altitude: 200.0, 17 | heading: 0.0}] 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/marti_common/swri_string_util/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_string_util 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | 8 | 0.2.3 (2016-12-09) 9 | ------------------ 10 | 11 | 0.2.2 (2016-12-07) 12 | ------------------ 13 | 14 | 0.2.1 (2016-10-23) 15 | ------------------ 16 | 17 | 0.2.0 (2016-06-21) 18 | ------------------ 19 | 20 | 0.1.5 (2016-05-13) 21 | ------------------ 22 | 23 | 0.1.4 (2016-05-12) 24 | ------------------ 25 | 26 | 0.1.3 (2016-03-04) 27 | ------------------ 28 | 29 | 0.1.2 (2016-01-06) 30 | ------------------ 31 | 32 | 0.1.1 (2015-11-17) 33 | ------------------ 34 | 35 | 0.1.0 (2015-09-29) 36 | ------------------ 37 | 38 | 0.0.5 (2015-09-27) 39 | ------------------ 40 | 41 | 0.0.4 (2015-09-27) 42 | ------------------ 43 | 44 | 0.0.3 (2015-09-26) 45 | ------------------ 46 | 47 | 0.0.2 (2015-09-25) 48 | ------------------ 49 | * Renames string_util to swri_string_util. Refs `#231 `_. 50 | * Contributors: Edward Venator 51 | 52 | 0.0.1 (2015-09-25) 53 | ------------------ 54 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/launch/transform_manager.test: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | [{ name: swri, 8 | latitude: 29.45196669, 9 | longitude: -98.61370577, 10 | altitude: 233.719, 11 | heading: 0.0}, 12 | 13 | { name: back_40, 14 | latitude: 29.447507, 15 | longitude: -98.629367, 16 | altitude: 200.0, 17 | heading: 0.0}] 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/marti_common/swri_route_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_route_util) 3 | 4 | set(BUILD_DEPS 5 | marti_nav_msgs 6 | roscpp 7 | swri_transform_util 8 | ) 9 | 10 | set(RUNTIME_DEPS ${BUILD_DEPS}) 11 | 12 | 13 | ### Catkin ### 14 | find_package(catkin REQUIRED COMPONENTS ${BUILD_DEPS}) 15 | catkin_package(CATKIN_DEPENDS ${RUNTIME_DEPS} 16 | INCLUDE_DIRS include 17 | LIBRARIES ${PROJECT_NAME}) 18 | 19 | include_directories(include ${catkin_INCLUDE_DIRS}) 20 | 21 | ### Build Library ### 22 | add_library(${PROJECT_NAME} 23 | src/route.cpp 24 | src/route_point.cpp 25 | src/util.cpp 26 | ) 27 | set_target_properties(${PROJECT_NAME} PROPERTIES COMPILE_FLAGS "-std=c++0x") 28 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES}) 29 | 30 | ### Install Libraries and Executables ### 31 | install(TARGETS ${PROJECT_NAME} 32 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 33 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 35 | ) 36 | 37 | ### Install Header Files ### 38 | install(DIRECTORY include/${PROJECT_NAME}/ 39 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 40 | ) 41 | 42 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_image_util 3 | 0.2.4 4 | 5 | 6 | swri_image_util 7 | 8 | 9 | Kris Kozak 10 | Kris Kozak 11 | BSD 12 | https://github.com/swri-robotics/marti_common 13 | 14 | catkin 15 | pkg-config 16 | 17 | camera_calibration_parsers 18 | cv_bridge 19 | eigen 20 | geometry_msgs 21 | image_geometry 22 | image_transport 23 | libqt5-opengl-dev 24 | libqt5-gui 25 | message_filters 26 | nav_msgs 27 | nodelet 28 | roscpp 29 | rospy 30 | std_msgs 31 | swri_math_util 32 | swri_opencv_util 33 | tf 34 | 35 | rostest 36 | 37 | 38 | 39 | 40 | 41 | 42 | -------------------------------------------------------------------------------- /src/marti_common/marti_data_structures/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package marti_data_structures 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | 8 | 0.2.3 (2016-12-09) 9 | ------------------ 10 | 11 | 0.2.2 (2016-12-07) 12 | ------------------ 13 | 14 | 0.2.1 (2016-10-23) 15 | ------------------ 16 | 17 | 0.2.0 (2016-06-21) 18 | ------------------ 19 | 20 | 0.1.5 (2016-05-13) 21 | ------------------ 22 | 23 | 0.1.4 (2016-05-12) 24 | ------------------ 25 | 26 | 0.1.3 (2016-03-04) 27 | ------------------ 28 | 29 | 0.1.2 (2016-01-06) 30 | ------------------ 31 | 32 | 0.1.1 (2015-11-17) 33 | ------------------ 34 | 35 | 0.1.0 (2015-09-29) 36 | ------------------ 37 | 38 | 0.0.5 (2015-09-27) 39 | ------------------ 40 | 41 | 0.0.4 (2015-09-27) 42 | ------------------ 43 | 44 | 0.0.3 (2015-09-26) 45 | ------------------ 46 | 47 | 0.0.2 (2015-09-25) 48 | ------------------ 49 | 50 | 0.0.1 (2015-09-25) 51 | ------------------ 52 | * update cmake version to squash the CMP0003 warning 53 | * remove dependencies on build_tools 54 | * use format 2 package definition 55 | * fix missing organization in license text 56 | * catkinize marti_common 57 | * change license to BSD 58 | * Contributors: Marc Alban 59 | -------------------------------------------------------------------------------- /src/tractobots_description/urdf/includes/tractobots_materials.urdf.xacro: -------------------------------------------------------------------------------- 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 | 49 | -------------------------------------------------------------------------------- /src/marti_common/swri_opencv_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_opencv_util) 3 | 4 | find_package(catkin REQUIRED COMPONENTS swri_math_util) 5 | 6 | find_package(OpenCV 3.1 COMPONENTS core imgproc REQUIRED) 7 | 8 | # The Boost Random library headers and namespaces changed between version 9 | # 1.46 and 1.47 10 | find_package(Boost REQUIRED COMPONENTS serialization thread) 11 | if(${Boost_LIB_VERSION} MATCHES "1_46*") 12 | add_definitions( -DBOOST_1_46 ) 13 | endif(${Boost_LIB_VERSION} MATCHES "1_46*") 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | LIBRARIES ${PROJECT_NAME} 18 | CATKIN_DEPENDS swri_math_util 19 | ) 20 | 21 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 22 | 23 | add_library(${PROJECT_NAME} 24 | src/models.cpp 25 | src/model_fit.cpp 26 | src/show.cpp 27 | src/convert.cpp) 28 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES} ${OpenCV_LIBS}) 29 | 30 | install(DIRECTORY include/${PROJECT_NAME}/ 31 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 32 | FILES_MATCHING PATTERN "*.h" 33 | ) 34 | 35 | install(TARGETS ${PROJECT_NAME} 36 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 37 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 38 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 39 | ) 40 | -------------------------------------------------------------------------------- /src/marti_common/swri_prefix_tools/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_prefix_tools 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | 8 | 0.2.3 (2016-12-09) 9 | ------------------ 10 | 11 | 0.2.2 (2016-12-07) 12 | ------------------ 13 | 14 | 0.2.1 (2016-10-23) 15 | ------------------ 16 | 17 | 0.2.0 (2016-06-21) 18 | ------------------ 19 | 20 | 0.1.5 (2016-05-13) 21 | ------------------ 22 | 23 | 0.1.4 (2016-05-12) 24 | ------------------ 25 | * Adds missing dependency for swri_prefix_tools 26 | * Contributors: Edward Venator 27 | 28 | 0.1.3 (2016-03-04) 29 | ------------------ 30 | 31 | 0.1.2 (2016-01-06) 32 | ------------------ 33 | 34 | 0.1.1 (2015-11-17) 35 | ------------------ 36 | * Properly installs the prefix scripts to the package share destination. 37 | * Contributors: Edward Venator 38 | 39 | 0.1.0 (2015-09-29) 40 | ------------------ 41 | 42 | 0.0.5 (2015-09-27) 43 | ------------------ 44 | 45 | 0.0.4 (2015-09-27) 46 | ------------------ 47 | 48 | 0.0.3 (2015-09-26) 49 | ------------------ 50 | 51 | 0.0.2 (2015-09-25) 52 | ------------------ 53 | * Renames prefix_tools to swri_prefix_tools. Refs `#231 `_. 54 | * Contributors: Edward Venator 55 | 56 | 0.0.1 (2015-09-25) 57 | ------------------ 58 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Tractobots Project 2 | 3 | Field robots 4 | 5 | Dependencies installation (ROS Kinetic) 6 | ------------ 7 | ROS packages: 8 | 9 | sudo apt-get install -y ros-$ROS_DISTRO-mapviz ros-$ROS_DISTRO-mapviz-plugins ros-$ROS_DISTRO-tile-map ros-$ROS_DISTRO-robot-localization 10 | 11 | Docker: 12 | 13 | sudo apt-key adv --keyserver hkp://p80.pool.sks-keyservers.net:80 --recv-keys 58118E89F3A912897C070ADBF76221572C52609D 14 | sudo apt-add-repository 'deb https://apt.dockerproject.org/repo ubuntu-xenial main' 15 | sudo apt-get update 16 | sudo apt-get install -y docker-engine 17 | 18 | ROS Offline Google Maps with MapViz ([thanks to danielsnider](https://github.com/danielsnider/MapViz-Tile-Map-Google-Maps-Satellite/blob/master/README.md)): 19 | 20 | mkdir ~/mapproxy 21 | sudo docker run -p 8080:8080 -d -t -v ~/mapproxy:/mapproxy danielsnider/mapproxy 22 | 23 | Optional: [catkin_tools](http://catkin-tools.readthedocs.io/en/latest/installing.html) 24 | 25 | Download repository and compile it: 26 | 27 | git clone https://github.com/kylerlaird/tractobots.git 28 | cd tractobots 29 | catkin build 30 | source devel/setup.bash 31 | 32 | Usage 33 | ------------ 34 | On different terminals run: 35 | 36 | roslaunch tractobots_launchers bringup.launch 37 | roslaunch tractobots_launchers mapviz.launch 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/marti_common/swri_console_util/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_console_util 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | 8 | 0.2.3 (2016-12-09) 9 | ------------------ 10 | * Fix OpenCV dependencies for Kinetic build (`#400 `_) 11 | * Contributors: P. J. Reed 12 | 13 | 0.2.2 (2016-12-07) 14 | ------------------ 15 | 16 | 0.2.1 (2016-10-23) 17 | ------------------ 18 | 19 | 0.2.0 (2016-06-21) 20 | ------------------ 21 | 22 | 0.1.5 (2016-05-13) 23 | ------------------ 24 | 25 | 0.1.4 (2016-05-12) 26 | ------------------ 27 | 28 | 0.1.3 (2016-03-04) 29 | ------------------ 30 | 31 | 0.1.2 (2016-01-06) 32 | ------------------ 33 | 34 | 0.1.1 (2015-11-17) 35 | ------------------ 36 | 37 | 0.1.0 (2015-09-29) 38 | ------------------ 39 | 40 | 0.0.5 (2015-09-27) 41 | ------------------ 42 | 43 | 0.0.4 (2015-09-27) 44 | ------------------ 45 | 46 | 0.0.3 (2015-09-26) 47 | ------------------ 48 | * Correct OpenCV rosdep key 49 | * Contributors: Jerry Towler 50 | 51 | 0.0.2 (2015-09-25) 52 | ------------------ 53 | * Renames console util to swri_console util. Refs `#231 `_. 54 | * Contributors: Edward Venator 55 | 56 | 0.0.1 (2015-09-25) 57 | ------------------ 58 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_math_util 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | 8 | 0.2.3 (2016-12-09) 9 | ------------------ 10 | 11 | 0.2.2 (2016-12-07) 12 | ------------------ 13 | 14 | 0.2.1 (2016-10-23) 15 | ------------------ 16 | 17 | 0.2.0 (2016-06-21) 18 | ------------------ 19 | 20 | 0.1.5 (2016-05-13) 21 | ------------------ 22 | 23 | 0.1.4 (2016-05-12) 24 | ------------------ 25 | 26 | 0.1.3 (2016-03-04) 27 | ------------------ 28 | 29 | 0.1.2 (2016-01-06) 30 | ------------------ 31 | * Refactors RANSAC matching code to use more matrix operations. 32 | * Contributors: Marc Alban 33 | 34 | 0.1.1 (2015-11-17) 35 | ------------------ 36 | * Minor change: marks single-argument constructor for Ransac explicit. 37 | * Contributors: Marc Alban 38 | 39 | 0.1.0 (2015-09-29) 40 | ------------------ 41 | 42 | 0.0.5 (2015-09-27) 43 | ------------------ 44 | 45 | 0.0.4 (2015-09-27) 46 | ------------------ 47 | 48 | 0.0.3 (2015-09-26) 49 | ------------------ 50 | * Format package files 51 | * Contributors: Jerry Towler 52 | 53 | 0.0.2 (2015-09-25) 54 | ------------------ 55 | * Renames math_util to swri_math_util. Refs `#231 `_. 56 | * Contributors: Edward Venator 57 | 58 | 0.0.1 (2015-09-25) 59 | ------------------ 60 | -------------------------------------------------------------------------------- /src/tractobots_description/urdf/includes/tractobots_properties.urdf.xacro: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /src/marti_common/swri_system_util/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_system_util 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | 8 | 0.2.3 (2016-12-09) 9 | ------------------ 10 | 11 | 0.2.2 (2016-12-07) 12 | ------------------ 13 | * Add helper method to find files within a directory and subdirectories based on regular expression matching for the filename. 14 | * Contributors: P. J. Reed 15 | 16 | 0.2.1 (2016-10-23) 17 | ------------------ 18 | 19 | 0.2.0 (2016-06-21) 20 | ------------------ 21 | 22 | 0.1.5 (2016-05-13) 23 | ------------------ 24 | 25 | 0.1.4 (2016-05-12) 26 | ------------------ 27 | 28 | 0.1.3 (2016-03-04) 29 | ------------------ 30 | 31 | 0.1.2 (2016-01-06) 32 | ------------------ 33 | 34 | 0.1.1 (2015-11-17) 35 | ------------------ 36 | 37 | 0.1.0 (2015-09-29) 38 | ------------------ 39 | 40 | 0.0.5 (2015-09-27) 41 | ------------------ 42 | 43 | 0.0.4 (2015-09-27) 44 | ------------------ 45 | 46 | 0.0.3 (2015-09-26) 47 | ------------------ 48 | * Format package files 49 | * Clean up dependencies 50 | Remove unneeded ones, add required ones not specified 51 | * Contributors: Jerry Towler 52 | 53 | 0.0.2 (2015-09-25) 54 | ------------------ 55 | * Renames system_util to swri_system_util. Refs `#231 `_. 56 | * Contributors: Edward Venator 57 | 58 | 0.0.1 (2015-09-25) 59 | ------------------ 60 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/package.xml: -------------------------------------------------------------------------------- 1 | 2 | swri_transform_util 3 | 0.2.4 4 | 5 | 6 | The swri_transform_util package contains utility functions and classes for 7 | transforming between coordinate frames. 8 | 9 | 10 | Marc Alban 11 | Marc Alban 12 | BSD 13 | https://github.com/swri-robotics/marti_common 14 | 15 | catkin 16 | 17 | boost 18 | cv_bridge 19 | diagnostic_msgs 20 | dynamic_reconfigure 21 | geographic_msgs 22 | geometry_msgs 23 | libgeos++-dev 24 | nodelet 25 | pluginlib 26 | proj 27 | roscpp 28 | rospy 29 | sensor_msgs 30 | swri_math_util 31 | swri_roscpp 32 | swri_yaml_util 33 | tf 34 | topic_tools 35 | yaml-cpp 36 | 37 | rostest 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/test/params.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | 3 | #include 4 | #include 5 | #include 6 | 7 | TEST(ParamTests, getUnusedParamKeys) 8 | { 9 | ros::NodeHandle pnh("~"); 10 | std::string ns = pnh.getNamespace(); 11 | 12 | int int_var; 13 | swri::getParam(pnh, "used_int_var_1", int_var); 14 | swri::param(pnh, "used_int_var_2", int_var, 2); 15 | swri::param(pnh, "unset_int_var", int_var, 2); 16 | 17 | std::string str_var; 18 | swri::getParam(pnh, "used_str_var_1", str_var); 19 | swri::param(pnh, "used_str_var_2", str_var, "foo"); 20 | swri::param(pnh, "unset_str_var", str_var, "foo"); 21 | 22 | double dbl_var; 23 | swri::getParam(pnh, "used_dbl_var_1", dbl_var); 24 | swri::param(pnh, "used_dbl_var_2", dbl_var, 2.0); 25 | swri::param(pnh, "unset_dbl_var", dbl_var, 2.0); 26 | 27 | bool bool_var; 28 | swri::getParam(pnh, "used_bool_var_1", bool_var); 29 | swri::param(pnh, "used_bool_var_2", bool_var, true); 30 | swri::param(pnh, "unset_bool_var", bool_var, true); 31 | 32 | std::vector unused = swri::getUnusedParamKeys(pnh); 33 | ASSERT_EQ(4, unused.size()); 34 | ASSERT_EQ(ns + "/unused_bool_var", unused[0]); 35 | ASSERT_EQ(ns + "/unused_dbl_var", unused[1]); 36 | ASSERT_EQ(ns + "/unused_int_var", unused[2]); 37 | ASSERT_EQ(ns + "/unused_str_var", unused[3]); 38 | } 39 | 40 | int main(int argc, char **argv) 41 | { 42 | ros::init(argc, argv, "param_test"); 43 | testing::InitGoogleTest(&argc, argv); 44 | return RUN_ALL_TESTS(); 45 | } 46 | 47 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/nodelet_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Nodelet to rotate an image a multiple of 90 degrees. 6 | 7 | 8 | 9 | 10 | 11 | Nodelet to scale an image. 12 | 13 | 14 | 15 | 16 | 17 | Nodelet to contrast stretch an image. 18 | 19 | 20 | 21 | 22 | 23 | Nodelet to normalize the response of an image. 24 | 25 | 26 | 27 | 28 | 29 | Nodelet to draw text on an image. 30 | 31 | 32 | 33 | 34 | 35 | Nodelet that blends one image onto the top of another. 36 | 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/marti_common/swri_geometry_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(swri_geometry_util) 4 | 5 | find_package(catkin REQUIRED COMPONENTS 6 | cmake_modules 7 | cv_bridge 8 | roscpp 9 | tf 10 | ) 11 | 12 | set(CMAKE_MODULE_PATH ${CMAKE_MODULE_PATH} ${CMAKE_CURRENT_SOURCE_DIR}) 13 | 14 | find_package(GEOS REQUIRED) 15 | 16 | find_package(Eigen3) 17 | if(NOT EIGEN3_FOUND) 18 | # Fallback to pkg-config 19 | find_package(PkgConfig) 20 | pkg_check_modules(EIGEN3 REQUIRED eigen3) 21 | else() 22 | set(EIGEN3_INCLUDE_DIRS ${EIGEN3_INCLUDE_DIR}) 23 | endif() 24 | add_definitions(${EIGEN3_DEFINITIONS}) 25 | 26 | find_package(OpenCV 3.1 COMPONENTS core REQUIRED) 27 | 28 | catkin_package( 29 | INCLUDE_DIRS include 30 | LIBRARIES ${PROJECT_NAME} 31 | CATKIN_DEPENDS 32 | roscpp 33 | tf 34 | DEPENDS 35 | OpenCV 36 | ) 37 | 38 | include_directories(include ${catkin_INCLUDE_DIRS} ${OpenCV_INCLUDE_DIRS} ${GEOS_INCLUDE_DIRS} ${EIGEN3_INCLUDE_DIRS}) 39 | 40 | add_library(${PROJECT_NAME} 41 | src/cubic_spline.cpp 42 | src/geometry_util.cpp 43 | src/intersection.cpp 44 | src/polygon.cpp 45 | ) 46 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${GEOS_LIBRARIES}) 47 | 48 | install(DIRECTORY include/${PROJECT_NAME}/ 49 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 50 | FILES_MATCHING PATTERN "*.h" 51 | ) 52 | 53 | install(TARGETS ${PROJECT_NAME} 54 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 55 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 56 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 57 | ) 58 | -------------------------------------------------------------------------------- /src/marti_common/LICENSE: -------------------------------------------------------------------------------- 1 | Copyright (c) 2014, Southwest Research Institute® (SwRI®) 2 | All rights reserved. 3 | 4 | Redistribution and use in source and binary forms, with or without 5 | modification, are permitted provided that the following conditions are met: 6 | * Redistributions of source code must retain the above copyright 7 | notice, this list of conditions and the following disclaimer. 8 | * Redistributions in binary form must reproduce the above copyright 9 | notice, this list of conditions and the following disclaimer in the 10 | documentation and/or other materials provided with the distribution. 11 | * Neither the name of Southwest Research Institute® (SwRI®) nor the 12 | names of its contributors may be used to endorse or promote products 13 | derived from this software without specific prior written permission. 14 | 15 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 16 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 17 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 18 | DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 19 | DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 20 | (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 21 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 22 | ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 23 | (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 24 | SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 | -------------------------------------------------------------------------------- /src/marti_common/swri_system_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_system_util) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp) 6 | 7 | find_package(Boost REQUIRED COMPONENTS filesystem regex) 8 | 9 | if(${Boost_MAJOR_VERSION} LESS 2 AND ${Boost_MINOR_VERSION} LESS 48) 10 | add_definitions( -DBOOST_FILESYSTEM_VERSION=2 ) 11 | message(STATUS " Using Boost Filesystem Version 2") 12 | else(${Boost_MAJOR_VERSION} LESS 2 AND ${Boost_MINOR_VERSION} LESS 48) 13 | add_definitions( -DBOOST_FILESYSTEM_VERSION=3 ) 14 | message(STATUS " Using Boost Filesystem Version 3") 15 | endif(${Boost_MAJOR_VERSION} LESS 2 AND ${Boost_MINOR_VERSION} LESS 48) 16 | 17 | catkin_package( 18 | INCLUDE_DIRS include 19 | LIBRARIES ${PROJECT_NAME} 20 | CATKIN_DEPENDS roscpp 21 | ) 22 | 23 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS} ) 24 | 25 | add_library(${PROJECT_NAME} 26 | src/file_util.cpp) 27 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 28 | 29 | if(CATKIN_ENABLE_TESTING) 30 | find_package(rostest REQUIRED) 31 | 32 | add_rostest_gtest(test_file_util launch/file_util.test test/test_file_util.cpp) 33 | target_link_libraries(test_file_util ${PROJECT_NAME}) 34 | endif() 35 | 36 | install(DIRECTORY include/${PROJECT_NAME}/ 37 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 38 | FILES_MATCHING PATTERN "*.h" 39 | ) 40 | 41 | install(TARGETS ${PROJECT_NAME} 42 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 43 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 44 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 45 | ) 46 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_math_util) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | ) 7 | 8 | # The Boost Random library headers and namespaces changed between version 9 | # 1.46 and 1.47 10 | find_package(Boost COMPONENTS random) 11 | if(${Boost_LIB_VERSION} MATCHES "1_46*") 12 | add_definitions( -DBOOST_1_46 ) 13 | endif(${Boost_LIB_VERSION} MATCHES "1_46*") 14 | 15 | catkin_package( 16 | INCLUDE_DIRS include 17 | LIBRARIES ${PROJECT_NAME} 18 | CATKIN_DEPENDS roscpp 19 | ) 20 | 21 | include_directories(include ${catkin_INCLUDE_DIRS} ${Boost_INCLUDE_DIRS}) 22 | 23 | add_library(${PROJECT_NAME} 24 | src/math_util.cpp 25 | src/trig_util.cpp 26 | src/random.cpp 27 | src/interpolation_1d.cpp) 28 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 29 | 30 | if(CATKIN_ENABLE_TESTING) 31 | find_package(rostest REQUIRED) 32 | 33 | add_rostest_gtest(test_trig_util launch/trig_util.test test/test_trig_util.cpp) 34 | target_link_libraries(test_trig_util ${PROJECT_NAME}) 35 | 36 | add_rostest_gtest(test_math_util launch/math_util.test test/test_math_util.cpp) 37 | target_link_libraries(test_math_util ${PROJECT_NAME}) 38 | 39 | add_rostest_gtest(test_random launch/random.test test/test_random.cpp) 40 | target_link_libraries(test_random ${PROJECT_NAME}) 41 | endif() 42 | 43 | install(DIRECTORY include/${PROJECT_NAME}/ 44 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 45 | FILES_MATCHING PATTERN "*.h" 46 | ) 47 | 48 | install(TARGETS ${PROJECT_NAME} 49 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 52 | ) 53 | 54 | -------------------------------------------------------------------------------- /src/tractobots_robot_localization/scripts/pose_transformer.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | import tf 5 | import tf2_ros 6 | import tf2_geometry_msgs 7 | from geometry_msgs.msg import PoseStamped 8 | 9 | class PoseTransformer: 10 | 11 | def __init__(self, from_frame, to_frame): 12 | 13 | pose_stamped = PoseStamped() 14 | pose_stamped.header.frame_id = from_frame 15 | pose_stamped.header.stamp = rospy.Time.now() 16 | 17 | tf_buffer = tf2_ros.Buffer(rospy.Duration(1.0)) # tf buffer length 18 | tf_listener = tf2_ros.TransformListener(tf_buffer) 19 | 20 | while not rospy.is_shutdown(): 21 | try: 22 | rospy.sleep(0.1) 23 | transform = tf_buffer.lookup_transform(to_frame, 24 | from_frame, # source frame 25 | rospy.Time(0), # get the tf at first available time 26 | rospy.Duration(0.1)) # wait for 1 second 27 | 28 | pose_transformed = tf2_geometry_msgs.do_transform_pose(pose_stamped, transform) 29 | 30 | pose_pub.publish(pose_transformed) 31 | 32 | except Exception as e: 33 | rospy.logwarn(e) 34 | continue 35 | 36 | if __name__ == '__main__': 37 | rospy.init_node('pose_transformer') 38 | from_frame = rospy.get_param('~from_frame') 39 | to_frame = rospy.get_param('~to_frame') 40 | pose_pub = rospy.Publisher('pose_example_publisher', PoseStamped, queue_size=10) 41 | ptf = PoseTransformer(from_frame, to_frame) 42 | rospy.loginfo("Transforming pose from frame: %s to frame: %s" % (from_frame, to_frame)) 43 | 44 | rospy.spin() 45 | -------------------------------------------------------------------------------- /src/marti_common/swri_serial_util/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_serial_util 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | * Support higher serial baud rates 8 | The system header `/usr/include/asm-generic/termbits.h` has constants for 9 | supporting baud rates up to 4000000, but swri_serial_util only allows up 10 | to 230400. Some devices support these higher rates and there's no reason 11 | to not support them, so this adds support for them. 12 | * Check for error from ioctl in serial_port 13 | Fixes `#406 `_ 14 | * Contributors: Edward Venator, P. J. Reed 15 | 16 | 0.2.3 (2016-12-09) 17 | ------------------ 18 | 19 | 0.2.2 (2016-12-07) 20 | ------------------ 21 | 22 | 0.2.1 (2016-10-23) 23 | ------------------ 24 | 25 | 0.2.0 (2016-06-21) 26 | ------------------ 27 | 28 | 0.1.5 (2016-05-13) 29 | ------------------ 30 | 31 | 0.1.4 (2016-05-12) 32 | ------------------ 33 | 34 | 0.1.3 (2016-03-04) 35 | ------------------ 36 | 37 | 0.1.2 (2016-01-06) 38 | ------------------ 39 | 40 | 0.1.1 (2015-11-17) 41 | ------------------ 42 | 43 | 0.1.0 (2015-09-29) 44 | ------------------ 45 | 46 | 0.0.5 (2015-09-27) 47 | ------------------ 48 | 49 | 0.0.4 (2015-09-27) 50 | ------------------ 51 | 52 | 0.0.3 (2015-09-26) 53 | ------------------ 54 | * Fixes missing boost dependency in swri_serial_util. 55 | Refs `#234 `_. 56 | * Contributors: Ed Venator 57 | 58 | 0.0.2 (2015-09-25) 59 | ------------------ 60 | * Renames serial_util to swri_serial_util. Refs `#231 `_. 61 | * Contributors: Edward Venator 62 | 63 | 0.0.1 (2015-09-25) 64 | ------------------ 65 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_nodelet 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | 8 | 0.2.3 (2016-12-09) 9 | ------------------ 10 | 11 | 0.2.2 (2016-12-07) 12 | ------------------ 13 | 14 | 0.2.1 (2016-10-23) 15 | ------------------ 16 | * Add C++ and CMake macros for wrapper nodes 17 | Defines a C++ macro to replace the normal nodelet export wrapper that 18 | also creates a factory function returning a pointer to that nodelet. 19 | Defines a CMake macro 20 | swri_nodelet_add_node(NODELET_NODENAME NODELET_NAMESPACE NODELET_CLASS) 21 | that automatically generates the c++ code for a node wrapper with node 22 | name NODELET_NODENAME that wraps the nodelet and makes a CMake target 23 | to build the node. 24 | * Add tests for swri_nodelet with manager and standalone 25 | * Contributors: Ed Venator, Edward Venator 26 | 27 | 0.2.0 (2016-06-21) 28 | ------------------ 29 | 30 | 0.1.5 (2016-05-13) 31 | ------------------ 32 | 33 | 0.1.4 (2016-05-12) 34 | ------------------ 35 | * Add swri_nodelet package. 36 | This package simplifies launch files that can easily change between 37 | running nodelets in a shared manager or as standalone processes. 38 | * Contributors: Elliot Johnson 39 | 40 | 0.1.3 (2016-03-04) 41 | ------------------ 42 | 43 | 0.1.2 (2016-01-06) 44 | ------------------ 45 | 46 | 0.1.1 (2015-11-17) 47 | ------------------ 48 | 49 | 0.1.0 (2015-09-29) 50 | ------------------ 51 | 52 | 0.0.5 (2015-09-27 15:27) 53 | ------------------------ 54 | 55 | 0.0.4 (2015-09-27 11:35) 56 | ------------------------ 57 | 58 | 0.0.3 (2015-09-26) 59 | ------------------ 60 | 61 | 0.0.2 (2015-09-25 15:00) 62 | ------------------------ 63 | 64 | 0.0.1 (2015-09-25 09:06) 65 | ------------------------ 66 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_nodelet) 3 | 4 | set(BUILD_DEPS 5 | nodelet 6 | roscpp 7 | std_msgs 8 | ) 9 | 10 | set(RUNTIME_DEPS ${BUILD_DEPS}) 11 | 12 | find_package(catkin REQUIRED COMPONENTS ${BUILD_DEPS}) 13 | catkin_package( 14 | CATKIN_DEPENDS ${RUNTIME_DEPS} 15 | INCLUDE_DIRS include 16 | CFG_EXTRAS swri_nodelet-extras.cmake 17 | ) 18 | 19 | # nodelet.cpp.in must be copied before include(...) or clean builds will fail 20 | file(COPY nodelet.cpp.in 21 | DESTINATION ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_SHARE_DESTINATION} 22 | ) 23 | set(swri_nodelet_PREFIX ${CATKIN_DEVEL_PREFIX}) # Allows use of nodelet.cpp.in in swri_nodelet_add_node macro for this build 24 | include(${CATKIN_DEVEL_PREFIX}/share/${PROJECT_NAME}/cmake/swri_nodelet-extras.cmake) # Allows use of swri_nodelet_add_node macro for this build 25 | 26 | include_directories(${catkin_INCLUDE_DIRS} include) 27 | 28 | add_library(swri_nodelet_test src/test_nodelet.cpp) 29 | target_link_libraries(swri_nodelet_test ${catkin_LIBRARIES}) 30 | 31 | swri_nodelet_add_node(test_node swri_nodelet TestNodelet) 32 | target_link_libraries(test_node swri_nodelet_test) 33 | 34 | if(CATKIN_ENABLE_TESTING) 35 | find_package(rostest REQUIRED) 36 | add_rostest(test/test_manager.test DEPENDENCIES swri_nodelet_test) 37 | add_rostest(test/test_standalone.test DEPENDENCIES swri_nodelet_test) 38 | add_rostest(test/test_node_wrapper.test DEPENDENCIES test_node) 39 | endif() 40 | 41 | install(PROGRAMS 42 | nodes/nodelet 43 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 44 | 45 | install(FILES nodelet.cpp.in 46 | nodelet_plugins.xml 47 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 48 | ) 49 | 50 | install(DIRECTORY include/${PROJECT_NAME}/ 51 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 52 | FILES_MATCHING PATTERN "*.h" 53 | ) 54 | -------------------------------------------------------------------------------- /src/marti_common/swri_yaml_util/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(swri_yaml_util) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | find_package(PkgConfig) 7 | pkg_check_modules(YamlCpp yaml-cpp) 8 | message(STATUS "YAML-CPP VERSION: " ${YamlCpp_VERSION}) 9 | message(STATUS "YAML-CPP LIBRARY: " ${YamlCpp_LIBRARIES}) 10 | 11 | catkin_destinations() 12 | file(MAKE_DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 13 | catkin_package( 14 | INCLUDE_DIRS include ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION} 15 | LIBRARIES ${PROJECT_NAME} ${YamlCpp_LIBRARIES} 16 | ) 17 | 18 | if( ${YamlCpp_VERSION} MATCHES "0.[23].*") 19 | set(YAMLCPP_OLD_API ON) 20 | message(STATUS " Using old yaml-cpp API.") 21 | else() 22 | set(YAMLCPP_OLD_API OFF) 23 | message(STATUS " Using new yaml-cpp API.") 24 | endif() 25 | 26 | configure_file(version.h.in ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION}/version.h) 27 | 28 | find_package(Boost REQUIRED) 29 | 30 | include_directories(include 31 | ${CATKIN_DEVEL_PREFIX}/${CATKIN_GLOBAL_INCLUDE_DESTINATION} 32 | ${Boost_INCLUDE_DIRS} 33 | ${catkin_INCLUDE_DIRS} 34 | ${YamlCpp_INCLUDE_DIR}) 35 | 36 | add_library(${PROJECT_NAME} 37 | src/yaml_util.cpp 38 | ) 39 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${YamlCpp_LIBRARIES}) 40 | 41 | install(DIRECTORY include/${PROJECT_NAME}/ 42 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 43 | FILES_MATCHING PATTERN "*.h" 44 | ) 45 | 46 | install( 47 | DIRECTORY ${CATKIN_DEVEL_PREFIX}/${CATKIN_PACKAGE_INCLUDE_DESTINATION}/ 48 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 49 | ) 50 | 51 | install(TARGETS ${PROJECT_NAME} 52 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 53 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | ) 56 | 57 | -------------------------------------------------------------------------------- /src/marti_common/README.md: -------------------------------------------------------------------------------- 1 | marti_common 2 | ============== 3 | 4 | [![Build Status](https://travis-ci.org/swri-robotics/marti_common.svg?branch=kinetic-devel)](https://travis-ci.org/swri-robotics/marti_common) 5 | 6 | This repository provides various utility packages created at [Southwest Reseach Institute](http://www.swri.org)'s [Intelligent Vehicle Systems](http://www.swri.org/4org/d10/isd/ivs/default.htm) section for working with [Robot Operating System(ROS)](http://www.ros.org). 7 | 8 | ## Installation (ROS Indigo, Jade) 9 | 10 | If you have installed ROS Indigo or Jade, you can install any of the packages in this repository with apt-get: 11 | 12 | sudo apt-get install ros-$ROS_DISTRO- 13 | 14 | Building From Source (ROS Indigo, Jade, Kinetic) 15 | ------------ 16 | 17 | These directions assume you have already set up a catkin workspace. See [this tutorial](http://wiki.ros.org/catkin/Tutorials/create_a_workspace) on the ROS Wiki for help setting up a catkin workspace. 18 | 19 | ### Checking out the source code (wstool) 20 | 21 | If you're using wstool, add this repository to your wstool workspace: 22 | 23 | # Replace $ROS_DISTRO with indigo, jade, or kinetic as appropriate 24 | wstool set marti_common --git https://github.com/swri-robotics/marti_common.git -v $ROS_DISTRO-devel 25 | 26 | ### Checking out the source code (git) 27 | 28 | If you're not using wstool, you can check out the repositories with git: 29 | 30 | # Replace $ROS_DISTRO with indigo, jade, or kinetic as appropriate 31 | git clone https://github.com/swri-robotics/marti_common.git --branch $ROS_DISTRO-devel 32 | 33 | ### Installing dependencies and building 34 | 35 | Install all of the dependencies using rosdep by running the following command from the root of your catkin workspace: 36 | 37 | rosdep install --from-paths src --ignore-src 38 | 39 | Build the workspace with catkin_make: 40 | 41 | catkin_make 42 | -------------------------------------------------------------------------------- /src/tractobots_robot_localization/scripts/imu_publisher.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Node for changing frame of IMU msg when working with a rosbag file and 3 | # publishing a marker for rviz visualization. 4 | 5 | import rospy 6 | import tf 7 | from math import pi 8 | from sensor_msgs.msg import Imu 9 | from visualization_msgs.msg import Marker 10 | 11 | def imuCallback(data): 12 | imu_msg = Imu() 13 | # imu_msg = data 14 | imu_msg.header.stamp = rospy.Time.now() 15 | imu_msg.header.frame_id = "imu_link" 16 | quat = ( 17 | data.orientation.x, 18 | data.orientation.y, 19 | data.orientation.z, 20 | data.orientation.w) 21 | euler = tf.transformations.euler_from_quaternion(quat) 22 | quat2 = tf.transformations.quaternion_from_euler(0, 0, euler[0]+pi) 23 | imu_msg.orientation.x = quat2[0] 24 | imu_msg.orientation.y = quat2[1] 25 | imu_msg.orientation.z = quat2[2] 26 | imu_msg.orientation.w = quat2[3] 27 | 28 | imu_pub.publish(imu_msg) 29 | 30 | marker_msg = Marker() 31 | marker_msg.header = imu_msg.header 32 | marker_msg.action = 0 # ADD 33 | marker_msg.type = 0 # ARROW 34 | marker_msg.scale.x = 1 35 | marker_msg.scale.y = 0.1 36 | marker_msg.scale.z = 0.1 37 | marker_msg.color.a = 1.0 38 | marker_msg.color.r = 0.0; 39 | marker_msg.color.g = 1.0; 40 | marker_msg.color.b = 0.0; 41 | marker_msg.pose.position.x = 0 42 | marker_msg.pose.position.y = 0 43 | marker_msg.pose.orientation.x = imu_msg.orientation.x 44 | marker_msg.pose.orientation.y = imu_msg.orientation.y 45 | marker_msg.pose.orientation.z = imu_msg.orientation.z 46 | marker_msg.pose.orientation.w = imu_msg.orientation.w 47 | 48 | marker_pub.publish(marker_msg) 49 | 50 | if __name__ == '__main__': 51 | 52 | rospy.init_node('imuPublisher') 53 | 54 | rospy.Subscriber('/tfsensors/imu1', Imu, imuCallback) 55 | 56 | imu_pub = rospy.Publisher('imu', Imu, queue_size=10) 57 | marker_pub = rospy.Publisher('imu/marker', Marker, queue_size=10) 58 | 59 | rospy.spin() 60 | 61 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/src/ransac.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | namespace swri_math_util 33 | { 34 | 35 | } 36 | -------------------------------------------------------------------------------- /src/tractobots_launchers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tractobots_launchers 4 | 0.0.0 5 | The tractobots_launchers package 6 | 7 | 8 | 9 | 10 | atroya 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 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /src/marti_common/swri_yaml_util/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_yaml_util 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | * Make swri_yaml_util build out-of-source. 8 | * Contributors: Marc Alban 9 | 10 | 0.2.3 (2016-12-09) 11 | ------------------ 12 | 13 | 0.2.2 (2016-12-07) 14 | ------------------ 15 | 16 | 0.2.1 (2016-10-23) 17 | ------------------ 18 | 19 | 0.2.0 (2016-06-21) 20 | ------------------ 21 | 22 | 0.1.5 (2016-05-13) 23 | ------------------ 24 | * Add an explicit dependency on pkg-config 25 | * Contributors: P. J. Reed 26 | 27 | 0.1.4 (2016-05-12) 28 | ------------------ 29 | * Add support to load YAML from string and dictionary. 30 | * Fix linking with yaml-cpp. 31 | * Contributors: Elliot Johnson, Marc Alban 32 | 33 | 0.1.3 (2016-03-04) 34 | ------------------ 35 | * Adds uint16 support to swri_yaml_util 36 | * Contributors: P. J. Reed 37 | 38 | 0.1.2 (2016-01-06) 39 | ------------------ 40 | 41 | 0.1.1 (2015-11-17) 42 | ------------------ 43 | 44 | 0.1.0 (2015-09-29) 45 | ------------------ 46 | 47 | 0.0.5 (2015-09-27) 48 | ------------------ 49 | * Adds missing package dependency and find_package() for boost in 50 | swri_yaml_util. See issue `#240 `_ 51 | * Cleans up all catkin_lint by adding a proper package description 52 | and fixing some formatting in CMakeLists.txt. 53 | * Contributors: Ed Venator 54 | 55 | 0.0.4 (2015-09-27) 56 | ------------------ 57 | * Adds boost include directories to yaml_util because yaml-cpp uses boost and doesn't export the include directory. 58 | * Contributors: Ed Venator 59 | 60 | 0.0.3 (2015-09-26) 61 | ------------------ 62 | * Fixes missing yaml-cpp dependency in swri_yaml_util. 63 | Refs `#233 `_. 64 | * Contributors: Ed Venator 65 | 66 | 0.0.2 (2015-09-25) 67 | ------------------ 68 | * Renames yaml_util to swri_yaml_util. Refs `#231 `_. 69 | * Contributors: Edward Venator 70 | 71 | 0.0.1 (2015-09-25) 72 | ------------------ 73 | -------------------------------------------------------------------------------- /src/tractobots_robot_localization/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | tractobots_robot_localization 4 | 0.0.0 5 | The tractobots_robot_localization package 6 | 7 | 8 | 9 | 10 | atroya 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 | catkin 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | 3 | project(swri_roscpp) 4 | 5 | set(BUILD_DEPS 6 | diagnostic_updater 7 | nav_msgs 8 | roscpp 9 | std_msgs 10 | std_srvs 11 | ) 12 | 13 | set(RUNTIME_DEPS 14 | diagnostic_updater 15 | nav_msgs 16 | roscpp 17 | std_msgs 18 | std_srvs 19 | ) 20 | 21 | ### Catkin ### 22 | find_package(catkin REQUIRED COMPONENTS ${BUILD_DEPS}) 23 | include_directories(include 24 | ${catkin_INCLUDE_DIRS}) 25 | catkin_package(CATKIN_DEPENDS ${RUNTIME_DEPS} 26 | INCLUDE_DIRS include) 27 | 28 | ### Build Test Node ### 29 | add_executable(subscriber_test src/nodes/subscriber_test.cpp) 30 | target_link_libraries(subscriber_test ${catkin_LIBRARIES}) 31 | 32 | add_executable(latched_subscriber_test src/nodes/latched_subscriber_test.cpp) 33 | target_link_libraries(latched_subscriber_test ${catkin_LIBRARIES}) 34 | 35 | add_executable(storing_subscriber_test src/nodes/storing_subscriber_test.cpp) 36 | target_link_libraries(storing_subscriber_test ${catkin_LIBRARIES}) 37 | 38 | add_executable(service_server_test src/nodes/service_server_test.cpp) 39 | target_link_libraries(service_server_test ${catkin_LIBRARIES}) 40 | 41 | add_executable(timer_test src/nodes/timer_test.cpp) 42 | target_link_libraries(timer_test ${catkin_LIBRARIES}) 43 | 44 | add_executable(param_example src/nodes/param_example.cpp) 45 | target_link_libraries(param_example ${catkin_LIBRARIES}) 46 | 47 | if (CATKIN_ENABLE_TESTING) 48 | find_package(rostest REQUIRED) 49 | add_rostest_gtest(test_params 50 | test/params.test 51 | test/params.cpp) 52 | target_link_libraries(test_params ${catkin_LIBRARIES}) 53 | endif() 54 | 55 | ### Install Test Node and Headers ### 56 | install(TARGETS subscriber_test latched_subscriber_test param_example 57 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 58 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 60 | ) 61 | install(DIRECTORY include/${PROJECT_NAME}/ 62 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 63 | ) 64 | install(DIRECTORY launch/ 65 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 66 | ) 67 | 68 | -------------------------------------------------------------------------------- /src/marti_common/swri_geometry_util/include/swri_geometry_util/intersection.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | 33 | namespace swri_geometry_util 34 | { 35 | bool PolygonsIntersect( 36 | const std::vector& a, 37 | const std::vector& b); 38 | 39 | double PolygonIntersectionArea( 40 | const std::vector& a, 41 | const std::vector& b); 42 | } 43 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/include/swri_roscpp/swri_roscpp.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2015, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | #ifndef SWRI_ROSCPP_SWRI_ROSCPP_H_ 30 | #define SWRI_ROSCPP_SWRI_ROSCPP_H_ 31 | 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | #endif // SWRI_ROSCPP_SWRI_ROSCPP_H_ 40 | -------------------------------------------------------------------------------- /src/marti_common/swri_opencv_util/include/swri_opencv_util/show.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef OPENCV_UTIL_SHOW_H_ 31 | #define OPENCV_UTIL_SHOW_H_ 32 | 33 | #include 34 | 35 | namespace swri_opencv_util 36 | { 37 | void ShowScaled( 38 | const std::string& name, 39 | const cv::Mat& mat, 40 | const cv::Mat& mask = cv::Mat(), 41 | double a = -1.0, // assume auto-scaling 42 | double b = 0.0); 43 | } 44 | 45 | #endif // OPENCV_UTIL_SHOW_H_ 46 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/src/nodes/draw_text.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | 33 | int main(int argc, char **argv) 34 | { 35 | ros::init(argc, argv, "draw_text", ros::init_options::AnonymousName); 36 | 37 | nodelet::Loader manager(false); 38 | 39 | nodelet::M_string remappings; 40 | nodelet::V_string my_argv; 41 | manager.load(ros::this_node::getName(), "swri_image_util/draw_text", 42 | remappings, my_argv); 43 | 44 | ros::spin(); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/src/nodes/scale_image.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | 33 | int main(int argc, char **argv) 34 | { 35 | ros::init(argc, argv, "scale_image", ros::init_options::AnonymousName); 36 | 37 | nodelet::Loader manager(false); 38 | 39 | nodelet::M_string remappings; 40 | nodelet::V_string my_argv; 41 | manager.load(ros::this_node::getName(), "swri_image_util/scale_image", 42 | remappings, my_argv); 43 | 44 | ros::spin(); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/src/nodes/rotate_image.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | 33 | int main(int argc, char **argv) 34 | { 35 | ros::init(argc, argv, "rotate_image", ros::init_options::AnonymousName); 36 | 37 | nodelet::Loader manager(false); 38 | 39 | nodelet::M_string remappings; 40 | nodelet::V_string my_argv; 41 | manager.load(ros::this_node::getName(), "swri_image_util/rotate_image", 42 | remappings, my_argv); 43 | 44 | ros::spin(); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/src/nodes/contrast_stretch.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | 33 | int main(int argc, char **argv) 34 | { 35 | ros::init(argc, argv, "contrast_stretch", ros::init_options::AnonymousName); 36 | 37 | nodelet::Loader manager(false); 38 | 39 | nodelet::M_string remappings; 40 | nodelet::V_string my_argv; 41 | manager.load(ros::this_node::getName(), "swri_image_util/contrast_stretch", 42 | remappings, my_argv); 43 | 44 | ros::spin(); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/src/nodes/normalize_response.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | 33 | int main(int argc, char **argv) 34 | { 35 | ros::init(argc, argv, "normalize_response", ros::init_options::AnonymousName); 36 | 37 | nodelet::Loader manager(false); 38 | 39 | nodelet::M_string remappings; 40 | nodelet::V_string my_argv; 41 | manager.load(ros::this_node::getName(), "swri_image_util/normalize_response", 42 | remappings, my_argv); 43 | 44 | ros::spin(); 45 | return 0; 46 | } 47 | -------------------------------------------------------------------------------- /src/marti_common/swri_opencv_util/include/swri_opencv_util/convert.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef OPENCV_UTIL_CONVERT_H_ 31 | #define OPENCV_UTIL_CONVERT_H_ 32 | 33 | #include 34 | 35 | namespace swri_opencv_util 36 | { 37 | cv::Mat ToBgra8( 38 | const cv::Mat& mat, 39 | const cv::Mat& mask = cv::Mat(), 40 | bool is_rgb = false, 41 | double a = 0.0, // assume auto-scaling 42 | double b = 0.0); 43 | 44 | void SetAlpha(cv::Mat& mat, uint8_t alpha); 45 | } 46 | 47 | #endif // OPENCV_UTIL_CONVERT_H_ 48 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/src/nodes/blend_images_node.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2017, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | 33 | int main(int argc, char **argv) 34 | { 35 | ros::init(argc, argv, "blend_images", ros::init_options::AnonymousName); 36 | nodelet::Loader manager(false); 37 | 38 | nodelet::M_string remappings; 39 | nodelet::V_string my_argv; 40 | manager.load( 41 | ros::this_node::getName(), 42 | "swri_image_util/blend_images", 43 | remappings, 44 | my_argv); 45 | ros::spin(); 46 | return 0; 47 | } 48 | -------------------------------------------------------------------------------- /src/tractobots_robot_localization/params/navsat_transform.yaml: -------------------------------------------------------------------------------- 1 | # Frequency of the main run loop 2 | frequency: 10 3 | 4 | # Delay time, in seconds, before we calculate the transform from the UTM frame to your world frame. This is especially 5 | # important if you have use_odometry_yaw set to true. Defaults to 0. 6 | delay: 0.0 7 | 8 | # PLEASE READ: Like all nodes in robot_localization, this node assumes that your IMU data is reported in the ENU frame. 9 | # Many IMUs report data in the NED frame, so you'll want to verify that your data is in the correct frame before using 10 | # it. 11 | 12 | # If your IMU does not account for magnetic declination, enter the value for your location here. If you don't know it, 13 | # see http://www.ngdc.noaa.gov/geomag-web/ (make sure to convert the value to radians). This parameter is mandatory. 14 | magnetic_declination_radians: 0.0712094335 # This should be replaced with the magnetic declination for your location 15 | 16 | # Your IMU's yaw, once the magnetic_declination_radians value is added to it, should report 0 when facing east. If it 17 | # doesn't, enter the offset here. Defaults to 0. 18 | yaw_offset: 1.5707963268 19 | 20 | # If this is true, the altitude is set to 0 in the output odometry message. Defaults to false. 21 | zero_altitude: true 22 | 23 | # If this is true, the transform between the UTM grid and the frame of the input odometry data is broadcasted for being 24 | # use by other nodes. Defaults to false. 25 | broadcast_utm_transform: true 26 | 27 | # If this is true, all received odometry data is converted back to a lat/lon and published as a NavSatFix message as 28 | # /gps/filtered. Defaults to false. 29 | publish_filtered_gps: true 30 | 31 | # If this is true, the node ignores the IMU data and gets its heading from the odometry source (typically the 32 | # /odometry/filtered topic coming from one of robot_localization's state estimation nodes). BE CAREFUL when using this! 33 | # The yaw value in your odometry source *must* be world-referenced, e.g., you cannot use your odometry source for yaw 34 | # if your yaw data is based purely on integrated velocities. Defaults to false. 35 | use_odometry_yaw: false 36 | 37 | # If true, will retrieve the datum from the 'datum' parameter below, if available. If no 'datum' parameter exists, 38 | # navsat_transform_node will wait until the user calls the 'datum' service with the SetDatum service message. 39 | wait_for_datum: false 40 | 41 | -------------------------------------------------------------------------------- /src/marti_common/swri_geometry_util/include/swri_geometry_util/cubic_spline.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2015, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace swri_geometry_util 35 | { 36 | bool CubicSplineInterpolation( 37 | const std::vector& points, 38 | double delta, 39 | std::vector >& splines); 40 | 41 | bool CubicSplineInterpolation( 42 | const std::vector& points, 43 | double delta, 44 | std::vector >& splines); 45 | } 46 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/src/random.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | namespace swri_math_util 33 | { 34 | RandomGenerator::RandomGenerator(int32_t seed) : 35 | rng_(seed == -1 ? seed_() : seed) 36 | { 37 | } 38 | 39 | void RandomGenerator::GetUniformRandomSample( 40 | int32_t min, 41 | int32_t max, 42 | int32_t count, 43 | std::vector& sample) 44 | { 45 | boost::unique_lock lock(mutex_); 46 | swri_math_util::GetUniformRandomSample(rng_, min, max, count, sample); 47 | } 48 | } 49 | -------------------------------------------------------------------------------- /src/marti_common/swri_string_util/include/swri_string_util/string_util.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef STRING_UTIL_STRING_UTIL_H_ 31 | #define STRING_UTIL_STRING_UTIL_H_ 32 | 33 | #include 34 | #include 35 | 36 | namespace swri_string_util 37 | { 38 | bool ToDouble(const std::string& string, double& value); 39 | 40 | bool ToFloat(const std::string& string, float& value); 41 | 42 | bool ToInt32(const std::string& string, int32_t& value, int32_t base = 10); 43 | 44 | bool ToUInt32(const std::string& string, uint32_t& value, int32_t base = 10); 45 | } 46 | 47 | #endif // STRING_UTIL_STRING_UTIL_H_ 48 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/include/swri_image_util/blend_images_util.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2017, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef IMAGE_UTIL_BLEND_IMAGES_H_ 31 | #define IMAGE_UTIL_BLEND_IMAGES_H_ 32 | 33 | #include 34 | 35 | namespace swri_image_util 36 | { 37 | /** 38 | * Blends two images together. top_image will be drawn on top of base_image 39 | * with a blending level of alpha. The blended image will be placed in 40 | * dest_image 41 | */ 42 | void blendImages( 43 | const cv::Mat& base_image, 44 | const cv::Mat& top_image, 45 | const double alpha, 46 | cv::Mat& dest_image); 47 | } 48 | 49 | #endif // IMAGE_UTIL_BLEND_IMAGES_H_ 50 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/include/swri_roscpp/publisher.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2015, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | #ifndef SWRI_ROSCPP_PUBLISHER_H_ 30 | #define SWRI_ROSCPP_PUBLISHER_H_ 31 | 32 | #include 33 | 34 | namespace swri 35 | { 36 | template 37 | ros::Publisher advertise( 38 | ros::NodeHandle &nh, 39 | const std::string name, 40 | uint32_t queue_size, 41 | bool latched=false) 42 | { 43 | const std::string resolved_name = nh.resolveName(name); 44 | ROS_INFO("Publishing [%s] to '%s'.", 45 | name.c_str(), 46 | resolved_name.c_str()); 47 | return nh.advertise(name, queue_size, latched); 48 | } 49 | } // namespace swri 50 | #endif // SWRI_ROSCPP_PUBLISHER_H_ 51 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/src/trig_util.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace swri_math_util 35 | { 36 | double WrapRadians(double angle, double center) 37 | { 38 | double wrapped = angle; 39 | while (wrapped < center && center - wrapped > _pi) 40 | { 41 | wrapped += _2pi; 42 | } 43 | 44 | while (wrapped > center && wrapped - center > _pi) 45 | { 46 | wrapped -= _2pi; 47 | } 48 | 49 | return wrapped; 50 | } 51 | 52 | double ToRadians(double degrees) 53 | { 54 | return degrees * _pi / 180.0; 55 | } 56 | 57 | double ToDegrees(double radians) 58 | { 59 | return radians * 180.0 / _pi; 60 | } 61 | } 62 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/include/swri_image_util/rolling_normalization.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef IMAGE_UTIL_ROLLING_NORMALIZATION_H_ 31 | #define IMAGE_UTIL_ROLLING_NORMALIZATION_H_ 32 | 33 | #include 34 | 35 | // ROS Libraries 36 | #include 37 | 38 | // OpenCV Libraries 39 | #include 40 | #include 41 | 42 | namespace swri_image_util 43 | { 44 | class RollingNormalization 45 | { 46 | public: 47 | explicit RollingNormalization(int32_t size); 48 | ~RollingNormalization(); 49 | 50 | cv::Mat AddSample(const cv::Mat& image); 51 | private: 52 | int32_t max_size_; 53 | int32_t samples_; 54 | cv::Mat average_image_; 55 | cv::Mat norm_image_; 56 | }; 57 | } 58 | 59 | 60 | #endif // IMAGE_UTIL_ROLLING_NORMALIZATION_H_ 61 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_roscpp 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | * Port `#385 `_ and `#419 `_ to kinetic. (`#420 `_) 8 | A common error when using unfamiliar ROS nodes is to accidentally set parameters 9 | by the wrong name. This feature allows the node author to output a WARNING 10 | for any unused parameters. 11 | Ported forward from indigo-devel 12 | * Contributors: Edward Venator 13 | 14 | 0.2.3 (2016-12-09) 15 | ------------------ 16 | 17 | 0.2.2 (2016-12-07) 18 | ------------------ 19 | * Deprecate LatchedSubscriber. (`#391 `_) 20 | LatchedSubscriber should be replaced with a swri::Subscriber that is 21 | initialized with the address of a location to store messages. This change 22 | makes for a simpler and more consistent interface, and avoids the confusion 23 | that comes from overloading the -> operator. 24 | * Contributors: P. J. Reed 25 | 26 | 0.2.1 (2016-10-23) 27 | ------------------ 28 | 29 | 0.2.0 (2016-06-21) 30 | ------------------ 31 | 32 | 0.1.5 (2016-05-13) 33 | ------------------ 34 | 35 | 0.1.4 (2016-05-12) 36 | ------------------ 37 | * Add timeoutParam() method to swri::Subscriber. 38 | This commit adds a new convenience method, timeoutParam, to 39 | swri::Subscriber that reads a specified parameter directly from the 40 | parameter server and sets it as the subscriber's timeout value. This 41 | is to simplify setup code that currently has to define a temporary 42 | variable, read the parameter in the temp, and then set the timeout. 43 | * Contributors: Elliot Johnson 44 | 45 | 0.1.3 (2016-03-04) 46 | ------------------ 47 | * Adds getParam() functions to swri_roscpp. 48 | These functions wrap NodeHandle::getParam(). If the parameter does 49 | not exist, they emit an error message and return false. 50 | * Contributors: Edward Venator 51 | 52 | 0.1.2 (2016-01-06) 53 | ------------------ 54 | 55 | 0.1.1 (2015-11-17) 56 | ------------------ 57 | * First jade release of swri_roscpp 58 | * Contributors: Edward Venator 59 | 60 | 0.1.0 (2015-09-29) 61 | ------------------ 62 | 63 | 0.0.5 (2015-09-27 15:27) 64 | ------------------------ 65 | 66 | 0.0.4 (2015-09-27 11:35) 67 | ------------------------ 68 | 69 | 0.0.3 (2015-09-26) 70 | ------------------ 71 | 72 | 0.0.2 (2015-09-25 15:00) 73 | ------------------------ 74 | 75 | 0.0.1 (2015-09-25 09:06) 76 | ------------------------ 77 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/include/swri_math_util/constants.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef MATH_UTIL_CONSTANTS_H_ 31 | #define MATH_UTIL_CONSTANTS_H_ 32 | 33 | namespace swri_math_util 34 | { 35 | static const long double _pi = 3.1415926535897932384626433832795029L; 36 | 37 | static const long double _2pi = 6.28318530717958647692528676655900576L; 38 | 39 | static const long double _half_pi = 1.57079632679489661923132169163975144L; 40 | 41 | static const long double _deg_2_rad = 0.01745329251994329576923690768488612L; 42 | 43 | static const long double _rad_2_deg = 57.2957795130823208767981548141051703L; 44 | 45 | static const double _kph_to_mps = 0.2777777777777778; 46 | 47 | static const double _mps_to_mph = 2.2369362920544025; 48 | 49 | static const double _feet_to_meters = 0.3048; 50 | 51 | static const double _yards_to_meters = 0.9144; 52 | } 53 | 54 | #endif // MATH_UTIL_CONSTANTS_H_ 55 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/src/test_nodelet.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2016, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL SOUTHWEST RESEARCH INSTITUTE BE LIABLE FOR 21 | // ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 22 | // DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 23 | // SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 24 | // CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 25 | // OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 26 | // OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | namespace swri_nodelet 35 | { 36 | class TestNodelet : public nodelet::Nodelet 37 | { 38 | public: 39 | virtual void onInit() 40 | { 41 | publisher_ = getNodeHandle().advertise("numbers", 2); 42 | timer_ = getNodeHandle().createTimer(ros::Duration(0.1), &TestNodelet::timerCallback, this); 43 | } 44 | private: 45 | ros::Publisher publisher_; 46 | ros::Timer timer_; 47 | 48 | void timerCallback(ros::TimerEvent const& e) 49 | { 50 | std_msgs::Int32 msg; 51 | msg.data = 1337; 52 | publisher_.publish(msg); 53 | } 54 | }; 55 | } 56 | 57 | #include 58 | SWRI_NODELET_EXPORT_CLASS(swri_nodelet, TestNodelet); 59 | -------------------------------------------------------------------------------- /src/marti_common/swri_opencv_util/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package swri_opencv_util 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 0.2.4 (2017-04-11) 6 | ------------------ 7 | 8 | 0.2.3 (2016-12-09) 9 | ------------------ 10 | * Fix OpenCV dependencies for Kinetic build (`#400 `_) 11 | * Contributors: P. J. Reed 12 | 13 | 0.2.2 (2016-12-07) 14 | ------------------ 15 | * Migrated OpenCV to 3.1 (default in Kinetic) 16 | * Contributors: Brian Holt 17 | 18 | 0.2.1 (2016-10-23) 19 | ------------------ 20 | 21 | 0.2.0 (2016-06-21) 22 | ------------------ 23 | 24 | 0.1.5 (2016-05-13) 25 | ------------------ 26 | 27 | 0.1.4 (2016-05-12) 28 | ------------------ 29 | * Fixed compile error when ros-indigo-opencv3 is installed (`#307 `_) 30 | * Fixed compile error when package ros-indigo-opencv3 is installed. 31 | swri_geometry_util uses wrong version of OpenCV when the package 32 | ros-indigo-opencv3 is installed. This patch fixes the issue. 33 | * Updated all CMakeFiles.txt to specify OpenCV version 2 34 | The find_package for OpenCV is now: 35 | ./swri_opencv_util/CMakeLists.txt:find_package(OpenCV 2 REQUIRED) 36 | ./swri_geometry_util/CMakeLists.txt:find_package(OpenCV 2 REQUIRED) 37 | ./swri_image_util/CMakeLists.txt:find_package(OpenCV 2) 38 | ./swri_transform_util/CMakeLists.txt:find_package(OpenCV 2 REQUIRED) 39 | Conflicts: 40 | swri_geometry_util/CMakeLists.txt 41 | * Contributors: Kim Mathiassen 42 | 43 | 0.1.3 (2016-03-04) 44 | ------------------ 45 | 46 | 0.1.2 (2016-01-06) 47 | ------------------ 48 | * Mark some constructors explicit. 49 | * Refactor RANSAC matching code to use more matrix operations. 50 | * Fix bugs in FitRigidTransform2d. 51 | The main problem was that reshape was being incorrectly, causing the 52 | points to get shuffled around. Once that was fixed, it was clear that 53 | the rotation should not be inverted. Also added a comment to clarify 54 | the significance of the returned transform. 55 | * Contributors: Elliot Johnson, Marc Alban 56 | 57 | 0.1.1 (2015-11-17) 58 | ------------------ 59 | 60 | 0.1.0 (2015-09-29) 61 | ------------------ 62 | 63 | 0.0.5 (2015-09-27) 64 | ------------------ 65 | 66 | 0.0.4 (2015-09-27) 67 | ------------------ 68 | 69 | 0.0.3 (2015-09-26) 70 | ------------------ 71 | * Clean up dependencies 72 | Remove unneeded ones, add required ones not specified 73 | * Contributors: Jerry Towler 74 | 75 | 0.0.2 (2015-09-25) 76 | ------------------ 77 | * Renames opencv_util package to swri_opencv_util. Refs `#231 `_ 78 | * Contributors: Edward Venator 79 | 80 | 0.0.1 (2015-09-25) 81 | ------------------ 82 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/include/swri_math_util/trig_util.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef MATH_UTIL_TRIG_UTIL_H_ 31 | #define MATH_UTIL_TRIG_UTIL_H_ 32 | 33 | namespace swri_math_util 34 | { 35 | /** 36 | * Normalize an angle to be within a 2pi range centered at a given value. 37 | * 38 | * @param[in]: angle The input angle in radians. 39 | * @param[in]: center The center of the range in radians. 40 | * 41 | * @returns An equivalent angle in the desired range. 42 | */ 43 | double WrapRadians(double angle, double center); 44 | 45 | /** 46 | * Convert radians to degrees. 47 | * 48 | * @param[in] degrees Input angle. 49 | 50 | * 51 | * @return The angle in radians. 52 | */ 53 | double ToRadians(double degrees); 54 | 55 | /** 56 | * Convert degrees to radians. 57 | * 58 | * @param[in] radians Input angle. 59 | * 60 | * @return The angle in degrees. 61 | */ 62 | double ToDegrees(double radians); 63 | } 64 | 65 | #endif // MATH_UTIL_TRIG_UTIL_H_ 66 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/include/swri_transform_util/frames.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef TRANSFORM_UTIL_FRAMES_H_ 31 | #define TRANSFORM_UTIL_FRAMES_H_ 32 | 33 | #include 34 | 35 | namespace swri_transform_util 36 | { 37 | /** 38 | * Special frame id for data defined in the WGS84 lat/lon coordinate system. 39 | */ 40 | static const std::string _wgs84_frame = "/wgs84"; 41 | 42 | /** 43 | * Special frame id for data defined in the UTM coordinate system. 44 | * 45 | * The zone is assumed to be the same as the LocalXY origin of the system. 46 | * Because of this zone transitions are not supported. 47 | */ 48 | static const std::string _utm_frame = "/utm"; 49 | 50 | /** 51 | * Special frame id for data defined a LocalXY coordinate system. 52 | * 53 | * Dependent on the LocalXY origin of the system. 54 | */ 55 | static const std::string _local_xy_frame = "/local_xy"; 56 | 57 | static const std::string _tf_frame = "/tf"; 58 | } 59 | 60 | #endif // TRANSFORM_UTIL_FRAMES_H_ 61 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/include/swri_nodelet/class_list_macros.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2016, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef SWRI_NODELET_CLASS_LIST_MACROS_H_ 31 | #define SWRI_NODELET_CLASS_LIST_MACROS_H_ 32 | 33 | #include 34 | 35 | /* 36 | Macro to define a nodelet with a factory function, so that it can be used in 37 | boilerplate wrapper nodes that do not rely on dynamic class loading. 38 | 39 | The macro calls PLUGINLIB_EXPORT_CLASS with plugin type nodelet::Nodelet and 40 | creates a factory function NS::createCLASS() that returns a 41 | boost::shared_ptr to NS::CLASS 42 | 43 | @param NS The namespace of the class to be used for the nodelet 44 | @param CLASS The classname of the class to be used for the nodelet 45 | */ 46 | #define SWRI_NODELET_EXPORT_CLASS(NS, CLASS) PLUGINLIB_EXPORT_CLASS(NS::CLASS, nodelet::Nodelet);\ 47 | namespace NS\ 48 | {\ 49 | boost::shared_ptr create ## CLASS()\ 50 | {\ 51 | return boost::make_shared();\ 52 | }\ 53 | } 54 | 55 | #endif // SWRI_NODELET_CLASS_LIST_MACROS_H_ 56 | 57 | -------------------------------------------------------------------------------- /src/marti_common/swri_console_util/include/swri_console_util/progress_bar.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef CONSOLE_UTIL_PROGRESS_BAR_H_ 31 | #define CONSOLE_UTIL_PROGRESS_BAR_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace swri_console_util 39 | { 40 | class ProgressBar 41 | { 42 | public: 43 | ProgressBar(); 44 | ~ProgressBar(); 45 | 46 | void SetStartTime(const ros::WallTime& start_time); 47 | void SetProgress(double percent_complete); 48 | void PrintTime(); 49 | void CheckForPause(); 50 | char ReadCharFromStdin(); 51 | 52 | static std::string GetTimeString(double seconds); 53 | static std::string IntToString(int64_t i, int width = 0); 54 | 55 | private: 56 | void SetupTerminal(); 57 | void RestoreTerminal(); 58 | 59 | bool paused_; 60 | 61 | double percent_complete_; 62 | 63 | ros::WallTime start_time_; 64 | ros::WallDuration paused_time_; 65 | 66 | termios orig_flags_; 67 | fd_set stdin_fdset_; 68 | int maxfd_; 69 | }; 70 | } 71 | 72 | #endif // CONSOLE_UTIL_PROGRESS_BAR_H_ 73 | -------------------------------------------------------------------------------- /src/marti_common/swri_geometry_util/include/swri_geometry_util/geometry_util.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef GEOMETRY_UTIL_GEOMETRY_UTIL_H_ 31 | #define GEOMETRY_UTIL_GEOMETRY_UTIL_H_ 32 | 33 | #include 34 | 35 | namespace swri_geometry_util 36 | { 37 | /** 38 | * Calculate the distance from a point to a plane. 39 | * 40 | * @param[in] plane_normal The normal vector of the plane. 41 | * @param[in] plane_point A point on the plane. 42 | * @param[in] point The point to measure the distance of. 43 | * 44 | * @returns The distance of the point from the plane. 45 | */ 46 | double DistanceFromPlane( 47 | const tf::Vector3& plane_normal, 48 | const tf::Vector3& plane_point, 49 | const tf::Vector3& point); 50 | 51 | double DistanceFromLineSegment( 52 | const tf::Vector3& line_start, 53 | const tf::Vector3& line_end, 54 | const tf::Vector3& point); 55 | 56 | tf::Vector3 ProjectToLineSegment( 57 | const tf::Vector3& line_start, 58 | const tf::Vector3& line_end, 59 | const tf::Vector3& point); 60 | } 61 | 62 | #endif // GEOMETRY_UTIL_GEOMETRY_UTIL_H_ 63 | -------------------------------------------------------------------------------- /src/marti_common/swri_geometry_util/src/geometry_util.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | namespace swri_geometry_util 33 | { 34 | double DistanceFromPlane( 35 | const tf::Vector3& plane_normal, 36 | const tf::Vector3& plane_point, 37 | const tf::Vector3& point) 38 | { 39 | return plane_normal.normalized().dot(point - plane_point); 40 | } 41 | 42 | double DistanceFromLineSegment( 43 | const tf::Vector3& line_start, 44 | const tf::Vector3& line_end, 45 | const tf::Vector3& point) 46 | { 47 | return point.distance(ProjectToLineSegment(line_start, line_end, point)); 48 | } 49 | 50 | tf::Vector3 ProjectToLineSegment( 51 | const tf::Vector3& line_start, 52 | const tf::Vector3& line_end, 53 | const tf::Vector3& point) 54 | { 55 | tf::Vector3 v = line_end - line_start; 56 | tf::Vector3 r = point - line_start; 57 | 58 | double t = r.dot(v); 59 | if (t <= 0) 60 | { 61 | return line_start; 62 | } 63 | 64 | double b = v.dot(v); 65 | if (t >= b) 66 | { 67 | return line_end; 68 | } 69 | 70 | return line_start + (t / b) * v; 71 | } 72 | } 73 | -------------------------------------------------------------------------------- /src/marti_common/swri_nodelet/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 49 | 50 | 51 | 52 | 53 | 54 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/include/swri_image_util/geometry_util.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (C) 2012 All Right Reserved, Southwest Research Institute® (SwRI®) 4 | // 5 | // Contract No. 10-R8248 6 | // Contractor Southwest Research Institute® (SwRI®) 7 | // Address 6220 Culebra Road, San Antonio, Texas 78228-0510 8 | // Contact Kris Kozak (210) 522-3854 9 | // 10 | // This code was developed as part of an internal research project fully funded 11 | // by Southwest Research Institute®. 12 | // 13 | // THIS CODE AND INFORMATION ARE PROVIDED "AS IS" WITHOUT WARRANTY OF ANY 14 | // KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE 15 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A 16 | // PARTICULAR PURPOSE. 17 | // 18 | // ***************************************************************************** 19 | 20 | #ifndef IMAGE_UTIL_GEOMETRY_UTIL_H_ 21 | #define IMAGE_UTIL_GEOMETRY_UTIL_H_ 22 | 23 | #include 24 | 25 | #include 26 | 27 | #include 28 | 29 | namespace swri_image_util 30 | { 31 | typedef cv::Rect_ BoundingBox; 32 | 33 | /** 34 | * Calculate the overlapping area of a rectangle an a rigidly transformed 35 | * version of itself. 36 | * 37 | * @param[in] rect The rectangle. 38 | * @param[in] rigid_transform The rigid transform. 39 | * 40 | * @returns The area of intersection of the two rectangles. 41 | */ 42 | double GetOverlappingArea( 43 | const cv::Rect& rect, 44 | const cv::Mat& rigid_transform); 45 | 46 | /** 47 | * Determine if two aligned rectangles intersect one another. 48 | * 49 | * @param[in] box1 The first rectangle. 50 | * @param[in] box2 The second rectangle. 51 | * 52 | * @returns True if box1 intersects with box2. False otherwise. 53 | */ 54 | bool Intersects(const BoundingBox& box1, const BoundingBox& box2); 55 | 56 | /** 57 | * Projects a 3D ellipsoid to an ellipse on the XY-plane. 58 | * 59 | * @param[in] ellipsoid The ellipsoid represented as a 3x3 float matrix. 60 | * 61 | * @returns The ellipse as a 2x2 float matrix if successful. An empty matrix 62 | * otherwise. 63 | */ 64 | cv::Mat ProjectEllipsoid(const cv::Mat& ellipsiod); 65 | 66 | /** 67 | * Gets a list of points on the perimeter of an ellipse. 68 | * 69 | * @param[in] ellipse The ellipse represented as a 2x2 float matrix. 70 | * @param[in] center The center of the ellipse. 71 | * @param[in] scale A scale factor. 72 | * @param[in] num_points The number of points to use. 73 | * 74 | * @returns A list of points on the perimeter of the ellipse if successful. 75 | * An empty list otherwise. 76 | */ 77 | std::vector GetEllipsePoints( 78 | const cv::Mat& ellipse, 79 | const tf::Vector3& center, 80 | double scale, 81 | int32_t num_points); 82 | } 83 | 84 | #endif // IMAGE_UTIL_GEOMETRY_UTIL_H_ 85 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/test/test_random.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | #ifdef BOOST_1_46 37 | namespace boost_random = boost; 38 | #else 39 | namespace boost_random = boost::random; 40 | #endif 41 | 42 | TEST(RandomTests, GetUniformRandomSample) 43 | { 44 | boost_random::mt19937 gen; 45 | 46 | std::vector sample; 47 | swri_math_util::GetUniformRandomSample(gen, 0, 100, 10, sample); 48 | 49 | EXPECT_EQ(10, sample.size()); 50 | 51 | swri_math_util::GetUniformRandomSample(gen, 0, 100, 90, sample); 52 | EXPECT_EQ(90, sample.size()); 53 | } 54 | 55 | TEST(RandomTests, RandomGenerator) 56 | { 57 | swri_math_util::RandomGenerator gen; 58 | 59 | std::vector sample; 60 | gen.GetUniformRandomSample(0, 100, 10, sample); 61 | 62 | EXPECT_EQ(10, sample.size()); 63 | 64 | gen.GetUniformRandomSample(0, 100, 90, sample); 65 | EXPECT_EQ(90, sample.size()); 66 | } 67 | 68 | // Run all the tests that were declared with TEST() 69 | int main(int argc, char **argv) 70 | { 71 | testing::InitGoogleTest(&argc, argv); 72 | 73 | return RUN_ALL_TESTS(); 74 | } 75 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/include/swri_transform_util/earth_constants.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef TRANSFORM_UTIL_EARTH_CONSTANTS_H_ 31 | #define TRANSFORM_UTIL_EARTH_CONSTANTS_H_ 32 | 33 | namespace swri_transform_util 34 | { 35 | /** 36 | * Earth equatorial radius in meters according to WGS84. 37 | */ 38 | static const double _earth_equator_radius = 6378137.0; 39 | 40 | static const double _earth_mean_radius = 6371009.0; 41 | 42 | /** 43 | * Earth equatorial circumference in meters according to WGS84. 44 | */ 45 | static const double _earth_equator_circumference = 40075016.69; 46 | 47 | /** 48 | * Earth 'first' eccentricity according to WGS84. 49 | */ 50 | static const double _earth_eccentricity = 0.08181919084261; 51 | 52 | /** 53 | * Earth flattening according to WGS84. 54 | * 55 | * Flattening is a measure of the compression of a sphere along a diameter to 56 | * form an ellipsoid of revolution. 57 | * 58 | * See: http://en.wikipedia.org/wiki/Flattening 59 | */ 60 | static const double _earth_flattening = 3.35281066475e-3; 61 | 62 | /** 63 | * Earth rotation rate in radians per second. 64 | */ 65 | static const double _earth_rotation_rate = 7.292115e-5; 66 | } 67 | 68 | #endif // TRANSFORM_UTIL_EARTH_CONSTANTS_H_ 69 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/src/math_util.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | #include 33 | 34 | namespace swri_math_util 35 | { 36 | double Round(double value) 37 | { 38 | return (value > 0.0) ? std::floor(value + 0.5) : std::ceil(value - 0.5); 39 | } 40 | 41 | double ToNearest(double value, double multiple) 42 | { 43 | if (multiple == 0) 44 | { 45 | return 0; 46 | } 47 | 48 | return Round(value / multiple) * multiple; 49 | } 50 | 51 | double UpToNearest(double value, double multiple) 52 | { 53 | if (multiple == 0) 54 | { 55 | return 0; 56 | } 57 | 58 | return std::ceil(value / multiple) * multiple; 59 | } 60 | 61 | bool IsNear(double v1, double v2, double epsilon) 62 | { 63 | return std::fabs(v1 - v2) <= epsilon; 64 | } 65 | 66 | double unWrapAngle(double static_angle, 67 | double variable_angle, 68 | double threshold) 69 | { 70 | if (std::abs(static_angle - variable_angle) > threshold) 71 | { 72 | if (variable_angle < static_angle) 73 | { 74 | variable_angle += swri_math_util::_2pi; 75 | } 76 | else 77 | { 78 | variable_angle -= swri_math_util::_2pi; 79 | } 80 | } 81 | return variable_angle; 82 | } 83 | } 84 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/include/swri_transform_util/transformer.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef TRANSFORM_UTIL_TRANSFORMER_H_ 31 | #define TRANSFORM_UTIL_TRANSFORMER_H_ 32 | 33 | #include 34 | #include 35 | #include 36 | 37 | #include 38 | 39 | #include 40 | #include 41 | 42 | #include 43 | 44 | namespace swri_transform_util 45 | { 46 | class Transformer 47 | { 48 | public: 49 | Transformer(); 50 | virtual ~Transformer(); 51 | 52 | void Initialize(const boost::shared_ptr tf); 53 | 54 | virtual std::map > Supports() const = 0; 55 | 56 | virtual bool GetTransform( 57 | const std::string& target_frame, 58 | const std::string& source_frame, 59 | const ros::Time& time, 60 | Transform& transform) = 0; 61 | 62 | protected: 63 | bool initialized_; 64 | boost::shared_ptr tf_listener_; 65 | 66 | virtual bool Initialize(); 67 | 68 | virtual bool GetTransform( 69 | const std::string& target_frame, 70 | const std::string& source_frame, 71 | const ros::Time& time, 72 | tf::StampedTransform& transform) const; 73 | }; 74 | } 75 | 76 | #endif // TRANSFORM_UTIL_TRANSFORMER_H_ 77 | -------------------------------------------------------------------------------- /src/marti_common/swri_geometry_util/include/swri_geometry_util/polygon.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef POLYGON_H_ 31 | #define POLYGON_H_ 32 | 33 | #include 34 | 35 | #ifndef DEG_TO_RAD 36 | #define DEG_TO_RAD 0.0174532925 37 | #endif 38 | 39 | namespace swri_geometry_util 40 | { 41 | //structure for defining the vertices of a polygon 42 | typedef struct 43 | { 44 | //vertices 45 | double *x; 46 | double *y; 47 | }PolygonD; 48 | 49 | typedef struct 50 | { 51 | //vertex 52 | double x; 53 | double y; 54 | }Vertex; 55 | 56 | class Polygon{ 57 | public: 58 | 59 | Polygon(); 60 | Polygon(const Polygon & other); 61 | Polygon & operator= (const Polygon & other); 62 | 63 | Polygon(double Xs[], double Ys[], int numVertx); 64 | 65 | bool VertexInPolygon(Vertex vertex); 66 | 67 | double* GetXVerticies(); 68 | 69 | double* GetYVerticies(); 70 | 71 | double GetXVerticie(int num); 72 | 73 | double GetYVerticie(int num); 74 | 75 | int GetNumVerticies(); 76 | 77 | bool LineOverlapsPolygon(Vertex start, Vertex end); 78 | 79 | ~Polygon(); 80 | 81 | private: 82 | 83 | Vertex FindLineIntersectLine(Vertex start1, Vertex end1, Vertex start2, 84 | Vertex end2); 85 | 86 | PolygonD _shape; //list of polygon vertices 87 | int _nvert; //number of vertices in this polygon 88 | }; 89 | } // end namespace swri_geometry_util 90 | #endif /* POLYGON_H_ */ 91 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/src/blend_images_util.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2017, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | #include 32 | 33 | namespace swri_image_util 34 | { 35 | void blendImages( 36 | const cv::Mat& base_image, 37 | const cv::Mat& top_image, 38 | const double alpha, 39 | cv::Mat& dest_image) 40 | { 41 | // All images must have the same shape. Return without modifying anything 42 | // if this is not the case 43 | if ((base_image.rows != top_image.rows) 44 | || (base_image.cols != top_image.cols) 45 | || (base_image.rows != dest_image.rows) 46 | || (base_image.cols != dest_image.cols)) 47 | { 48 | ROS_ERROR("Images to blend had incorrect shapes"); 49 | return; 50 | } 51 | 52 | // Make sure all the image have the same type before modifying anything 53 | if ((base_image.type() != top_image.type()) 54 | || (base_image.type() != dest_image.type())) 55 | { 56 | ROS_ERROR("Images to blend must have the same type"); 57 | return; 58 | } 59 | 60 | // Sanity check the alpha value to make sure it is reasonable. Do not 61 | // modify anything if it is outside the expected range 62 | if ((alpha < 0.0) || (alpha > 1.0)) 63 | { 64 | ROS_ERROR("Alpha value must be in the range [0, 1]"); 65 | return; 66 | } 67 | 68 | cv::addWeighted(base_image, 1.0 - alpha, top_image, alpha, 0, dest_image); 69 | } 70 | } 71 | -------------------------------------------------------------------------------- /src/marti_common/swri_roscpp/src/nodes/timer_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include 6 | 7 | namespace du = diagnostic_updater; 8 | 9 | // Alias type for easier access to DiagnosticStatus enumerations. 10 | typedef diagnostic_msgs::DiagnosticStatus DS; 11 | 12 | class TimerTest 13 | { 14 | ros::NodeHandle nh_; 15 | ros::WallTimer init_timer_; 16 | swri::Timer update_timer_; 17 | swri::Timer diag_timer_; 18 | 19 | du::Updater diagnostic_updater_; 20 | 21 | int fibonacci_index_; 22 | 23 | public: 24 | TimerTest() 25 | { 26 | // Setup a one-shot timer to initialize the node after a brief 27 | // delay so that /rosout is always fully initialized. 28 | ROS_INFO("Starting initialization timer..."); 29 | init_timer_ = nh_.createWallTimer(ros::WallDuration(1.0), 30 | &TimerTest::initialize, 31 | this, 32 | true); 33 | } 34 | 35 | void initialize(const ros::WallTimerEvent &ignored) 36 | { 37 | update_timer_ = swri::Timer(nh_, ros::Duration(1.0/50.0), 38 | &TimerTest::handleUpdateTimer, 39 | this); 40 | 41 | ros::NodeHandle pnh("~"); 42 | swri::param(pnh, "fibonacci_index", fibonacci_index_, 30); 43 | 44 | diagnostic_updater_.setHardwareID("none"); 45 | diagnostic_updater_.add( 46 | "swri::Timer test", this, 47 | &TimerTest::timerDiagnostics); 48 | 49 | diag_timer_ = swri::Timer(nh_, ros::Duration(1.0), 50 | &TimerTest::handleDiagnosticsTimer, 51 | this); 52 | } 53 | 54 | void handleUpdateTimer(const ros::TimerEvent &ignored) 55 | { 56 | // Do some work to give us a measurable time. 57 | size_t number = super_slow_fibonacci(fibonacci_index_); 58 | ROS_INFO("The %d-th number of the fibonacci sequence is %lu", 59 | fibonacci_index_, number); 60 | } 61 | 62 | int super_slow_fibonacci(int x) 63 | { 64 | if (x <= 0) { 65 | return 0; 66 | } else if (x == 1) { 67 | return 1; 68 | } else { 69 | return super_slow_fibonacci(x-1) + super_slow_fibonacci(x-2); 70 | } 71 | } 72 | 73 | void handleDiagnosticsTimer(const ros::TimerEvent &ignored) 74 | { 75 | diagnostic_updater_.update(); 76 | } 77 | 78 | void timerDiagnostics(du::DiagnosticStatusWrapper& status) // NOLINT 79 | { 80 | status.summary(DS::OK, "No errors reported."); 81 | 82 | // This example uses swri::Timer's appendDiagnostics method to 83 | // include diagnostics using a common format and common summary 84 | // semantics. If we didn't care about specific bits of 85 | // information, we use more specific flags. 86 | update_timer_.appendDiagnostics(status, "Update", 87 | swri::Timer::DIAG_ALL); 88 | } 89 | 90 | }; // class TimerTest 91 | 92 | int main(int argc, char **argv) 93 | { 94 | ros::init(argc, argv, "timer_test"); 95 | 96 | TimerTest node; 97 | ros::spin(); 98 | 99 | return 0; 100 | } 101 | -------------------------------------------------------------------------------- /src/marti_common/swri_system_util/test/test_file_util.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | TEST(FileUtilTests, Uncomplete) 39 | { 40 | std::string path1; 41 | ASSERT_TRUE(ros::param::get("path1", path1)); 42 | 43 | std::string path2; 44 | ASSERT_TRUE(ros::param::get("path2", path2)); 45 | 46 | std::string path3; 47 | ASSERT_TRUE(ros::param::get("path3", path3)); 48 | 49 | EXPECT_EQ(boost::filesystem::path("./"), swri_system_util::NaiveUncomplete(path1, path1)); 50 | EXPECT_EQ(boost::filesystem::path("src"), swri_system_util::NaiveUncomplete(path2, path1)); 51 | EXPECT_EQ(boost::filesystem::path("include/swri_system_util"), swri_system_util::NaiveUncomplete(path3, path1)); 52 | EXPECT_EQ(boost::filesystem::path("../"), swri_system_util::NaiveUncomplete(path1, path2)); 53 | EXPECT_EQ(boost::filesystem::path("../../"), swri_system_util::NaiveUncomplete(path1, path3)); 54 | EXPECT_EQ(boost::filesystem::path(""), swri_system_util::NaiveUncomplete(boost::filesystem::path(""), path1)); 55 | EXPECT_EQ(boost::filesystem::path(""), swri_system_util::NaiveUncomplete(path1, boost::filesystem::path(""))); 56 | } 57 | 58 | // Run all the tests that were declared with TEST() 59 | int main(int argc, char **argv) 60 | { 61 | testing::InitGoogleTest(&argc, argv); 62 | 63 | ros::init(argc, argv, "test_file_util"); 64 | ros::NodeHandle nh; 65 | 66 | return RUN_ALL_TESTS(); 67 | } 68 | -------------------------------------------------------------------------------- /src/marti_common/swri_yaml_util/include/swri_yaml_util/yaml_util.h: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #ifndef YAML_UTIL_YAML_UTIL_H_ 31 | #define YAML_UTIL_YAML_UTIL_H_ 32 | 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | #include 39 | 40 | #ifndef YAMLCPP_OLD_API 41 | namespace YAML 42 | { 43 | void operator >> (const YAML::Node& node, float& value); 44 | void operator >> (const YAML::Node& node, double& value); 45 | void operator >> (const YAML::Node& node, bool& value); 46 | void operator >> (const YAML::Node& node, int16_t& value); 47 | void operator >> (const YAML::Node& node, uint16_t& value); 48 | void operator >> (const YAML::Node& node, int32_t& value); 49 | void operator >> (const YAML::Node& node, uint32_t& value); 50 | void operator >> (const YAML::Node& node, int64_t& value); 51 | void operator >> (const YAML::Node& node, uint64_t& value); 52 | void operator >> (const YAML::Node& node, std::string& value); 53 | } 54 | #endif // YAMLCPP_OLD_API 55 | 56 | namespace swri_yaml_util 57 | { 58 | bool LoadFile(const std::string& path, YAML::Node& yaml); 59 | bool LoadString(const std::string& input, YAML::Node& yaml); 60 | bool LoadMap(const std::map& dict, YAML::Node& yaml); 61 | bool FindValue(const YAML::Node& node, const std::string& name); 62 | 63 | std::auto_ptr Clone(const YAML::Node& node); 64 | 65 | std::string ToString(double value, int32_t precision); 66 | } 67 | 68 | #endif // YAML_UTIL_YAML_UTIL_H_ 69 | -------------------------------------------------------------------------------- /src/marti_common/swri_image_util/src/nodes/dummy_image_publisher.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | // ROS Libraries 33 | #include 34 | #include 35 | 36 | class DummyImagePublisherNode 37 | { 38 | public: 39 | DummyImagePublisherNode() 40 | { 41 | ros::NodeHandle node; 42 | ros::NodeHandle priv("~"); 43 | image_pub_ = node.advertise("image", 100); 44 | 45 | priv.param("rate", rate_, 10.0); 46 | priv.param("width", width_, 640); 47 | priv.param("height", height_, 480); 48 | priv.param("encoding", encoding_, std::string("mono8")); 49 | } 50 | 51 | void Spin() 52 | { 53 | sensor_msgs::Image image; 54 | image.encoding = encoding_; 55 | image.width = width_; 56 | image.height = height_; 57 | image.step = width_; 58 | image.data.resize(height_ * width_); 59 | 60 | ros::Rate rate(rate_); 61 | while (ros::ok()) 62 | { 63 | image.header.stamp = ros::Time::now(); 64 | image_pub_.publish(image); 65 | 66 | ros::spinOnce(); 67 | rate.sleep(); 68 | } 69 | } 70 | 71 | private: 72 | double rate_; 73 | int32_t width_; 74 | int32_t height_; 75 | std::string encoding_; 76 | 77 | ros::Publisher image_pub_; 78 | }; 79 | 80 | int main(int argc, char **argv) 81 | { 82 | ros::init(argc, argv, "dummy_image_publisher", ros::init_options::AnonymousName); 83 | 84 | DummyImagePublisherNode node; 85 | node.Spin(); 86 | } 87 | -------------------------------------------------------------------------------- /src/marti_common/swri_transform_util/test/test_georeference.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | #include 33 | 34 | #include 35 | 36 | TEST(GeoreferenceTests, Load) 37 | { 38 | std::string filename; 39 | ASSERT_TRUE(ros::param::get("geo_file", filename)); 40 | 41 | swri_transform_util::GeoReference georeference(filename); 42 | ASSERT_TRUE(georeference.Load()); 43 | 44 | EXPECT_EQ(std::string("wgs84"), georeference.Datum()); 45 | EXPECT_EQ(std::string("utm"), georeference.Projection()); 46 | EXPECT_EQ(29184, georeference.Width()); 47 | EXPECT_EQ(15872, georeference.Height()); 48 | EXPECT_EQ(512, georeference.TileSize()); 49 | EXPECT_EQ("jpg", georeference.Extension()); 50 | } 51 | 52 | TEST(GeoreferenceTests, LoadExtension) 53 | { 54 | std::string filename; 55 | ASSERT_TRUE(ros::param::get("geo_file_extension", filename)); 56 | 57 | swri_transform_util::GeoReference georeference(filename); 58 | ASSERT_TRUE(georeference.Load()); 59 | 60 | EXPECT_EQ(std::string("wgs84"), georeference.Datum()); 61 | EXPECT_EQ(std::string("utm"), georeference.Projection()); 62 | EXPECT_EQ(29184, georeference.Width()); 63 | EXPECT_EQ(15872, georeference.Height()); 64 | EXPECT_EQ(512, georeference.TileSize()); 65 | EXPECT_EQ("png", georeference.Extension()); 66 | } 67 | 68 | // TODO(malban): Test coordinate/pixel transforms. 69 | 70 | // Run all the tests that were declared with TEST() 71 | int main(int argc, char **argv) 72 | { 73 | testing::InitGoogleTest(&argc, argv); 74 | 75 | // Initialize the ROS core parameters can be loaded from the launch file 76 | ros::init(argc, argv, "test_georeference"); 77 | 78 | return RUN_ALL_TESTS(); 79 | } 80 | -------------------------------------------------------------------------------- /src/tractobots_robot_localization/launch/navsat_transform_node.launch: -------------------------------------------------------------------------------- 1 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | -------------------------------------------------------------------------------- /src/marti_common/swri_math_util/test/test_trig_util.cpp: -------------------------------------------------------------------------------- 1 | // ***************************************************************************** 2 | // 3 | // Copyright (c) 2014, Southwest Research Institute® (SwRI®) 4 | // All rights reserved. 5 | // 6 | // Redistribution and use in source and binary forms, with or without 7 | // modification, are permitted provided that the following conditions are met: 8 | // * Redistributions of source code must retain the above copyright 9 | // notice, this list of conditions and the following disclaimer. 10 | // * Redistributions in binary form must reproduce the above copyright 11 | // notice, this list of conditions and the following disclaimer in the 12 | // documentation and/or other materials provided with the distribution. 13 | // * Neither the name of Southwest Research Institute® (SwRI®) nor the 14 | // names of its contributors may be used to endorse or promote products 15 | // derived from this software without specific prior written permission. 16 | // 17 | // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | // ARE DISCLAIMED. IN NO EVENT SHALL BE LIABLE FOR ANY 21 | // DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES 22 | // (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 23 | // LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND 24 | // ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT 25 | // (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 26 | // SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 27 | // 28 | // ***************************************************************************** 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | 35 | TEST(TrigUtilTests, WrapRadians) 36 | { 37 | // Test values wrapped between [-pi, pi] 38 | EXPECT_FLOAT_EQ(0, swri_math_util::WrapRadians(0, 0)); 39 | EXPECT_FLOAT_EQ(swri_math_util::_pi, swri_math_util::WrapRadians(swri_math_util::_pi, 0)); 40 | EXPECT_FLOAT_EQ(swri_math_util::_pi, swri_math_util::WrapRadians(swri_math_util::_pi * 3.0, 0)); 41 | EXPECT_FLOAT_EQ(swri_math_util::_pi * -0.5, swri_math_util::WrapRadians(swri_math_util::_pi * 1.5, 0)); 42 | EXPECT_FLOAT_EQ(swri_math_util::_pi * 0.5, swri_math_util::WrapRadians(swri_math_util::_pi * -1.5, 0)); 43 | 44 | // Test values wrapped between [0, 2pi] 45 | EXPECT_FLOAT_EQ(0, swri_math_util::WrapRadians(0, swri_math_util::_pi)); 46 | EXPECT_FLOAT_EQ(swri_math_util::_pi, swri_math_util::WrapRadians(swri_math_util::_pi, swri_math_util::_pi)); 47 | EXPECT_FLOAT_EQ(swri_math_util::_pi, swri_math_util::WrapRadians(swri_math_util::_pi * 3.0, swri_math_util::_pi)); 48 | EXPECT_FLOAT_EQ(swri_math_util::_pi, swri_math_util::WrapRadians(-swri_math_util::_pi, swri_math_util::_pi)); 49 | EXPECT_FLOAT_EQ(swri_math_util::_pi, swri_math_util::WrapRadians(swri_math_util::_pi * -3.0, swri_math_util::_pi)); 50 | EXPECT_FLOAT_EQ(swri_math_util::_pi * 0.5, swri_math_util::WrapRadians(swri_math_util::_pi * 2.5, swri_math_util::_pi)); 51 | EXPECT_FLOAT_EQ(swri_math_util::_pi * 1.5, swri_math_util::WrapRadians(swri_math_util::_pi * 3.5, swri_math_util::_pi)); 52 | } 53 | 54 | // Run all the tests that were declared with TEST() 55 | int main(int argc, char **argv) 56 | { 57 | testing::InitGoogleTest(&argc, argv); 58 | 59 | return RUN_ALL_TESTS(); 60 | } 61 | --------------------------------------------------------------------------------