├── .gitignore ├── README.md ├── habitat_spec_file.txt ├── img ├── map_cropping.jpg ├── map_loading.jpg ├── ps3_controller.jpg ├── rviz_control_panel.jpg ├── rviz_full.jpg ├── start_point.jpg ├── trav_area_saved.jpg └── trav_area_selection.jpg ├── src ├── CMakeLists.txt ├── joystick_drivers │ ├── README.md │ ├── joy │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── config │ │ │ └── ps4joy.yaml │ │ ├── launch │ │ │ └── ps4joy.launch │ │ ├── mainpage.dox │ │ ├── migration_rules │ │ │ └── Joy.bmr │ │ ├── package.xml │ │ ├── scripts │ │ │ └── joy_remap.py │ │ ├── src │ │ │ └── joy_node.cpp │ │ └── test │ │ │ ├── saved │ │ │ └── Joy.saved │ │ │ └── test_joy_msg_migration.py │ ├── joystick_drivers │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── package.xml │ └── ps3joy │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── diagnostics.yaml │ │ ├── doc │ │ ├── bluetooth_devices.md │ │ ├── testing.md │ │ └── troubleshooting.md │ │ ├── launch │ │ └── ps3.launch │ │ ├── package.xml │ │ ├── scripts │ │ ├── ps3joy.py │ │ ├── ps3joy_node.py │ │ └── ps3joysim.py │ │ └── src │ │ └── sixpair.c ├── local_planner │ ├── CMakeLists.txt │ ├── launch │ │ └── local_planner.launch │ ├── package.xml │ ├── paths │ │ ├── correspondences.txt │ │ ├── pathList.ply │ │ ├── path_generator.m │ │ ├── paths.ply │ │ └── startPaths.ply │ └── src │ │ ├── localPlanner.cpp │ │ └── pathFollower.cpp ├── segmentation_proc │ ├── CMakeLists.txt │ ├── LICENSE │ ├── launch │ │ └── segmentation_proc.launch │ ├── package.xml │ ├── scripts │ │ ├── camera_models.py │ │ ├── compatible_np.py │ │ ├── habitat_online_360_v0.2.1.py │ │ ├── planar_as_base.py │ │ ├── radian.py │ │ ├── register.py │ │ ├── shape_struct.py │ │ ├── six_images_common.py │ │ ├── six_images_numba.py │ │ └── six_images_py_numba.py │ └── src │ │ └── segmentationProc.cpp ├── semantic_scan_generation │ ├── CMakeLists.txt │ ├── launch │ │ └── semantic_scan_generation.launch │ ├── package.xml │ └── src │ │ └── semanticScanGeneration.cpp ├── sensor_scan_generation │ ├── CMakeLists.txt │ ├── launch │ │ └── sensor_scan_generation.launch │ ├── package.xml │ └── src │ │ └── sensorScanGeneration.cpp ├── teleop_rviz_plugin │ ├── CMakeLists.txt │ ├── package.xml │ ├── plugin_description.xml │ └── src │ │ ├── drive_widget.cpp │ │ ├── drive_widget.h │ │ ├── teleop_panel.cpp │ │ └── teleop_panel.h ├── terrain_analysis │ ├── CMakeLists.txt │ ├── launch │ │ └── terrain_analysis.launch │ ├── package.xml │ └── src │ │ └── terrainAnalysis.cpp ├── terrain_analysis_ext │ ├── CMakeLists.txt │ ├── launch │ │ └── terrain_analysis_ext.launch │ ├── package.xml │ └── src │ │ └── terrainAnalysisExt.cpp ├── vehicle_simulator │ ├── CMakeLists.txt │ ├── launch │ │ ├── system_matterport.launch │ │ └── vehicle_simulator.launch │ ├── log │ │ └── readme.txt │ ├── mesh │ │ └── matterport │ │ │ ├── meshes │ │ │ └── readme.txt │ │ │ ├── model.config │ │ │ ├── model.sdf │ │ │ ├── pointclouds │ │ │ └── readme.txt │ │ │ └── segmentations │ │ │ └── readme.txt │ ├── package.xml │ ├── rviz │ │ └── vehicle_simulator.rviz │ ├── src │ │ └── vehicleSimulator.cpp │ ├── urdf │ │ ├── lidar.urdf.xacro │ │ └── robot.urdf.xacro │ └── world │ │ └── matterport.world ├── velodyne_simulator │ ├── LICENSE │ ├── README.md │ ├── velodyne_description │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── launch │ │ │ └── example.launch │ │ ├── meshes │ │ │ ├── HDL32E_base.dae │ │ │ ├── HDL32E_base.stl │ │ │ ├── HDL32E_scan.dae │ │ │ ├── HDL32E_scan.stl │ │ │ ├── VLP16_base_1.dae │ │ │ ├── VLP16_base_1.stl │ │ │ ├── VLP16_base_2.dae │ │ │ ├── VLP16_base_2.stl │ │ │ ├── VLP16_scan.dae │ │ │ └── VLP16_scan.stl │ │ ├── package.xml │ │ ├── rviz │ │ │ └── example.rviz │ │ ├── urdf │ │ │ ├── HDL-32E.urdf.xacro │ │ │ ├── VLP-16.urdf.xacro │ │ │ └── example.urdf.xacro │ │ └── world │ │ │ └── example.world │ ├── velodyne_gazebo_plugins │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── include │ │ │ └── velodyne_gazebo_plugins │ │ │ │ └── GazeboRosVelodyneLaser.h │ │ ├── package.xml │ │ └── src │ │ │ └── GazeboRosVelodyneLaser.cpp │ └── velodyne_simulator │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── package.xml ├── visualization_tools │ ├── CMakeLists.txt │ ├── launch │ │ └── visualization_tools.launch │ ├── package.xml │ └── src │ │ └── visualizationTools.cpp ├── waypoint_converter │ ├── CMakeLists.txt │ ├── launch │ │ └── waypoint_converter.launch │ ├── package.xml │ └── src │ │ └── waypointConverter.cpp └── waypoint_rviz_plugin │ ├── CMakeLists.txt │ ├── include │ └── waypoint_tool.h │ ├── package.xml │ ├── plugin_description.xml │ └── src │ └── waypoint_tool.cpp └── system_bring_up.sh /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | 3 | devel 4 | 5 | install 6 | 7 | .catkin_workspace 8 | 9 | __pycache__ 10 | 11 | *~ 12 | 13 | .DS_Store 14 | 15 | src/vehicle_simulator/mesh/* 16 | 17 | !src/vehicle_simulator/mesh/matterport 18 | 19 | src/vehicle_simulator/mesh/matterport/meshes/* 20 | 21 | src/vehicle_simulator/mesh/matterport/segmentations/* 22 | 23 | src/vehicle_simulator/mesh/matterport/pointclouds/* 24 | 25 | src/vehicle_simulator/log/* 26 | 27 | !readme.txt 28 | -------------------------------------------------------------------------------- /img/map_cropping.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/img/map_cropping.jpg -------------------------------------------------------------------------------- /img/map_loading.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/img/map_loading.jpg -------------------------------------------------------------------------------- /img/ps3_controller.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/img/ps3_controller.jpg -------------------------------------------------------------------------------- /img/rviz_control_panel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/img/rviz_control_panel.jpg -------------------------------------------------------------------------------- /img/rviz_full.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/img/rviz_full.jpg -------------------------------------------------------------------------------- /img/start_point.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/img/start_point.jpg -------------------------------------------------------------------------------- /img/trav_area_saved.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/img/trav_area_saved.jpg -------------------------------------------------------------------------------- /img/trav_area_selection.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/img/trav_area_selection.jpg -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/joystick_drivers/README.md: -------------------------------------------------------------------------------- 1 | # ROS Joystick Drivers Stack # 2 | 3 | [![](https://github.com/ros-drivers/joystick_drivers/workflows/Basic%20Build%20Workflow/badge.svg?branch=master)](https://github.com/ros-drivers/joystick_drivers/actions) 4 | 5 | A simple set of nodes for supporting various types of joystick inputs and producing ROS messages from the underlying OS messages. 6 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package joy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.15.1 (2021-06-07) 6 | ------------------- 7 | 8 | 1.15.0 (2020-10-12) 9 | ------------------- 10 | * Added autodetection for force-feedback devices. (`#169 `_) 11 | * Added autodetection for force-feedback devices. 12 | * RAII for closedir 13 | * joy: Little fixes for force feedback. (`#167 `_) 14 | This commit increases the maximum magnitude of the FF effects to double the previous maximum. 15 | * Print out joystick name on initialization. (`#168 `_) 16 | This helps figuring out what string to give to the `dev_name` parameter. 17 | * Contributors: Martin Pecka 18 | 19 | 1.14.0 (2020-07-07) 20 | ------------------- 21 | * frame_id in the header of the joystick msg (`#166 `_) 22 | * roslint and Generic Clean-Up (`#161 `_) 23 | * Merge pull request `#158 `_ from clalancette/ros1-cleanups 24 | ROS1 joy cleanups 25 | * Greatly simplify the sticky_buttons support. 26 | * Small fixes to rumble support. 27 | * Use C++ style casts. 28 | * Use empty instead of length. 29 | * joy_def_ff -> joy_dev_ff 30 | * Cleanup header includes. 31 | * Use size_t appropriately. 32 | * NULL -> nullptr everywhere. 33 | * Style cleanup in joy_node.cpp. 34 | * Merge pull request `#154 `_ from zchen24/master 35 | Minor: moved default to right indent level 36 | * Contributors: Chris Lalancette, Joshua Whitley, Mamoun Gharbi, Zihan Chen 37 | 38 | 1.13.0 (2019-06-24) 39 | ------------------- 40 | * Merge pull request `#120 `_ from furushchev/remap 41 | add joy_remap and its sample 42 | * Merge pull request `#128 `_ from ros-drivers/fix/tab_errors 43 | Cleaning up Python indentation. 44 | * Merge pull request `#111 `_ from matt-attack/indigo-devel 45 | Add Basic Force Feedback Support 46 | * Merge pull request `#126 `_ from clalancette/minor-formatting 47 | * Put brackets around ROS\_* macros. 48 | In some circumstances they may be defined to empty, so we need 49 | to have brackets to ensure that they are syntactically valid. 50 | Signed-off-by: Chris Lalancette 51 | * Merge pull request `#122 `_ from lbucklandAS/fix-publish-timestamp 52 | Add timestamp to all joy messages 53 | * Change error messages and set ps3 as default controller 54 | * Better handle device loss 55 | Allow for loss and redetection of device with force feedback 56 | * Add basic force feedback over usb 57 | Addresses `#89 `_ 58 | * Contributors: Chris Lalancette, Furushchev, Joshua Whitley, Lucas Buckland, Matthew, Matthew Bries 59 | 60 | 1.12.0 (2018-06-11) 61 | ------------------- 62 | * Update timestamp when using autorepeat_rate 63 | * Added dev_name parameter to select joystick by name 64 | * Added Readme for joy package with description of new device name parameter 65 | * Fixed numerous outstanding PRs. 66 | * Added sticky buttons 67 | * Changed package xml to format 2 68 | * Fixed issue when the joystick data did not got send until changed. 69 | * Changed messaging to better reflect what the script is doing 70 | * Contributors: Dino Hüllmann, Jonathan Bohren, Joshua Whitley, Miklos Marton, Naoki Mizuno, jprod123, psimona 71 | 72 | 1.11.0 (2017-02-10) 73 | ------------------- 74 | * fixed joy/Cmakelists for osx 75 | * Update dependencies to remove warnings 76 | * Contributors: Marynel Vazquez, Mark D Horn 77 | 78 | 1.10.1 (2015-05-24) 79 | ------------------- 80 | * Remove stray architechture_independent flags 81 | * Contributors: Jonathan Bohren, Scott K Logan 82 | 83 | 1.10.0 (2014-06-26) 84 | ------------------- 85 | * First indigo release 86 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(joy) 3 | 4 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 5 | 6 | # Load catkin and all dependencies required for this package 7 | set(CATKIN_DEPS roscpp diagnostic_updater sensor_msgs roslint) 8 | find_package(catkin REQUIRED ${CATKIN_DEPS}) 9 | 10 | roslint_cpp() 11 | 12 | catkin_package(DEPENDS sensor_msgs) 13 | 14 | # Look for 15 | include(CheckIncludeFiles) 16 | check_include_files(linux/joystick.h HAVE_LINUX_JOYSTICK_H) 17 | 18 | if(CATKIN_ENABLE_TESTING) 19 | roslint_add_test() 20 | endif() 21 | 22 | if(HAVE_LINUX_JOYSTICK_H) 23 | include_directories(msg/cpp ${catkin_INCLUDE_DIRS}) 24 | add_executable(joy_node src/joy_node.cpp) 25 | target_link_libraries(joy_node ${catkin_LIBRARIES}) 26 | 27 | # Install targets 28 | install(TARGETS joy_node 29 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 30 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 32 | else() 33 | message("Warning: no ; won't build joy node") 34 | endif() 35 | 36 | install(DIRECTORY migration_rules scripts config launch 37 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 38 | USE_SOURCE_PERMISSIONS) 39 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/README.md: -------------------------------------------------------------------------------- 1 | # ROS Driver for Generic Linux Joysticks 2 | 3 | The joy package contains joy_node, a node that interfaces a generic Linux joystick to ROS. This node publishes a "Joy" message, which contains the current state of each one of the joystick's buttons and axes. 4 | 5 | ## Supported Hardware 6 | 7 | This node should work with any joystick that is supported by Linux. 8 | 9 | ## Published Topics 10 | 11 | * joy ([sensor_msgs/Joy](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html)): outputs the joystick state. 12 | 13 | ## Device Selection 14 | 15 | There are two parameters controlling which device should be used: 16 | 17 | * ~dev (string, default: "/dev/input/js0") 18 | * ~dev_name (string, default: "" (empty = disabled)) 19 | 20 | If ~dev_name is empty, ~dev defines the Linux joystick device from which to read joystick events. 21 | 22 | If ~dev_name is defined, the node enumerates all available joysticks, i.e. /dev/input/js*. The first joystick matching ~dev_name is opened. If no joystick matches the desired device name, the device specified by ~dev is used as a fallback. 23 | 24 | To get a list of the names of all connected joysticks, an invalid ~dev_name can be specified. For example: 25 | 26 | `rosrun joy joy_node _dev_name:="*"` 27 | 28 | The output might look like this: 29 | 30 | ``` 31 | [ INFO]: Found joystick: ST LIS3LV02DL Accelerometer (/dev/input/js1). 32 | [ INFO]: Found joystick: Microsoft X-Box 360 pad (/dev/input/js0). 33 | [ERROR]: Couldn't find a joystick with name *. Falling back to default device. 34 | ``` 35 | 36 | Then the node can be started with the device name given in the list. For example: 37 | 38 | `rosrun joy joy_node _dev_name:="Microsoft X-Box 360 pad"` 39 | 40 | ## Advanced Parameters 41 | 42 | * ~deadzone (double, default: 0.05) 43 | * Amount by which the joystick has to move before it is considered to be off-center. This parameter is specified relative to an axis normalized between -1 and 1. Thus, 0.1 means that the joystick has to move 10% of the way to the edge of an axis's range before that axis will output a non-zero value. Linux does its own deadzone processing, so in many cases this value can be set to zero. 44 | 45 | * ~autorepeat_rate (double, default: 0.0 (disabled)) 46 | * Rate in Hz at which a joystick that has a non-changing state will resend the previously sent message. 47 | 48 | * ~coalesce_interval (double, default: 0.001) 49 | * Axis events that are received within coalesce_interval (seconds) of each other are sent out in a single ROS message. Since the kernel sends each axis motion as a separate event, coalescing greatly reduces the rate at which messages are sent. This option can also be used to limit the rate of outgoing messages. Button events are always sent out immediately to avoid missing button presses. 50 | 51 | ## Further Information 52 | 53 | For further information have a look at the [Wiki page](http://wiki.ros.org/joy). -------------------------------------------------------------------------------- /src/joystick_drivers/joy/config/ps4joy.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | axes: 3 | - axes[0] 4 | - axes[1] 5 | - axes[2] 6 | - axes[5] 7 | - axes[6] * 2.0 8 | - axes[7] * 2.0 9 | - axes[8] * -2.0 10 | - 0.0 11 | - max(axes[10], 0.0) * -1.0 12 | - min(axes[9], 0.0) 13 | - min(axes[10], 0.0) 14 | - max(axes[9], 0.0) * -1.0 15 | - (axes[3] - 1.0) * 0.5 16 | - (axes[4] - 1.0) * 0.5 17 | - buttons[4] * -1.0 18 | - buttons[5] * -1.0 19 | - buttons[3] * -1.0 20 | - buttons[2] * -1.0 21 | - buttons[1] * -1.0 22 | - buttons[0] * -1.0 23 | - 0.0 24 | - 0.0 25 | - 0.0 26 | - 0.0 27 | - 0.0 28 | - 0.0 29 | - 0.0 30 | buttons: 31 | - buttons[8] 32 | - buttons[10] 33 | - buttons[11] 34 | - buttons[9] 35 | - max(axes[10], 0.0) 36 | - min(axes[9], 0.0) * -1.0 37 | - min(axes[10], 0.0) * -1.0 38 | - max(axes[9], 0.0) 39 | - buttons[6] 40 | - buttons[7] 41 | - buttons[4] 42 | - buttons[5] 43 | - buttons[3] 44 | - buttons[2] 45 | - buttons[1] 46 | - buttons[0] 47 | - buttons[12] 48 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/launch/ps4joy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \mainpage 4 | \htmlinclude manifest.html 5 | 6 | \b joy ROS joystick drivers for Linux. This package works with Logitech joysticks and PS3 SIXAXIS/DUALSHOCK devices. This package will only build is linux/joystick.h 7 | 8 | \section rosapi ROS API 9 | 10 | This package contains the message "Joy", which carries data from the joystick's axes and buttons. It also has the joy_node, which reads from a given joystick port and publishes "Joy" messages. 11 | 12 | List of nodes: 13 | - \b joy_node 14 | 15 | Deprecated nodes: 16 | - \b txjoy 17 | - \b joy 18 | The deprecated nodes are the same as the joy_node, but they will warn users when used. They are actually just bash scripts that call "joy_node" and print a warning. 19 | 20 |
21 | 22 | \subsection joy joy 23 | 24 | \b joy ROS joystick driver for all linux joysticks. The driver will poll a given port until it can read from it, the publish Joy messages of the joystick state. If the port closes, or it reads an error, it will reopen the port. All axes are in the range [-1, 1], and all buttons are 0 (off) or 1 (on). 25 | 26 | Since the driver will poll for the joystick port, and automatically reopen the port if it's closed, the joy_node should be "on" whenever possible. It is typically part of the robot's launch file. 27 | 28 | \subsubsection autorepeat Auto-Repeat/Signal Loss 29 | 30 | The joy_node takes an "~autorepeat_rate" parameter. If the linux kernal receives no events during the autorepeat interval, it will automatically repeat the last value of the joystick. This is an important safety feature, and allows users to recover from a joystick that has timed out. 31 | 32 | \subsubsection usage Usage 33 | \verbatim 34 | $ joy [standard ROS args] 35 | \endverbatim 36 | 37 | \subsubsection topic ROS topics 38 | 39 | Subscribes to (name / type): 40 | - None 41 | 42 | Publishes to (name / type): 43 | - \b "joy/Joy" : Joystick output. Axes are [-1, 1], buttons are 0 or 1 (depressed). 44 | 45 | \subsubsection parameters ROS parameters 46 | - \b "~dev" : Input device for joystick. Default: /dev/input/js0 47 | - \b "~deadzone" : Output is zero for axis in deadzone. Range: [-0.9, 0.9]. Default 0.05 48 | - \b "~autorepeat_rate" : If no events, repeats last known state at this rate. Defualt: 0 (disabled) 49 | - \b "~coalesce_interval" : Waits for this interval (seconds) after receiving an event. If multiple events happen in this interval, only one message will be sent. Reduces number of messages. Default: 0.001. 50 | 51 | 52 |
53 | 54 | 55 | 56 | */ 57 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/migration_rules/Joy.bmr: -------------------------------------------------------------------------------- 1 | class update_joy_Joy_e3ef016fcdf22397038b36036c66f7c8(MessageUpdateRule): 2 | old_type = "joy/Joy" 3 | old_full_text = """ 4 | float32[] axes 5 | int32[] buttons 6 | """ 7 | 8 | new_type = "sensor_msgs/Joy" 9 | new_full_text = """ 10 | # Reports the state of a joysticks axes and buttons. 11 | Header header # timestamp in the header is the time the data is received from the joystick 12 | float32[] axes # the axes measurements from a joystick 13 | int32[] buttons # the buttons measurements from a joystick 14 | 15 | ================================================================================ 16 | MSG: std_msgs/Header 17 | # Standard metadata for higher-level stamped data types. 18 | # This is generally used to communicate timestamped data 19 | # in a particular coordinate frame. 20 | # 21 | # sequence ID: consecutively increasing ID 22 | uint32 seq 23 | #Two-integer timestamp that is expressed as: 24 | # * stamp.secs: seconds (stamp_secs) since epoch 25 | # * stamp.nsecs: nanoseconds since stamp_secs 26 | # time-handling sugar is provided by the client library 27 | time stamp 28 | #Frame this data is associated with 29 | # 0: no frame 30 | # 1: global frame 31 | string frame_id 32 | """ 33 | 34 | order = 0 35 | migrated_types = [] 36 | 37 | valid = True 38 | 39 | def update(self, old_msg, new_msg): 40 | #No matching field name in old message 41 | new_msg.header = self.get_new_class('Header')() 42 | new_msg.axes = old_msg.axes 43 | new_msg.buttons = old_msg.buttons 44 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | joy 3 | 1.15.1 4 | BSD 5 | 6 | 7 | ROS driver for a generic Linux joystick. 8 | The joy package contains joy_node, a node that interfaces a 9 | generic Linux joystick to ROS. This node publishes a "Joy" 10 | message, which contains the current state of each one of the 11 | joystick's buttons and axes. 12 | 13 | 14 | Jonathan Bohren 15 | Morgan Quigley 16 | Brian Gerkey 17 | Kevin Watts 18 | Blaise Gassend 19 | 20 | http://www.ros.org/wiki/joy 21 | https://github.com/ros-drivers/joystick_drivers 22 | https://github.com/ros-drivers/joystick_drivers/issues 23 | 24 | catkin 25 | roslint 26 | 27 | diagnostic_updater 28 | joystick 29 | roscpp 30 | sensor_msgs 31 | 32 | rosbag 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/scripts/joy_remap.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # Author: furushchev 4 | 5 | import ast 6 | import operator as op 7 | import rospy 8 | import traceback 9 | from sensor_msgs.msg import Joy 10 | 11 | 12 | class RestrictedEvaluator(object): 13 | def __init__(self): 14 | self.operators = { 15 | ast.Add: op.add, 16 | ast.Sub: op.sub, 17 | ast.Mult: op.mul, 18 | ast.Div: op.truediv, 19 | ast.BitXor: op.xor, 20 | ast.USub: op.neg, 21 | } 22 | self.functions = { 23 | 'abs': lambda x: abs(x), 24 | 'max': lambda *x: max(*x), 25 | 'min': lambda *x: min(*x), 26 | } 27 | 28 | def _reval_impl(self, node, variables): 29 | if isinstance(node, ast.Num): 30 | return node.n 31 | elif isinstance(node, ast.BinOp): 32 | op = self.operators[type(node.op)] 33 | return op(self._reval_impl(node.left, variables), 34 | self._reval_impl(node.right, variables)) 35 | elif isinstance(node, ast.UnaryOp): 36 | op = self.operators[type(node.op)] 37 | return op(self._reval_impl(node.operand, variables)) 38 | elif isinstance(node, ast.Call) and node.func.id in self.functions: 39 | func = self.functions[node.func.id] 40 | args = [self._reval_impl(n, variables) for n in node.args] 41 | return func(*args) 42 | elif isinstance(node, ast.Name) and node.id in variables: 43 | return variables[node.id] 44 | elif isinstance(node, ast.Subscript) and node.value.id in variables: 45 | var = variables[node.value.id] 46 | idx = node.slice.value.n 47 | try: 48 | return var[idx] 49 | except IndexError: 50 | raise IndexError("Variable '%s' out of range: %d >= %d" % (node.value.id, idx, len(var))) 51 | else: 52 | raise TypeError("Unsupported operation: %s" % node) 53 | 54 | def reval(self, expr, variables): 55 | expr = str(expr) 56 | if len(expr) > 1000: 57 | raise ValueError("The length of an expression must not be more than 1000 characters") 58 | try: 59 | return self._reval_impl(ast.parse(expr, mode='eval').body, variables) 60 | except Exception as e: 61 | rospy.logerr(traceback.format_exc()) 62 | raise e 63 | 64 | 65 | class JoyRemap(object): 66 | def __init__(self): 67 | self.evaluator = RestrictedEvaluator() 68 | self.mappings = self.load_mappings("~mappings") 69 | self.warn_remap("joy_out") 70 | self.pub = rospy.Publisher( 71 | "joy_out", Joy, queue_size=1) 72 | self.warn_remap("joy_in") 73 | self.sub = rospy.Subscriber( 74 | "joy_in", Joy, self.callback, 75 | queue_size=rospy.get_param("~queue_size", None)) 76 | 77 | def load_mappings(self, ns): 78 | btn_remap = rospy.get_param(ns + "/buttons", []) 79 | axes_remap = rospy.get_param(ns + "/axes", []) 80 | rospy.loginfo("loaded remapping: %d buttons, %d axes" % (len(btn_remap), len(axes_remap))) 81 | return {"buttons": btn_remap, "axes": axes_remap} 82 | 83 | def warn_remap(self, name): 84 | if name == rospy.remap_name(name): 85 | rospy.logwarn("topic '%s' is not remapped" % name) 86 | 87 | def callback(self, in_msg): 88 | out_msg = Joy(header=in_msg.header) 89 | map_axes = self.mappings["axes"] 90 | map_btns = self.mappings["buttons"] 91 | out_msg.axes = [0.0] * len(map_axes) 92 | out_msg.buttons = [0] * len(map_btns) 93 | in_dic = {"axes": in_msg.axes, "buttons": in_msg.buttons} 94 | for i, exp in enumerate(map_axes): 95 | try: 96 | out_msg.axes[i] = self.evaluator.reval(exp, in_dic) 97 | except NameError as e: 98 | rospy.logerr("You are using vars other than 'buttons' or 'axes': %s" % e) 99 | except UnboundLocalError as e: 100 | rospy.logerr("Wrong form: %s" % e) 101 | except Exception as e: 102 | raise e 103 | 104 | for i, exp in enumerate(map_btns): 105 | try: 106 | if self.evaluator.reval(exp, in_dic) > 0: 107 | out_msg.buttons[i] = 1 108 | except NameError as e: 109 | rospy.logerr("You are using vars other than 'buttons' or 'axes': %s" % e) 110 | except UnboundLocalError as e: 111 | rospy.logerr("Wrong form: %s" % e) 112 | except Exception as e: 113 | raise e 114 | 115 | self.pub.publish(out_msg) 116 | 117 | 118 | if __name__ == '__main__': 119 | rospy.init_node("joy_remap") 120 | n = JoyRemap() 121 | rospy.spin() 122 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/test/saved/Joy.saved: -------------------------------------------------------------------------------- 1 | [joy/Joy]: 2 | float32[] axes 3 | int32[] buttons 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/test/test_joy_msg_migration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Willow Garage, Inc. nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | 35 | import roslib 36 | roslib.load_manifest('joy') 37 | 38 | import sys 39 | import struct 40 | 41 | import unittest 42 | 43 | import rostest 44 | import rosbag 45 | import rosbagmigration 46 | 47 | import re 48 | from cStringIO import StringIO 49 | import os 50 | 51 | import rospy 52 | 53 | 54 | 55 | migrator = rosbagmigration.MessageMigrator() 56 | 57 | 58 | def repack(x): 59 | return struct.unpack('`_) 14 | * roslint and Generic Clean-Up (`#161 `_) 15 | * Contributors: Joshua Whitley 16 | 17 | 1.13.0 (2019-06-24) 18 | ------------------- 19 | 20 | 1.12.0 (2018-06-11) 21 | ------------------- 22 | * Addressed numerous outstanding PRs. 23 | * Changed package xml to format 2 24 | * Contributors: Jonathan Bohren, jprod123 25 | 26 | 1.11.0 (2017-02-10) 27 | ------------------- 28 | 29 | 1.10.1 (2015-05-24) 30 | ------------------- 31 | 32 | 1.10.0 (2014-06-26) 33 | ------------------- 34 | * First indigo release 35 | -------------------------------------------------------------------------------- /src/joystick_drivers/joystick_drivers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(joystick_drivers) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /src/joystick_drivers/joystick_drivers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | joystick_drivers 3 | 1.15.1 4 | BSD 5 | 6 | 7 | This metapackage depends on packages for interfacing common 8 | joysticks and human input devices with ROS. 9 | 10 | 11 | http://www.ros.org/wiki/joystick_drivers 12 | https://github.com/ros-drivers/joystick_drivers 13 | https://github.com/ros-drivers/joystick_drivers/issues 14 | 15 | Jonathan Bohren 16 | 17 | catkin 18 | 19 | joy 20 | ps3joy 21 | spacenav_node 22 | wiimote 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ps3joy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.15.0 (2020-10-12) 6 | ------------------- 7 | 8 | 1.14.0 (2020-07-07) 9 | ------------------- 10 | * Fixing linter errors for Noetic. (`#174 `_) 11 | * Make sure to import struct where it is used. (`#162 `_) 12 | * roslint and Generic Clean-Up (`#161 `_) 13 | * Contributors: Chris Lalancette, Joshua Whitley 14 | 15 | 1.13.0 (2019-06-24) 16 | ------------------- 17 | * Merge pull request `#128 `_ from ros-drivers/fix/tab_errors 18 | * Cleaning up Python indentation. 19 | * Merge pull request `#123 `_ from cclauss/modernize-python2-code 20 | * Modernize Python 2 code to get ready for Python 3 21 | * Merge branch 'master' into indigo-devel 22 | * Contributors: Joshua Whitley, Matthew, cclauss 23 | 24 | 1.12.0 (2018-06-11) 25 | ------------------- 26 | * Addressed numerous outstanding PRs. 27 | * Created bluetooth_devices.md 28 | * Created testing guide for ps3joy. 29 | * Create procedure_test.md 30 | * Let ps3joy_node not quit on inactivity-timeout. 31 | * Refine diagnostics message usage in ps3joy_node 32 | * Improve ps3joy_node with rospy.init_node and .is_shutdown 33 | * Remove quit on failed root level check, part one of issue `#53 `_ 34 | * Create README 35 | * Changed package xml to format 2 36 | * Contributors: Alenso Labady, Felix Kolbe, Jonathan Bohren, alab288, jprod123 37 | 38 | 1.11.0 (2017-02-10) 39 | ------------------- 40 | * Update dependencies to remove warnings 41 | * Contributors: Mark D Horn 42 | 43 | 1.10.1 (2015-05-24) 44 | ------------------- 45 | * Remove stray architechture_independent flags 46 | * Contributors: Jonathan Bohren, Scott K Logan 47 | 48 | 1.10.0 (2014-06-26) 49 | ------------------- 50 | * First indigo reelase 51 | * Update ps3joy/package.xml URLs with github user ros to ros-drivers 52 | * Prompt for sudo password when required 53 | * Contributors: Felix Kolbe, Jonathan Bohren, dawonn 54 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ps3joy) 3 | 4 | find_package(PkgConfig REQUIRED) 5 | pkg_search_module(LIBUSB REQUIRED libusb) 6 | 7 | if(LIBUSB_FOUND) 8 | include_directories(SYSTEM ${LIBUSB_INCLUDE_DIRS}) 9 | link_directories(${LIBUSB_LIBRARY_DIRS}) 10 | else() 11 | message( FATAL_ERROR "Failed to find libusb" ) 12 | endif() 13 | 14 | # Load catkin and all dependencies required for this package 15 | set(CATKIN_DEPS diagnostic_msgs sensor_msgs rospy rosgraph roslint) 16 | find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPS}) 17 | catkin_package(CATKIN_DEPENDS diagnostic_msgs sensor_msgs) 18 | 19 | roslint_python( 20 | scripts/ps3joy.py 21 | scripts/ps3joy_node.py 22 | scripts/ps3joysim.py 23 | ) 24 | 25 | include_directories(${catkin_INCLUDE_DIRS}) 26 | add_executable(sixpair src/sixpair.c) 27 | target_link_libraries(sixpair -lusb ${catkin_LIBRARIES}) 28 | 29 | if(CATKIN_ENABLE_TESTING) 30 | roslint_add_test() 31 | endif() 32 | 33 | # Install targets 34 | install(TARGETS sixpair 35 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | 40 | install(DIRECTORY launch 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 42 | ) 43 | 44 | install(FILES diagnostics.yaml 45 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 46 | ) 47 | 48 | catkin_install_python(PROGRAMS 49 | scripts/ps3joy.py 50 | scripts/ps3joy_node.py 51 | scripts/ps3joysim.py 52 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 53 | ) 54 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/README.md: -------------------------------------------------------------------------------- 1 | # PlayStation 3 Joystick Driver for ROS 2 | 3 | This package provides a driver for the PS3 (SIXAXIS or DUALSHOCK3) bluetooth joystick. 4 | 5 | This driver provides a more reliable connection, and provides access to the joystick's accelerometers and gyroscope. Linux's native support for the PS3 joystick does lacks this functionality. 6 | 7 | Additional documentation: 8 | 9 | * [Troubleshooting](doc/troubleshooting.md) 10 | * [Testing Instructions](doc/testing.md) 11 | * [Bluetooth Device Compatibility](doc/bluetooth_devices.md) 12 | 13 | ## Dependencies 14 | 15 | * joystick 16 | * libusb-dev 17 | * bluez-5.37 18 | 19 | ## Pairing instructions 20 | 21 | 1. If you can connect the joystick and the bluetooth dongle into the same 22 | computer connect the joystick to the computer using a USB cable. 23 | 24 | 2. Load the bluetooth dongle's MAC address into the ps3 joystick using: 25 | ``` 26 | sudo bash 27 | rosrun ps3joy sixpair 28 | ``` 29 | If you cannot connect the joystick to the same computer as the dongle, 30 | find out the bluetooth dongle's MAC address by running (on the computer 31 | that has the bluetooth dongle): 32 | ``` 33 | hciconfig 34 | ``` 35 | If this does not work, you may need to do 36 | ``` 37 | sudo hciconfig hci0 up 38 | ``` 39 | and retry 40 | ``` 41 | hciconfig 42 | ``` 43 | 3. Plug the PS3 joystick into some other computer using a USB cable. 44 | 45 | 4. Replace the joystick's mac address in the following command: 46 | ``` 47 | sudo rosrun ps3joy sixpair 01:23:45:67:89:ab 48 | ``` 49 | 50 | ## Starting the PS3 joystick 51 | 52 | 5. Run the following command 53 | ``` 54 | rosrun ps3joy ps3joy.py 55 | ``` 56 | 6. Open a new terminal and reboot bluez and run joy with: 57 | ``` 58 | sudo systemctl restart bluetooth 59 | rosrun joy joy_node 60 | ``` 61 | 7. Open a new terminal and echo the joy topic 62 | ``` 63 | rostopic echo joy 64 | ``` 65 | 8. This should make a joystick appear at /dev/input/js? 66 | 67 | 9. You can check that it is working with 68 | jstest /dev/input/js? 69 | (replace ? with the name of your joystick) 70 | 71 | ## Command-line Options 72 | 73 | ### ps3joy.py 74 | 75 | ``` 76 | usage: ps3joy.py [--inactivity-timeout=] [--no-disable-bluetoothd] [--redirect-output] [--continuous-output]= 77 | : inactivity timeout in seconds (saves battery life). 78 | : file name to redirect output to. 79 | ``` 80 | 81 | `--inactivity-timeout` 82 | This may be useful for saving battery life and reducing contention on the 2.4 GHz network.Your PS3 controller 83 | will shutdown after a given amount of time of inactivity. 84 | 85 | `--no-disable-bluetoothd` 86 | ps3joy.py will not take down bluetoothd. Bluetoothd must be configured to not handle input device, otherwise 87 | you will receive an error saying "Error binding to socket". 88 | 89 | `--redirect-output` 90 | This can be helpful when ps3joy.py is running in the backgound. This will allow the standard output 91 | and error messages to redirected into a file. 92 | 93 | `--continuous-output` 94 | This will output continuous motion streams and as a result this will no longer leave extended periods of 95 | no messages on the /joy topic. ( This only works for ps3joy.py. Entering this parameter in ps3joy_node.py will 96 | result in the parameter being ignored.) 97 | 98 | ## Limitations 99 | 100 | This driver will not coexist with any other bluetooth device. In future releases, we plan to allow first non-HID and later any bluetooth device to coexist with this driver. The following devices do coexist: 101 | 102 | * Non-HID devices using a userland driver, such as one written using pybluez. 103 | * Keyboards or mice running in HID proxy mode, which appear to the kernel as USB devices. 104 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/diagnostics.yaml: -------------------------------------------------------------------------------- 1 | type: AnalyzerGroup 2 | pub_rate: 1.0 # Optional 3 | base_path: '' # Optional, prepended to all diagnostic output 4 | analyzers: 5 | PS3State: 6 | type: diagnostic_aggregator/GenericAnalyzer 7 | path: 'PS3 State' 8 | timeout: 5.0 9 | startswith: ['Battery', 'Charging State', 'Connection', 'ps3_joy'] 10 | remove_prefix: 'ps3_joy' 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/doc/bluetooth_devices.md: -------------------------------------------------------------------------------- 1 | 2 | ## Bluetooth Device Compatibility 3 | 4 | This driver works with most 2.x Bluetooth adapters.This driver should also work with 3.0 and 4.0 Bluetooth adapters. 5 | Please report other compatible and incompatible Bluetooth adapters through a pull request to this page. 6 | 7 | ### Adapters that are known to work 8 | 9 | * Cambridge Silicon Radio, Ltd Bluetooth Dongle (Bluetooth 4.0) 10 | 11 | ### Adapters that are known not to work 12 | 13 | * Linksys USBBT100 version 2 (Bluetooth 1.1) 14 | * USB device 0a12:0x0001 15 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/doc/testing.md: -------------------------------------------------------------------------------- 1 | # Testing procedures for the ps3joy package # 2 | 3 | ## Pairing the ps3 controller via bluetooth ## 4 | If the controller is not paired to the bluetooth dongle connect 5 | the controller via usb 6 | 7 | Enter the following commands: 8 | 9 | ``` 10 | sudo bash 11 | rosrun ps3joy sixpair 12 | ``` 13 | The current bluetooth master address and setting master address should the same. 14 | 15 | ``` 16 | Current Bluetooth master: 00:15:83:ed:3f:21 17 | Setting master bd_addr to 00:15:83:ed:3f:21 18 | ``` 19 | 20 | ## Running the ps3joy nodes ## 21 | The package should consists of the following nodes: 22 | * ps3joy.py 23 | * ps3joy_node.py 24 | 25 | 26 | Running ps3joy_node.py will **require** being root to run if the user does not have 27 | permissions to the hardware. 28 | 29 | Enter the following commands in the terminal: 30 | 31 | ``` 32 | sudo bash 33 | rosrun ps3joy ps3joy_node.py 34 | ``` 35 | The following message should be diplayed aftwards: 36 | 37 | ``` 38 | Waiting Connection. Disconnect your PS3 joystick from USB and press the pairing button. 39 | ``` 40 | 41 | If your joystick does not pair, open a new terminal and restart bluez by 42 | entering the following command: 43 | 44 | ``` 45 | sudo systemctl restart bluetooth 46 | ``` 47 | The terminal where ps3joy_node.py was launched should display the following message: 48 | ``` 49 | Connection is Activated. 50 | ``` 51 | ## ps3joy Diagnostics test ## 52 | Open a new terminal and enter the following following command: 53 | ``` 54 | rostopic list 55 | ``` 56 | You should see the following: 57 | ``` 58 | /diagnostics 59 | /joy/set_feedback 60 | /rosout 61 | /rosout_agg 62 | ``` 63 | Enter the following command to diagnose the current state of the joystick 64 | ``` 65 | rostopic echo /diagnostics 66 | ``` 67 | You will see the charging state, battery percentage and, connection type in your terminal: 68 | ``` 69 | header: 70 | seq: 1 71 | stamp: 72 | secs: 1498667204 73 | nsecs: 603754043 74 | frame_id: '' 75 | status: 76 | - 77 | level: 0 78 | name: Battery 79 | message: 100% Charge 80 | hardware_id: '' 81 | values: [] 82 | - 83 | level: 0 84 | name: Connection 85 | message: Bluetooth Connection 86 | hardware_id: '' 87 | values: [] 88 | - 89 | level: 0 90 | name: Charging State 91 | message: Not Charging 92 | hardware_id: '' 93 | values: [] 94 | ``` 95 | 96 | If you plug in the usb cable both the connection type and charging state will change: 97 | ``` 98 | header: 99 | seq: 8 100 | stamp: 101 | secs: 1498667507 102 | nsecs: 798973083 103 | frame_id: '' 104 | status: 105 | - 106 | level: 0 107 | name: Battery 108 | message: Charging 109 | hardware_id: '' 110 | values: [] 111 | - 112 | level: 0 113 | name: Connection 114 | message: USB Connection 115 | hardware_id: '' 116 | values: [] 117 | - 118 | level: 0 119 | name: Charging State 120 | message: Charging 121 | hardware_id: '' 122 | values: [] 123 | ``` 124 | 125 | ## Confirming the ps3 joystick input ## 126 | Check to see if your joystick is recgonized by your computer. 127 | 128 | ``` 129 | ls /dev/input/ 130 | ``` 131 | Tell the ps3joy node which device is the ps3 joystick 132 | ``` 133 | rosparam set joy_node/dev "dev/input/jsX" 134 | ``` 135 | X would be the number your device was given. 136 | you can now start the joy node: 137 | 138 | ``` 139 | rosrun joy joy_node 140 | ``` 141 | You should see something that looks like this: 142 | ``` 143 | [ INFO] [1498673536.447541090]: Opened joystick: /dev/input/js0. deadzone_: 0.050000. 144 | ``` 145 | Open a new terminal and use rostopic to observe the data from the controller. 146 | ``` 147 | rostopic echo joy 148 | ``` 149 | You should see the input data dipslayed on your terminal. 150 | 151 | ## Recharging the PS3 joystick 152 | 1. Have an available USB port on a computer, and the computer must be on while the joystick is 153 | charging. 154 | 155 | 2. Connect the PS3 joystick to a computer using an A to mini-B USB cable. 156 | 157 | 3. The LEDs on the joystick should blink at about 1Hz to indicate the the joystick is charging. 158 | 159 | ## Shutting down the ps3 joystick 160 | There are two ways to turn of the ps3 joystick 161 | 1. Press and hold the pairing button on the joystick for approximately 10 seconds 162 | 163 | 2. You can also also shut the controller down by killing the process of ps3joy_node.py 164 | press Ctrl-c on your keyboard to kill the processes and the joystick will turn off 165 | as well. 166 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/doc/troubleshooting.md: -------------------------------------------------------------------------------- 1 | ## Troubleshooting ## 2 | ------------------------- 3 | #### Issues pairing the PS3 joystick #### 4 | When pairing your joystick via bluetooth, you may recieve the following message on 5 | your terminal: 6 | ``` 7 | Current Bluetooth master: 00:15:83:ed:3f:21 8 | Unable to retrieve local bd_addr from `hcitool dev`. 9 | Please enable Bluetooth or specify an address manually. 10 | ``` 11 | This would indicate that your bluetooth is disabled. To enable your bluetooth, try the 12 | following: 13 | 14 | 1. Check the status of your bluetooth by entering the following: 15 | ``` 16 | sudo systemctl status bluetooth 17 | ``` 18 | You may see something like this: 19 | 20 | ``` 21 | ● bluetooth.service - Bluetooth service 22 | Loaded: loaded (/lib/systemd/system/bluetooth.service; disabled; vendor preset: enabled) 23 | Active: inactive (dead) 24 | Docs: man:bluetoothd(8) 25 | ``` 26 | 27 | If you do, that means your bluetooth service is disabled. Turn enable it enter 28 | ``` 29 | sudo systemctl start bluetooth 30 | sudo systemctl status bluetooth 31 | ``` 32 | After running these commands your bluetooth service should be up and running: 33 | 34 | ``` 35 | ● bluetooth.service - Bluetooth service 36 | Loaded: loaded (/lib/systemd/system/bluetooth.service; disabled; vendor preset: enabled) 37 | Active: active (running) since Thu 2017-06-29 16:21:43 EDT; 16s ago 38 | Docs: man:bluetoothd(8) 39 | Main PID: 27362 (bluetoothd) 40 | Status: "Running" 41 | CGroup: /system.slice/bluetooth.service 42 | └─27362 /usr/local/libexec/bluetooth/bluetoothd 43 | ``` 44 | Retry the commands that were mentioned in step 2 for pairing the PS3 joystick. 45 | 46 | 2. Run the following command: 47 | ``` 48 | hciconfig hci0 reset 49 | ``` 50 | followed by: 51 | ``` 52 | sudo bash 53 | rosrun ps3joy sixpair 54 | ``` 55 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/launch/ps3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ps3joy 3 | 1.15.0 4 | BSD 5 | 6 | 7 | Playstation 3 SIXAXIS or DUAL SHOCK 3 joystick driver. 8 | Driver for the Sony PlayStation 3 SIXAXIS or DUAL SHOCK 3 9 | joysticks. In its current state, this driver is not compatible 10 | with the use of other Bluetooth HID devices. The driver listens 11 | for a connection on the HID ports, starts the joystick 12 | streaming data, and passes the data to the Linux uinput device 13 | so that it shows up as a normal joystick. 14 | 15 | 16 | Jonathan Bohren 17 | Blaise Gassend 18 | pascal@pabr.org 19 | Melonee Wise 20 | 21 | http://www.ros.org/wiki/ps3joy 22 | https://github.com/ros-drivers/joystick_drivers 23 | https://github.com/ros-drivers/joystick_drivers/issues 24 | 25 | catkin 26 | 27 | roslint 28 | 29 | bluez 30 | diagnostic_msgs 31 | joystick 32 | libusb-dev 33 | python3-bluez 34 | rosgraph 35 | rospy 36 | sensor_msgs 37 | 38 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/scripts/ps3joysim.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2009, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | from __future__ import print_function 35 | from bluetooth import * 36 | import select 37 | import fcntl 38 | import os 39 | import struct 40 | import time 41 | import sys 42 | import traceback 43 | import threading 44 | import ps3joy 45 | import socket 46 | import signal 47 | 48 | 49 | def mk_in_socket(): 50 | sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 51 | sock.bind(("127.0.0.1", 0)) 52 | sock.listen(1) 53 | return sock, sock.getsockname()[1] 54 | 55 | 56 | # Class to spawn the ps3joy.py infrastructure in its own thread 57 | class driversim(threading.Thread): 58 | def __init__(self, intr, ctrl): 59 | threading.Thread.__init__(self) 60 | self.intr = intr 61 | self.ctrl = ctrl 62 | self.start() 63 | 64 | def run(self): 65 | self.cm = ps3joy.connection_manager(ps3joy.decoder()) 66 | self.cm.listen(self.intr, self.ctrl) 67 | print("driversim exiting") 68 | 69 | def shutdown(self): 70 | self.cm.shutdown = True 71 | 72 | 73 | class joysim(threading.Thread): 74 | def __init__(self, intr, ctrl): 75 | threading.Thread.__init__(self) 76 | print("Starting joystick simulator on ports", intr, "and", ctrl) 77 | self.intr = socket.socket() 78 | self.intr.connect(("127.0.0.1", intr)) 79 | if self.intr == -1: 80 | raise "Error creating interrput socket." 81 | self.ctrl = socket.socket() 82 | self.ctrl.connect(("127.0.0.1", ctrl)) 83 | if self.ctrl == -1: 84 | raise "Error creating control socket." 85 | self.active = False 86 | self.shutdown = False 87 | self.start() 88 | 89 | def run(self): 90 | while not self.active and not self.shutdown: 91 | (rd, wr, err) = select.select([self.ctrl], [], [], 1) 92 | if len(rd) == 1: 93 | cmd = self.ctrl.recv(128) 94 | if cmd == "\x53\xf4\x42\x03\x00\x00": 95 | self.active = True 96 | print("Got activate command") 97 | else: 98 | print("Got unknown command (len=%i)" % len(cmd), end=' ') 99 | time.sleep(1) 100 | for c in cmd: 101 | print("%x" % ord(c), end=' ') 102 | print() 103 | print("joyactivate exiting") 104 | 105 | def publishstate(self, ax, butt): 106 | if self.active: 107 | ranges = [255] * 17 + [8191] * 20 108 | axval = [int((v + 1) * s / 2) for (v, s) in zip(ax, ranges)] 109 | buttout = [] 110 | for i in range(0, 2): 111 | newval = 0 112 | for j in range(0, 8): 113 | newval = (newval << 1) 114 | if butt[i * 8 + j]: 115 | newval = newval + 1 116 | buttout.append(newval) 117 | joy_coding = "!1B2x3B1x4B4x12B15x4H" 118 | self.intr.send(struct.pack(joy_coding, 161, *(buttout + [0] + axval))) 119 | else: 120 | print("Tried to publish while inactive") 121 | 122 | 123 | if __name__ == "__main__": 124 | def stop_all_threads(a, b): 125 | exit(0) 126 | 127 | signal.signal(signal.SIGINT, stop_all_threads) 128 | 129 | # Create sockets for the driver side and pass them to the driver 130 | (intr_in, intr_port) = mk_in_socket() 131 | (ctrl_in, ctrl_port) = mk_in_socket() 132 | 133 | ds = driversim(intr_in, ctrl_in) 134 | 135 | # Give the simulator a chance to get going 136 | time.sleep(2) 137 | 138 | # Call up the simulator telling it which ports to connect to. 139 | js = joysim(intr_port, ctrl_port) 140 | buttons1 = [True] * 16 141 | axes1 = [1, 0, -1, .5] * 5 142 | buttons2 = [False] * 16 143 | axes2 = [-1] * 20 144 | buttons3 = [False] * 16 145 | axes3 = [0] * 20 146 | shutdown = False 147 | while not js.active and not shutdown: 148 | time.sleep(0.2) 149 | time.sleep(0.01) 150 | time.sleep(0.01) 151 | while not shutdown: 152 | js.publishstate(axes1, buttons2) 153 | time.sleep(0.01) 154 | axes1[0] = -axes1[0] 155 | js.publishstate(axes2, buttons2) 156 | time.sleep(0.01) 157 | 158 | print("main exiting") 159 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/src/sixpair.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2007, 2008 pascal@pabr.org 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 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 the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * 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 THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #define VENDOR 0x054c 36 | #define PRODUCT 0x0268 37 | 38 | #define USB_DIR_IN 0x80 39 | #define USB_DIR_OUT 0 40 | 41 | void fatal(char *msg) { perror(msg); exit(1); } 42 | 43 | void show_master(usb_dev_handle *devh, int itfnum) { 44 | printf("Current Bluetooth master: "); 45 | unsigned char msg[8]; 46 | int res = usb_control_msg 47 | (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE, 48 | 0x01, 0x03f5, itfnum, (void*)msg, sizeof(msg), 5000); 49 | if ( res < 0 ) { perror("USB_REQ_GET_CONFIGURATION"); return; } 50 | printf("%02x:%02x:%02x:%02x:%02x:%02x\n", 51 | msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]); 52 | } 53 | 54 | void set_master(usb_dev_handle *devh, int itfnum, int mac[6]) { 55 | printf("Setting master bd_addr to %02x:%02x:%02x:%02x:%02x:%02x\n", 56 | mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); 57 | char msg[8]= { 0x01, 0x00, mac[0],mac[1],mac[2],mac[3],mac[4],mac[5] }; 58 | int res = usb_control_msg 59 | (devh, 60 | USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE, 61 | 0x09, 62 | 0x03f5, itfnum, msg, sizeof(msg), 63 | 5000); 64 | if ( res < 0 ) fatal("USB_REQ_SET_CONFIGURATION"); 65 | } 66 | 67 | void process_device(int argc, char **argv, struct usb_device *dev, 68 | struct usb_config_descriptor *cfg, int itfnum) { 69 | int mac[6]; 70 | 71 | usb_dev_handle *devh = usb_open(dev); 72 | if ( ! devh ) fatal("usb_open"); 73 | 74 | usb_detach_kernel_driver_np(devh, itfnum); 75 | 76 | int res = usb_claim_interface(devh, itfnum); 77 | if ( res < 0 ) fatal("usb_claim_interface"); 78 | 79 | show_master(devh, itfnum); 80 | 81 | if ( argc >= 2 ) { 82 | if ( sscanf(argv[1], "%x:%x:%x:%x:%x:%x", 83 | &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) { 84 | 85 | printf("usage: %s []\n", argv[0]); 86 | exit(1); 87 | } 88 | } else { 89 | FILE *f = popen("hcitool dev", "r"); 90 | if ( !f || 91 | fscanf(f, "%*s\n%*s %x:%x:%x:%x:%x:%x", 92 | &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) { 93 | printf("Unable to retrieve local bd_addr from `hcitool dev`.\n"); 94 | printf("Please enable Bluetooth or specify an address manually.\n"); 95 | exit(1); 96 | } 97 | pclose(f); 98 | } 99 | 100 | set_master(devh, itfnum, mac); 101 | 102 | usb_close(devh); 103 | } 104 | 105 | int main(int argc, char *argv[]) { 106 | 107 | usb_init(); 108 | if ( usb_find_busses() < 0 ) fatal("usb_find_busses"); 109 | if ( usb_find_devices() < 0 ) fatal("usb_find_devices"); 110 | struct usb_bus *busses = usb_get_busses(); 111 | if ( ! busses ) fatal("usb_get_busses"); 112 | 113 | int found = 0; 114 | 115 | struct usb_bus *bus; 116 | for ( bus=busses; bus; bus=bus->next ) { 117 | struct usb_device *dev; 118 | for ( dev=bus->devices; dev; dev=dev->next) { 119 | struct usb_config_descriptor *cfg; 120 | for ( cfg = dev->config; 121 | cfg < dev->config + dev->descriptor.bNumConfigurations; 122 | ++cfg ) { 123 | int itfnum; 124 | for ( itfnum=0; itfnumbNumInterfaces; ++itfnum ) { 125 | struct usb_interface *itf = &cfg->interface[itfnum]; 126 | struct usb_interface_descriptor *alt; 127 | for ( alt = itf->altsetting; 128 | alt < itf->altsetting + itf->num_altsetting; 129 | ++alt ) { 130 | if ( dev->descriptor.idVendor == VENDOR && 131 | dev->descriptor.idProduct == PRODUCT && 132 | alt->bInterfaceClass == 3 ) { 133 | process_device(argc, argv, dev, cfg, itfnum); 134 | ++found; 135 | } 136 | } 137 | } 138 | } 139 | } 140 | } 141 | 142 | if ( ! found ) { 143 | printf("No controller found on USB busses. Please connect your joystick via USB.\n"); 144 | return 1; 145 | } 146 | 147 | return 0; 148 | 149 | } 150 | 151 | -------------------------------------------------------------------------------- /src/local_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(local_planner) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(PCL REQUIRED) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | std_msgs 27 | sensor_msgs 28 | pcl_ros 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | ## Specify additional locations of header files 36 | ## Your package locations should be listed before other locations 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${PCL_INCLUDE_DIRS} 40 | "${PROJECT_SOURCE_DIR}/include" 41 | /usr/local/include # Location when using 'make system_install' 42 | /usr/include # More usual location (e.g. when installing using a package) 43 | ) 44 | 45 | ## Specify additional locations for library files 46 | link_directories( 47 | /usr/local/lib # Location when using 'make system_install' 48 | /usr/lib # More usual location (e.g. when installing using a package) 49 | ) 50 | 51 | ## Declare executables 52 | add_executable(localPlanner src/localPlanner.cpp) 53 | add_executable(pathFollower src/pathFollower.cpp) 54 | 55 | ## Specify libraries to link a library or executable target against 56 | target_link_libraries(localPlanner ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 57 | target_link_libraries(pathFollower ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS localPlanner pathFollower 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | install(DIRECTORY paths/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/paths 70 | ) 71 | -------------------------------------------------------------------------------- /src/local_planner/launch/local_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /src/local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | local_planner 3 | 0.0.1 4 | Local Planner 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/local_planner/paths/path_generator.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear all; 3 | close all; 4 | 5 | %% generate path 6 | %{. 7 | dis = 1.0; 8 | angle = 27; 9 | deltaAngle = angle / 3; 10 | scale = 0.65; 11 | 12 | pathStartAll = zeros(4, 0); 13 | pathAll = zeros(5, 0); 14 | pathList = zeros(5, 0); 15 | pathID = 0; 16 | groupID = 0; 17 | 18 | figure; 19 | hold on; 20 | box on; 21 | axis equal; 22 | xlabel('X (m)'); 23 | ylabel('Y (m)'); 24 | 25 | fprintf('\nGenerating paths\n'); 26 | 27 | for shift1 = -angle : deltaAngle : angle 28 | wayptsStart = [0, 0, 0; 29 | dis, shift1, 0]; 30 | 31 | pathStartR = 0 : 0.01 : dis; 32 | pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR); 33 | 34 | pathStartX = pathStartR .* cos(pathStartShift * pi / 180); 35 | pathStartY = pathStartR .* sin(pathStartShift * pi / 180); 36 | pathStartZ = zeros(size(pathStartX)); 37 | 38 | pathStart = [pathStartX; pathStartY; pathStartZ; ones(size(pathStartX)) * groupID]; 39 | pathStartAll = [pathStartAll, pathStart]; 40 | 41 | for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1 42 | for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2 43 | waypts = [pathStartR', pathStartShift', pathStartZ'; 44 | 2 * dis, shift2, 0; 45 | 3 * dis - 0.001, shift3, 0; 46 | 3 * dis, shift3, 0]; 47 | 48 | pathR = 0 : 0.01 : waypts(end, 1); 49 | pathShift = spline(waypts(:, 1), waypts(:, 2), pathR); 50 | 51 | pathX = pathR .* cos(pathShift * pi / 180); 52 | pathY = pathR .* sin(pathShift * pi / 180); 53 | pathZ = zeros(size(pathX)); 54 | 55 | path = [pathX; pathY; pathZ; ones(size(pathX)) * pathID; ones(size(pathX)) * groupID]; 56 | pathAll = [pathAll, path]; 57 | pathList = [pathList, [pathX(end); pathY(end); pathZ(end); pathID; groupID]]; 58 | 59 | pathID = pathID + 1; 60 | 61 | plot3(pathX, pathY, pathZ); 62 | end 63 | end 64 | 65 | groupID = groupID + 1 66 | end 67 | 68 | pathID 69 | 70 | fileID = fopen('startPaths.ply', 'w'); 71 | fprintf(fileID, 'ply\n'); 72 | fprintf(fileID, 'format ascii 1.0\n'); 73 | fprintf(fileID, 'element vertex %d\n', size(pathStartAll, 2)); 74 | fprintf(fileID, 'property float x\n'); 75 | fprintf(fileID, 'property float y\n'); 76 | fprintf(fileID, 'property float z\n'); 77 | fprintf(fileID, 'property int group_id\n'); 78 | fprintf(fileID, 'end_header\n'); 79 | fprintf(fileID, '%f %f %f %d\n', pathStartAll); 80 | fclose(fileID); 81 | 82 | fileID = fopen('paths.ply', 'w'); 83 | fprintf(fileID, 'ply\n'); 84 | fprintf(fileID, 'format ascii 1.0\n'); 85 | fprintf(fileID, 'element vertex %d\n', size(pathAll, 2)); 86 | fprintf(fileID, 'property float x\n'); 87 | fprintf(fileID, 'property float y\n'); 88 | fprintf(fileID, 'property float z\n'); 89 | fprintf(fileID, 'property int path_id\n'); 90 | fprintf(fileID, 'property int group_id\n'); 91 | fprintf(fileID, 'end_header\n'); 92 | fprintf(fileID, '%f %f %f %d %d\n', pathAll); 93 | fclose(fileID); 94 | 95 | fileID = fopen('pathList.ply', 'w'); 96 | fprintf(fileID, 'ply\n'); 97 | fprintf(fileID, 'format ascii 1.0\n'); 98 | fprintf(fileID, 'element vertex %d\n', size(pathList, 2)); 99 | fprintf(fileID, 'property float end_x\n'); 100 | fprintf(fileID, 'property float end_y\n'); 101 | fprintf(fileID, 'property float end_z\n'); 102 | fprintf(fileID, 'property int path_id\n'); 103 | fprintf(fileID, 'property int group_id\n'); 104 | fprintf(fileID, 'end_header\n'); 105 | fprintf(fileID, '%f %f %f %d %d\n', pathList); 106 | fclose(fileID); 107 | 108 | pause(1.0); 109 | %} 110 | 111 | %% find correspondence 112 | %{. 113 | voxelSize = 0.02; 114 | searchRadius = 0.45; 115 | offsetX = 3.2; 116 | offsetY = 4.5; 117 | voxelNumX = 161; 118 | voxelNumY = 451; 119 | 120 | fprintf('\nPreparing voxels\n'); 121 | 122 | indPoint = 1; 123 | voxelPointNum = voxelNumX * voxelNumY; 124 | voxelPoints = zeros(voxelPointNum, 2); 125 | for indX = 0 : voxelNumX - 1 126 | x = offsetX - voxelSize * indX; 127 | scaleY = x / offsetX + searchRadius / offsetY * (offsetX - x) / offsetX; 128 | for indY = 0 : voxelNumY - 1 129 | y = scaleY * (offsetY - voxelSize * indY); 130 | 131 | voxelPoints(indPoint, 1) = x; 132 | voxelPoints(indPoint, 2) = y; 133 | 134 | indPoint = indPoint + 1; 135 | end 136 | end 137 | 138 | plot3(voxelPoints(:, 1), voxelPoints(:, 2), zeros(voxelPointNum, 1), 'k.'); 139 | pause(1.0); 140 | 141 | fprintf('\nCollision checking\n'); 142 | 143 | [ind, dis] = rangesearch(pathAll(1 : 2, :)', voxelPoints, searchRadius); 144 | 145 | fprintf('\nSaving correspondences\n'); 146 | 147 | fileID = fopen('correspondences.txt', 'w'); 148 | 149 | for i = 1 : voxelPointNum 150 | fprintf(fileID, '%d ', i - 1); 151 | 152 | indVoxel = sort(ind{i}); 153 | indVoxelNum = size(indVoxel, 2); 154 | 155 | pathIndRec = -1; 156 | for j = 1 : indVoxelNum 157 | pathInd = pathAll(4, indVoxel(j)); 158 | if pathInd == pathIndRec 159 | continue; 160 | end 161 | 162 | fprintf(fileID, '%d ', pathInd); 163 | pathIndRec = pathInd; 164 | end 165 | fprintf(fileID, '-1\n'); 166 | 167 | if mod(i, 1000) == 0 168 | i 169 | end 170 | end 171 | 172 | fclose(fileID); 173 | 174 | fprintf('\nProcessing complete\n'); 175 | %} 176 | -------------------------------------------------------------------------------- /src/segmentation_proc/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(segmentation_proc) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(PCL REQUIRED) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | std_msgs 27 | sensor_msgs 28 | pcl_ros 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | ## Specify additional locations of header files 36 | ## Your package locations should be listed before other locations 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${PCL_INCLUDE_DIRS} 40 | "${PROJECT_SOURCE_DIR}/include" 41 | /usr/local/include # Location when using 'make system_install' 42 | /usr/include # More usual location (e.g. when installing using a package) 43 | ) 44 | 45 | ## Specify additional locations for library files 46 | link_directories( 47 | /usr/local/lib # Location when using 'make system_install' 48 | /usr/lib # More usual location (e.g. when installing using a package) 49 | ) 50 | 51 | ## Declare executables 52 | add_executable(segmentationProc src/segmentationProc.cpp) 53 | 54 | ## Specify libraries to link a library or executable target against 55 | target_link_libraries(segmentationProc ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 56 | 57 | install(TARGETS segmentationProc 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 61 | ) 62 | 63 | install(DIRECTORY launch/ 64 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 65 | ) 66 | 67 | install(DIRECTORY scripts/ 68 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts 69 | ) 70 | 71 | install(FILES LICENSE 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 73 | ) 74 | 75 | -------------------------------------------------------------------------------- /src/segmentation_proc/LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) Facebook, Inc. and its affiliates. 4 | 5 | Copyright (c) 2017 Angel Chang, Angela Dai, Thomas Funkhouser, Maciej Halber, Matthias Nießner, Manolis Savva, Shuran Song, Andy Zeng, Yinda Zhang 6 | 7 | Permission is hereby granted, free of charge, to any person obtaining a copy 8 | of this software and associated documentation files (the "Software"), to deal 9 | in the Software without restriction, including without limitation the rights 10 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 11 | copies of the Software, and to permit persons to whom the Software is 12 | furnished to do so, subject to the following conditions: 13 | 14 | The above copyright notice and this permission notice shall be included in all 15 | copies or substantial portions of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 22 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 23 | SOFTWARE. 24 | -------------------------------------------------------------------------------- /src/segmentation_proc/launch/segmentation_proc.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /src/segmentation_proc/package.xml: -------------------------------------------------------------------------------- 1 | 2 | segmentation_proc 3 | 0.0.1 4 | Segmentation Processing 5 | Ji Zhang 6 | MIT 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/segmentation_proc/scripts/compatible_np.py: -------------------------------------------------------------------------------- 1 | 2 | # The following is for compatibility issue with PyTorch 1.8 on 3 | # Jetson Xavier NX with Jetpack 4.6, Python 3.6. 4 | 5 | import numpy as np 6 | 7 | 8 | np.meshgrid( np.random.rand(3), np.random.rand(3), indexing='ij' ) 9 | 10 | # No error. 11 | def np_meshgrid(x, y, indexing='xy'): 12 | res = np.meshgrid(x, y, indexing=indexing) 13 | return [ np.ascontiguousarray(r) for r in res ] 14 | -------------------------------------------------------------------------------- /src/segmentation_proc/scripts/planar_as_base.py: -------------------------------------------------------------------------------- 1 | 2 | # Author: 3 | # Yaoyu Hu 4 | # Date: 5 | # 2021-05-06 6 | 7 | import copy 8 | import cv2 9 | import numpy as np 10 | 11 | INTER_MAP_OCV = { 12 | 'linear': cv2.INTER_LINEAR, 13 | 'nearest': cv2.INTER_NEAREST 14 | } 15 | 16 | INTER_MAP = { 17 | 'nearest': 'nearest', 18 | 'linear': 'bilinear', 19 | } 20 | 21 | INTER_BLENDED = 'blended' 22 | 23 | class PlanarAsBase(object): 24 | def __init__(self, 25 | fov, 26 | camera_model, 27 | cached_raw_shape=(1024, 2048), 28 | default_invalid_value=0): 29 | ''' 30 | NOTE: If convert_output=False, then the output is a Tensor WITH the batch dimension. 31 | That is, the output is a 4D Tensor no matter whether the input is a single image 32 | or a collection of images. 33 | 34 | Arguments: 35 | fov (float): Full FoV of the lens in degrees. 36 | camera_model (camera_model.CameraModel): Target camera model. 37 | cached_raw_shape (two-element): The tentative shape of the support raw image. Use some positive values if not sure. 38 | convert_output (bool): True if the output needs to be converted to NumPy (OpenCV). 39 | default_invalid_value (scalar): The default value for the invalid output pixels. 40 | ''' 41 | super(PlanarAsBase, self).__init__() 42 | 43 | self.fov = fov # Degree. 44 | 45 | self.camera_model = copy.deepcopy(camera_model) 46 | self.shape = self.camera_model.shape 47 | 48 | # The rotation matrix of the fisheye camera. 49 | # The notation is R__ or R__. 50 | # This rotation matrix is the orientation of the fisheye camera w.r.t 51 | # the frame where we take the raw images. And the orientation is measured 52 | # in the raw image frame. 53 | 54 | self.cached_raw_shape = cached_raw_shape 55 | 56 | self.default_invalid_value = default_invalid_value 57 | 58 | def is_same_as_cached_shape(self, new_shape): 59 | return new_shape[0] == self.cached_raw_shape[0] and new_shape[1] == self.cached_raw_shape[1] 60 | 61 | def get_xyz(self, back_shift_pixel=False): 62 | ''' 63 | Compute the ray vectors for all valid pixels in the fisheye image. 64 | A ray vector is represented as a unit vector. 65 | All ray vectors will be transformed such that their coordiantes are 66 | measured in the raw frame where z-forward, x-right, and y-downward. 67 | 68 | Some pixels are not going to have valid rays. There is a mask of valid 69 | pixels that is also returned by this function. 70 | 71 | Returns: 72 | xyz (Tensor): 3xN, where N is the number of pixels. 73 | valid_mask (Tensor): 1xN, where N is the number of pixels. A binary mask. 74 | ''' 75 | # The pixel coordinates. 76 | # # xx, yy = self.mesh_grid_pixels(self.shape, flag_flatten=True) # 1D. 77 | # xx, yy = self.camera_model.pixel_meshgrid( flatten=True ) 78 | 79 | # if back_shift_pixel: 80 | # xx -= 0.5 81 | # yy -= 0.5 82 | 83 | # pixel_coor = torch.stack( (xx, yy), dim=0 ) # 2xN 84 | 85 | # xyz, valid_mask = \ 86 | # self.camera_model.pixel_2_ray(pixel_coor) 87 | 88 | shift = 0 if back_shift_pixel else 0.5 89 | xyz, valid_mask = self.camera_model.get_rays_wrt_sensor_frame(shift=shift) 90 | 91 | # xyz and valid_mask are torch.Tensor. 92 | # xyz = xyz.astype(np.float32) 93 | 94 | return xyz, valid_mask -------------------------------------------------------------------------------- /src/segmentation_proc/scripts/radian.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | _LOCAL_2_PI = 2 * np.pi 5 | 6 | def check_valid_range(r0, r1, raise_exception=False): 7 | ''' 8 | Return True if: 9 | - r0 and r1 are in the range of [-2pi, 2pi] 10 | - r0 < r1 11 | - r1 - r0 <= 2pi 12 | ''' 13 | 14 | global _LOCAL_2_PI 15 | 16 | flag = True 17 | 18 | if r0 >= r1: 19 | flag = False 20 | reason = f'r0 ({r0}) >= r1 ({r1}). ' 21 | elif r0 < -_LOCAL_2_PI or r1 > _LOCAL_2_PI: 22 | flag = False 23 | reason = f'r0 ({r0}) or r1 ({r1}) has a wrong value. The value must be in [-2pi, 2pi]. ' 24 | elif r1 - r0 > _LOCAL_2_PI: 25 | flag = False 26 | reason = f'r1 ({r1}) - r0 ({r0}) = {r1-r0} > 2pi. ' 27 | 28 | if not flag and raise_exception: 29 | raise ValueError(reason) 30 | 31 | return flag -------------------------------------------------------------------------------- /src/segmentation_proc/scripts/register.py: -------------------------------------------------------------------------------- 1 | # Author: Yaoyu Hu 2 | # Date: 2022-07-31 3 | 4 | import copy 5 | 6 | SAMPLERS=dict() 7 | BLEND_FUNCTIONS=dict() 8 | 9 | def register(dst): 10 | '''Register a class to a dstination dictionary. ''' 11 | def dec_register(cls): 12 | dst[cls.__name__] = cls 13 | return cls 14 | return dec_register 15 | 16 | def make_object(typeD, argD): 17 | '''Make an object from type collection typeD. ''' 18 | 19 | assert( isinstance(typeD, dict) ), f'typeD must be dict. typeD is {type(typeD)}' 20 | assert( isinstance(argD, dict) ), f'argD must be dict. argD is {type(argD)}' 21 | 22 | # Make a deep copy of the input dict. 23 | d = copy.deepcopy(argD) 24 | 25 | # Get the type. 26 | typeName = typeD[ d['type'] ] 27 | 28 | # Remove the type string from the input dictionary. 29 | d.pop('type') 30 | 31 | # Create the model. 32 | return typeName( **d ) -------------------------------------------------------------------------------- /src/segmentation_proc/scripts/shape_struct.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | class ShapeStruct(object): 5 | def __init__(self, H, W, C=-1, **kwargs): 6 | super().__init__() 7 | 8 | self.H = H 9 | self.W = W 10 | self._C = C 11 | 12 | @property 13 | def shape(self): 14 | ''' 15 | This funtion is meant to be used with NumPy, PyTorch, etc. 16 | ''' 17 | return (self.H, self.W) 18 | 19 | @property 20 | def size(self): 21 | ''' 22 | This function is meant to be used with OpenCV APIs. 23 | ''' 24 | return (self.W, self.H) 25 | 26 | @property 27 | def shape_numpy(self): 28 | return np.array( [ self.H, self.W ], dtype=np.int32 ) 29 | 30 | @staticmethod 31 | def read_shape_struct(dict_like): 32 | ''' 33 | Read shape information from a dict-like object. 34 | ''' 35 | return ShapeStruct( **dict_like ) \ 36 | if not isinstance(dict_like, ShapeStruct) \ 37 | else dict_like 38 | 39 | @property 40 | def C(self): 41 | return self._C 42 | 43 | def __str__(self) -> str: 44 | return f'{{ "H": {self.H}, "W": {self.W}, "C": {self.C} }}' 45 | 46 | def __repr__(self) -> str: 47 | return f'ShapeStruct(H={self.H}, W={self.W})' 48 | 49 | def __eq__(self, other): 50 | return self.H == other.H and self.W == other.W and self.C == other.C -------------------------------------------------------------------------------- /src/segmentation_proc/scripts/six_images_common.py: -------------------------------------------------------------------------------- 1 | 2 | import numpy as np 3 | 4 | # --> +---------+ 5 | # | x |0,W |0,2W 6 | # v | 4 | 7 | # y | top | 8 | # +---------+---------+---------+---------+ 9 | # |H,0 |H,W |H,2W |H,3W |H,4W 10 | # | 3 | 0 | 1 | 5 | 11 | # | left | front | right | back | 12 | # +---------+---------+---------+---------+ 13 | # 2H,0 |2H,W |2H,2W 2H,3W 2H,4W 14 | # | 2 | 15 | # | bottom | 16 | # +---------+ 17 | # 3H,W 3H,2W 18 | 19 | FRONT = 'front' 20 | BACK = 'back' 21 | LEFT = 'left' 22 | RIGHT = 'right' 23 | TOP = 'top' 24 | BOTTOM = 'bottom' 25 | 26 | OFFSETS = offsets=np.array( 27 | [ [1, 2, 1, 0, 1, 3], 28 | [1, 1, 2, 1, 0, 1] ], dtype=np.int32) 29 | 30 | def make_image_cross_npy(imgs): 31 | ''' 32 | Arguments: 33 | imgs (dict of arrays): The six images with the keys as front, back, left, right, top, and bottom. 34 | 35 | Returns: 36 | A image cross with shape (3*H, 4*W). 37 | ''' 38 | 39 | global FRONT, BACK, LEFT, RIGHT, TOP, BOTTOM 40 | 41 | H, W = imgs[FRONT].shape[:2] 42 | d_type = imgs[FRONT].dtype 43 | 44 | if ( imgs[FRONT].ndim == 3 ): 45 | # Get the last dimension of the input image. 46 | last_dim = imgs[FRONT].shape[2] 47 | canvas = np.zeros( ( 3*H, 4*W, last_dim ), dtype=d_type ) 48 | elif ( imgs[FRONT].ndim == 2 ): 49 | canvas = np.zeros( ( 3*H, 4*W ), dtype=d_type ) 50 | else: 51 | raise Exception(f'Wrong dimension of the input images. imgs[FRONT].shape = {imgs[FRONT].shape}') 52 | 53 | canvas[ H:2*H, W:2*W, ...] = imgs[FRONT] # Front. 54 | canvas[ H:2*H, 2*W:3*W, ...] = imgs[RIGHT] # Right. 55 | canvas[2*H:3*H, W:2*W, ...] = imgs[BOTTOM] # Bottom. 56 | canvas[ H:2*H, 0:W, ...] = imgs[LEFT] # Left. 57 | canvas[ 0:H, W:2*W, ...] = imgs[TOP] # Top. 58 | canvas[ H:2*H, 3*W:4*W, ...] = imgs[BACK] # Top. 59 | 60 | # Padding. 61 | # Right. 62 | canvas[ H-1, 2*W:3*W, ... ] = imgs[TOP][ ::-1, -1, ... ] 63 | canvas[ 2*H, 2*W:3*W, ... ] = imgs[BOTTOM][ :, -1, ... ] 64 | # Bottom. 65 | canvas[ 2*H:3*H, 2*W, ... ] = imgs[RIGHT][ -1, :, ... ] 66 | canvas[ 2*H:3*H, W-1, ... ] = imgs[LEFT][ -1, ::-1, ... ] 67 | # Left. 68 | canvas[ H-1, 0:W, ... ] = imgs[TOP][ :, 0, ...] 69 | canvas[ 2*H, 0:W, ... ] = imgs[BOTTOM][ ::-1, 0, ...] 70 | # Top. 71 | canvas[ 0:H, W-1, ... ] = imgs[LEFT][ 0, :, ...] 72 | canvas[ 0:H, 2*W, ... ] = imgs[RIGHT][ 0, ::-1, ...] 73 | # Back. 74 | canvas[ H-1, 3*W:4*W, ... ] = imgs[TOP][ 0, ::-1, ... ] 75 | canvas[ 2*H, 3*W:4*W, ... ] = imgs[BOTTOM][ -1, ::-1, ... ] 76 | 77 | return canvas 78 | -------------------------------------------------------------------------------- /src/segmentation_proc/scripts/six_images_numba.py: -------------------------------------------------------------------------------- 1 | 2 | import math 3 | from numba import jit 4 | import numpy as np 5 | 6 | from six_images_common import OFFSETS 7 | 8 | @jit(nopython=True) 9 | def sample_coor(xyz, 10 | valid_mask, 11 | offsets=OFFSETS): 12 | output = np.zeros( ( 2, xyz.shape[1] ), dtype=np.float32 ) 13 | out_offsets = np.zeros_like(output) 14 | 15 | one_fourth_pi = np.pi / 4 16 | half_pi = np.pi / 2 17 | three_fourth_pi = one_fourth_pi + half_pi 18 | 19 | for i in range(xyz.shape[1]): 20 | # already rotated to the raw frame, z-forward, x-right, y-downwards. 21 | x = xyz[0, i] 22 | y = xyz[1, i] 23 | z = xyz[2, i] 24 | 25 | a_y = math.atan2(x, y) # Angle w.r.t. y+ axis projected to the x-y plane. 26 | a_z = math.atan2(z, y) # Angle w.r.t. y+ axis projected to the y-z plane. 27 | azimuth = math.atan2(z, x) # Angle w.r.t. x+ axis projected to the z-x plane. 28 | 29 | if ( -one_fourth_pi < a_y and a_y < one_fourth_pi and \ 30 | -one_fourth_pi < a_z and a_z < one_fourth_pi ): 31 | # Bottom. 32 | output[0, i] = min( max( ( 1 + x/y ) / 2, 0 ), 1 ) 33 | output[1, i] = min( max( ( 1 - z/y ) / 2, 0 ), 1 ) 34 | out_offsets[0, i] = offsets[0][2] 35 | out_offsets[1, i] = offsets[1][2] 36 | elif ( (three_fourth_pi < a_y or a_y < -three_fourth_pi) and \ 37 | (three_fourth_pi < a_z or a_z < -three_fourth_pi) ): 38 | # Top. 39 | output[0, i] = min( max( ( 1 - x/y ) / 2, 0 ), 1 ) 40 | output[1, i] = min( max( ( 1 - z/y ) / 2, 0 ), 1 ) 41 | out_offsets[0, i] = offsets[0][4] 42 | out_offsets[1, i] = offsets[1][4] 43 | elif ( one_fourth_pi <= azimuth and azimuth < three_fourth_pi ): 44 | # Front. 45 | output[0, i] = min( max( ( 1 + x/z ) / 2, 0 ), 1 ) 46 | output[1, i] = min( max( ( 1 + y/z ) / 2, 0 ), 1 ) 47 | out_offsets[0, i] = offsets[0][0] 48 | out_offsets[1, i] = offsets[1][0] 49 | elif ( -one_fourth_pi <= azimuth and azimuth < one_fourth_pi ): 50 | # Right. 51 | output[0, i] = min( max( ( 1 - z/x ) / 2, 0 ), 1 ) 52 | output[1, i] = min( max( ( 1 + y/x ) / 2, 0 ), 1 ) 53 | out_offsets[0, i] = offsets[0][1] 54 | out_offsets[1, i] = offsets[1][1] 55 | elif ( -three_fourth_pi <= azimuth and azimuth < -one_fourth_pi ): 56 | # Back. 57 | output[0, i] = min( max( ( 1 + x/z ) / 2, 0 ), 1 ) 58 | output[1, i] = min( max( ( 1 - y/z ) / 2, 0 ), 1 ) 59 | out_offsets[0, i] = offsets[0][5] 60 | out_offsets[1, i] = offsets[1][5] 61 | elif ( three_fourth_pi <= azimuth or azimuth < -three_fourth_pi ): 62 | # Left. 63 | output[0, i] = min( max( ( 1 - z/x ) / 2, 0 ), 1 ) 64 | output[1, i] = min( max( ( 1 - y/x ) / 2, 0 ), 1 ) 65 | out_offsets[0, i] = offsets[0][3] 66 | out_offsets[1, i] = offsets[1][3] 67 | else: 68 | raise Exception('xy invalid.') 69 | # raise Exception(f'x = {x}, y = {y}, z = {z}, a_y = {a_y}, a_z = {a_z}, one_fourth_pi = {one_fourth_pi}, half_pi = {half_pi}') 70 | 71 | output[:, np.logical_not(valid_mask)] = -1 72 | 73 | return output, out_offsets 74 | -------------------------------------------------------------------------------- /src/segmentation_proc/scripts/six_images_py_numba.py: -------------------------------------------------------------------------------- 1 | # Author: 2 | # Yaoyu Hu 3 | # Date: 4 | # 2022-06-17 5 | # 2022-12-26 6 | 7 | import cv2 8 | import numpy as np 9 | 10 | # Local package. 11 | from planar_as_base import ( PlanarAsBase, INTER_MAP_OCV, INTER_MAP ) 12 | from register import (SAMPLERS, register) 13 | 14 | from six_images_common import (FRONT, make_image_cross_npy) 15 | from six_images_numba import ( sample_coor) 16 | 17 | @register(SAMPLERS) 18 | class SixPlanarNumba(PlanarAsBase): 19 | def __init__(self, camera_model, cached_raw_shape=(640, 640)): 20 | ''' 21 | Arguments: 22 | fov (float): Full FoV of the lens in degrees. 23 | camera_model: A camera model for the fisheye camera. 24 | ''' 25 | super().__init__( 26 | camera_model.fov_degree, 27 | camera_model=camera_model, 28 | cached_raw_shape=cached_raw_shape) 29 | 30 | # The 3D coordinates of the hyper-surface. 31 | # xyz, valid_mask = self.get_xyz(back_shift_pixel=True) 32 | xyz, valid_mask = self.get_xyz(back_shift_pixel=False) 33 | self.xyz = xyz 34 | self.valid_mask = valid_mask.reshape(self.shape).astype(bool) 35 | 36 | # Explicity set the device to 'cuda' for better speed during construction. 37 | # Specifically, the call to self.update_remap_coordinates(). 38 | 39 | # The remap coordinates. 40 | self.mx = None 41 | self.my = None 42 | self.update_remap_coordinates( self.cached_raw_shape ) 43 | 44 | 45 | def __repr__(self): 46 | s = ( f'SixPlanar\n' 47 | f'fov = {self.fov}\n' 48 | f'shape = {self.shape}' ) 49 | 50 | return s 51 | 52 | def update_remap_coordinates(self, support_shape): 53 | H, W = support_shape 54 | 55 | if not isinstance(support_shape, np.ndarray): 56 | support_shape = np.array( [H, W], dtype=np.float32 ).reshape((2, 1)) 57 | 58 | # Get the sample locations. 59 | m, offsets = sample_coor(self.xyz, self.valid_mask.reshape((-1,))) 60 | 61 | # We need to properly scale the dimensionless values in m to use cv2.remap(). 62 | # Refer to self.convert_dimensionless_torch_grid_2_ocv_remap_format() for consistency. 63 | m[0, :] = W / ( W - 0.5 ) * ( m[0, :] - 0.5 ) + 0.5 64 | m[1, :] = H / ( H - 0.5 ) * ( m[1, :] - 0.5 ) + 0.5 65 | 66 | m = m * ( support_shape - 1 ) + offsets * support_shape 67 | 68 | self.mx = m[0, :].reshape(self.shape) 69 | self.my = m[1, :].reshape(self.shape) 70 | 71 | def check_shape_and_make_image_cross(self, imgs): 72 | global FRONT 73 | 74 | # Get the original shape of the input images. 75 | img_shape = np.array(imgs[FRONT].shape[:2], dtype=np.float32).reshape((2, 1)) 76 | 77 | # Check the input shape. 78 | if not self.is_same_as_cached_shape( img_shape ): 79 | self.update_remap_coordinates( img_shape ) 80 | self.cached_raw_shape = img_shape 81 | 82 | # Make the image cross. 83 | img_cross = make_image_cross_npy( imgs ) 84 | # cv2.imwrite('img_cross.png', img_cross) 85 | 86 | return img_cross 87 | 88 | def __call__(self, imgs, interpolation='linear', invalid_pixel_value=127): 89 | ''' 90 | Arguments: 91 | imgs (list of arrays): The six images in the order of front, right, bottom, left, top, and back. 92 | interpolation (str): The interpolation method, could be linear or nearest. 93 | invalid_pixel_value (int): The value of the invalid pixel. For RGB images, it is normally 127. For depth, it is -1. 94 | 95 | Returns: 96 | The generated fisheye image. 97 | ''' 98 | 99 | global INTER_MAP_OCV 100 | 101 | img_cross = self.check_shape_and_make_image_cross(imgs) 102 | 103 | # Get the interpolation method. 104 | interp_method = INTER_MAP_OCV[interpolation] 105 | 106 | # Sample. 107 | sampled = cv2.remap( 108 | img_cross, 109 | self.mx, self.my, 110 | interpolation=interp_method ) 111 | 112 | # Apply gray color on invalid coordinates. 113 | invalid = np.logical_not(self.valid_mask) 114 | sampled[invalid, ...] = invalid_pixel_value 115 | 116 | return sampled, self.valid_mask 117 | 118 | def blend_interpolation(self, imgs, blend_func, invalid_pixel_value=127): 119 | ''' 120 | This function blends the results of linear interpolation and nearest neighbor interpolation. 121 | The user is supposed to provide a callable object, blend_func, which takes in img and produces 122 | a blending factor. The blending factor is a float number between 0 and 1. 1 means only nearest. 123 | ''' 124 | 125 | img_cross = self.check_shape_and_make_image_cross(imgs) 126 | 127 | # Sample. 128 | sampled_linear = cv2.remap( 129 | img_cross, 130 | self.mx, self.my, 131 | interpolation=cv2.INTER_LINEAR ) 132 | 133 | sampled_nearest = cv2.remap( 134 | img_cross, 135 | self.mx, self.my, 136 | interpolation=cv2.INTER_NEAREST ) 137 | 138 | # Blend factor. 139 | f = blend_func(img_cross) 140 | 141 | # Sample from the blend factor. 142 | f = cv2.remap( 143 | f, 144 | self.mx, self.my, 145 | interpolation=cv2.INTER_NEAREST ) 146 | 147 | sampled = f * sampled_nearest.astype(np.float32) + (1 - f) * sampled_linear.astype(np.float32) 148 | 149 | # Apply gray color on invalid coordinates. 150 | invalid = np.logical_not(self.valid_mask) 151 | sampled[invalid, ...] = invalid_pixel_value 152 | 153 | return sampled, self.valid_mask 154 | -------------------------------------------------------------------------------- /src/semantic_scan_generation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_scan_generation) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | 8 | ## Find catkin macros and libraries 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | cv_bridge 14 | pcl_ros 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | find_package(Eigen3 REQUIRED) 20 | 21 | ################################### 22 | ## catkin specific configuration ## 23 | ################################### 24 | 25 | catkin_package( 26 | CATKIN_DEPENDS 27 | roscpp 28 | std_msgs 29 | sensor_msgs 30 | cv_bridge 31 | pcl_ros 32 | ) 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | 38 | ## Specify additional locations of header files 39 | ## Your package locations should be listed before other locations 40 | include_directories( 41 | ${catkin_INCLUDE_DIRS} 42 | ${OpenCV_INCLUDE_DIRS} 43 | ${PCL_INCLUDE_DIRS} 44 | ${EIGEN3_INCLUDE_DIR} 45 | "${PROJECT_SOURCE_DIR}/include" 46 | /usr/local/include # Location when using 'make system_install' 47 | /usr/include # More usual location (e.g. when installing using a package) 48 | ) 49 | 50 | ## Specify additional locations for library files 51 | link_directories( 52 | /usr/local/lib # Location when using 'make system_install' 53 | /usr/lib # More usual location (e.g. when installing using a package) 54 | ) 55 | 56 | ## Declare executables 57 | add_executable(semanticScanGeneration src/semanticScanGeneration.cpp) 58 | 59 | ## Specify libraries to link a library or executable target against 60 | target_link_libraries(semanticScanGeneration ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 61 | 62 | install(TARGETS semanticScanGeneration 63 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 65 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 66 | ) 67 | 68 | install(DIRECTORY launch/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 70 | ) 71 | -------------------------------------------------------------------------------- /src/semantic_scan_generation/launch/semantic_scan_generation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/semantic_scan_generation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_scan_generation 3 | 0.0.1 4 | Semantic Scan Generation 5 | Ji Zhang 6 | BSD 7 | 8 | Ji Zhang 9 | 10 | catkin 11 | 12 | roscpp 13 | std_msgs 14 | sensor_msgs 15 | cv_bridge 16 | pcl_ros 17 | 18 | roscpp 19 | std_msgs 20 | sensor_msgs 21 | cv_bridge 22 | pcl_ros 23 | 24 | -------------------------------------------------------------------------------- /src/semantic_scan_generation/src/semanticScanGeneration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace std; 27 | using namespace cv; 28 | 29 | const double PI = 3.1415926; 30 | 31 | double cameraOffsetZ = 0; 32 | 33 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 34 | pcl::PointCloud::Ptr laserCloudSeg(new pcl::PointCloud()); 35 | 36 | const int stackNum = 400; 37 | float lidarXStack[stackNum]; 38 | float lidarYStack[stackNum]; 39 | float lidarZStack[stackNum]; 40 | float lidarRollStack[stackNum]; 41 | float lidarPitchStack[stackNum]; 42 | float lidarYawStack[stackNum]; 43 | double odomTimeStack[stackNum]; 44 | int odomIDPointer = -1; 45 | int imageIDPointer = 0; 46 | 47 | bool imageInit = false; 48 | double imageTime = 0; 49 | 50 | bool newLaserCloud = false; 51 | double laserCloudTime = 0; 52 | 53 | ros::Publisher *pubLaserCloudPointer = NULL; 54 | cv_bridge::CvImageConstPtr segImageCv; 55 | 56 | void odomHandler(const nav_msgs::Odometry::ConstPtr& odom) 57 | { 58 | double roll, pitch, yaw; 59 | geometry_msgs::Quaternion geoQuat = odom->pose.pose.orientation; 60 | tf::Matrix3x3(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w)).getRPY(roll, pitch, yaw); 61 | 62 | odomIDPointer = (odomIDPointer + 1) % stackNum; 63 | odomTimeStack[odomIDPointer] = odom->header.stamp.toSec(); 64 | lidarXStack[odomIDPointer] = odom->pose.pose.position.x; 65 | lidarYStack[odomIDPointer] = odom->pose.pose.position.y; 66 | lidarZStack[odomIDPointer] = odom->pose.pose.position.z; 67 | lidarRollStack[odomIDPointer] = roll; 68 | lidarPitchStack[odomIDPointer] = pitch; 69 | lidarYawStack[odomIDPointer] = yaw; 70 | } 71 | 72 | void semImageHandler(const sensor_msgs::ImageConstPtr& image) 73 | { 74 | imageTime = image->header.stamp.toSec(); 75 | segImageCv = cv_bridge::toCvShare(image, "bgr8"); 76 | 77 | imageInit = true; 78 | } 79 | 80 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn) 81 | { 82 | laserCloudTime = laserCloudIn->header.stamp.toSec(); 83 | 84 | laserCloud->clear(); 85 | pcl::fromROSMsg(*laserCloudIn, *laserCloud); 86 | 87 | newLaserCloud = true; 88 | } 89 | 90 | int main(int argc, char** argv) 91 | { 92 | ros::init(argc, argv, "semanticScanGeneration"); 93 | ros::NodeHandle nh; 94 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 95 | 96 | nhPrivate.getParam("cameraOffsetZ", cameraOffsetZ); 97 | 98 | ros::Subscriber subOdom = nh.subscribe ("/state_estimation", 50, odomHandler); 99 | 100 | ros::Subscriber subSegImage = nh.subscribe ("/camera/semantic_image", 2, semImageHandler); 101 | 102 | ros::Subscriber subLaserCloud = nh.subscribe ("/registered_scan", 2, laserCloudHandler); 103 | 104 | ros::Publisher pubLaserCloud = nh.advertise ("/semantic_scan", 2); 105 | pubLaserCloudPointer = &pubLaserCloud; 106 | 107 | ros::Rate rate(200); 108 | bool status = ros::ok(); 109 | while (status) 110 | { 111 | ros::spinOnce(); 112 | 113 | if (imageInit && newLaserCloud) { 114 | newLaserCloud = false; 115 | 116 | int laserCloudSize = laserCloud->points.size(); 117 | if (laserCloudSize <= 0) continue; 118 | 119 | if (odomIDPointer < 0) continue; 120 | while (odomTimeStack[imageIDPointer] < imageTime - 0.001 && 121 | imageIDPointer != (odomIDPointer + 1) % stackNum) { 122 | imageIDPointer = (imageIDPointer + 1) % stackNum; 123 | } 124 | if (fabs(odomTimeStack[imageIDPointer] - imageTime) > 0.001) continue; 125 | 126 | float lidarX = lidarXStack[imageIDPointer]; 127 | float lidarY = lidarYStack[imageIDPointer]; 128 | float lidarZ = lidarZStack[imageIDPointer]; 129 | float lidarRoll = lidarRollStack[imageIDPointer]; 130 | float lidarPitch = lidarPitchStack[imageIDPointer]; 131 | float lidarYaw = lidarYawStack[imageIDPointer]; 132 | 133 | int imageWidth = segImageCv->image.size().width; 134 | int imageHeight = segImageCv->image.size().height; 135 | 136 | float sinLidarRoll = sin(lidarRoll); 137 | float cosLidarRoll = cos(lidarRoll); 138 | float sinLidarPitch = sin(lidarPitch); 139 | float cosLidarPitch = cos(lidarPitch); 140 | float sinLidarYaw = sin(lidarYaw); 141 | float cosLidarYaw = cos(lidarYaw); 142 | 143 | pcl::PointXYZRGB point; 144 | laserCloudSeg->clear(); 145 | for (int i = 0; i < laserCloudSize; i++) { 146 | float x1 = laserCloud->points[i].x - lidarX; 147 | float y1 = laserCloud->points[i].y - lidarY; 148 | float z1 = laserCloud->points[i].z - lidarZ; 149 | 150 | float x2 = x1 * cosLidarYaw + y1 * sinLidarYaw; 151 | float y2 = -x1 * sinLidarYaw + y1 * cosLidarYaw; 152 | float z2 = z1; 153 | 154 | float x3 = x2 * cosLidarPitch - z2 * sinLidarPitch; 155 | float y3 = y2; 156 | float z3 = x2 * sinLidarPitch + z2 * cosLidarPitch; 157 | 158 | float x4 = x3; 159 | float y4 = y3 * cosLidarRoll + z3 * sinLidarRoll; 160 | float z4 = -y3 * sinLidarRoll + z3 * cosLidarRoll - cameraOffsetZ; 161 | 162 | float horiDis = sqrt(x4 * x4 + y4 * y4); 163 | int horiPixelID = -imageWidth / (2 * PI) * atan2(y4, x4) + imageWidth / 2 + 1; 164 | int vertPixelID = -imageWidth / (2 * PI) * atan(z4 / horiDis) + imageHeight / 2 + 1; 165 | int pixelID = imageWidth * vertPixelID + horiPixelID; 166 | 167 | if (horiPixelID >= 0 && horiPixelID < imageWidth && vertPixelID >= 0 && vertPixelID < imageHeight) { 168 | point.x = laserCloud->points[i].x; 169 | point.y = laserCloud->points[i].y; 170 | point.z = laserCloud->points[i].z; 171 | point.b = segImageCv->image.data[3 * pixelID]; 172 | point.g = segImageCv->image.data[3 * pixelID + 1]; 173 | point.r = segImageCv->image.data[3 * pixelID + 2]; 174 | 175 | laserCloudSeg->push_back(point); 176 | } 177 | } 178 | 179 | sensor_msgs::PointCloud2 laserCloudOut; 180 | pcl::toROSMsg(*laserCloudSeg, laserCloudOut); 181 | laserCloudOut.header.stamp = ros::Time().fromSec(laserCloudTime); 182 | laserCloudOut.header.frame_id = "map"; 183 | pubLaserCloudPointer->publish(laserCloudOut); 184 | } 185 | 186 | status = ros::ok(); 187 | rate.sleep(); 188 | } 189 | 190 | return 0; 191 | } 192 | -------------------------------------------------------------------------------- /src/sensor_scan_generation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sensor_scan_generation) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | 8 | ## Find catkin macros and libraries 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | pcl_ros 14 | ) 15 | 16 | find_package(PCL REQUIRED) 17 | find_package(Eigen3 REQUIRED) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | std_msgs 27 | sensor_msgs 28 | pcl_ros 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | ## Specify additional locations of header files 36 | ## Your package locations should be listed before other locations 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${PCL_INCLUDE_DIRS} 40 | ${EIGEN3_INCLUDE_DIR} 41 | "${PROJECT_SOURCE_DIR}/include" 42 | /usr/local/include # Location when using 'make system_install' 43 | /usr/include # More usual location (e.g. when installing using a package) 44 | ) 45 | 46 | ## Specify additional locations for library files 47 | link_directories( 48 | /usr/local/lib # Location when using 'make system_install' 49 | /usr/lib # More usual location (e.g. when installing using a package) 50 | ) 51 | 52 | 53 | ## Declare executables 54 | add_executable(sensorScanGeneration src/sensorScanGeneration.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(sensorScanGeneration ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS sensorScanGeneration 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | -------------------------------------------------------------------------------- /src/sensor_scan_generation/launch/sensor_scan_generation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/sensor_scan_generation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | sensor_scan_generation 3 | 0.0.1 4 | sensor_scan_generation is a package to generate scan data in sensor frame with registered_scan in map frame. 5 | Hongbiao Zhu 6 | BSD 7 | 8 | Hongbiao Zhu 9 | 10 | catkin 11 | 12 | roscpp 13 | std_msgs 14 | sensor_msgs 15 | pcl_ros 16 | 17 | roscpp 18 | std_msgs 19 | sensor_msgs 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/sensor_scan_generation/src/sensorScanGeneration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | pcl::PointCloud::Ptr laserCloudIn(new pcl::PointCloud()); 24 | pcl::PointCloud::Ptr laserCLoudInSensorFrame(new pcl::PointCloud()); 25 | 26 | double robotX = 0; 27 | double robotY = 0; 28 | double robotZ = 0; 29 | double roll = 0; 30 | double pitch = 0; 31 | double yaw = 0; 32 | 33 | bool newTransformToMap = false; 34 | 35 | nav_msgs::Odometry odometryIn; 36 | ros::Publisher *pubOdometryPointer = NULL; 37 | tf::StampedTransform transformToMap; 38 | tf::TransformBroadcaster *tfBroadcasterPointer = NULL; 39 | 40 | ros::Publisher pubLaserCloud; 41 | 42 | void laserCloudAndOdometryHandler(const nav_msgs::Odometry::ConstPtr& odometry, 43 | const sensor_msgs::PointCloud2ConstPtr& laserCloud2) 44 | { 45 | laserCloudIn->clear(); 46 | laserCLoudInSensorFrame->clear(); 47 | 48 | pcl::fromROSMsg(*laserCloud2, *laserCloudIn); 49 | 50 | odometryIn = *odometry; 51 | 52 | transformToMap.setOrigin( 53 | tf::Vector3(odometryIn.pose.pose.position.x, odometryIn.pose.pose.position.y, odometryIn.pose.pose.position.z)); 54 | transformToMap.setRotation(tf::Quaternion(odometryIn.pose.pose.orientation.x, odometryIn.pose.pose.orientation.y, 55 | odometryIn.pose.pose.orientation.z, odometryIn.pose.pose.orientation.w)); 56 | 57 | int laserCloudInNum = laserCloudIn->points.size(); 58 | 59 | pcl::PointXYZ p1; 60 | tf::Vector3 vec; 61 | 62 | for (int i = 0; i < laserCloudInNum; i++) 63 | { 64 | p1 = laserCloudIn->points[i]; 65 | vec.setX(p1.x); 66 | vec.setY(p1.y); 67 | vec.setZ(p1.z); 68 | 69 | vec = transformToMap.inverse() * vec; 70 | 71 | p1.x = vec.x(); 72 | p1.y = vec.y(); 73 | p1.z = vec.z(); 74 | 75 | laserCLoudInSensorFrame->points.push_back(p1); 76 | } 77 | 78 | odometryIn.header.stamp = laserCloud2->header.stamp; 79 | odometryIn.header.frame_id = "map"; 80 | odometryIn.child_frame_id = "sensor_at_scan"; 81 | pubOdometryPointer->publish(odometryIn); 82 | 83 | transformToMap.stamp_ = laserCloud2->header.stamp; 84 | transformToMap.frame_id_ = "map"; 85 | transformToMap.child_frame_id_ = "sensor_at_scan"; 86 | tfBroadcasterPointer->sendTransform(transformToMap); 87 | 88 | sensor_msgs::PointCloud2 scan_data; 89 | pcl::toROSMsg(*laserCLoudInSensorFrame, scan_data); 90 | scan_data.header.stamp = laserCloud2->header.stamp; 91 | scan_data.header.frame_id = "sensor_at_scan"; 92 | pubLaserCloud.publish(scan_data); 93 | } 94 | 95 | int main(int argc, char** argv) 96 | { 97 | ros::init(argc, argv, "sensor_scan"); 98 | ros::NodeHandle nh; 99 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 100 | 101 | // ROS message filters 102 | message_filters::Subscriber subOdometry; 103 | message_filters::Subscriber subLaserCloud; 104 | typedef message_filters::sync_policies::ApproximateTime syncPolicy; 105 | typedef message_filters::Synchronizer Sync; 106 | boost::shared_ptr sync_; 107 | subOdometry.subscribe(nh, "/state_estimation", 1); 108 | subLaserCloud.subscribe(nh, "/registered_scan", 1); 109 | sync_.reset(new Sync(syncPolicy(100), subOdometry, subLaserCloud)); 110 | sync_->registerCallback(boost::bind(laserCloudAndOdometryHandler, _1, _2)); 111 | 112 | ros::Publisher pubOdometry = nh.advertise ("/state_estimation_at_scan", 5); 113 | pubOdometryPointer = &pubOdometry; 114 | 115 | tf::TransformBroadcaster tfBroadcaster; 116 | tfBroadcasterPointer = &tfBroadcaster; 117 | 118 | pubLaserCloud = nh.advertise("/sensor_scan", 2); 119 | 120 | ros::spin(); 121 | 122 | return 0; 123 | } 124 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(teleop_rviz_plugin) 3 | find_package(catkin REQUIRED COMPONENTS rviz) 4 | catkin_package() 5 | include_directories(${catkin_INCLUDE_DIRS}) 6 | link_directories(${catkin_LIBRARY_DIRS}) 7 | 8 | set(CMAKE_AUTOMOC ON) 9 | 10 | if(rviz_QT_VERSION VERSION_LESS "5") 11 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 12 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 13 | include(${QT_USE_FILE}) 14 | else() 15 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 16 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 17 | set(QT_LIBRARIES Qt5::Widgets) 18 | endif() 19 | 20 | add_definitions(-DQT_NO_KEYWORDS) 21 | 22 | set(SRC_FILES 23 | src/drive_widget.cpp 24 | src/teleop_panel.cpp 25 | ) 26 | 27 | add_library(${PROJECT_NAME} ${SRC_FILES}) 28 | 29 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 30 | 31 | install(TARGETS 32 | ${PROJECT_NAME} 33 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 36 | ) 37 | 38 | install(FILES 39 | plugin_description.xml 40 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 41 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | teleop_rviz_plugin 3 | 0.0.1 4 | 5 | Teleop RVIZ Plugin 6 | 7 | Ji Zhang 8 | BSD 9 | 10 | catkin 11 | 12 | qtbase5-dev 13 | rviz 14 | 15 | libqt5-core 16 | libqt5-gui 17 | libqt5-widgets 18 | rviz 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel for driving diff-drive style robot base control. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/drive_widget.cpp: -------------------------------------------------------------------------------- 1 | #include "drive_widget.h" 2 | 3 | namespace teleop_rviz_plugin 4 | { 5 | 6 | DriveWidget::DriveWidget( QWidget* parent ) 7 | : QWidget( parent ) 8 | , linear_velocity_( 0 ) 9 | , angular_velocity_( 0 ) 10 | , linear_scale_( 1 ) 11 | , angular_scale_( 1 ) 12 | , x_mouse_( 0 ) 13 | , y_mouse_( 0 ) 14 | , mouse_pressed_(false) 15 | { 16 | } 17 | 18 | void DriveWidget::paintEvent( QPaintEvent* event ) 19 | { 20 | QColor background; 21 | QColor crosshair; 22 | if( isEnabled() ) 23 | { 24 | background = Qt::white; 25 | crosshair = Qt::black; 26 | } 27 | else 28 | { 29 | background = Qt::lightGray; 30 | crosshair = Qt::darkGray; 31 | } 32 | 33 | int w = width(); 34 | int h = height(); 35 | int size = (( w > h ) ? h : w) - 1; 36 | int hpad = ( w - size ) / 2; 37 | int vpad = ( h - size ) / 2; 38 | 39 | QPainter painter( this ); 40 | painter.setBrush( background ); 41 | painter.setPen( crosshair ); 42 | 43 | painter.drawRect( QRect( hpad, vpad, size, size )); 44 | 45 | painter.drawLine( hpad, height() / 2, hpad + size, height() / 2 ); 46 | painter.drawLine( width() / 2, vpad, width() / 2, vpad + size ); 47 | 48 | if( isEnabled() && (angular_velocity_ != 0 || linear_velocity_ != 0 )) 49 | { 50 | QPen pen; 51 | pen.setWidth( size/150 ); 52 | pen.setColor( Qt::darkRed ); 53 | pen.setCapStyle( Qt::RoundCap ); 54 | pen.setJoinStyle( Qt::RoundJoin ); 55 | painter.setPen( pen ); 56 | 57 | QPointF joystick[ 2 ]; 58 | joystick[ 0 ].setX( w/2 ); 59 | joystick[ 0 ].setY( h/2 ); 60 | joystick[ 1 ].setX( x_mouse_ ); 61 | joystick[ 1 ].setY( y_mouse_ ); 62 | 63 | painter.drawPolyline( joystick, 2 ); 64 | painter.drawEllipse( x_mouse_ - 10, y_mouse_ - 10, 20, 20 ); 65 | } 66 | } 67 | 68 | void DriveWidget::mouseMoveEvent( QMouseEvent* event ) 69 | { 70 | sendVelocitiesFromMouse( event->x(), event->y(), width(), height() ); 71 | } 72 | 73 | void DriveWidget::mousePressEvent( QMouseEvent* event ) 74 | { 75 | sendVelocitiesFromMouse( event->x(), event->y(), width(), height() ); 76 | } 77 | 78 | void DriveWidget::leaveEvent( QEvent* event ) 79 | { 80 | stop(); 81 | } 82 | 83 | void DriveWidget::mouseReleaseEvent( QMouseEvent* event ) 84 | { 85 | stop(); 86 | } 87 | 88 | void DriveWidget::sendVelocitiesFromMouse( int x, int y, int width, int height ) 89 | { 90 | int size = (( width > height ) ? height : width ); 91 | int hpad = ( width - size ) / 2; 92 | int vpad = ( height - size ) / 2; 93 | 94 | x_mouse_ = x; 95 | if ( x_mouse_ < width / 2 - size / 2 ) x_mouse_ = width / 2 - size / 2; 96 | else if ( x_mouse_ > width / 2 + size / 2 ) x_mouse_ = width / 2 + size / 2; 97 | y_mouse_ = y; 98 | if ( y_mouse_ < height / 2 - size / 2 ) y_mouse_ = height / 2 - size / 2; 99 | else if ( y_mouse_ > height / 2 + size / 2 ) y_mouse_ = height / 2 + size / 2; 100 | 101 | linear_velocity_ = (1.0 - float( y - vpad ) / float( size / 2 )) * linear_scale_; 102 | if ( linear_velocity_ < -1.0 ) linear_velocity_ = -1.0; 103 | else if ( linear_velocity_ > 1.0 ) linear_velocity_ = 1.0; 104 | if ( fabs( linear_velocity_ ) < 0.1 ) linear_velocity_ = 0; 105 | angular_velocity_ = ( 1.0 - float( x - hpad ) / float( size / 2 )) * angular_scale_; 106 | if ( angular_velocity_ < -1.0 ) angular_velocity_ = -1.0; 107 | else if ( angular_velocity_ > 1.0 ) angular_velocity_ = 1.0; 108 | 109 | mouse_pressed_ = true; 110 | 111 | Q_EMIT outputVelocity( linear_velocity_, angular_velocity_, mouse_pressed_ ); 112 | update(); 113 | } 114 | 115 | void DriveWidget::stop() 116 | { 117 | linear_velocity_ = 0; 118 | angular_velocity_ = 0; 119 | mouse_pressed_ = false; 120 | 121 | Q_EMIT outputVelocity( linear_velocity_, angular_velocity_, mouse_pressed_ ); 122 | update(); 123 | } 124 | 125 | } 126 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/drive_widget.h: -------------------------------------------------------------------------------- 1 | #ifndef DRIVE_WIDGET_H 2 | #define DRIVE_WIDGET_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace teleop_rviz_plugin 13 | { 14 | 15 | class DriveWidget: public QWidget 16 | { 17 | Q_OBJECT 18 | public: 19 | DriveWidget( QWidget* parent = 0 ); 20 | virtual void paintEvent( QPaintEvent* event ); 21 | virtual void mouseMoveEvent( QMouseEvent* event ); 22 | virtual void mousePressEvent( QMouseEvent* event ); 23 | virtual void mouseReleaseEvent( QMouseEvent* event ); 24 | virtual void leaveEvent( QEvent* event ); 25 | virtual QSize sizeHint() const { return QSize( 150, 150 ); } 26 | 27 | Q_SIGNALS: 28 | void outputVelocity( float linear, float angular, bool pressed ); 29 | 30 | protected: 31 | void sendVelocitiesFromMouse( int x, int y, int width, int height ); 32 | void stop(); 33 | 34 | float linear_velocity_; 35 | float angular_velocity_; 36 | float linear_scale_; 37 | float angular_scale_; 38 | float x_mouse_, y_mouse_; 39 | bool mouse_pressed_; 40 | }; 41 | 42 | } 43 | 44 | #endif // DRIVE_WIDGET_H 45 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/teleop_panel.cpp: -------------------------------------------------------------------------------- 1 | #include "drive_widget.h" 2 | #include "teleop_panel.h" 3 | 4 | namespace teleop_rviz_plugin 5 | { 6 | 7 | TeleopPanel::TeleopPanel( QWidget* parent ) 8 | : rviz::Panel( parent ) 9 | , linear_velocity_( 0 ) 10 | , angular_velocity_( 0 ) 11 | , mouse_pressed_( false ) 12 | , mouse_pressed_sent_( false ) 13 | { 14 | QVBoxLayout* layout = new QVBoxLayout; 15 | push_button_1_ = new QPushButton( "Resume Navigation to Goal", this ); 16 | layout->addWidget( push_button_1_ ); 17 | drive_widget_ = new DriveWidget; 18 | layout->addWidget( drive_widget_ ); 19 | setLayout( layout ); 20 | 21 | QTimer* output_timer = new QTimer( this ); 22 | 23 | connect( push_button_1_, SIGNAL( pressed() ), this, SLOT( pressButton1() )); 24 | connect( drive_widget_, SIGNAL( outputVelocity( float, float, bool )), this, SLOT( setVel( float, float, bool ))); 25 | connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() )); 26 | 27 | output_timer->start( 100 ); 28 | 29 | velocity_publisher_ = nh_.advertise( "/joy", 5 ); 30 | drive_widget_->setEnabled( true ); 31 | } 32 | 33 | void TeleopPanel::pressButton1() 34 | { 35 | if ( ros::ok() && velocity_publisher_ ) 36 | { 37 | sensor_msgs::Joy joy; 38 | 39 | joy.axes.push_back(0); 40 | joy.axes.push_back(0); 41 | joy.axes.push_back(-1.0); 42 | joy.axes.push_back(0); 43 | joy.axes.push_back(0); 44 | joy.axes.push_back(1.0); 45 | joy.axes.push_back(0); 46 | joy.axes.push_back(0); 47 | 48 | joy.buttons.push_back(0); 49 | joy.buttons.push_back(0); 50 | joy.buttons.push_back(0); 51 | joy.buttons.push_back(0); 52 | joy.buttons.push_back(0); 53 | joy.buttons.push_back(0); 54 | joy.buttons.push_back(0); 55 | joy.buttons.push_back(1); 56 | joy.buttons.push_back(0); 57 | joy.buttons.push_back(0); 58 | joy.buttons.push_back(0); 59 | 60 | joy.header.stamp = ros::Time::now(); 61 | joy.header.frame_id = "teleop_panel"; 62 | velocity_publisher_.publish( joy ); 63 | } 64 | } 65 | 66 | void TeleopPanel::setVel( float lin, float ang, bool pre ) 67 | { 68 | linear_velocity_ = lin; 69 | angular_velocity_ = ang; 70 | mouse_pressed_ = pre; 71 | } 72 | 73 | void TeleopPanel::sendVel() 74 | { 75 | if( ros::ok() && velocity_publisher_ && ( mouse_pressed_ || mouse_pressed_sent_ )) 76 | { 77 | sensor_msgs::Joy joy; 78 | 79 | joy.axes.push_back( 0 ); 80 | joy.axes.push_back( 0 ); 81 | joy.axes.push_back( 1.0 ); 82 | joy.axes.push_back( angular_velocity_ ); 83 | joy.axes.push_back( linear_velocity_ ); 84 | joy.axes.push_back( 1.0 ); 85 | joy.axes.push_back( 0 ); 86 | joy.axes.push_back( 0 ); 87 | 88 | joy.buttons.push_back( 0 ); 89 | joy.buttons.push_back( 0 ); 90 | joy.buttons.push_back( 0 ); 91 | joy.buttons.push_back( 0 ); 92 | joy.buttons.push_back( 0 ); 93 | joy.buttons.push_back( 0 ); 94 | joy.buttons.push_back( 0 ); 95 | joy.buttons.push_back( 1 ); 96 | joy.buttons.push_back( 0 ); 97 | joy.buttons.push_back( 0 ); 98 | joy.buttons.push_back( 0 ); 99 | 100 | joy.header.stamp = ros::Time::now(); 101 | joy.header.frame_id = "teleop_panel"; 102 | velocity_publisher_.publish( joy ); 103 | 104 | mouse_pressed_sent_ = mouse_pressed_; 105 | } 106 | } 107 | 108 | } 109 | #include 110 | PLUGINLIB_EXPORT_CLASS( teleop_rviz_plugin::TeleopPanel,rviz::Panel ) 111 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/teleop_panel.h: -------------------------------------------------------------------------------- 1 | #ifndef TELEOP_PANEL_H 2 | #define TELEOP_PANEL_H 3 | 4 | #ifndef Q_MOC_RUN 5 | # include 6 | 7 | # include 8 | #endif 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | class QLineEdit; 26 | 27 | namespace teleop_rviz_plugin 28 | { 29 | 30 | class DriveWidget; 31 | 32 | class TeleopPanel: public rviz::Panel 33 | { 34 | Q_OBJECT 35 | public: 36 | TeleopPanel( QWidget* parent = 0 ); 37 | 38 | public Q_SLOTS: 39 | void setVel( float linear_velocity_, float angular_velocity_, bool mouse_pressed_ ); 40 | 41 | protected Q_SLOTS: 42 | void pressButton1(); 43 | void sendVel(); 44 | 45 | protected: 46 | DriveWidget* drive_widget_; 47 | ros::Publisher velocity_publisher_; 48 | ros::NodeHandle nh_; 49 | 50 | QPushButton *push_button_1_; 51 | 52 | float linear_velocity_; 53 | float angular_velocity_; 54 | bool mouse_pressed_; 55 | bool mouse_pressed_sent_; 56 | }; 57 | 58 | } 59 | 60 | #endif // TELEOP_PANEL_H 61 | -------------------------------------------------------------------------------- /src/terrain_analysis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(terrain_analysis) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | roscpp 27 | std_msgs 28 | sensor_msgs 29 | pcl_ros 30 | ) 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | 36 | ## Specify additional locations of header files 37 | ## Your package locations should be listed before other locations 38 | include_directories( 39 | ${catkin_INCLUDE_DIRS} 40 | ${OpenCV_INCLUDE_DIRS} 41 | ${PCL_INCLUDE_DIRS} 42 | "${PROJECT_SOURCE_DIR}/include" 43 | /usr/local/include # Location when using 'make system_install' 44 | /usr/include # More usual location (e.g. when installing using a package) 45 | ) 46 | 47 | ## Specify additional locations for library files 48 | link_directories( 49 | /usr/local/lib # Location when using 'make system_install' 50 | /usr/lib # More usual location (e.g. when installing using a package) 51 | ) 52 | 53 | ## Declare executables 54 | add_executable(terrainAnalysis src/terrainAnalysis.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(terrainAnalysis ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS terrainAnalysis 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | -------------------------------------------------------------------------------- /src/terrain_analysis/launch/terrain_analysis.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /src/terrain_analysis/package.xml: -------------------------------------------------------------------------------- 1 | 2 | terrain_analysis 3 | 0.0.1 4 | Terrain Analysis 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/terrain_analysis_ext/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(terrain_analysis_ext) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | roscpp 27 | std_msgs 28 | sensor_msgs 29 | pcl_ros 30 | ) 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | 36 | ## Specify additional locations of header files 37 | ## Your package locations should be listed before other locations 38 | include_directories( 39 | ${catkin_INCLUDE_DIRS} 40 | ${OpenCV_INCLUDE_DIRS} 41 | ${PCL_INCLUDE_DIRS} 42 | "${PROJECT_SOURCE_DIR}/include" 43 | /usr/local/include # Location when using 'make system_install' 44 | /usr/include # More usual location (e.g. when installing using a package) 45 | ) 46 | 47 | ## Specify additional locations for library files 48 | link_directories( 49 | /usr/local/lib # Location when using 'make system_install' 50 | /usr/lib # More usual location (e.g. when installing using a package) 51 | ) 52 | 53 | ## Declare executables 54 | add_executable(terrainAnalysisExt src/terrainAnalysisExt.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(terrainAnalysisExt ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS terrainAnalysisExt 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | -------------------------------------------------------------------------------- /src/terrain_analysis_ext/launch/terrain_analysis_ext.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/terrain_analysis_ext/package.xml: -------------------------------------------------------------------------------- 1 | 2 | terrain_analysis_ext 3 | 0.0.1 4 | Terrain Analysis in Extended Scale 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/vehicle_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vehicle_simulator) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | roscpp 27 | std_msgs 28 | sensor_msgs 29 | pcl_ros 30 | ) 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | 36 | ## Specify additional locations of header files 37 | ## Your package locations should be listed before other locations 38 | include_directories( 39 | ${catkin_INCLUDE_DIRS} 40 | ${OpenCV_INCLUDE_DIRS} 41 | ${PCL_INCLUDE_DIRS} 42 | "${PROJECT_SOURCE_DIR}/include" 43 | /usr/local/include # Location when using 'make system_install' 44 | /usr/include # More usual location (e.g. when installing using a package) 45 | ) 46 | 47 | ## Specify additional locations for library files 48 | link_directories( 49 | /usr/local/lib # Location when using 'make system_install' 50 | /usr/lib # More usual location (e.g. when installing using a package) 51 | ) 52 | 53 | ## Declare executables 54 | add_executable(vehicleSimulator src/vehicleSimulator.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(vehicleSimulator ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS vehicleSimulator 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | install(DIRECTORY log/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/log 70 | ) 71 | install(DIRECTORY mesh/ 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/mesh 73 | ) 74 | install(DIRECTORY rviz/ 75 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz 76 | ) 77 | install(DIRECTORY urdf/ 78 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/urdf 79 | ) 80 | install(DIRECTORY world/ 81 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/world 82 | ) 83 | -------------------------------------------------------------------------------- /src/vehicle_simulator/launch/system_matterport.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | -------------------------------------------------------------------------------- /src/vehicle_simulator/launch/vehicle_simulator.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | -------------------------------------------------------------------------------- /src/vehicle_simulator/log/readme.txt: -------------------------------------------------------------------------------- 1 | Exploration metrics and vehicle trajectory are saved in this folder. 2 | 3 | 'metric_xxx.txt' contains four columns, respectively explored volume (m^3), traveling distance (m), algorithm runtime (second), and time duration from start of the run (second). Note that the runtime is recorded by receiving numbers as 'std_msgs::Float32' typed messages on ROS topic '/runtime'. 4 | 5 | 'trajectory_xxx.txt' contains seven columns, respectively x (m), y (m), z (m), roll (rad), pitch (rad), yaw (rad), and time duration from start of the run (second). 6 | -------------------------------------------------------------------------------- /src/vehicle_simulator/mesh/matterport/meshes/readme.txt: -------------------------------------------------------------------------------- 1 | Put Matterport3D mesh files (.mtl, .obj, .jpg) in this folder. 2 | -------------------------------------------------------------------------------- /src/vehicle_simulator/mesh/matterport/model.config: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | matterport 5 | 1.0 6 | model.sdf 7 | 8 | 9 | Matterport3D 10 | matterport3d@googlegroups.com 11 | 12 | 13 | 14 | Matterport3D Photorealistic Model 15 | 16 | 17 | 18 | -------------------------------------------------------------------------------- /src/vehicle_simulator/mesh/matterport/model.sdf: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | true 5 | 6 | 7 | 0 0 0 0 0 0 8 | 10402000 9 | 10 | 3970000000 11 | 20876000 12 | 613230000 13 | 9033200000 14 | -269580000 15 | 5839000000 16 | 17 | 18 | 19 | 20 | 21 | model://matterport/meshes/matterport.obj 22 | 23 | 24 | 25 | 26 | 27 | 28 | model://matterport/meshes/matterport.obj 29 | 30 | 31 | 32 | 33 | 34 | 35 | -------------------------------------------------------------------------------- /src/vehicle_simulator/mesh/matterport/pointclouds/readme.txt: -------------------------------------------------------------------------------- 1 | Put Matterport3D pointcloud files (.ply, etc) in this folder. 2 | -------------------------------------------------------------------------------- /src/vehicle_simulator/mesh/matterport/segmentations/readme.txt: -------------------------------------------------------------------------------- 1 | Put Matterport3D segmentation files (.glb, .house, .ply, .navmesh) in this folder. 2 | -------------------------------------------------------------------------------- /src/vehicle_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | vehicle_simulator 3 | 0.0.1 4 | Vehicle Simulator 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | gazebo_ros 16 | 17 | roscpp 18 | std_msgs 19 | sensor_msgs 20 | message_filters 21 | pcl_ros 22 | gazebo_ros 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/vehicle_simulator/urdf/lidar.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/vehicle_simulator/urdf/robot.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | Gazebo/${color} 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 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | 93 | 94 | 95 | 96 | 97 | 98 | 99 | 100 | 101 | 102 | 103 | 104 | 105 | 106 | 107 | 108 | 109 | 110 | 111 | 112 | 113 | 114 | 115 | 116 | 117 | 118 | 119 | 120 | 121 | 122 | 123 | 124 | 125 | 126 | 127 | 128 | 129 | 130 | 131 | 132 | 133 | 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | 147 | 148 | 149 | -------------------------------------------------------------------------------- /src/vehicle_simulator/world/matterport.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 1 5 | 0 0 300 0 0 0 6 | 1 1 1 1 7 | 0 0 0 1 8 | 9 | 1000 10 | 1 11 | 0 12 | 0 13 | 14 | 0 1 0 15 | 16 | 17 | 0.001 18 | 1 19 | 1000 20 | 21 | 22 | 1 1 1 1 23 | 0 0 0 1 24 | 0 25 | 26 | 27 | EARTH_WGS84 28 | 0 29 | 0 30 | 0 31 | 0 32 | 33 | 34 | 4176 536000000 35 | 49 494850140 36 | 1541177620 221891024 37 | 48963 38 | 39 | 0 0 0 0 0 0 40 | 1 1 1 41 | 42 | 0 0 0 0 0 0 43 | 0 0 0 0 0 0 44 | 0 0 0 0 0 0 45 | 0 0 0 0 0 0 46 | 47 | 48 | 49 | 0 0 300 0 0 0 50 | 51 | 52 | 53 | 54 | 0 0 20 0 1.5708 0 55 | orbit 56 | perspective 57 | 58 | 59 | 0 0 0 60 | 0 0 0 61 | 62 | 63 | 1 64 | 65 | 66 | 67 | 68 | model://matterport/meshes/matterport.obj 69 | 70 | 71 | 10 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | model://matterport/meshes/matterport.obj 89 | 90 | 91 | 92 | 0 93 | 0 94 | 95 | 0 0 0 0 0 0 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /src/velodyne_simulator/LICENSE: -------------------------------------------------------------------------------- 1 | Software License Agreement (BSD License) 2 | 3 | Copyright (c) 2015-2021, Dataspeed Inc. 4 | All rights reserved. 5 | 6 | Redistribution and use in source and binary forms, with or without modification, 7 | are permitted provided that the following conditions are met: 8 | 9 | * Redistributions of source code must retain the above copyright notice, 10 | this list of conditions and the following disclaimer. 11 | * Redistributions in binary form must reproduce the above copyright notice, 12 | this list of conditions and the following disclaimer in the documentation 13 | and/or other materials provided with the distribution. 14 | * Neither the name of Dataspeed Inc. nor the names of its 15 | contributors may be used to endorse or promote products derived from this 16 | software without specific prior written permission. 17 | 18 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND 19 | ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED 20 | WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 21 | DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 22 | FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 23 | DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 24 | SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 25 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 26 | OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 27 | OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 28 | -------------------------------------------------------------------------------- /src/velodyne_simulator/README.md: -------------------------------------------------------------------------------- 1 | # Velodyne Simulator 2 | URDF description and Gazebo plugins to simulate Velodyne laser scanners 3 | 4 | ![rviz screenshot](img/rviz.png) 5 | 6 | # Features 7 | * URDF with colored meshes 8 | * Gazebo plugin based on [gazebo_plugins/gazebo_ros_block_laser](https://github.com/ros-simulation/gazebo_ros_pkgs/blob/kinetic-devel/gazebo_plugins/src/gazebo_ros_block_laser.cpp) 9 | * Publishes PointCloud2 with same structure (x, y, z, intensity, ring, time) 10 | * Simulated Gaussian noise 11 | * GPU acceleration ([with a modern Gazebo build](gazebo_upgrade.md)) 12 | * Supported models: 13 | * [VLP-16](velodyne_description/urdf/VLP-16.urdf.xacro) 14 | * [HDL-32E](velodyne_description/urdf/HDL-32E.urdf.xacro) 15 | * Pull requests for other models are welcome 16 | * Experimental support for clipping low-intensity returns 17 | 18 | # Parameters 19 | * ```*origin``` URDF transform from parent link. 20 | * ```parent``` URDF parent link name. Default ```base_link``` 21 | * ```name``` URDF model name. Also used as tf frame_id for PointCloud2 output. Default ```velodyne``` 22 | * ```topic``` PointCloud2 output topic name. Default ```/velodyne_points``` 23 | * ```hz``` Update rate in hz. Default ```10``` 24 | * ```lasers``` Number of vertical spinning lasers. Default ```VLP-16: 16, HDL-32E: 32``` 25 | * ```samples``` Nuber of horizontal rotating samples. Default ```VLP-16: 1875, HDL-32E: 2187``` 26 | * ```organize_cloud``` Organize PointCloud2 into 2D array with NaN placeholders, otherwise 1D array and leave out invlaid points. Default ```false``` 27 | * ```min_range``` Minimum range value in meters. Default ```0.9``` 28 | * ```max_range``` Maximum range value in meters. Default ```130.0``` 29 | * ```noise``` Gausian noise value in meters. Default ```0.008``` 30 | * ```min_angle``` Minimum horizontal angle in radians. Default ```-3.14``` 31 | * ```max_angle``` Maximum horizontal angle in radians. Default ```3.14``` 32 | * ```gpu``` Use gpu_ray sensor instead of the standard ray sensor. Default ```false``` 33 | * ```min_intensity``` The minimum intensity beneath which returns will be clipped. Can be used to remove low-intensity objects. 34 | 35 | # Known Issues 36 | * At full sample resolution, Gazebo can take up to 30 seconds to load the VLP-16 pluggin, 60 seconds for the HDL-32E 37 | * With the default Gazebo version shipped with ROS, ranges are incorrect when accelerated with the GPU option ([issue](https://bitbucket.org/osrf/gazebo/issues/946/),[image](img/gpu.png)) 38 | * Solution: Upgrade to a [modern Gazebo version](gazebo_upgrade.md) 39 | * Gazebo cannot maintain 10Hz with large pointclouds 40 | * Solution: User can reduce number of points (samples) or frequency (hz) in the urdf parameters, see [example.urdf.xacro](velodyne_description/urdf/example.urdf.xacro) 41 | * Gazebo crashes when updating HDL-32E sensors with default number of points. "Took over 1.0 seconds to update a sensor." 42 | * Solution: User can reduce number of points in urdf (same as above) 43 | 44 | # Example Gazebo Robot 45 | ```roslaunch velodyne_description example.launch``` 46 | 47 | # Example Gazebo Robot (with GPU) 48 | ```roslaunch velodyne_description example.launch gpu:=true``` 49 | 50 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package velodyne_description 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.11 (2021-02-23) 6 | ------------------- 7 | * Add organize_cloud parameter to match velodyne_pointcloud 8 | * Contributors: Kevin Hallenbeck 9 | 10 | 1.0.10 (2020-08-03) 11 | ------------------- 12 | * Change PointCloud visualization type from flat squares to points in example rviz config 13 | * Bump minimum CMake version to 3.0.2 in all CMakeLists.txt 14 | * Fix xacro macro instantiation 15 | * Contributors: Kevin Hallenbeck, Micho Radovnikovich 16 | 17 | 1.0.9 (2019-03-08) 18 | ------------------ 19 | 20 | 1.0.8 (2018-09-08) 21 | ------------------ 22 | 23 | 1.0.7 (2018-07-03) 24 | ------------------ 25 | * Added GPU support 26 | * Updated inertia tensors for VLP-16 and HDL-32E to realistic values 27 | * Removed unnecessary file extraction code in cmake 28 | * Contributors: Kevin Hallenbeck, Max Schwarz 29 | 30 | 1.0.6 (2017-10-17) 31 | ------------------ 32 | * Use robotNamespace as prefix for PointCloud2 topic frame_id by default 33 | * Contributors: Micho Radovnikovich 34 | 35 | 1.0.5 (2017-09-05) 36 | ------------------ 37 | * Increased minimum collision range to prevent self-clipping when in motion 38 | * Added many URDF parameters, and set example sample count to reasonable values 39 | * Launch rviz with gazebo 40 | * Contributors: Kevin Hallenbeck 41 | 42 | 1.0.4 (2017-04-24) 43 | ------------------ 44 | * Updated package.xml format to version 2 45 | * Contributors: Kevin Hallenbeck 46 | 47 | 1.0.3 (2016-08-13) 48 | ------------------ 49 | * Contributors: Kevin Hallenbeck 50 | 51 | 1.0.2 (2016-02-03) 52 | ------------------ 53 | * Moved M_PI property out of macro to support multiple instances 54 | * Materials caused problems with more than one sensors. Removed. 55 | * Added example urdf and gazebo 56 | * Changed to DAE meshes 57 | * Added meshes. Added HDL-32E. 58 | * Start from block laser 59 | * Contributors: Kevin Hallenbeck 60 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(velodyne_description) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | catkin_package() 7 | 8 | install(DIRECTORY launch meshes rviz urdf world 9 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 10 | ) 11 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/launch/example.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/HDL32E_base.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/src/velodyne_simulator/velodyne_description/meshes/HDL32E_base.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/HDL32E_scan.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/src/velodyne_simulator/velodyne_description/meshes/HDL32E_scan.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/VLP16_base_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/src/velodyne_simulator/velodyne_description/meshes/VLP16_base_1.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/VLP16_base_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/src/velodyne_simulator/velodyne_description/meshes/VLP16_base_2.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/VLP16_scan.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_matterport/95878da68290b26557f2c78baa6ea94253c0bbac/src/velodyne_simulator/velodyne_description/meshes/VLP16_scan.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_description 4 | 1.0.11 5 | 6 | URDF and meshes describing Velodyne laser scanners. 7 | 8 | 9 | BSD 10 | Kevin Hallenbeck 11 | Kevin Hallenbeck 12 | http://wiki.ros.org/velodyne_description 13 | https://bitbucket.org/dataspeedinc/velodyne_simulator 14 | https://bitbucket.org/dataspeedinc/velodyne_simulator/issues 15 | 16 | catkin 17 | 18 | urdf 19 | xacro 20 | 21 | 22 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/urdf/HDL-32E.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 0 0 0 0 0 0 58 | false 59 | ${hz} 60 | 61 | 62 | 63 | ${samples} 64 | 1 65 | ${min_angle} 66 | ${max_angle} 67 | 68 | 69 | ${lasers} 70 | 1 71 | -${30.67*M_PI/180.0} 72 | ${10.67*M_PI/180.0} 73 | 74 | 75 | 76 | ${collision_range} 77 | ${max_range+1} 78 | 0.001 79 | 80 | 81 | gaussian 82 | 0.0 83 | 0.0 84 | 85 | 86 | 87 | ${topic} 88 | ${name} 89 | ${organize_cloud} 90 | ${min_range} 91 | ${max_range} 92 | ${noise} 93 | 94 | 95 | 96 | 97 | 98 | 0 0 0 0 0 0 99 | false 100 | ${hz} 101 | 102 | 103 | 104 | ${samples} 105 | 1 106 | ${min_angle} 107 | ${max_angle} 108 | 109 | 110 | ${lasers} 111 | 1 112 | -${30.67*M_PI/180.0} 113 | ${10.67*M_PI/180.0} 114 | 115 | 116 | 117 | ${collision_range} 118 | ${max_range+1} 119 | 0.001 120 | 121 | 122 | gaussian 123 | 0.0 124 | 0.0 125 | 126 | 127 | 128 | ${topic} 129 | ${name} 130 | ${organize_cloud} 131 | ${min_range} 132 | ${max_range} 133 | ${noise} 134 | 135 | 136 | 137 | 138 | 139 | 140 | 141 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/urdf/VLP-16.urdf.xacro: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 0 0 0 0 0 0 63 | false 64 | ${hz} 65 | 66 | 67 | 68 | ${samples} 69 | 1 70 | ${min_angle} 71 | ${max_angle} 72 | 73 | 74 | ${lasers} 75 | 1 76 | -${15.0*M_PI/180.0} 77 | ${15.0*M_PI/180.0} 78 | 79 | 80 | 81 | ${collision_range} 82 | ${max_range+1} 83 | 0.001 84 | 85 | 86 | gaussian 87 | 0.0 88 | 0.0 89 | 90 | 91 | 92 | ${topic} 93 | ${name} 94 | ${organize_cloud} 95 | ${min_range} 96 | ${max_range} 97 | ${noise} 98 | 99 | 100 | 101 | 102 | 103 | 0 0 0 0 0 0 104 | false 105 | ${hz} 106 | 107 | 108 | 109 | ${samples} 110 | 1 111 | ${min_angle} 112 | ${max_angle} 113 | 114 | 115 | ${lasers} 116 | 1 117 | -${15.0*M_PI/180.0} 118 | ${15.0*M_PI/180.0} 119 | 120 | 121 | 122 | ${collision_range} 123 | ${max_range+1} 124 | 0.001 125 | 126 | 127 | gaussian 128 | 0.0 129 | 0.0 130 | 131 | 132 | 133 | ${topic} 134 | ${name} 135 | ${organize_cloud} 136 | ${min_range} 137 | ${max_range} 138 | ${noise} 139 | 140 | 141 | 142 | 143 | 144 | 145 | 146 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/urdf/example.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 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/world/example.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | model://sun 7 | 8 | 9 | 10 | model://ground_plane 11 | 12 | 13 | 14 | -1.5 2.5 0.5 0 -0 0 15 | 16 | 17 | 1 18 | 19 | 0.166667 20 | 0 21 | 0 22 | 0.166667 23 | 0 24 | 0.166667 25 | 26 | 27 | 28 | 29 | 30 | 1 1 1 31 | 32 | 33 | 10 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 1 1 1 51 | 52 | 53 | 54 | 58 | 59 | 60 | 0 61 | 0 62 | 1 63 | 64 | 65 | 66 | 67 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_gazebo_plugins/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package velodyne_gazebo_plugins 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.11 (2021-02-23) 6 | ------------------- 7 | * Remove support for end-of-life ROS distributions 8 | * Add organize_cloud parameter to match velodyne_pointcloud 9 | * Contributors: Kevin Hallenbeck 10 | 11 | 1.0.10 (2020-08-03) 12 | ------------------- 13 | * Change PointCloud2 structure to match updated velodyne_pointcloud package 14 | * Bump minimum CMake version to 3.0.2 in all CMakeLists.txt 15 | * Contributors: Kevin Hallenbeck, Micho Radovnikovich 16 | 17 | 1.0.9 (2019-03-08) 18 | ------------------ 19 | * Added min_intensity parameter to support cliping of low intensity returns 20 | * Contributors: Jonathan Wheare, Kevin Hallenbeck 21 | 22 | 1.0.8 (2018-09-08) 23 | ------------------ 24 | * Changed iteration order to more closely represent the live velodyne driver 25 | * Contributors: Kevin Hallenbeck 26 | 27 | 1.0.7 (2018-07-03) 28 | ------------------ 29 | * Added GPU support 30 | * Added support for Gazebo 9 31 | * Improved behavior of max range calculation 32 | * Removed trailing slashes in robot namespace 33 | * Fixed resolution of 1 not supported 34 | * Fixed issue with only 1 vert or horiz ray 35 | * Fixed cmake exports and warning 36 | * Contributors: Kevin Hallenbeck, Jacob Seibert, Naoki Mizuno 37 | 38 | 1.0.6 (2017-10-17) 39 | ------------------ 40 | * Use robotNamespace as prefix for PointCloud2 topic frame_id by default 41 | * Use Gazebo LaserScan message instead of direct LaserShape access, fixes timestamp issue 42 | * Contributors: Kevin Hallenbeck, Max Schwarz, Micho Radovnikovich 43 | 44 | 1.0.5 (2017-09-05) 45 | ------------------ 46 | * Fixed ground plane projection by removing interpolation 47 | * Contributors: Kevin Hallenbeck, Micho Radovnikovich 48 | 49 | 1.0.4 (2017-04-24) 50 | ------------------ 51 | * Updated package.xml format to version 2 52 | * Removed gazebo_plugins dependency 53 | * Contributors: Kevin Hallenbeck 54 | 55 | 1.0.3 (2016-08-13) 56 | ------------------ 57 | * Gazebo7 integration 58 | * Contributors: Kevin Hallenbeck, Konstantin Sorokin 59 | 60 | 1.0.2 (2016-02-03) 61 | ------------------ 62 | * Display laser count when loading gazebo plugin 63 | * Don't reverse ring for newer gazebo versions 64 | * Changed to PointCloud2. Handle min and max range. Noise. General cleanup. 65 | * Start from block laser 66 | * Contributors: Kevin Hallenbeck 67 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_gazebo_plugins/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(velodyne_gazebo_plugins) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | sensor_msgs 7 | tf 8 | gazebo_ros 9 | ) 10 | find_package(gazebo REQUIRED) 11 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${GAZEBO_CXX_FLAGS}") 12 | 13 | catkin_package( 14 | INCLUDE_DIRS include ${GAZEBO_INCLUDE_DIRS} 15 | LIBRARIES gazebo_ros_velodyne_laser gazebo_ros_velodyne_gpu_laser 16 | CATKIN_DEPENDS roscpp sensor_msgs gazebo_ros 17 | ) 18 | 19 | include_directories( 20 | include 21 | ${catkin_INCLUDE_DIRS} 22 | ${GAZEBO_INCLUDE_DIRS} 23 | ) 24 | 25 | link_directories( 26 | ${GAZEBO_LIBRARY_DIRS} 27 | ) 28 | 29 | add_library(gazebo_ros_velodyne_laser src/GazeboRosVelodyneLaser.cpp) 30 | target_link_libraries(gazebo_ros_velodyne_laser 31 | ${catkin_LIBRARIES} 32 | ${GAZEBO_LIBRARIES} 33 | RayPlugin 34 | ) 35 | 36 | add_library(gazebo_ros_velodyne_gpu_laser src/GazeboRosVelodyneLaser.cpp) 37 | target_link_libraries(gazebo_ros_velodyne_gpu_laser 38 | ${catkin_LIBRARIES} 39 | ${GAZEBO_LIBRARIES} 40 | GpuRayPlugin 41 | ) 42 | target_compile_definitions(gazebo_ros_velodyne_gpu_laser PRIVATE GAZEBO_GPU_RAY=1) 43 | 44 | install(TARGETS gazebo_ros_velodyne_laser gazebo_ros_velodyne_gpu_laser 45 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 46 | ) 47 | 48 | install(DIRECTORY include/${PROJECT_NAME}/ 49 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 50 | ) 51 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_gazebo_plugins/include/velodyne_gazebo_plugins/GazeboRosVelodyneLaser.h: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2015-2021, Dataspeed Inc. 5 | * All rights reserved. 6 | * 7 | * Redistribution and use in source and binary forms, with or without 8 | * modification, are permitted provided that the following conditions 9 | * are met: 10 | * 11 | * * Redistributions of source code must retain the above copyright 12 | * notice, this list of conditions and the following disclaimer. 13 | * * Redistributions in binary form must reproduce the above 14 | * copyright notice, this list of conditions and the following 15 | * disclaimer in the documentation and/or other materials provided 16 | * with the distribution. 17 | * * Neither the name of Dataspeed Inc. nor the names of its 18 | * contributors may be used to endorse or promote products derived 19 | * from this software without specific prior written permission. 20 | * 21 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | * POSSIBILITY OF SUCH DAMAGE. 33 | *********************************************************************/ 34 | 35 | #ifndef GAZEBO_ROS_VELODYNE_LASER_H_ 36 | #define GAZEBO_ROS_VELODYNE_LASER_H_ 37 | 38 | // Use the same source code for CPU and GPU plugins 39 | #ifndef GAZEBO_GPU_RAY 40 | #define GAZEBO_GPU_RAY 0 41 | #endif 42 | 43 | // Custom Callback Queue 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | #include 54 | #include 55 | #include 56 | #if GAZEBO_GPU_RAY 57 | #include 58 | #else 59 | #include 60 | #endif 61 | 62 | #include 63 | #include 64 | #include 65 | #include 66 | #include 67 | 68 | #if GAZEBO_GPU_RAY 69 | #define GazeboRosVelodyneLaser GazeboRosVelodyneGpuLaser 70 | #define RayPlugin GpuRayPlugin 71 | #define RaySensorPtr GpuRaySensorPtr 72 | #endif 73 | 74 | namespace gazebo 75 | { 76 | 77 | class GazeboRosVelodyneLaser : public RayPlugin 78 | { 79 | /// \brief Constructor 80 | /// \param parent The parent entity, must be a Model or a Sensor 81 | public: GazeboRosVelodyneLaser(); 82 | 83 | /// \brief Destructor 84 | public: ~GazeboRosVelodyneLaser(); 85 | 86 | /// \brief Load the plugin 87 | /// \param take in SDF root element 88 | public: void Load(sensors::SensorPtr _parent, sdf::ElementPtr _sdf); 89 | 90 | /// \brief Subscribe on-demand 91 | private: void ConnectCb(); 92 | 93 | /// \brief The parent ray sensor 94 | private: sensors::RaySensorPtr parent_ray_sensor_; 95 | 96 | /// \brief Pointer to ROS node 97 | private: ros::NodeHandle* nh_; 98 | 99 | /// \brief ROS publisher 100 | private: ros::Publisher pub_; 101 | 102 | /// \brief topic name 103 | private: std::string topic_name_; 104 | 105 | /// \brief frame transform name, should match link name 106 | private: std::string frame_name_; 107 | 108 | /// \brief organize cloud 109 | private: bool organize_cloud_; 110 | 111 | /// \brief the intensity beneath which points will be filtered 112 | private: double min_intensity_; 113 | 114 | /// \brief Minimum range to publish 115 | private: double min_range_; 116 | 117 | /// \brief Maximum range to publish 118 | private: double max_range_; 119 | 120 | /// \brief Gaussian noise 121 | private: double gaussian_noise_; 122 | 123 | /// \brief Gaussian noise generator 124 | private: static double gaussianKernel(double mu, double sigma) 125 | { 126 | // using Box-Muller transform to generate two independent standard normally distributed normal variables 127 | // see wikipedia 128 | double U = (double)rand() / (double)RAND_MAX; // normalized uniform random variable 129 | double V = (double)rand() / (double)RAND_MAX; // normalized uniform random variable 130 | return sigma * (sqrt(-2.0 * ::log(U)) * cos(2.0 * M_PI * V)) + mu; 131 | } 132 | 133 | /// \brief A mutex to lock access 134 | private: boost::mutex lock_; 135 | 136 | /// \brief For setting ROS name space 137 | private: std::string robot_namespace_; 138 | 139 | // Custom Callback Queue 140 | private: ros::CallbackQueue laser_queue_; 141 | private: void laserQueueThread(); 142 | private: boost::thread callback_laser_queue_thread_; 143 | 144 | // Subscribe to gazebo laserscan 145 | private: gazebo::transport::NodePtr gazebo_node_; 146 | private: gazebo::transport::SubscriberPtr sub_; 147 | private: void OnScan(const ConstLaserScanStampedPtr &_msg); 148 | 149 | }; 150 | 151 | } // namespace gazebo 152 | 153 | #endif /* GAZEBO_ROS_VELODYNE_LASER_H_ */ 154 | 155 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_gazebo_plugins/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_gazebo_plugins 4 | 1.0.11 5 | 6 | Gazebo plugin to provide simulated data from Velodyne laser scanners. 7 | 8 | 9 | BSD 10 | Kevin Hallenbeck 11 | Kevin Hallenbeck 12 | http://wiki.ros.org/velodyne_gazebo_plugins 13 | https://bitbucket.org/dataspeedinc/velodyne_simulator 14 | https://bitbucket.org/dataspeedinc/velodyne_simulator/issues 15 | 16 | catkin 17 | 18 | roscpp 19 | sensor_msgs 20 | tf 21 | gazebo_ros 22 | 23 | 24 | 25 | 26 | 27 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_simulator/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package velodyne_simulator 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.11 (2021-02-23) 6 | ------------------- 7 | 8 | 1.0.10 (2020-08-03) 9 | ------------------- 10 | * Bump minimum CMake version to 3.0.2 in all CMakeLists.txt 11 | * Contributors: Micho Radovnikovich 12 | 13 | 1.0.9 (2019-03-08) 14 | ------------------ 15 | 16 | 1.0.8 (2018-09-08) 17 | ------------------ 18 | 19 | 1.0.7 (2018-07-03) 20 | ------------------ 21 | 22 | 1.0.6 (2017-10-17) 23 | ------------------ 24 | 25 | 1.0.5 (2017-09-05) 26 | ------------------ 27 | 28 | 1.0.4 (2017-04-24) 29 | ------------------ 30 | * Updated package.xml format to version 2 31 | * Contributors: Kevin Hallenbeck 32 | 33 | 1.0.3 (2016-08-13) 34 | ------------------ 35 | * Contributors: Kevin Hallenbeck 36 | 37 | 1.0.2 (2016-02-03) 38 | ------------------ 39 | * Contributors: Kevin Hallenbeck 40 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(velodyne_simulator) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_simulator/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | velodyne_simulator 4 | 1.0.11 5 | 6 | Metapackage allowing easy installation of Velodyne simulation components. 7 | 8 | 9 | BSD 10 | Kevin Hallenbeck 11 | Kevin Hallenbeck 12 | http://wiki.ros.org/velodyne_simulator 13 | https://bitbucket.org/dataspeedinc/velodyne_simulator 14 | https://bitbucket.org/dataspeedinc/velodyne_simulator/issues 15 | 16 | catkin 17 | 18 | velodyne_description 19 | velodyne_gazebo_plugins 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/visualization_tools/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(visualization_tools) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | rospy 12 | roscpp 13 | std_msgs 14 | sensor_msgs 15 | pcl_ros 16 | ) 17 | 18 | find_package(PCL REQUIRED) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | rospy 27 | roscpp 28 | std_msgs 29 | sensor_msgs 30 | pcl_ros 31 | ) 32 | 33 | ########### 34 | ## Build ## 35 | ########### 36 | 37 | ## Specify additional locations of header files 38 | ## Your package locations should be listed before other locations 39 | include_directories( 40 | ${catkin_INCLUDE_DIRS} 41 | ${PCL_INCLUDE_DIRS} 42 | "${PROJECT_SOURCE_DIR}/include" 43 | /usr/local/include # Location when using 'make system_install' 44 | /usr/include # More usual location (e.g. when installing using a package) 45 | ) 46 | 47 | ## Specify additional locations for library files 48 | link_directories( 49 | /usr/local/lib # Location when using 'make system_install' 50 | /usr/lib # More usual location (e.g. when installing using a package) 51 | ) 52 | 53 | ## Declare executables 54 | add_executable(visualizationTools src/visualizationTools.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(visualizationTools ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS visualizationTools 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | 69 | -------------------------------------------------------------------------------- /src/visualization_tools/launch/visualization_tools.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | -------------------------------------------------------------------------------- /src/visualization_tools/package.xml: -------------------------------------------------------------------------------- 1 | 2 | visualization_tools 3 | 0.0.1 4 | Visulization Tools 5 | Hongbiao Zhu 6 | BSD 7 | 8 | Hongbiao Zhu 9 | 10 | catkin 11 | 12 | rospy 13 | roscpp 14 | std_msgs 15 | sensor_msgs 16 | pcl_ros 17 | 18 | rospy 19 | roscpp 20 | std_msgs 21 | sensor_msgs 22 | pcl_ros 23 | 24 | -------------------------------------------------------------------------------- /src/waypoint_converter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_converter) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(PCL REQUIRED) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | std_msgs 27 | sensor_msgs 28 | pcl_ros 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | ## Specify additional locations of header files 36 | ## Your package locations should be listed before other locations 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${PCL_INCLUDE_DIRS} 40 | "${PROJECT_SOURCE_DIR}/include" 41 | /usr/local/include # Location when using 'make system_install' 42 | /usr/include # More usual location (e.g. when installing using a package) 43 | ) 44 | 45 | ## Specify additional locations for library files 46 | link_directories( 47 | /usr/local/lib # Location when using 'make system_install' 48 | /usr/lib # More usual location (e.g. when installing using a package) 49 | ) 50 | 51 | ## Declare executables 52 | add_executable(waypointConverter src/waypointConverter.cpp) 53 | 54 | ## Specify libraries to link a library or executable target against 55 | target_link_libraries(waypointConverter ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 56 | 57 | install(TARGETS waypointConverter 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 61 | ) 62 | 63 | install(DIRECTORY launch/ 64 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 65 | ) 66 | install(DIRECTORY data/ 67 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/data 68 | ) 69 | -------------------------------------------------------------------------------- /src/waypoint_converter/launch/waypoint_converter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/waypoint_converter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | waypoint_converter 3 | 0.0.1 4 | Waypoint Converter from Waypoint with Heading 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_rviz_plugin) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | rviz 8 | ) 9 | 10 | set(CMAKE_AUTOMOC ON) 11 | if(rviz_QT_VERSION VERSION_LESS "5") 12 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 13 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 14 | include(${QT_USE_FILE}) 15 | else() 16 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 17 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 18 | set(QT_LIBRARIES Qt5::Widgets) 19 | endif() 20 | 21 | add_definitions(-DQT_NO_KEYWORDS) 22 | 23 | catkin_package( 24 | LIBRARIES ${PROJECT_NAME} 25 | CATKIN_DEPENDS roscpp rviz 26 | ) 27 | 28 | include_directories( 29 | include 30 | ${QT_INCLUDE_DIRS} 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | set(HEADER_FILES 35 | include/waypoint_tool.h 36 | ) 37 | set(SRC_FILES 38 | src/waypoint_tool.cpp 39 | ) 40 | 41 | add_library(${PROJECT_NAME} 42 | ${SRC_FILES} ${HEADER_FILES} 43 | ) 44 | 45 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 46 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} ${OGRE_OV_LIBRARIES_ABS}) 47 | 48 | install(TARGETS ${PROJECT_NAME} 49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ) 53 | 54 | install(FILES plugin_description.xml 55 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 56 | ) 57 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/include/waypoint_tool.h: -------------------------------------------------------------------------------- 1 | #ifndef WAYPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H 2 | #define WAYPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include "rviz/display_context.h" 12 | #include "rviz/properties/string_property.h" 13 | #include "rviz/default_plugin/tools/pose_tool.h" 14 | 15 | namespace rviz 16 | { 17 | class StringProperty; 18 | 19 | class WaypointTool : public PoseTool 20 | { 21 | Q_OBJECT 22 | public: 23 | WaypointTool(); 24 | virtual ~WaypointTool() 25 | { 26 | } 27 | virtual void onInitialize(); 28 | 29 | protected: 30 | virtual void onPoseSet(double x, double y, double theta); 31 | 32 | private Q_SLOTS: 33 | void updateTopic(); 34 | 35 | private: 36 | ros::NodeHandle nh_; 37 | ros::Subscriber sub_; 38 | ros::Publisher pub_; 39 | ros::Publisher pub_joy_; 40 | 41 | StringProperty* topic_property_; 42 | }; 43 | } 44 | 45 | #endif // WAYPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H 46 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waypoint_rviz_plugin 4 | 0.0.1 5 | Waypoint RVIZ Plugin 6 | Chao Cao 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | rospy 13 | rviz 14 | qtbase5-dev 15 | 16 | roscpp 17 | rospy 18 | rviz 19 | 20 | roscpp 21 | rospy 22 | rviz 23 | libqt5-core 24 | libqt5-gui 25 | libqt5-widgets 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A tool for sending waypoints 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/src/waypoint_tool.cpp: -------------------------------------------------------------------------------- 1 | #include "waypoint_tool.h" 2 | 3 | namespace rviz 4 | { 5 | WaypointTool::WaypointTool() 6 | { 7 | shortcut_key_ = 'w'; 8 | 9 | topic_property_ = new StringProperty("Topic", "waypoint", "The topic on which to publish navigation waypionts.", 10 | getPropertyContainer(), SLOT(updateTopic()), this); 11 | } 12 | 13 | void WaypointTool::onInitialize() 14 | { 15 | PoseTool::onInitialize(); 16 | setName("Waypoint with Heading"); 17 | updateTopic(); 18 | } 19 | 20 | void WaypointTool::updateTopic() 21 | { 22 | pub_ = nh_.advertise("/way_point_with_heading", 5); 23 | pub_joy_ = nh_.advertise("/joy", 5); 24 | } 25 | 26 | void WaypointTool::onPoseSet(double x, double y, double theta) 27 | { 28 | sensor_msgs::Joy joy; 29 | 30 | joy.axes.push_back(0); 31 | joy.axes.push_back(0); 32 | joy.axes.push_back(-1.0); 33 | joy.axes.push_back(0); 34 | joy.axes.push_back(0); 35 | joy.axes.push_back(1.0); 36 | joy.axes.push_back(0); 37 | joy.axes.push_back(0); 38 | 39 | joy.buttons.push_back(0); 40 | joy.buttons.push_back(0); 41 | joy.buttons.push_back(0); 42 | joy.buttons.push_back(0); 43 | joy.buttons.push_back(0); 44 | joy.buttons.push_back(0); 45 | joy.buttons.push_back(0); 46 | joy.buttons.push_back(1); 47 | joy.buttons.push_back(0); 48 | joy.buttons.push_back(0); 49 | joy.buttons.push_back(0); 50 | 51 | joy.header.stamp = ros::Time::now(); 52 | joy.header.frame_id = "waypoint_tool"; 53 | pub_joy_.publish(joy); 54 | 55 | geometry_msgs::Pose2D waypoint; 56 | waypoint.x = x; 57 | waypoint.y = y; 58 | waypoint.theta = theta; 59 | 60 | pub_.publish(waypoint); 61 | usleep(10000); 62 | pub_.publish(waypoint); 63 | } 64 | } 65 | 66 | #include 67 | PLUGINLIB_EXPORT_CLASS(rviz::WaypointTool, rviz::Tool) 68 | -------------------------------------------------------------------------------- /system_bring_up.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | CONDA_DIR="/home/$USER/anaconda3" 4 | CONDA_BIN_DIR="$CONDA_DIR/bin" 5 | CONDA_SETUP_FILE="$CONDA_DIR/bin/conda" 6 | CONDA_PROFILE_FILE="$CONDA_DIR/etc/profile.d/conda.sh" 7 | 8 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" 9 | 10 | # >>> conda initialize >>> 11 | # !! Contents within this block are managed by 'conda init' !! 12 | __conda_setup="$($CONDA_SETUP_FILE 'shell.bash' 'hook' 2> /dev/null)" 13 | if [ $? -eq 0 ]; then 14 | eval "$__conda_setup" 15 | else 16 | if [ -f $CONDA_PROFILE_FILE ]; then 17 | . $CONDA_PROFILE_FILE 18 | else 19 | export PATH="$CONDA_BIN_DIR:$PATH" 20 | fi 21 | fi 22 | unset __conda_setup 23 | # <<< conda initialize <<< 24 | 25 | conda activate habitat 26 | cd $SCRIPT_DIR/src/segmentation_proc/scripts 27 | python3 ./habitat_online_360_v0.2.1.py & 28 | sleep 2 29 | cd ../../.. 30 | source ./devel/setup.bash 31 | roslaunch vehicle_simulator system_matterport.launch 32 | --------------------------------------------------------------------------------