├── .gitignore ├── README.md ├── img └── header.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 ├── loam_interface ├── CMakeLists.txt ├── launch │ └── loam_interface.launch ├── package.xml └── src │ └── loamInterface.cpp ├── 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 ├── sensor_scan_generation ├── CMakeLists.txt ├── launch │ └── sensor_scan_generation.launch ├── package.xml └── src │ └── sensorScanGeneration.cpp ├── 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_campus.launch │ ├── system_forest.launch │ ├── system_garage.launch │ ├── system_indoor.launch │ ├── system_real_robot.launch │ ├── system_tunnel.launch │ └── vehicle_simulator.launch ├── log │ └── readme.txt ├── mesh │ └── download_environments.sh ├── package.xml ├── rviz │ └── vehicle_simulator.rviz ├── src │ └── vehicleSimulator.cpp ├── urdf │ ├── camera.urdf.xacro │ ├── lidar.urdf.xacro │ └── robot.urdf.xacro └── world │ ├── campus.world │ ├── forest.world │ ├── garage.world │ ├── indoor.world │ └── tunnel.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 ├── scripts │ └── realTimePlot.py └── src │ └── visualizationTools.cpp ├── waypoint_example ├── CMakeLists.txt ├── data │ ├── boundary_garage.ply │ └── waypoints_garage.ply ├── launch │ └── waypoint_example_garage.launch ├── package.xml └── src │ └── waypointExample.cpp └── waypoint_rviz_plugin ├── CMakeLists.txt ├── include └── waypoint_tool.h ├── package.xml ├── plugin_description.xml └── src └── waypoint_tool.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | 3 | devel 4 | 5 | install 6 | 7 | .catkin_workspace 8 | 9 | *~ 10 | 11 | .DS_Store 12 | 13 | src/vehicle_simulator/mesh/* 14 | 15 | !src/vehicle_simulator/mesh/matterport 16 | 17 | src/vehicle_simulator/mesh/matterport/meshes/* 18 | 19 | src/vehicle_simulator/mesh/matterport/segmentations/* 20 | 21 | src/vehicle_simulator/mesh/matterport/preview/* 22 | 23 | src/vehicle_simulator/log/* 24 | 25 | !download_environments.sh 26 | 27 | !readme.txt 28 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Header 2 | 3 | The repository is meant for leveraging system development and robot deployment for ground-based autonomous navigation and exploration. Containing a variety of simulation environments, autonomous navigation modules such as collision avoidance, terrain traversability analysis, waypoint following, etc, and a set of visualization tools, users can develop autonomous navigation systems and later on port those systems onto real robots for deployment. 4 | 5 | Please use instructions on our [project page](https://www.cmu-exploration.com). 6 | -------------------------------------------------------------------------------- /img/header.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongbiaoZ/autonomous_exploration_development_environment/d396f27c4b128b0df901e0f59ad27d259084b3fc/img/header.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 11) 5 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 6 | 7 | # Load catkin and all dependencies required for this package 8 | set(CATKIN_DEPS roscpp diagnostic_updater sensor_msgs roslint) 9 | find_package(catkin REQUIRED ${CATKIN_DEPS}) 10 | 11 | roslint_cpp() 12 | 13 | catkin_package(DEPENDS sensor_msgs) 14 | 15 | # Look for 16 | include(CheckIncludeFiles) 17 | check_include_files(linux/joystick.h HAVE_LINUX_JOYSTICK_H) 18 | 19 | if(CATKIN_ENABLE_TESTING) 20 | roslint_add_test() 21 | endif() 22 | 23 | if(HAVE_LINUX_JOYSTICK_H) 24 | include_directories(msg/cpp ${catkin_INCLUDE_DIRS}) 25 | add_executable(joy_node src/joy_node.cpp) 26 | target_link_libraries(joy_node ${catkin_LIBRARIES}) 27 | 28 | # Install targets 29 | install(TARGETS joy_node 30 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 32 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 33 | else() 34 | message("Warning: no ; won't build joy node") 35 | endif() 36 | 37 | install(DIRECTORY migration_rules scripts config launch 38 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 39 | USE_SOURCE_PERMISSIONS) 40 | -------------------------------------------------------------------------------- /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/loam_interface/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(loam_interface) 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(loamInterface src/loamInterface.cpp) 53 | 54 | ## Specify libraries to link a library or executable target against 55 | target_link_libraries(loamInterface ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 56 | 57 | install(TARGETS loamInterface 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 | -------------------------------------------------------------------------------- /src/loam_interface/launch/loam_interface.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/loam_interface/package.xml: -------------------------------------------------------------------------------- 1 | 2 | loam_interface 3 | 0.0.1 4 | Loam Interface 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/loam_interface/src/loamInterface.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace std; 27 | 28 | const double PI = 3.1415926; 29 | 30 | string stateEstimationTopic = "/integrated_to_init"; 31 | string registeredScanTopic = "/velodyne_cloud_registered"; 32 | bool flipStateEstimation = true; 33 | bool flipRegisteredScan = true; 34 | bool sendTF = true; 35 | bool reverseTF = false; 36 | 37 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 38 | 39 | nav_msgs::Odometry odomData; 40 | tf::StampedTransform odomTrans; 41 | ros::Publisher *pubOdometryPointer = NULL; 42 | tf::TransformBroadcaster *tfBroadcasterPointer = NULL; 43 | ros::Publisher *pubLaserCloudPointer = NULL; 44 | 45 | void odometryHandler(const nav_msgs::Odometry::ConstPtr& odom) 46 | { 47 | double roll, pitch, yaw; 48 | geometry_msgs::Quaternion geoQuat = odom->pose.pose.orientation; 49 | odomData = *odom; 50 | 51 | if (flipStateEstimation) { 52 | tf::Matrix3x3(tf::Quaternion(geoQuat.z, -geoQuat.x, -geoQuat.y, geoQuat.w)).getRPY(roll, pitch, yaw); 53 | 54 | pitch = -pitch; 55 | yaw = -yaw; 56 | 57 | geoQuat = tf::createQuaternionMsgFromRollPitchYaw(roll, pitch, yaw); 58 | 59 | odomData.pose.pose.orientation = geoQuat; 60 | odomData.pose.pose.position.x = odom->pose.pose.position.z; 61 | odomData.pose.pose.position.y = odom->pose.pose.position.x; 62 | odomData.pose.pose.position.z = odom->pose.pose.position.y; 63 | } 64 | 65 | // publish odometry messages 66 | odomData.header.frame_id = "map"; 67 | odomData.child_frame_id = "sensor"; 68 | pubOdometryPointer->publish(odomData); 69 | 70 | // publish tf messages 71 | odomTrans.stamp_ = odom->header.stamp; 72 | odomTrans.frame_id_ = "map"; 73 | odomTrans.child_frame_id_ = "sensor"; 74 | odomTrans.setRotation(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w)); 75 | odomTrans.setOrigin(tf::Vector3(odomData.pose.pose.position.x, odomData.pose.pose.position.y, odomData.pose.pose.position.z)); 76 | 77 | if (sendTF) { 78 | if (!reverseTF) { 79 | tfBroadcasterPointer->sendTransform(odomTrans); 80 | } else { 81 | tfBroadcasterPointer->sendTransform(tf::StampedTransform(odomTrans.inverse(), odom->header.stamp, "sensor", "map")); 82 | } 83 | } 84 | } 85 | 86 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn) 87 | { 88 | laserCloud->clear(); 89 | pcl::fromROSMsg(*laserCloudIn, *laserCloud); 90 | 91 | if (flipRegisteredScan) { 92 | int laserCloudSize = laserCloud->points.size(); 93 | for (int i = 0; i < laserCloudSize; i++) { 94 | float temp = laserCloud->points[i].x; 95 | laserCloud->points[i].x = laserCloud->points[i].z; 96 | laserCloud->points[i].z = laserCloud->points[i].y; 97 | laserCloud->points[i].y = temp; 98 | } 99 | } 100 | 101 | // publish registered scan messages 102 | sensor_msgs::PointCloud2 laserCloud2; 103 | pcl::toROSMsg(*laserCloud, laserCloud2); 104 | laserCloud2.header.stamp = laserCloudIn->header.stamp; 105 | laserCloud2.header.frame_id = "map"; 106 | pubLaserCloudPointer->publish(laserCloud2); 107 | } 108 | 109 | int main(int argc, char** argv) 110 | { 111 | ros::init(argc, argv, "loamInterface"); 112 | ros::NodeHandle nh; 113 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 114 | 115 | nhPrivate.getParam("stateEstimationTopic", stateEstimationTopic); 116 | nhPrivate.getParam("registeredScanTopic", registeredScanTopic); 117 | nhPrivate.getParam("flipStateEstimation", flipStateEstimation); 118 | nhPrivate.getParam("flipRegisteredScan", flipRegisteredScan); 119 | nhPrivate.getParam("sendTF", sendTF); 120 | nhPrivate.getParam("reverseTF", reverseTF); 121 | 122 | ros::Subscriber subOdometry = nh.subscribe (stateEstimationTopic, 5, odometryHandler); 123 | 124 | ros::Subscriber subLaserCloud = nh.subscribe (registeredScanTopic, 5, laserCloudHandler); 125 | 126 | ros::Publisher pubOdometry = nh.advertise ("/state_estimation", 5); 127 | pubOdometryPointer = &pubOdometry; 128 | 129 | tf::TransformBroadcaster tfBroadcaster; 130 | tfBroadcasterPointer = &tfBroadcaster; 131 | 132 | ros::Publisher pubLaserCloud = nh.advertise ("/registered_scan", 5); 133 | pubLaserCloudPointer = &pubLaserCloud; 134 | 135 | ros::spin(); 136 | 137 | return 0; 138 | } 139 | -------------------------------------------------------------------------------- /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/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/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_campus.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 | -------------------------------------------------------------------------------- /src/vehicle_simulator/launch/system_forest.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 | -------------------------------------------------------------------------------- /src/vehicle_simulator/launch/system_garage.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 | -------------------------------------------------------------------------------- /src/vehicle_simulator/launch/system_indoor.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 | -------------------------------------------------------------------------------- /src/vehicle_simulator/launch/system_real_robot.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/vehicle_simulator/launch/system_tunnel.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 | -------------------------------------------------------------------------------- /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 | 76 | 77 | 78 | 79 | 80 | -------------------------------------------------------------------------------- /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/download_environments.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | cd "$( cd "$( dirname "${BASH_SOURCE[0]}" )" >/dev/null 2>&1 && pwd )" 4 | 5 | echo "" 6 | echo "Downloading simulation environments..." 7 | echo "" 8 | 9 | ggID='1GMT8tptb3nAb87F8eFfmIgjma6Bu0reV' 10 | ggURL='https://drive.usercontent.google.com/download' 11 | 12 | filename="$(curl -sc /tmp/gcokie "${ggURL}?id=${ggID}" | grep -o '="uc-name.*' | sed 's/.*">//;s/<.a> .*//')" 13 | curl -Lb /tmp/gcokie "${ggURL}?id=${ggID}&confirm=xxx" -o ${filename} 14 | 15 | echo "" 16 | echo "Unzipping files..." 17 | echo "" 18 | 19 | unzip "${filename}" 20 | 21 | rm "${filename}" 22 | 23 | echo "" 24 | echo "Done, simulation environments are kept in 'src/vehicle_simulator/mesh'." 25 | echo "" 26 | -------------------------------------------------------------------------------- /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/camera.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 | 15.0 28 | 29 | 2.0 30 | 31 | 320 32 | 180 33 | R8G8B8 34 | 35 | 36 | 0.1 37 | 200.0 38 | 39 | 40 | 41 | true 42 | 0.0 43 | camera 44 | image 45 | camera_info 46 | camera 47 | 0.0 48 | 0.0 49 | 0.0 50 | 0.0 51 | 0.0 52 | 0.0 53 | 54 | 55 | 56 | 57 | 58 | -------------------------------------------------------------------------------- /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/campus.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 -1 15 | 16 | 17 | 0.001 18 | 1 19 | 1000 20 | 21 | 22 | 0.8 0.8 0.8 1 23 | 0.3 0.3 0.3 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 150 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://campus/meshes/campus.dae 69 | 70 | 71 | 10 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | model://campus/meshes/campus.dae 89 | 90 | 91 | 92 | 0 93 | 0 94 | 95 | 0 0 1 0 0 0 96 | 97 | 98 | 99 | -------------------------------------------------------------------------------- /src/vehicle_simulator/world/garage.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | model://garage 9 | 10 | 11 | 12 | 13 | EARTH_WGS84 14 | 47.3667 15 | 8.5500 16 | 500.0 17 | 0 18 | 19 | 20 | 21 | 22 | quick 23 | 1000 24 | 1.3 25 | 26 | 27 | 0 28 | 0.2 29 | 100 30 | 0.001 31 | 32 | 33 | 0.001 34 | 1 35 | 1000 36 | 0 0 -9.8 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/vehicle_simulator/world/indoor.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | model://indoor 9 | 10 | 11 | 12 | 13 | EARTH_WGS84 14 | 47.3667 15 | 8.5500 16 | 500.0 17 | 0 18 | 19 | 20 | 21 | 22 | quick 23 | 1000 24 | 1.3 25 | 26 | 27 | 0 28 | 0.2 29 | 100 30 | 0.001 31 | 32 | 33 | 0.001 34 | 1 35 | 1000 36 | 0 0 -9.8 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /src/vehicle_simulator/world/tunnel.world: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | model://sun 6 | 7 | 8 | model://tunnel 9 | 10 | 11 | 12 | 13 | EARTH_WGS84 14 | 47.3667 15 | 8.5500 16 | 500.0 17 | 0 18 | 19 | 20 | 21 | 22 | quick 23 | 1000 24 | 1.3 25 | 26 | 27 | 0 28 | 0.2 29 | 100 30 | 0.001 31 | 32 | 33 | 0.001 34 | 1 35 | 1000 36 | 0 0 -9.8 37 | 38 | 39 | 40 | -------------------------------------------------------------------------------- /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/HongbiaoZ/autonomous_exploration_development_environment/d396f27c4b128b0df901e0f59ad27d259084b3fc/src/velodyne_simulator/velodyne_description/meshes/HDL32E_base.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/HDL32E_scan.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongbiaoZ/autonomous_exploration_development_environment/d396f27c4b128b0df901e0f59ad27d259084b3fc/src/velodyne_simulator/velodyne_description/meshes/HDL32E_scan.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/VLP16_base_1.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongbiaoZ/autonomous_exploration_development_environment/d396f27c4b128b0df901e0f59ad27d259084b3fc/src/velodyne_simulator/velodyne_description/meshes/VLP16_base_1.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/VLP16_base_2.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongbiaoZ/autonomous_exploration_development_environment/d396f27c4b128b0df901e0f59ad27d259084b3fc/src/velodyne_simulator/velodyne_description/meshes/VLP16_base_2.stl -------------------------------------------------------------------------------- /src/velodyne_simulator/velodyne_description/meshes/VLP16_scan.stl: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/HongbiaoZ/autonomous_exploration_development_environment/d396f27c4b128b0df901e0f59ad27d259084b3fc/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/rviz/example.rviz: -------------------------------------------------------------------------------- 1 | Panels: 2 | - Class: rviz/Displays 3 | Help Height: 78 4 | Name: Displays 5 | Property Tree Widget: 6 | Expanded: 7 | - /Global Options1 8 | - /TF1/Frames1 9 | Splitter Ratio: 0.5 10 | Tree Height: 565 11 | - Class: rviz/Selection 12 | Name: Selection 13 | - Class: rviz/Tool Properties 14 | Expanded: 15 | - /2D Pose Estimate1 16 | - /2D Nav Goal1 17 | - /Publish Point1 18 | Name: Tool Properties 19 | Splitter Ratio: 0.588679016 20 | - Class: rviz/Views 21 | Expanded: 22 | - /Current View1 23 | Name: Views 24 | Splitter Ratio: 0.5 25 | - Class: rviz/Time 26 | Experimental: false 27 | Name: Time 28 | SyncMode: 0 29 | SyncSource: VLP-16 30 | Visualization Manager: 31 | Class: "" 32 | Displays: 33 | - Alpha: 0.5 34 | Cell Size: 1 35 | Class: rviz/Grid 36 | Color: 160; 160; 164 37 | Enabled: true 38 | Line Style: 39 | Line Width: 0.0299999993 40 | Value: Lines 41 | Name: Grid 42 | Normal Cell Count: 0 43 | Offset: 44 | X: 0 45 | Y: 0 46 | Z: 0 47 | Plane: XY 48 | Plane Cell Count: 10 49 | Reference Frame: 50 | Value: true 51 | - Class: rviz/TF 52 | Enabled: true 53 | Frame Timeout: 15 54 | Frames: 55 | All Enabled: true 56 | base_footprint: 57 | Value: true 58 | base_link: 59 | Value: true 60 | velodyne: 61 | Value: true 62 | velodyne2: 63 | Value: true 64 | velodyne2_base_link: 65 | Value: true 66 | velodyne_base_link: 67 | Value: true 68 | Marker Scale: 1 69 | Name: TF 70 | Show Arrows: true 71 | Show Axes: true 72 | Show Names: true 73 | Tree: 74 | base_footprint: 75 | base_link: 76 | velodyne2_base_link: 77 | velodyne2: 78 | {} 79 | velodyne_base_link: 80 | velodyne: 81 | {} 82 | Update Interval: 0 83 | Value: true 84 | - Alpha: 1 85 | Class: rviz/RobotModel 86 | Collision Enabled: false 87 | Enabled: true 88 | Links: 89 | All Links Enabled: true 90 | Expand Joint Details: false 91 | Expand Link Details: false 92 | Expand Tree: false 93 | Link Tree Style: Links in Alphabetic Order 94 | base_footprint: 95 | Alpha: 1 96 | Show Axes: false 97 | Show Trail: false 98 | base_link: 99 | Alpha: 1 100 | Show Axes: false 101 | Show Trail: false 102 | Value: true 103 | velodyne: 104 | Alpha: 1 105 | Show Axes: false 106 | Show Trail: false 107 | Value: true 108 | velodyne2: 109 | Alpha: 1 110 | Show Axes: false 111 | Show Trail: false 112 | Value: true 113 | velodyne2_base_link: 114 | Alpha: 1 115 | Show Axes: false 116 | Show Trail: false 117 | Value: true 118 | velodyne_base_link: 119 | Alpha: 1 120 | Show Axes: false 121 | Show Trail: false 122 | Value: true 123 | Name: RobotModel 124 | Robot Description: robot_description 125 | TF Prefix: "" 126 | Update Interval: 0 127 | Value: true 128 | Visual Enabled: true 129 | - Alpha: 1 130 | Autocompute Intensity Bounds: true 131 | Autocompute Value Bounds: 132 | Max Value: 10 133 | Min Value: -10 134 | Value: true 135 | Axis: Z 136 | Channel Name: ring 137 | Class: rviz/PointCloud2 138 | Color: 255; 255; 255 139 | Color Transformer: Intensity 140 | Decay Time: 0 141 | Enabled: true 142 | Invert Rainbow: false 143 | Max Color: 255; 255; 255 144 | Max Intensity: 7 145 | Min Color: 0; 0; 0 146 | Min Intensity: 0 147 | Name: VLP-16 148 | Position Transformer: XYZ 149 | Queue Size: 10 150 | Selectable: true 151 | Size (Pixels): 3 152 | Size (m): 0.00999999978 153 | Style: Points 154 | Topic: /velodyne_points 155 | Unreliable: false 156 | Use Fixed Frame: true 157 | Use rainbow: true 158 | Value: true 159 | - Alpha: 1 160 | Autocompute Intensity Bounds: true 161 | Autocompute Value Bounds: 162 | Max Value: 10 163 | Min Value: -10 164 | Value: true 165 | Axis: Z 166 | Channel Name: ring 167 | Class: rviz/PointCloud2 168 | Color: 255; 255; 255 169 | Color Transformer: Intensity 170 | Decay Time: 0 171 | Enabled: false 172 | Invert Rainbow: false 173 | Max Color: 255; 255; 255 174 | Max Intensity: 22 175 | Min Color: 0; 0; 0 176 | Min Intensity: 0 177 | Name: HDL-32E 178 | Position Transformer: XYZ 179 | Queue Size: 10 180 | Selectable: true 181 | Size (Pixels): 3 182 | Size (m): 0.00999999978 183 | Style: Points 184 | Topic: /velodyne_points2 185 | Unreliable: false 186 | Use Fixed Frame: true 187 | Use rainbow: true 188 | Value: false 189 | Enabled: true 190 | Global Options: 191 | Background Color: 48; 48; 48 192 | Fixed Frame: base_footprint 193 | Frame Rate: 30 194 | Name: root 195 | Tools: 196 | - Class: rviz/Interact 197 | Hide Inactive Objects: true 198 | - Class: rviz/MoveCamera 199 | - Class: rviz/Select 200 | - Class: rviz/FocusCamera 201 | - Class: rviz/Measure 202 | - Class: rviz/SetInitialPose 203 | Topic: /initialpose 204 | - Class: rviz/SetGoal 205 | Topic: /move_base_simple/goal 206 | - Class: rviz/PublishPoint 207 | Single click: true 208 | Topic: /clicked_point 209 | Value: true 210 | Views: 211 | Current: 212 | Class: rviz/XYOrbit 213 | Distance: 2.07834911 214 | Enable Stereo Rendering: 215 | Stereo Eye Separation: 0.0599999987 216 | Stereo Focal Distance: 1 217 | Swap Stereo Eyes: false 218 | Value: false 219 | Focal Point: 220 | X: 0 221 | Y: 0 222 | Z: 0 223 | Focal Shape Fixed Size: false 224 | Focal Shape Size: 0.0500000007 225 | Invert Z Axis: false 226 | Name: Current View 227 | Near Clip Distance: 0.00999999978 228 | Pitch: 0.370399028 229 | Target Frame: 230 | Value: XYOrbit (rviz) 231 | Yaw: 5.54361677 232 | Saved: ~ 233 | Window Geometry: 234 | Displays: 235 | collapsed: false 236 | Height: 846 237 | Hide Left Dock: false 238 | Hide Right Dock: true 239 | QMainWindow State: 000000ff00000000fd00000004000000000000016a000002c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000006400fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c0061007900730100000028000002c4000000dd00fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000002c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a005600690065007700730000000028000002c4000000b000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000004b00000003efc0100000002fb0000000800540069006d00650100000000000004b00000030000fffffffb0000000800540069006d0065010000000000000450000000000000000000000340000002c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 240 | Selection: 241 | collapsed: false 242 | Time: 243 | collapsed: false 244 | Tool Properties: 245 | collapsed: false 246 | Views: 247 | collapsed: true 248 | Width: 1200 249 | X: 2457 250 | Y: 75 251 | -------------------------------------------------------------------------------- /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 | install(PROGRAMS scripts/realTimePlot.py 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/scripts 70 | ) 71 | 72 | -------------------------------------------------------------------------------- /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/visualization_tools/scripts/realTimePlot.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/python3 2 | 3 | import numpy as np 4 | import matplotlib.pyplot as plt 5 | import matplotlib as mpl 6 | 7 | import rospy 8 | from std_msgs.msg import Float32 9 | 10 | mpl.rcParams['toolbar'] = 'None' 11 | plt.ion() 12 | 13 | time_duration = 0 14 | start_time_duration = 0 15 | first_iteration = 'True' 16 | 17 | explored_volume = 0; 18 | traveling_distance = 0; 19 | run_time = 0; 20 | max_explored_volume = 0 21 | max_traveling_diatance = 0 22 | max_run_time = 0 23 | 24 | time_list1 = np.array([]) 25 | time_list2 = np.array([]) 26 | time_list3 = np.array([]) 27 | run_time_list = np.array([]) 28 | explored_volume_list = np.array([]) 29 | traveling_distance_list = np.array([]) 30 | 31 | def timeDurationCallback(msg): 32 | global time_duration, start_time_duration, first_iteration 33 | time_duration = msg.data 34 | if first_iteration == 'True': 35 | start_time_duration = time_duration 36 | first_iteration = 'False' 37 | 38 | def runTimeCallback(msg): 39 | global run_time 40 | run_time = msg.data 41 | 42 | def exploredVolumeCallback(msg): 43 | global explored_volume 44 | explored_volume = msg.data 45 | 46 | 47 | def travelingDistanceCallback(msg): 48 | global traveling_distance 49 | traveling_distance = msg.data 50 | 51 | def listener(): 52 | global time_duration, start_time_duration, explored_volume, traveling_distance, run_time, max_explored_volume, max_traveling_diatance, max_run_time, time_list1, time_list2, time_list3, run_time_list, explored_volume_list, traveling_distance_list 53 | 54 | rospy.init_node('realTimePlot') 55 | rospy.Subscriber("/time_duration", Float32, timeDurationCallback) 56 | rospy.Subscriber("/runtime", Float32, runTimeCallback) 57 | rospy.Subscriber("/explored_volume", Float32, exploredVolumeCallback) 58 | rospy.Subscriber("/traveling_distance", Float32, travelingDistanceCallback) 59 | 60 | fig=plt.figure(figsize=(8,7)) 61 | fig1=fig.add_subplot(311) 62 | plt.title("Exploration Metrics\n", fontsize=14) 63 | plt.margins(x=0.001) 64 | fig1.set_ylabel("Explored\nVolume (m$^3$)", fontsize=12) 65 | l1, = fig1.plot(time_list2, explored_volume_list, color='r', label='Explored Volume') 66 | fig2=fig.add_subplot(312) 67 | fig2.set_ylabel("Traveling\nDistance (m)", fontsize=12) 68 | l2, = fig2.plot(time_list3, traveling_distance_list, color='r', label='Traveling Distance') 69 | fig3=fig.add_subplot(313) 70 | fig3.set_ylabel("Algorithm\nRuntime (s)", fontsize=12) 71 | fig3.set_xlabel("Time Duration (s)", fontsize=12) #only set once 72 | l3, = fig3.plot(time_list1, run_time_list, color='r', label='Algorithm Runtime') 73 | 74 | count = 0 75 | r = rospy.Rate(100) # 100hz 76 | while not rospy.is_shutdown(): 77 | r.sleep() 78 | count = count + 1 79 | 80 | if count % 25 == 0: 81 | max_explored_volume = explored_volume 82 | max_traveling_diatance = traveling_distance 83 | if run_time > max_run_time: 84 | max_run_time = run_time 85 | 86 | time_list2 = np.append(time_list2, time_duration) 87 | explored_volume_list = np.append(explored_volume_list, explored_volume) 88 | time_list3 = np.append(time_list3, time_duration) 89 | traveling_distance_list = np.append(traveling_distance_list, traveling_distance) 90 | time_list1 = np.append(time_list1, time_duration) 91 | run_time_list = np.append(run_time_list, run_time) 92 | 93 | if count >= 100: 94 | count = 0 95 | l1.set_xdata(time_list2) 96 | l2.set_xdata(time_list3) 97 | l3.set_xdata(time_list1) 98 | l1.set_ydata(explored_volume_list) 99 | l2.set_ydata(traveling_distance_list) 100 | l3.set_ydata(run_time_list) 101 | 102 | fig1.set_ylim(0, max_explored_volume + 500) 103 | fig1.set_xlim(start_time_duration, time_duration + 10) 104 | fig2.set_ylim(0, max_traveling_diatance + 20) 105 | fig2.set_xlim(start_time_duration, time_duration + 10) 106 | fig3.set_ylim(0, max_run_time + 0.2) 107 | fig3.set_xlim(start_time_duration, time_duration + 10) 108 | 109 | fig.canvas.draw() 110 | 111 | if __name__ == '__main__': 112 | listener() 113 | print("1") 114 | -------------------------------------------------------------------------------- /src/visualization_tools/src/visualizationTools.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace std; 28 | 29 | const double PI = 3.1415926; 30 | 31 | string metricFile; 32 | string trajFile; 33 | string mapFile; 34 | double overallMapVoxelSize = 0.5; 35 | double exploredAreaVoxelSize = 0.3; 36 | double exploredVolumeVoxelSize = 0.5; 37 | double transInterval = 0.2; 38 | double yawInterval = 10.0; 39 | int overallMapDisplayInterval = 2; 40 | int overallMapDisplayCount = 0; 41 | int exploredAreaDisplayInterval = 1; 42 | int exploredAreaDisplayCount = 0; 43 | 44 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 45 | pcl::PointCloud::Ptr overallMapCloud(new pcl::PointCloud()); 46 | pcl::PointCloud::Ptr overallMapCloudDwz(new pcl::PointCloud()); 47 | pcl::PointCloud::Ptr exploredAreaCloud(new pcl::PointCloud()); 48 | pcl::PointCloud::Ptr exploredAreaCloud2(new pcl::PointCloud()); 49 | pcl::PointCloud::Ptr exploredVolumeCloud(new pcl::PointCloud()); 50 | pcl::PointCloud::Ptr exploredVolumeCloud2(new pcl::PointCloud()); 51 | pcl::PointCloud::Ptr trajectory(new pcl::PointCloud()); 52 | 53 | const int systemDelay = 5; 54 | int systemDelayCount = 0; 55 | bool systemDelayInited = false; 56 | double systemTime = 0; 57 | double systemInitTime = 0; 58 | bool systemInited = false; 59 | 60 | float vehicleYaw = 0; 61 | float vehicleX = 0, vehicleY = 0, vehicleZ = 0; 62 | float exploredVolume = 0, travelingDis = 0, runtime = 0, timeDuration = 0; 63 | 64 | pcl::VoxelGrid overallMapDwzFilter; 65 | pcl::VoxelGrid exploredAreaDwzFilter; 66 | pcl::VoxelGrid exploredVolumeDwzFilter; 67 | 68 | sensor_msgs::PointCloud2 overallMap2; 69 | 70 | ros::Publisher *pubExploredAreaPtr = NULL; 71 | ros::Publisher *pubTrajectoryPtr = NULL; 72 | ros::Publisher *pubExploredVolumePtr = NULL; 73 | ros::Publisher *pubTravelingDisPtr = NULL; 74 | ros::Publisher *pubTimeDurationPtr = NULL; 75 | 76 | FILE *metricFilePtr = NULL; 77 | FILE *trajFilePtr = NULL; 78 | 79 | void odometryHandler(const nav_msgs::Odometry::ConstPtr& odom) 80 | { 81 | systemTime = odom->header.stamp.toSec(); 82 | 83 | double roll, pitch, yaw; 84 | geometry_msgs::Quaternion geoQuat = odom->pose.pose.orientation; 85 | tf::Matrix3x3(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w)).getRPY(roll, pitch, yaw); 86 | 87 | float dYaw = fabs(yaw - vehicleYaw); 88 | if (dYaw > PI) dYaw = 2 * PI - dYaw; 89 | 90 | float dx = odom->pose.pose.position.x - vehicleX; 91 | float dy = odom->pose.pose.position.y - vehicleY; 92 | float dz = odom->pose.pose.position.z - vehicleZ; 93 | float dis = sqrt(dx * dx + dy * dy + dz * dz); 94 | 95 | if (!systemDelayInited) { 96 | vehicleYaw = yaw; 97 | vehicleX = odom->pose.pose.position.x; 98 | vehicleY = odom->pose.pose.position.y; 99 | vehicleZ = odom->pose.pose.position.z; 100 | return; 101 | } 102 | 103 | if (systemInited) { 104 | timeDuration = systemTime - systemInitTime; 105 | 106 | std_msgs::Float32 timeDurationMsg; 107 | timeDurationMsg.data = timeDuration; 108 | pubTimeDurationPtr->publish(timeDurationMsg); 109 | } 110 | 111 | if (dis < transInterval && dYaw < yawInterval) { 112 | return; 113 | } 114 | 115 | if (!systemInited) { 116 | dis = 0; 117 | systemInitTime = systemTime; 118 | systemInited = true; 119 | } 120 | 121 | travelingDis += dis; 122 | 123 | vehicleYaw = yaw; 124 | vehicleX = odom->pose.pose.position.x; 125 | vehicleY = odom->pose.pose.position.y; 126 | vehicleZ = odom->pose.pose.position.z; 127 | 128 | fprintf(trajFilePtr, "%f %f %f %f %f %f %f\n", vehicleX, vehicleY, vehicleZ, roll, pitch, yaw, timeDuration); 129 | 130 | pcl::PointXYZI point; 131 | point.x = vehicleX; 132 | point.y = vehicleY; 133 | point.z = vehicleZ; 134 | point.intensity = travelingDis; 135 | trajectory->push_back(point); 136 | 137 | sensor_msgs::PointCloud2 trajectory2; 138 | pcl::toROSMsg(*trajectory, trajectory2); 139 | trajectory2.header.stamp = odom->header.stamp; 140 | trajectory2.header.frame_id = "map"; 141 | pubTrajectoryPtr->publish(trajectory2); 142 | } 143 | 144 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn) 145 | { 146 | if (!systemDelayInited) { 147 | systemDelayCount++; 148 | if (systemDelayCount > systemDelay) { 149 | systemDelayInited = true; 150 | } 151 | } 152 | 153 | if (!systemInited) { 154 | return; 155 | } 156 | 157 | laserCloud->clear(); 158 | pcl::fromROSMsg(*laserCloudIn, *laserCloud); 159 | 160 | *exploredVolumeCloud += *laserCloud; 161 | 162 | exploredVolumeCloud2->clear(); 163 | exploredVolumeDwzFilter.setInputCloud(exploredVolumeCloud); 164 | exploredVolumeDwzFilter.filter(*exploredVolumeCloud2); 165 | 166 | pcl::PointCloud::Ptr tempCloud = exploredVolumeCloud; 167 | exploredVolumeCloud = exploredVolumeCloud2; 168 | exploredVolumeCloud2 = tempCloud; 169 | 170 | exploredVolume = exploredVolumeVoxelSize * exploredVolumeVoxelSize * 171 | exploredVolumeVoxelSize * exploredVolumeCloud->points.size(); 172 | 173 | *exploredAreaCloud += *laserCloud; 174 | 175 | exploredAreaDisplayCount++; 176 | if (exploredAreaDisplayCount >= 5 * exploredAreaDisplayInterval) { 177 | exploredAreaCloud2->clear(); 178 | exploredAreaDwzFilter.setInputCloud(exploredAreaCloud); 179 | exploredAreaDwzFilter.filter(*exploredAreaCloud2); 180 | 181 | tempCloud = exploredAreaCloud; 182 | exploredAreaCloud = exploredAreaCloud2; 183 | exploredAreaCloud2 = tempCloud; 184 | 185 | sensor_msgs::PointCloud2 exploredArea2; 186 | pcl::toROSMsg(*exploredAreaCloud, exploredArea2); 187 | exploredArea2.header.stamp = laserCloudIn->header.stamp; 188 | exploredArea2.header.frame_id = "map"; 189 | pubExploredAreaPtr->publish(exploredArea2); 190 | 191 | exploredAreaDisplayCount = 0; 192 | } 193 | 194 | fprintf(metricFilePtr, "%f %f %f %f\n", exploredVolume, travelingDis, runtime, timeDuration); 195 | 196 | std_msgs::Float32 exploredVolumeMsg; 197 | exploredVolumeMsg.data = exploredVolume; 198 | pubExploredVolumePtr->publish(exploredVolumeMsg); 199 | 200 | std_msgs::Float32 travelingDisMsg; 201 | travelingDisMsg.data = travelingDis; 202 | pubTravelingDisPtr->publish(travelingDisMsg); 203 | } 204 | 205 | void runtimeHandler(const std_msgs::Float32::ConstPtr& runtimeIn) 206 | { 207 | runtime = runtimeIn->data; 208 | } 209 | 210 | int main(int argc, char** argv) 211 | { 212 | ros::init(argc, argv, "visualizationTools"); 213 | ros::NodeHandle nh; 214 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 215 | 216 | nhPrivate.getParam("metricFile", metricFile); 217 | nhPrivate.getParam("trajFile", trajFile); 218 | nhPrivate.getParam("mapFile", mapFile); 219 | nhPrivate.getParam("overallMapVoxelSize", overallMapVoxelSize); 220 | nhPrivate.getParam("exploredAreaVoxelSize", exploredAreaVoxelSize); 221 | nhPrivate.getParam("exploredVolumeVoxelSize", exploredVolumeVoxelSize); 222 | nhPrivate.getParam("transInterval", transInterval); 223 | nhPrivate.getParam("yawInterval", yawInterval); 224 | nhPrivate.getParam("overallMapDisplayInterval", overallMapDisplayInterval); 225 | nhPrivate.getParam("exploredAreaDisplayInterval", exploredAreaDisplayInterval); 226 | 227 | ros::Subscriber subOdometry = nh.subscribe ("/state_estimation", 5, odometryHandler); 228 | 229 | ros::Subscriber subLaserCloud = nh.subscribe ("/registered_scan", 5, laserCloudHandler); 230 | 231 | ros::Subscriber subRuntime = nh.subscribe ("/runtime", 5, runtimeHandler); 232 | 233 | ros::Publisher pubOverallMap = nh.advertise ("/overall_map", 5); 234 | 235 | ros::Publisher pubExploredArea = nh.advertise ("/explored_areas", 5); 236 | pubExploredAreaPtr = &pubExploredArea; 237 | 238 | ros::Publisher pubTrajectory = nh.advertise ("/trajectory", 5); 239 | pubTrajectoryPtr = &pubTrajectory; 240 | 241 | ros::Publisher pubExploredVolume = nh.advertise ("/explored_volume", 5); 242 | pubExploredVolumePtr = &pubExploredVolume; 243 | 244 | ros::Publisher pubTravelingDis = nh.advertise ("/traveling_distance", 5); 245 | pubTravelingDisPtr = &pubTravelingDis; 246 | 247 | ros::Publisher pubTimeDuration = nh.advertise ("/time_duration", 5); 248 | pubTimeDurationPtr = &pubTimeDuration; 249 | 250 | //ros::Publisher pubRuntime = nh.advertise ("/runtime", 5); 251 | 252 | overallMapDwzFilter.setLeafSize(overallMapVoxelSize, overallMapVoxelSize, overallMapVoxelSize); 253 | exploredAreaDwzFilter.setLeafSize(exploredAreaVoxelSize, exploredAreaVoxelSize, exploredAreaVoxelSize); 254 | exploredVolumeDwzFilter.setLeafSize(exploredVolumeVoxelSize, exploredVolumeVoxelSize, exploredVolumeVoxelSize); 255 | 256 | pcl::PLYReader ply_reader; 257 | if (ply_reader.read(mapFile, *overallMapCloud) == -1) { 258 | printf("\nCouldn't read pointcloud.ply file.\n\n"); 259 | } 260 | 261 | overallMapCloudDwz->clear(); 262 | overallMapDwzFilter.setInputCloud(overallMapCloud); 263 | overallMapDwzFilter.filter(*overallMapCloudDwz); 264 | overallMapCloud->clear(); 265 | 266 | pcl::toROSMsg(*overallMapCloudDwz, overallMap2); 267 | 268 | time_t logTime = time(0); 269 | tm *ltm = localtime(&logTime); 270 | string timeString = to_string(1900 + ltm->tm_year) + "-" + to_string(1 + ltm->tm_mon) + "-" + to_string(ltm->tm_mday) + "-" + 271 | to_string(ltm->tm_hour) + "-" + to_string(ltm->tm_min) + "-" + to_string(ltm->tm_sec); 272 | 273 | metricFile += "_" + timeString + ".txt"; 274 | trajFile += "_" + timeString + ".txt"; 275 | metricFilePtr = fopen(metricFile.c_str(), "w"); 276 | trajFilePtr = fopen(trajFile.c_str(), "w"); 277 | 278 | ros::Rate rate(100); 279 | bool status = ros::ok(); 280 | while (status) { 281 | ros::spinOnce(); 282 | 283 | overallMapDisplayCount++; 284 | if (overallMapDisplayCount >= 100 * overallMapDisplayInterval) { 285 | overallMap2.header.stamp = ros::Time().fromSec(systemTime); 286 | overallMap2.header.frame_id = "map"; 287 | pubOverallMap.publish(overallMap2); 288 | 289 | overallMapDisplayCount = 0; 290 | } 291 | 292 | status = ros::ok(); 293 | rate.sleep(); 294 | } 295 | 296 | fclose(metricFilePtr); 297 | fclose(trajFilePtr); 298 | 299 | printf("\nExploration metrics and vehicle trajectory are saved in 'src/vehicle_simulator/log'.\n\n"); 300 | 301 | return 0; 302 | } 303 | -------------------------------------------------------------------------------- /src/waypoint_example/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_example) 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(waypointExample src/waypointExample.cpp) 53 | 54 | ## Specify libraries to link a library or executable target against 55 | target_link_libraries(waypointExample ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 56 | 57 | install(TARGETS waypointExample 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_example/data/boundary_garage.ply: -------------------------------------------------------------------------------- 1 | ply 2 | format ascii 1.0 3 | element vertex 9 4 | property float x 5 | property float y 6 | property float z 7 | end_header 8 | -7.0 -4.0 0 9 | -7.0 84.0 0 10 | 54.5 84.0 0 11 | 54.5 8.0 0 12 | 28.0 8.0 0 13 | 28.0 70.0 0 14 | 24.0 70.0 0 15 | 24.0 -4.0 0 16 | -7.0 -4.0 0 17 | -------------------------------------------------------------------------------- /src/waypoint_example/data/waypoints_garage.ply: -------------------------------------------------------------------------------- 1 | ply 2 | format ascii 1.0 3 | element vertex 9 4 | property float x 5 | property float y 6 | property float z 7 | end_header 8 | 10.0 28.0 0.8 9 | 4.0 69.0 0.8 10 | 40.0 69.0 1.7 11 | 40.0 16.0 4.3 12 | 49.0 16.0 4.3 13 | 49.0 78.0 1.7 14 | -1.0 78.0 0.8 15 | 10.0 28.0 0.8 16 | 0 0 0.8 17 | -------------------------------------------------------------------------------- /src/waypoint_example/launch/waypoint_example_garage.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /src/waypoint_example/package.xml: -------------------------------------------------------------------------------- 1 | 2 | waypoint_example 3 | 0.0.1 4 | Waypoint Example 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_example/src/waypointExample.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace std; 27 | 28 | const double PI = 3.1415926; 29 | 30 | string waypoint_file_dir; 31 | string boundary_file_dir; 32 | double waypointXYRadius = 0.5; 33 | double waypointZBound = 5.0; 34 | double waitTime = 0; 35 | double waitTimeStart = 0; 36 | bool isWaiting = false; 37 | double frameRate = 5.0; 38 | double speed = 1.0; 39 | bool sendSpeed = true; 40 | bool sendBoundary = true; 41 | 42 | pcl::PointCloud::Ptr waypoints(new pcl::PointCloud()); 43 | pcl::PointCloud::Ptr boundary(new pcl::PointCloud()); 44 | 45 | float vehicleX = 0, vehicleY = 0, vehicleZ = 0; 46 | double curTime = 0, waypointTime = 0; 47 | 48 | // reading waypoints from file function 49 | void readWaypointFile() 50 | { 51 | FILE* waypoint_file = fopen(waypoint_file_dir.c_str(), "r"); 52 | if (waypoint_file == NULL) { 53 | printf ("\nCannot read input files, exit.\n\n"); 54 | exit(1); 55 | } 56 | 57 | char str[50]; 58 | int val, pointNum; 59 | string strCur, strLast; 60 | while (strCur != "end_header") { 61 | val = fscanf(waypoint_file, "%s", str); 62 | if (val != 1) { 63 | printf ("\nError reading input files, exit.\n\n"); 64 | exit(1); 65 | } 66 | 67 | strLast = strCur; 68 | strCur = string(str); 69 | 70 | if (strCur == "vertex" && strLast == "element") { 71 | val = fscanf(waypoint_file, "%d", &pointNum); 72 | if (val != 1) { 73 | printf ("\nError reading input files, exit.\n\n"); 74 | exit(1); 75 | } 76 | } 77 | } 78 | 79 | waypoints->clear(); 80 | pcl::PointXYZ point; 81 | int val1, val2, val3; 82 | for (int i = 0; i < pointNum; i++) { 83 | val1 = fscanf(waypoint_file, "%f", &point.x); 84 | val2 = fscanf(waypoint_file, "%f", &point.y); 85 | val3 = fscanf(waypoint_file, "%f", &point.z); 86 | 87 | if (val1 != 1 || val2 != 1 || val3 != 1) { 88 | printf ("\nError reading input files, exit.\n\n"); 89 | exit(1); 90 | } 91 | 92 | waypoints->push_back(point); 93 | } 94 | 95 | fclose(waypoint_file); 96 | } 97 | 98 | // reading boundary from file function 99 | void readBoundaryFile() 100 | { 101 | FILE* boundary_file = fopen(boundary_file_dir.c_str(), "r"); 102 | if (boundary_file == NULL) { 103 | printf ("\nCannot read input files, exit.\n\n"); 104 | exit(1); 105 | } 106 | 107 | char str[50]; 108 | int val, pointNum; 109 | string strCur, strLast; 110 | while (strCur != "end_header") { 111 | val = fscanf(boundary_file, "%s", str); 112 | if (val != 1) { 113 | printf ("\nError reading input files, exit.\n\n"); 114 | exit(1); 115 | } 116 | 117 | strLast = strCur; 118 | strCur = string(str); 119 | 120 | if (strCur == "vertex" && strLast == "element") { 121 | val = fscanf(boundary_file, "%d", &pointNum); 122 | if (val != 1) { 123 | printf ("\nError reading input files, exit.\n\n"); 124 | exit(1); 125 | } 126 | } 127 | } 128 | 129 | boundary->clear(); 130 | pcl::PointXYZ point; 131 | int val1, val2, val3; 132 | for (int i = 0; i < pointNum; i++) { 133 | val1 = fscanf(boundary_file, "%f", &point.x); 134 | val2 = fscanf(boundary_file, "%f", &point.y); 135 | val3 = fscanf(boundary_file, "%f", &point.z); 136 | 137 | if (val1 != 1 || val2 != 1 || val3 != 1) { 138 | printf ("\nError reading input files, exit.\n\n"); 139 | exit(1); 140 | } 141 | 142 | boundary->push_back(point); 143 | } 144 | 145 | fclose(boundary_file); 146 | } 147 | 148 | // vehicle pose callback function 149 | void poseHandler(const nav_msgs::Odometry::ConstPtr& pose) 150 | { 151 | curTime = pose->header.stamp.toSec(); 152 | 153 | vehicleX = pose->pose.pose.position.x; 154 | vehicleY = pose->pose.pose.position.y; 155 | vehicleZ = pose->pose.pose.position.z; 156 | } 157 | 158 | int main(int argc, char** argv) 159 | { 160 | ros::init(argc, argv, "waypointExample"); 161 | ros::NodeHandle nh; 162 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 163 | 164 | nhPrivate.getParam("waypoint_file_dir", waypoint_file_dir); 165 | nhPrivate.getParam("boundary_file_dir", boundary_file_dir); 166 | nhPrivate.getParam("waypointXYRadius", waypointXYRadius); 167 | nhPrivate.getParam("waypointZBound", waypointZBound); 168 | nhPrivate.getParam("waitTime", waitTime); 169 | nhPrivate.getParam("frameRate", frameRate); 170 | nhPrivate.getParam("speed", speed); 171 | nhPrivate.getParam("sendSpeed", sendSpeed); 172 | nhPrivate.getParam("sendBoundary", sendBoundary); 173 | 174 | ros::Subscriber subPose = nh.subscribe ("/state_estimation", 5, poseHandler); 175 | 176 | ros::Publisher pubWaypoint = nh.advertise ("/way_point", 5); 177 | geometry_msgs::PointStamped waypointMsgs; 178 | waypointMsgs.header.frame_id = "map"; 179 | 180 | ros::Publisher pubSpeed = nh.advertise ("/speed", 5); 181 | std_msgs::Float32 speedMsgs; 182 | 183 | ros::Publisher pubBoundary = nh.advertise ("/navigation_boundary", 5); 184 | geometry_msgs::PolygonStamped boundaryMsgs; 185 | boundaryMsgs.header.frame_id = "map"; 186 | 187 | // read waypoints from file 188 | readWaypointFile(); 189 | 190 | // read boundary from file 191 | if (sendBoundary) { 192 | readBoundaryFile(); 193 | 194 | int boundarySize = boundary->points.size(); 195 | boundaryMsgs.polygon.points.resize(boundarySize); 196 | for (int i = 0; i < boundarySize; i++) { 197 | boundaryMsgs.polygon.points[i].x = boundary->points[i].x; 198 | boundaryMsgs.polygon.points[i].y = boundary->points[i].y; 199 | boundaryMsgs.polygon.points[i].z = boundary->points[i].z; 200 | } 201 | } 202 | 203 | int wayPointID = 0; 204 | int waypointSize = waypoints->points.size(); 205 | 206 | if (waypointSize == 0) { 207 | printf ("\nNo waypoint available, exit.\n\n"); 208 | exit(1); 209 | } 210 | 211 | ros::Rate rate(100); 212 | bool status = ros::ok(); 213 | while (status) { 214 | ros::spinOnce(); 215 | 216 | float disX = vehicleX - waypoints->points[wayPointID].x; 217 | float disY = vehicleY - waypoints->points[wayPointID].y; 218 | float disZ = vehicleZ - waypoints->points[wayPointID].z; 219 | 220 | // start waiting if the current waypoint is reached 221 | if (sqrt(disX * disX + disY * disY) < waypointXYRadius && fabs(disZ) < waypointZBound && !isWaiting) { 222 | waitTimeStart = curTime; 223 | isWaiting = true; 224 | } 225 | 226 | // move to the next waypoint after waiting is over 227 | if (isWaiting && waitTimeStart + waitTime < curTime && wayPointID < waypointSize - 1) { 228 | wayPointID++; 229 | isWaiting = false; 230 | } 231 | 232 | // publish waypoint, speed, and boundary messages at certain frame rate 233 | if (curTime - waypointTime > 1.0 / frameRate) { 234 | if (!isWaiting) { 235 | waypointMsgs.header.stamp = ros::Time().fromSec(curTime); 236 | waypointMsgs.point.x = waypoints->points[wayPointID].x; 237 | waypointMsgs.point.y = waypoints->points[wayPointID].y; 238 | waypointMsgs.point.z = waypoints->points[wayPointID].z; 239 | pubWaypoint.publish(waypointMsgs); 240 | } 241 | 242 | if (sendSpeed) { 243 | speedMsgs.data = speed; 244 | pubSpeed.publish(speedMsgs); 245 | } 246 | 247 | if (sendBoundary) { 248 | boundaryMsgs.header.stamp = ros::Time().fromSec(curTime); 249 | pubBoundary.publish(boundaryMsgs); 250 | } 251 | 252 | waypointTime = curTime; 253 | } 254 | 255 | status = ros::ok(); 256 | rate.sleep(); 257 | } 258 | 259 | return 0; 260 | } 261 | -------------------------------------------------------------------------------- /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 | #include 11 | 12 | #include "rviz/display_context.h" 13 | #include "rviz/properties/string_property.h" 14 | #include "rviz/default_plugin/tools/pose_tool.h" 15 | 16 | namespace rviz 17 | { 18 | class StringProperty; 19 | 20 | class WaypointTool : public PoseTool 21 | { 22 | Q_OBJECT 23 | public: 24 | WaypointTool(); 25 | virtual ~WaypointTool() 26 | { 27 | } 28 | virtual void onInitialize(); 29 | 30 | protected: 31 | virtual void odomHandler(const nav_msgs::Odometry::ConstPtr& odom); 32 | virtual void onPoseSet(double x, double y, double theta); 33 | 34 | private Q_SLOTS: 35 | void updateTopic(); 36 | 37 | private: 38 | float vehicle_z; 39 | 40 | ros::NodeHandle nh_; 41 | ros::Subscriber sub_; 42 | ros::Publisher pub_; 43 | ros::Publisher pub_joy_; 44 | 45 | StringProperty* topic_property_; 46 | }; 47 | } 48 | 49 | #endif // WAYPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H 50 | -------------------------------------------------------------------------------- /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"); 17 | updateTopic(); 18 | vehicle_z = 0; 19 | } 20 | 21 | void WaypointTool::updateTopic() 22 | { 23 | sub_ = nh_.subscribe ("/state_estimation", 5, &WaypointTool::odomHandler, this); 24 | pub_ = nh_.advertise("/way_point", 5); 25 | pub_joy_ = nh_.advertise("/joy", 5); 26 | } 27 | 28 | void WaypointTool::odomHandler(const nav_msgs::Odometry::ConstPtr& odom) 29 | { 30 | vehicle_z = odom->pose.pose.position.z; 31 | } 32 | 33 | void WaypointTool::onPoseSet(double x, double y, double theta) 34 | { 35 | sensor_msgs::Joy joy; 36 | 37 | joy.axes.push_back(0); 38 | joy.axes.push_back(0); 39 | joy.axes.push_back(-1.0); 40 | joy.axes.push_back(0); 41 | joy.axes.push_back(1.0); 42 | joy.axes.push_back(1.0); 43 | joy.axes.push_back(0); 44 | joy.axes.push_back(0); 45 | 46 | joy.buttons.push_back(0); 47 | joy.buttons.push_back(0); 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(1); 54 | joy.buttons.push_back(0); 55 | joy.buttons.push_back(0); 56 | joy.buttons.push_back(0); 57 | 58 | joy.header.stamp = ros::Time::now(); 59 | joy.header.frame_id = "waypoint_tool"; 60 | pub_joy_.publish(joy); 61 | 62 | geometry_msgs::PointStamped waypoint; 63 | waypoint.header.frame_id = "map"; 64 | waypoint.header.stamp = joy.header.stamp; 65 | waypoint.point.x = x; 66 | waypoint.point.y = y; 67 | waypoint.point.z = vehicle_z; 68 | 69 | pub_.publish(waypoint); 70 | usleep(10000); 71 | pub_.publish(waypoint); 72 | } 73 | } 74 | 75 | #include 76 | PLUGINLIB_EXPORT_CLASS(rviz::WaypointTool, rviz::Tool) 77 | --------------------------------------------------------------------------------