├── .gitignore ├── README.md ├── img ├── ps3_controller.jpg ├── rviz_control_panel.jpg └── rviz_full.jpg ├── src ├── CMakeLists.txt ├── ROS-TCP-Endpoint │ ├── CHANGELOG.md │ ├── CMakeLists.txt │ ├── CODE_OF_CONDUCT.md │ ├── CONTRIBUTING.md │ ├── LICENSE │ ├── README.md │ ├── launch │ │ └── endpoint.launch │ ├── package.xml │ ├── requirements.txt │ ├── setup.py │ ├── src │ │ └── ros_tcp_endpoint │ │ │ ├── __init__.py │ │ │ ├── client.py │ │ │ ├── communication.py │ │ │ ├── default_server_endpoint.py │ │ │ ├── exceptions.py │ │ │ ├── publisher.py │ │ │ ├── server.py │ │ │ ├── service.py │ │ │ ├── subscriber.py │ │ │ ├── tcp_sender.py │ │ │ ├── thread_pauser.py │ │ │ └── unity_service.py │ └── test │ │ ├── __init__.py │ │ ├── test_client.py │ │ ├── test_publisher.py │ │ ├── test_ros_service.py │ │ ├── test_server.py │ │ ├── test_subscriber.py │ │ ├── test_tcp_sender.py │ │ ├── test_thread_pauser.py │ │ └── test_unity_service.py ├── bbox_calculation │ ├── CMakeLists.txt │ ├── launch │ │ ├── bbox_broadcast.launch │ │ └── bbox_calculation.launch │ ├── package.xml │ └── src │ │ ├── bboxBroadcast.cpp │ │ └── bboxCalculation.cpp ├── joystick_drivers │ ├── README.md │ ├── joy │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── config │ │ │ └── ps4joy.yaml │ │ ├── launch │ │ │ └── ps4joy.launch │ │ ├── mainpage.dox │ │ ├── migration_rules │ │ │ └── Joy.bmr │ │ ├── package.xml │ │ ├── scripts │ │ │ └── joy_remap.py │ │ ├── src │ │ │ └── joy_node.cpp │ │ └── test │ │ │ ├── saved │ │ │ └── Joy.saved │ │ │ └── test_joy_msg_migration.py │ ├── joystick_drivers │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ └── package.xml │ └── ps3joy │ │ ├── CHANGELOG.rst │ │ ├── CMakeLists.txt │ │ ├── README.md │ │ ├── diagnostics.yaml │ │ ├── doc │ │ ├── bluetooth_devices.md │ │ ├── testing.md │ │ └── troubleshooting.md │ │ ├── launch │ │ └── ps3.launch │ │ ├── package.xml │ │ ├── scripts │ │ ├── ps3joy.py │ │ ├── ps3joy_node.py │ │ └── ps3joysim.py │ │ └── src │ │ └── sixpair.c ├── local_planner │ ├── CMakeLists.txt │ ├── launch │ │ └── local_planner.launch │ ├── package.xml │ ├── paths │ │ ├── correspondences.txt │ │ ├── pathList.ply │ │ ├── path_generator.m │ │ ├── paths.ply │ │ └── startPaths.ply │ └── src │ │ ├── localPlanner.cpp │ │ └── pathFollower.cpp ├── score_calculation │ ├── CMakeLists.txt │ ├── data │ │ ├── trajectory_q4.ply │ │ └── trajectory_q5.ply │ ├── launch │ │ └── score_calculation.launch │ ├── package.xml │ └── src │ │ └── scoreCalculation.cpp ├── semantic_scan_generation │ ├── CMakeLists.txt │ ├── launch │ │ └── semantic_scan_generation.launch │ ├── package.xml │ └── src │ │ └── semanticScanGeneration.cpp ├── sensor_scan_generation │ ├── CMakeLists.txt │ ├── launch │ │ └── sensor_scan_generation.launch │ ├── package.xml │ └── src │ │ └── sensorScanGeneration.cpp ├── teleop_rviz_plugin │ ├── CMakeLists.txt │ ├── package.xml │ ├── plugin_description.xml │ └── src │ │ ├── drive_widget.cpp │ │ ├── drive_widget.h │ │ ├── teleop_panel.cpp │ │ └── teleop_panel.h ├── terrain_analysis │ ├── CMakeLists.txt │ ├── launch │ │ └── terrain_analysis.launch │ ├── package.xml │ └── src │ │ └── terrainAnalysis.cpp ├── terrain_analysis_ext │ ├── CMakeLists.txt │ ├── launch │ │ └── terrain_analysis_ext.launch │ ├── package.xml │ └── src │ │ └── terrainAnalysisExt.cpp ├── vehicle_simulator │ ├── CMakeLists.txt │ ├── launch │ │ ├── system_unity.launch │ │ └── vehicle_simulator.launch │ ├── log │ │ └── readme.txt │ ├── mesh │ │ └── unity │ │ │ └── readme.txt │ ├── package.xml │ ├── rviz │ │ └── vehicle_simulator.rviz │ └── src │ │ └── vehicleSimulator.cpp ├── visualization_tools │ ├── CMakeLists.txt │ ├── launch │ │ └── visualization_tools.launch │ ├── package.xml │ ├── scripts │ │ └── realTimePlot.py │ └── src │ │ └── visualizationTools.cpp ├── waypoint_converter │ ├── CMakeLists.txt │ ├── launch │ │ └── waypoint_converter.launch │ ├── package.xml │ └── src │ │ └── waypointConverter.cpp └── waypoint_rviz_plugin │ ├── CMakeLists.txt │ ├── include │ └── waypoint_tool.h │ ├── package.xml │ ├── plugin_description.xml │ └── src │ └── waypoint_tool.cpp └── system_bring_up.sh /.gitignore: -------------------------------------------------------------------------------- 1 | build 2 | 3 | devel 4 | 5 | install 6 | 7 | .catkin_workspace 8 | 9 | __pycache__ 10 | 11 | *~ 12 | 13 | .DS_Store 14 | 15 | src/vehicle_simulator/mesh/* 16 | 17 | !src/vehicle_simulator/mesh/unity 18 | 19 | src/vehicle_simulator/mesh/unity/* 20 | 21 | src/vehicle_simulator/log/* 22 | 23 | !readme.txt 24 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | ## Repository Setup 2 | 3 | The repository provides the base navigation system for the [CMU Vision-Language-Autonomy Challenge](https://www.ai-meets-autonomy.com/cmu-vla-challenge). The system is integrated with [Unity](https://unity.com) environment models. The repository has been tested in Ubuntu 20.04 with [ROS Noetic](http://wiki.ros.org/noetic/Installation). Install dependencies with the command lines below. 4 | ``` 5 | sudo apt update 6 | sudo apt install libusb-dev python-yaml python-is-python3 7 | ``` 8 | Clone the open-source repository. 9 | ``` 10 | git clone https://github.com/jizhang-cmu/cmu_vla_challenge_unity.git 11 | ``` 12 | In a terminal, go to the folder, checkout the 'noetic' branch, and compile. 13 | ``` 14 | cd cmu_vla_challenge_unity 15 | git checkout noetic 16 | catkin_make 17 | ``` 18 | Download any of our [Unity environment models](https://drive.google.com/drive/folders/1bmxdT6Oxzt0_0tohye2br7gqTnkMaq20?usp=share_link) and unzip the files to the 'src/vehicle_simulator/mesh/unity' folder. The environment model files should look like below. Note that the 'AssetList.csv' file is generated upon start of the system. 19 | 20 | mesh/
21 |     unity/
22 |         environment/
23 |             Model_Data/ (multiple files in the folder)
24 |             Model.x86_64
25 |             UnityPlayer.so
26 |             AssetList.csv (generated at runtime)
27 |             Dimensions.csv
28 |             Categories.csv
29 |         map.ply
30 |         object_list.txt
31 |         traversable_area.ply
32 |         map.jpg
33 |         render.jpg
34 | 35 | ## System Launch 36 | 37 | In a terminal, go to the 'cmu_vla_challenge_unity' folder and bring up the system. 38 | ``` 39 | ./system_bring_up.sh 40 | ``` 41 | Users should see data showing up in RVIZ. Click 'Panels->Display' and check the data to view. Users can also use the 'Waypoint with Heading' button to navigate the vehicle. **To set the waypoint, press the left button on the mouse to choose the position, then move the mouse to choose the orientation before releasing the left button.** The vehicle will navigate to the waypoint avoiding collisions on the way and then turn to the orientation. [A video showing the system in action is available.](https://youtu.be/KSoiDJHShU8) Note that the waypoints are meant to be relatively close to the vehicle. Setting the waypoint too far can cause the vehicle to stuck at a dead end. 42 | 43 |

44 | RVIZ Full 45 |

46 | 47 | Users can also use the control panel to navigate the vehicle by clicking inside the black box. The system will switch to *smart joystick* mode - the vehicle tries to follow the joystick command and avoid collisions at the same time. To resume waypoint navigation, press the 'Resume Navigation to Goal' button. Note that users can use a PS3/4 or Xbox controller with a USB or Bluetooth interface instead of the virtual joystick (If using the Xbox Wireless USB Adapter, please install [xow](https://github.com/medusalix/xow)). Users can use the right joystick on the controller to navigate the vehicle. Holding the obstacle-check button cancels obstacle checking and clicking the clear-terrain-map button reinitializes the terrain map. To resume waypoint navigation, hold the mode-switch button and at the same time push the right joystick. This way, the right joystick gives the speed. If only holding the mode-switch button, the system will use the default speed. 48 | 49 |

50 | RVIZ Control Panel 51 |      52 | PS3 Controller 53 |

54 | 55 | **Troubleshooting** - If the system does not launch correctly, users can launch the Unity environment and the autonomy system in two separate terminals. In a terminal, go to the 'cmu_vla_challange_unity' folder and launch the Unity environment. 56 | ``` 57 | ./src/vehicle_simulator/mesh/unity/environment/Model.x86_64 58 | ``` 59 | In a second terminal, go to the 'cmu_vla_challange_unity' folder and bring up the autonomy system. 60 | 61 | ``` 62 | source devel/setup.sh  63 | roslaunch vehicle_simulator system_unity.launch  64 | ``` 65 | 66 | ## Credits 67 | 68 | [ROS-TCP-Endpoint](https://github.com/Unity-Technologies/ROS-TCP-Endpoint) and [joystick_drivers](http://wiki.ros.org/joystick_drivers) packages are from open-source releases. 69 | 70 | ## Relevant Links 71 | 72 | The repository is based on [Autonomous Exploration Development Environment](https://www.cmu-exploration.com). 73 | 74 | [Far Planner](https://github.com/MichaelFYang/far_planner): a visibility graph-based route planner. 75 | -------------------------------------------------------------------------------- /img/ps3_controller.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_unity/b2349e730e3cd51abcbd1a626414fe85d398e1db/img/ps3_controller.jpg -------------------------------------------------------------------------------- /img/rviz_control_panel.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_unity/b2349e730e3cd51abcbd1a626414fe85d398e1db/img/rviz_control_panel.jpg -------------------------------------------------------------------------------- /img/rviz_full.jpg: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/jizhang-cmu/cmu_vla_challenge_unity/b2349e730e3cd51abcbd1a626414fe85d398e1db/img/rviz_full.jpg -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | /opt/ros/noetic/share/catkin/cmake/toplevel.cmake -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/CHANGELOG.md: -------------------------------------------------------------------------------- 1 | # Changelog 2 | 3 | All notable changes to this repository will be documented in this file. 4 | 5 | The format is based on [Keep a Changelog](http://keepachangelog.com/en/1.0.0/) and this project adheres to [Semantic Versioning](http://semver.org/spec/v2.0.0.html). 6 | 7 | ## Unreleased 8 | 9 | ### Upgrade Notes 10 | 11 | ### Known Issues 12 | 13 | ### Added 14 | 15 | ### Changed 16 | 17 | ### Deprecated 18 | 19 | ### Removed 20 | 21 | ### Fixed 22 | 23 | ## [0.7.0] - 2022-02-01 24 | 25 | ### Added 26 | 27 | Added Sonarqube Scanner 28 | 29 | Private ros params 30 | 31 | Send information during hand shaking for ros and package version checks 32 | 33 | Send service response as one queue item 34 | 35 | 36 | ## [0.6.0] - 2021-09-30 37 | 38 | Add the [Close Stale Issues](https://github.com/marketplace/actions/close-stale-issues) action 39 | 40 | ### Upgrade Notes 41 | 42 | ### Known Issues 43 | 44 | ### Added 45 | 46 | Support for queue_size and latch for publishers. (https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues/82) 47 | 48 | ### Changed 49 | 50 | ### Deprecated 51 | 52 | ### Removed 53 | 54 | ### Fixed 55 | 56 | ## [0.5.0] - 2021-07-15 57 | 58 | ### Upgrade Notes 59 | 60 | Upgrade the ROS communication to support ROS2 with Unity 61 | 62 | ### Known Issues 63 | 64 | ### Added 65 | 66 | ### Changed 67 | 68 | ### Deprecated 69 | 70 | ### Removed 71 | 72 | ### Fixed 73 | 74 | ## [0.4.0] - 2021-05-27 75 | 76 | Note: the logs only reflects the changes from version 0.3.0 77 | 78 | ### Upgrade Notes 79 | 80 | RosConnection 2.0: maintain a single constant connection from Unity to the Endpoint. This is more efficient than opening one connection per message, and it eliminates a whole bunch of user issues caused by ROS being unable to connect to Unity due to firewalls, proxies, etc. 81 | 82 | ### Known Issues 83 | 84 | ### Added 85 | 86 | Add a link to the Robotics forum, and add a config.yml to add a link in the Github Issues page 87 | 88 | Add linter, unit tests, and test coverage reporting 89 | 90 | ### Changed 91 | 92 | Improving the performance of the read_message in client.py, This is done by receiving the entire message all at once instead of reading 1024 byte chunks and stitching them together as you go. 93 | 94 | ### Deprecated 95 | 96 | ### Removed 97 | 98 | Remove outdated handshake references 99 | 100 | ### Fixed -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(ros_tcp_endpoint) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | rospy 6 | std_msgs 7 | ) 8 | 9 | catkin_python_setup() 10 | 11 | catkin_package(CATKIN_DEPENDS message_runtime ) 12 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/CODE_OF_CONDUCT.md: -------------------------------------------------------------------------------- 1 | # Contributor Covenant Code of Conduct 2 | 3 | ## Our Pledge 4 | 5 | In the interest of fostering an open and welcoming environment, we as 6 | contributors and maintainers pledge to making participation in our project and 7 | our community a harassment-free experience for everyone, regardless of age, body 8 | size, disability, ethnicity, gender identity and expression, level of experience, 9 | nationality, personal appearance, race, religion, or sexual identity and 10 | orientation. 11 | 12 | ## Our Standards 13 | 14 | Examples of behavior that contributes to creating a positive environment 15 | include: 16 | 17 | * Using welcoming and inclusive language 18 | * Being respectful of differing viewpoints and experiences 19 | * Gracefully accepting constructive criticism 20 | * Focusing on what is best for the community 21 | * Showing empathy towards other community members 22 | 23 | Examples of unacceptable behavior by participants include: 24 | 25 | * The use of sexualized language or imagery and unwelcome sexual attention or 26 | advances 27 | * Trolling, insulting/derogatory comments, and personal or political attacks 28 | * Public or private harassment 29 | * Publishing others' private information, such as a physical or electronic 30 | address, without explicit permission 31 | * Other conduct which could reasonably be considered inappropriate in a 32 | professional setting 33 | 34 | ## Our Responsibilities 35 | 36 | Project maintainers are responsible for clarifying the standards of acceptable 37 | behavior and are expected to take appropriate and fair corrective action in 38 | response to any instances of unacceptable behavior. 39 | 40 | Project maintainers have the right and responsibility to remove, edit, or 41 | reject comments, commits, code, wiki edits, issues, and other contributions 42 | that are not aligned to this Code of Conduct, or to ban temporarily or 43 | permanently any contributor for other behaviors that they deem inappropriate, 44 | threatening, offensive, or harmful. 45 | 46 | ## Scope 47 | 48 | This Code of Conduct applies both within project spaces and in public spaces 49 | when an individual is representing the project or its community. Examples of 50 | representing a project or community include using an official project e-mail 51 | address, posting via an official social media account, or acting as an appointed 52 | representative at an online or offline event. Representation of a project may be 53 | further defined and clarified by project maintainers. 54 | 55 | ## Enforcement 56 | 57 | Instances of abusive, harassing, or otherwise unacceptable behavior may be 58 | reported by contacting the project team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). All 59 | complaints will be reviewed and investigated and will result in a response that 60 | is deemed necessary and appropriate to the circumstances. The project team is 61 | obligated to maintain confidentiality with regard to the reporter of an incident. 62 | Further details of specific enforcement policies may be posted separately. 63 | 64 | Project maintainers who do not follow or enforce the Code of Conduct in good 65 | faith may face temporary or permanent repercussions as determined by other 66 | members of the project's leadership. 67 | 68 | ## Attribution 69 | 70 | This Code of Conduct is adapted from the [Contributor Covenant][homepage], 71 | version 1.4, available at 72 | https://www.contributor-covenant.org/version/1/4/code-of-conduct/ 73 | 74 | [homepage]: https://www.contributor-covenant.org -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/CONTRIBUTING.md: -------------------------------------------------------------------------------- 1 | # Contribution Guidelines 2 | 3 | Thank you for your interest in contributing to Unity Robotics! To facilitate your 4 | contributions, we've outlined a brief set of guidelines to ensure that your extensions 5 | can be easily integrated. 6 | 7 | ## Communication 8 | 9 | First, please read through our 10 | [code of conduct](CODE_OF_CONDUCT.md), 11 | as we expect all our contributors to follow it. 12 | 13 | Second, before starting on a project that you intend to contribute to any of our 14 | Unity Robotics packages or tutorials, we **strongly** recommend posting on the repository's 15 | [Issues page](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues) and 16 | briefly outlining the changes you plan to make. This will enable us to provide 17 | some context that may be helpful for you. This could range from advice and 18 | feedback on how to optimally perform your changes or reasons for not doing it. 19 | 20 | ## Git Branches 21 | 22 | The `main` branch corresponds to the most recent stable version of the project. The `dev` branch 23 | contains changes that are staged to be merged into `main` as the team sees fit. 24 | 25 | When contributing to the project, please make sure that your Pull Request (PR) 26 | does the following: 27 | 28 | - Is up-to-date with and targets the `dev` branch 29 | - Contains a detailed description of the changes performed 30 | - Has corresponding changes to documentation, unit tests and sample environments (if 31 | applicable) 32 | - Contains a summary of the tests performed to validate your changes 33 | - Links to issue numbers that the PR resolves (if any) 34 | 35 | 38 | 39 | ## Code style 40 | 41 | All Python code should follow the [PEP 8 style guidelines](https://pep8.org/). 42 | 43 | All C# code should follow the [Microsoft C# Coding Conventions](https://docs.microsoft.com/en-us/dotnet/csharp/programming-guide/inside-a-program/coding-conventions). 44 | Additionally, the [Unity Coding package](https://docs.unity3d.com/Packages/com.unity.coding@0.1/manual/index.html) 45 | can be used to format, encode, and lint your code according to the standard Unity 46 | development conventions. Be aware that these Unity conventions will supersede the 47 | Microsoft C# Coding Conventions where applicable. 48 | 49 | Please note that even if the code you are changing does not adhere to these guidelines, 50 | we expect your submissions to follow these conventions. 51 | 52 | ## Contributor License Agreements 53 | 54 | When you open a pull request, you will be asked to acknowledge our Contributor 55 | License Agreement. We allow both individual contributions and contributions made 56 | on behalf of companies. We use an open source tool called CLA assistant. If you 57 | have any questions on our CLA, please 58 | [submit an issue](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues) or 59 | email us at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). 60 | 61 | ## Contribution review 62 | 63 | Once you have a change ready following the above ground rules, simply make a 64 | pull request in GitHub. -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/README.md: -------------------------------------------------------------------------------- 1 | # ROS TCP Endpoint 2 | 3 | [![License](https://img.shields.io/badge/license-Apache--2.0-green.svg)](LICENSE.md) 4 | [![Version](https://img.shields.io/github/v/tag/Unity-Technologies/ROS-TCP-Endpoint)](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/releases) 5 | ![ROS](https://img.shields.io/badge/ros-melodic,noetic-brightgreen) 6 | ![ROS](https://img.shields.io/badge/ros2-foxy,galactic-brightgreen) 7 | 8 | --- 9 | 10 | We're currently working on lots of things! Please take a short moment fill out our [survey](https://unitysoftware.co1.qualtrics.com/jfe/form/SV_0ojVkDVW0nNrHkW) to help us identify what products and packages to build next. 11 | 12 | --- 13 | 14 | ## Introduction 15 | 16 | [ROS](https://www.ros.org/) package used to create an endpoint to accept ROS messages sent from a Unity scene. This ROS package works in tandem with the [ROS TCP Connector](https://github.com/Unity-Technologies/ROS-TCP-Connector) Unity package. 17 | 18 | Instructions and examples on how to use this ROS package can be found on the [Unity Robotics Hub](https://github.com/Unity-Technologies/Unity-Robotics-Hub/blob/master/tutorials/ros_unity_integration/README.md) repository. 19 | 20 | ## Community and Feedback 21 | 22 | The Unity Robotics projects are open-source and we encourage and welcome contributions. 23 | If you wish to contribute, be sure to review our [contribution guidelines](CONTRIBUTING.md) 24 | and [code of conduct](CODE_OF_CONDUCT.md). 25 | 26 | ## Support 27 | For questions or discussions about Unity Robotics package installations or how to best set up and integrate your robotics projects, please create a new thread on the [Unity Robotics forum](https://forum.unity.com/forums/robotics.623/) and make sure to include as much detail as possible. 28 | 29 | For feature requests, bugs, or other issues, please file a [GitHub issue](https://github.com/Unity-Technologies/ROS-TCP-Endpoint/issues) using the provided templates and the Robotics team will investigate as soon as possible. 30 | 31 | For any other questions or feedback, connect directly with the 32 | Robotics team at [unity-robotics@unity3d.com](mailto:unity-robotics@unity3d.com). 33 | 34 | ## License 35 | [Apache License 2.0](LICENSE) -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/launch/endpoint.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | ros_tcp_endpoint 4 | 0.7.0 5 | Acts as the bridge between Unity messages sent via Websocket and ROS messages. 6 | 7 | Unity Robotics 8 | 9 | Apache 2.0 10 | catkin 11 | rospy 12 | message_generation 13 | rospy 14 | rospy 15 | message_runtime 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/requirements.txt: -------------------------------------------------------------------------------- 1 | black 2 | pre-commit 3 | pytest-cov -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/setup.py: -------------------------------------------------------------------------------- 1 | ## ! DO NOT MANUALLY INVOKE THIS setup.py, USE CATKIN INSTEAD 2 | 3 | from setuptools import setup 4 | from catkin_pkg.python_setup import generate_distutils_setup 5 | 6 | # fetch values from package.xml 7 | setup_args = generate_distutils_setup(packages=["ros_tcp_endpoint"], package_dir={"": "src"}) 8 | 9 | setup(**setup_args) 10 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/__init__.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Unity Technologies 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | from .server import TcpServer 16 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/communication.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Unity Technologies 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | class RosSender: 17 | """ 18 | Base class for ROS communication where data is sent to the ROS network. 19 | """ 20 | 21 | def __init__(self, node_name): 22 | self.node_name = node_name 23 | pass 24 | 25 | def send(self, *args): 26 | raise NotImplementedError 27 | 28 | 29 | class RosReceiver: 30 | """ 31 | Base class for ROS communication where data is being sent outside of the ROS network. 32 | """ 33 | 34 | def __init__(self, node_name): 35 | self.node_name = node_name 36 | pass 37 | 38 | def send(self, *args): 39 | raise NotImplementedError 40 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/default_server_endpoint.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | import rospy 4 | 5 | from ros_tcp_endpoint import TcpServer 6 | 7 | 8 | def main(args=None): 9 | # Start the Server Endpoint 10 | rospy.init_node("unity_endpoint", anonymous=True) 11 | tcp_server = TcpServer(rospy.get_name()) 12 | tcp_server.start() 13 | rospy.spin() 14 | 15 | 16 | if __name__ == "__main__": 17 | main() 18 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/exceptions.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Unity Technologies 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | 16 | class RosTcpEndpointError(Exception): 17 | """Base class for this package's custom exceptions""" 18 | 19 | pass 20 | 21 | 22 | class TopicOrServiceNameDoesNotExistError(RosTcpEndpointError): 23 | """The topic or service name passed does not exist in the source destination dictionary.""" 24 | 25 | pass 26 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/publisher.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Unity Technologies 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | import re 17 | from .communication import RosSender 18 | 19 | 20 | class RosPublisher(RosSender): 21 | """ 22 | Class to publish messages to a ROS topic 23 | """ 24 | 25 | # TODO: surface latch functionality 26 | def __init__(self, topic, message_class, queue_size=10, latch=False): 27 | """ 28 | 29 | Args: 30 | topic: Topic name to publish messages to 31 | message_class: The message class in catkin workspace 32 | queue_size: Max number of entries to maintain in an outgoing queue 33 | """ 34 | strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic) 35 | node_name = "{}_RosPublisher".format(strippedTopic) 36 | RosSender.__init__(self, node_name) 37 | self.msg = message_class() 38 | self.pub = rospy.Publisher(topic, message_class, queue_size=queue_size, latch=latch) 39 | 40 | def send(self, data): 41 | """ 42 | Takes in serialized message data from source outside of the ROS network, 43 | deserializes it into it's message class, and publishes the message to ROS topic. 44 | 45 | Args: 46 | data: The already serialized message_class data coming from outside of ROS 47 | 48 | Returns: 49 | None: Explicitly return None so behaviour can be 50 | """ 51 | self.msg.deserialize(data) 52 | self.pub.publish(self.msg) 53 | 54 | return None 55 | 56 | def unregister(self): 57 | """ 58 | 59 | Returns: 60 | 61 | """ 62 | self.pub.unregister() 63 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/service.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Unity Technologies 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | from rospy.service import ServiceException 17 | import re 18 | 19 | from .communication import RosSender 20 | 21 | 22 | class RosService(RosSender): 23 | """ 24 | Class to send messages to a ROS service. 25 | """ 26 | 27 | def __init__(self, service, service_class): 28 | """ 29 | Args: 30 | service: The service name in ROS 31 | service_class: The service class in catkin workspace 32 | """ 33 | strippedService = re.sub("[^A-Za-z0-9_]+", "", service) 34 | node_name = "{}_RosService".format(strippedService) 35 | RosSender.__init__(self, node_name) 36 | 37 | self.srv_class = service_class._request_class() 38 | self.srv = rospy.ServiceProxy(service, service_class) 39 | 40 | def send(self, data): 41 | """ 42 | Takes in serialized message data from source outside of the ROS network, 43 | deserializes it into it's class, calls the service with the message, and returns 44 | the service's response. 45 | 46 | Args: 47 | data: The already serialized message_class data coming from outside of ROS 48 | 49 | Returns: 50 | service response 51 | """ 52 | message = self.srv_class.deserialize(data) 53 | 54 | attempt = 0 55 | 56 | while attempt < 3: 57 | try: 58 | service_response = self.srv(message) 59 | return service_response 60 | except ServiceException: 61 | attempt += 1 62 | print("Service Exception raised. Attempt: {}".format(attempt)) 63 | except Exception as e: 64 | print("Exception Raised: {}".format(e)) 65 | 66 | return None 67 | 68 | def unregister(self): 69 | """ 70 | 71 | Returns: 72 | 73 | """ 74 | if not self.srv is None: 75 | self.srv.close() 76 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/subscriber.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Unity Technologies 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | import socket 17 | import re 18 | 19 | from .communication import RosReceiver 20 | from .client import ClientThread 21 | 22 | 23 | class RosSubscriber(RosReceiver): 24 | """ 25 | Class to send messages outside of ROS network 26 | """ 27 | 28 | def __init__(self, topic, message_class, tcp_server, queue_size=10): 29 | """ 30 | 31 | Args: 32 | topic: Topic name to publish messages to 33 | message_class: The message class in catkin workspace 34 | queue_size: Max number of entries to maintain in an outgoing queue 35 | """ 36 | strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic) 37 | self.node_name = "{}_RosSubscriber".format(strippedTopic) 38 | RosReceiver.__init__(self, self.node_name) 39 | self.topic = topic 40 | self.msg = message_class 41 | self.tcp_server = tcp_server 42 | self.queue_size = queue_size 43 | 44 | # Start Subscriber listener function 45 | self.sub = rospy.Subscriber(self.topic, self.msg, self.send) 46 | 47 | def send(self, data): 48 | """ 49 | Connect to TCP endpoint on client and pass along message 50 | Args: 51 | data: message data to send outside of ROS network 52 | 53 | Returns: 54 | self.msg: The deserialize message 55 | 56 | """ 57 | self.tcp_server.send_unity_message(self.topic, data) 58 | return self.msg 59 | 60 | def unregister(self): 61 | """ 62 | 63 | Returns: 64 | 65 | """ 66 | if not self.sub is None: 67 | self.sub.unregister() 68 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/thread_pauser.py: -------------------------------------------------------------------------------- 1 | import threading 2 | 3 | 4 | class ThreadPauser: 5 | def __init__(self): 6 | self.condition = threading.Condition() 7 | self.result = None 8 | 9 | def sleep_until_resumed(self): 10 | with self.condition: 11 | self.condition.wait() 12 | 13 | def resume_with_result(self, result): 14 | self.result = result 15 | with self.condition: 16 | self.condition.notify() 17 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/src/ros_tcp_endpoint/unity_service.py: -------------------------------------------------------------------------------- 1 | # Copyright 2020 Unity Technologies 2 | # 3 | # Licensed under the Apache License, Version 2.0 (the "License"); 4 | # you may not use this file except in compliance with the License. 5 | # You may obtain a copy of the License at 6 | # 7 | # http://www.apache.org/licenses/LICENSE-2.0 8 | # 9 | # Unless required by applicable law or agreed to in writing, software 10 | # distributed under the License is distributed on an "AS IS" BASIS, 11 | # WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | # See the License for the specific language governing permissions and 13 | # limitations under the License. 14 | 15 | import rospy 16 | import socket 17 | import re 18 | 19 | from .communication import RosReceiver 20 | from .client import ClientThread 21 | 22 | 23 | class UnityService(RosReceiver): 24 | """ 25 | Class to register a ROS service that's implemented in Unity. 26 | """ 27 | 28 | def __init__(self, topic, service_class, tcp_server, queue_size=10): 29 | """ 30 | 31 | Args: 32 | topic: Topic name to publish messages to 33 | service_class: The message class in catkin workspace 34 | queue_size: Max number of entries to maintain in an outgoing queue 35 | """ 36 | strippedTopic = re.sub("[^A-Za-z0-9_]+", "", topic) 37 | self.node_name = "{}_service".format(strippedTopic) 38 | 39 | self.topic = topic 40 | self.service_class = service_class 41 | self.tcp_server = tcp_server 42 | self.queue_size = queue_size 43 | 44 | self.service = rospy.Service(self.topic, self.service_class, self.send) 45 | 46 | def send(self, request): 47 | """ 48 | Connect to TCP endpoint on client, pass along message and get reply 49 | Args: 50 | data: message data to send outside of ROS network 51 | 52 | Returns: 53 | The response message 54 | """ 55 | return self.tcp_server.send_unity_service(self.topic, self.service_class, request) 56 | 57 | def unregister(self): 58 | """ 59 | 60 | Returns: 61 | 62 | """ 63 | self.service.shutdown() 64 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/__init__.py: -------------------------------------------------------------------------------- 1 | 2 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/test_client.py: -------------------------------------------------------------------------------- 1 | from unittest import mock 2 | from unittest.mock import Mock 3 | from ros_tcp_endpoint.client import ClientThread 4 | from ros_tcp_endpoint.server import TcpServer 5 | import sys 6 | import threading 7 | 8 | 9 | def test_client_thread_initialization(): 10 | tcp_server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 11 | mock_conn = mock.Mock() 12 | client_thread = ClientThread(mock_conn, tcp_server, "127.0.0.1", 10000) 13 | assert client_thread.tcp_server.node_name == "test-tcp-server" 14 | assert client_thread.incoming_ip == "127.0.0.1" 15 | assert client_thread.incoming_port == 10000 16 | 17 | 18 | def test_recvall_should_read_bytes_exact_size(): 19 | mock_conn = mock.Mock() 20 | mock_conn.recv_into.return_value = 1 21 | result = ClientThread.recvall(mock_conn, 8) 22 | assert result == b"\x00\x00\x00\x00\x00\x00\x00\x00" 23 | 24 | 25 | @mock.patch.object(ClientThread, "recvall", return_value=b"\x01\x00\x00\x00") 26 | def test_read_int32_should_parse_int(mock_recvall): 27 | mock_conn = mock.Mock() 28 | result = ClientThread.read_int32(mock_conn) 29 | mock_recvall.assert_called_once 30 | assert result == 1 31 | 32 | 33 | @mock.patch.object(ClientThread, "read_int32", return_value=4) 34 | @mock.patch.object(ClientThread, "recvall", return_value=b"Hello world!") 35 | def test_read_string_should_decode(mock_recvall, mock_read_int): 36 | mock_conn = mock.Mock() 37 | result = ClientThread.read_string(mock_conn) 38 | mock_recvall.assert_called_once 39 | mock_read_int.assert_called_once 40 | assert result == "Hello world!" 41 | 42 | 43 | @mock.patch.object(ClientThread, "read_string", return_value="__srv") 44 | @mock.patch.object(ClientThread, "read_int32", return_value=4) 45 | @mock.patch.object(ClientThread, "recvall", return_value=b"Hello world!") 46 | def test_read_message_return_destination_data(mock_recvall, mock_read_int, mock_read_str): 47 | tcp_server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 48 | mock_conn = mock.Mock() 49 | client_thread = ClientThread(mock_conn, tcp_server, "127.0.0.1", 10000) 50 | result = client_thread.read_message(mock_conn) 51 | assert result == ("__srv", b"Hello world!") 52 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/test_publisher.py: -------------------------------------------------------------------------------- 1 | from unittest import mock 2 | from ros_tcp_endpoint.publisher import RosPublisher 3 | import rospy 4 | 5 | 6 | @mock.patch.object(rospy, "Publisher") 7 | def test_publisher_send(mock_ros): 8 | mock_tcp_server = mock.Mock() 9 | publisher = RosPublisher("color", mock.Mock(), mock_tcp_server) 10 | publisher.send("test data") 11 | publisher.msg.deserialize.assert_called_once() 12 | publisher.pub.publish.assert_called_once() 13 | 14 | 15 | @mock.patch.object(rospy, "Publisher") 16 | def test_publisher_unregister(mock_ros): 17 | mock_tcp_server = mock.Mock() 18 | publisher = RosPublisher("color", mock.Mock(), mock_tcp_server) 19 | publisher.unregister() 20 | publisher.pub.unregister.assert_called_once() 21 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/test_ros_service.py: -------------------------------------------------------------------------------- 1 | from unittest import mock 2 | from ros_tcp_endpoint.service import RosService 3 | import rospy 4 | 5 | 6 | @mock.patch.object(rospy, "ServiceProxy") 7 | def test_subscriber_send(mock_ros): 8 | ros_service = RosService("color", mock.Mock()) 9 | service_response = ros_service.send("test data") 10 | ros_service.srv_class.deserialize.assert_called_once() 11 | assert service_response is not None 12 | 13 | 14 | @mock.patch.object(rospy, "ServiceProxy") 15 | def test_subscriber_unregister(mock_ros): 16 | ros_service = RosService("color", mock.Mock()) 17 | ros_service.unregister() 18 | ros_service.srv.close.assert_called_once() 19 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/test_server.py: -------------------------------------------------------------------------------- 1 | from unittest import mock 2 | from ros_tcp_endpoint import TcpServer 3 | from ros_tcp_endpoint.server import SysCommands 4 | import importlib 5 | import rospy 6 | import sys 7 | import ros_tcp_endpoint 8 | 9 | 10 | @mock.patch("socket.socket") 11 | @mock.patch("ros_tcp_endpoint.server.rospy") 12 | def test_server_constructor(mock_ros, mock_socket): 13 | mock_ros.get_param = mock.Mock(return_value="127.0.0.1") 14 | server = TcpServer("test-tcp-server") 15 | assert server.node_name == "test-tcp-server" 16 | assert server.tcp_ip == "127.0.0.1" 17 | assert server.buffer_size == 1024 18 | assert server.connections == 10 19 | 20 | 21 | def test_start_server(): 22 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 23 | assert server.tcp_ip == "127.0.0.1" 24 | assert server.tcp_port == 10000 25 | assert server.connections == 10 26 | server.start() 27 | 28 | 29 | def test_unity_service_empty_topic_should_return_none(): 30 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 31 | system_cmds = SysCommands(server) 32 | result = system_cmds.unity_service("", "test message") 33 | assert result is None 34 | 35 | 36 | def test_unity_service_resolve_message_name_failure(): 37 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 38 | system_cmds = SysCommands(server) 39 | result = system_cmds.unity_service("get_pos", "unresolvable message") 40 | assert result is None 41 | 42 | 43 | @mock.patch.object(rospy, "Service") 44 | @mock.patch.object( 45 | SysCommands, "resolve_message_name", return_value="unity_interfaces.msg/RosUnitySrvMessage" 46 | ) 47 | def test_unity_service_resolve_news_service(mock_resolve_message, mock_ros_service): 48 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 49 | assert server.ros_services_table == {} 50 | system_cmds = SysCommands(server) 51 | result = system_cmds.unity_service("get_pos", "unity_interfaces.msg/RosUnitySrvMessage") 52 | mock_ros_service.assert_called_once 53 | assert result is None 54 | 55 | 56 | @mock.patch.object(rospy, "Service") 57 | @mock.patch.object( 58 | SysCommands, "resolve_message_name", return_value="unity_interfaces.msg/RosUnitySrvMessage" 59 | ) 60 | def test_unity_service_resolve_existing_service(mock_resolve_message, mock_ros_service): 61 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 62 | server.ros_services = {"get_pos": mock.Mock()} 63 | system_cmds = SysCommands(server) 64 | result = system_cmds.unity_service("get_pos", "unity_interfaces.msg/RosUnitySrvMessage") 65 | mock_ros_service.assert_called_once 66 | assert result is None 67 | 68 | 69 | @mock.patch.object(sys, "modules", return_value="unity_interfaces.msg") 70 | @mock.patch.object(importlib, "import_module") 71 | def test_resolve_message_name(mock_import_module, mock_sys_modules): 72 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 73 | msg_name = "unity_interfaces.msg/UnityColor.msg" 74 | result = SysCommands(server).resolve_message_name(msg_name) 75 | mock_import_module.assert_called_once 76 | mock_sys_modules.assert_called_once 77 | assert result is not None 78 | 79 | 80 | @mock.patch.object(rospy, "Publisher") 81 | def test_publish_add_new_topic(mock_ros_publisher): 82 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 83 | result = SysCommands(server).publish("object_pos_topic", "std_msgs/Bool") 84 | assert server.publishers_table != {} 85 | mock_ros_publisher.assert_called_once 86 | 87 | 88 | @mock.patch.object(rospy, "Publisher") 89 | def test_publish_existing_topic(mock_ros_publisher): 90 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 91 | server.publishers_table = {"object_pos_topic": mock.Mock()} 92 | result = SysCommands(server).publish("object_pos_topic", "std_msgs/Bool") 93 | assert server.publishers_table["object_pos_topic"] is not None 94 | mock_ros_publisher.assert_called_once 95 | 96 | 97 | def test_publish_empty_topic_should_return_none(): 98 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 99 | result = SysCommands(server).publish("", "pos") 100 | assert result is None 101 | assert server.publishers_table == {} 102 | 103 | 104 | def test_publish_empty_message_should_return_none(): 105 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 106 | result = SysCommands(server).publish("test-topic", "") 107 | assert result is None 108 | assert server.publishers_table == {} 109 | 110 | 111 | @mock.patch.object(rospy, "Subscriber") 112 | @mock.patch.object(SysCommands, "resolve_message_name", return_value="unity_interfaces.msg/Pos") 113 | def test_subscribe_to_new_topic(mock_resolve_msg, mock_ros_subscriber): 114 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 115 | result = SysCommands(server).subscribe("object_pos_topic", "pos") 116 | assert server.subscribers_table != {} 117 | mock_ros_subscriber.assert_called_once 118 | 119 | 120 | @mock.patch.object(rospy, "Subscriber") 121 | @mock.patch.object(SysCommands, "resolve_message_name", return_value="unity_interfaces.msg/Pos") 122 | def test_subscribe_to_existing_topic(mock_resolve_msg, mock_ros_subscriber): 123 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 124 | server.subscribers_table = {"object_pos_topic": mock.Mock()} 125 | result = SysCommands(server).subscribe("object_pos_topic", "pos") 126 | assert server.subscribers_table["object_pos_topic"] is not None 127 | mock_ros_subscriber.assert_called_once 128 | 129 | 130 | def test_subscribe_to_empty_topic_should_return_none(): 131 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 132 | result = SysCommands(server).subscribe("", "pos") 133 | assert result is None 134 | assert server.subscribers_table == {} 135 | 136 | 137 | def test_subscribe_to_empty_message_should_return_none(): 138 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 139 | result = SysCommands(server).subscribe("test-topic", "") 140 | assert result is None 141 | assert server.subscribers_table == {} 142 | 143 | 144 | @mock.patch.object(rospy, "ServiceProxy") 145 | @mock.patch.object(SysCommands, "resolve_message_name") 146 | def test_ros_service_new_topic(mock_resolve_msg, mock_ros_service): 147 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 148 | result = SysCommands(server).ros_service("object_pos_topic", "pos") 149 | assert server.ros_services_table != {} 150 | mock_ros_service.assert_called_once 151 | 152 | 153 | @mock.patch.object(rospy, "ServiceProxy") 154 | @mock.patch.object(SysCommands, "resolve_message_name") 155 | def test_ros_service_existing_topic(mock_resolve_msg, mock_ros_service): 156 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 157 | server.ros_services_table = {"object_pos_topic": mock.Mock()} 158 | result = SysCommands(server).ros_service("object_pos_topic", "pos") 159 | assert server.ros_services_table["object_pos_topic"] is not None 160 | mock_ros_service.assert_called_once 161 | 162 | 163 | def test_ros_service_empty_topic_should_return_none(): 164 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 165 | result = SysCommands(server).ros_service("", "pos") 166 | assert result is None 167 | assert server.ros_services_table == {} 168 | 169 | 170 | def test_ros_service_empty_message_should_return_none(): 171 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 172 | result = SysCommands(server).ros_service("test-topic", "") 173 | assert result is None 174 | assert server.ros_services_table == {} 175 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/test_subscriber.py: -------------------------------------------------------------------------------- 1 | from unittest import mock 2 | from ros_tcp_endpoint.subscriber import RosSubscriber 3 | import rospy 4 | 5 | 6 | @mock.patch.object(rospy, "Subscriber") 7 | def test_subscriber_send(mock_ros): 8 | mock_tcp_server = mock.Mock() 9 | subscriber = RosSubscriber("color", mock.Mock(), mock_tcp_server) 10 | assert subscriber.node_name == "color_RosSubscriber" 11 | subscriber.send("test data") 12 | mock_tcp_server.send_unity_message.assert_called_once() 13 | 14 | 15 | @mock.patch.object(rospy, "Subscriber") 16 | def test_subscriber_unregister(mock_ros): 17 | mock_tcp_server = mock.Mock() 18 | subscriber = RosSubscriber("color", mock.Mock(), mock_tcp_server) 19 | assert subscriber.node_name == "color_RosSubscriber" 20 | subscriber.unregister() 21 | subscriber.sub.unregister.assert_called_once() 22 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/test_tcp_sender.py: -------------------------------------------------------------------------------- 1 | import queue 2 | import socket 3 | from unittest import mock 4 | from ros_tcp_endpoint import TcpServer 5 | import ros_tcp_endpoint 6 | 7 | 8 | @mock.patch("ros_tcp_endpoint.tcp_sender.rospy") 9 | def test_tcp_sender_constructor(mock_ros): 10 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 11 | tcp_sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender(server) 12 | assert tcp_sender.sender_id == 1 13 | assert tcp_sender.time_between_halt_checks == 5 14 | assert tcp_sender.queue == None 15 | assert tcp_sender.next_srv_id == 1001 16 | assert tcp_sender.srv_lock != None 17 | assert tcp_sender.services_waiting == {} 18 | 19 | 20 | # @mock.patch("socket.socket") 21 | # @mock.patch.object(ros_tcp_endpoint.client.ClientThread, "serialize_message") 22 | # def test_send_unity_error_should_send_msg(mock_serialize_msg, mock_socket): 23 | # sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender() 24 | # sender.queue = queue.Queue() 25 | # sender.send_unity_error("Test error") 26 | # mock_serialize_msg.assert_called_once() 27 | 28 | 29 | @mock.patch.object(ros_tcp_endpoint.client.ClientThread, "serialize_message") 30 | def test_send_message_should_serialize_message(mock_serialize_msg): 31 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 32 | sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender(server) 33 | sender.queue = queue.Queue() 34 | sender.send_unity_message("test topic", "test msg") 35 | mock_serialize_msg.assert_called_once() 36 | 37 | 38 | # @mock.patch.object(ros_tcp_endpoint.thread_pauser.ThreadPauser, "sleep_until_resumed") 39 | # @mock.patch.object(ros_tcp_endpoint.client.ClientThread, "serialize_message") 40 | # def test_send_unity_service_should_serialize_ros_unity_srv(mock_serialize_msg, mock_thread_pauser): 41 | # sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender() 42 | # sender.send_unity_service_request(mock.Mock(), mock.Mock(), mock.Mock()) 43 | # mock_serialize_msg.assert_called_once() 44 | # # TODO: Test the scenario when the queue is not None 45 | # assert sender.queue == None 46 | 47 | 48 | @mock.patch("ros_tcp_endpoint.thread_pauser.ThreadPauser") 49 | def test_send_unity_service_response_should_resume(mock_thread_pauser_class): 50 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 51 | sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender(server) 52 | thread_pauser = mock_thread_pauser_class() 53 | sender.services_waiting = {"moveit": thread_pauser} 54 | sender.send_unity_service_response("moveit", "test data") 55 | thread_pauser.resume_with_result.assert_called_once() 56 | 57 | 58 | def test_start_sender_should_increase_sender_id(): 59 | server = TcpServer(node_name="test-tcp-server", tcp_ip="127.0.0.1", tcp_port=10000) 60 | sender = ros_tcp_endpoint.tcp_sender.UnityTcpSender(server) 61 | init_sender_id = 1 62 | assert sender.sender_id == init_sender_id 63 | sender.start_sender(mock.Mock(), mock.Mock()) 64 | assert sender.sender_id == init_sender_id + 1 65 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/test_thread_pauser.py: -------------------------------------------------------------------------------- 1 | import threading 2 | from unittest import mock 3 | from ros_tcp_endpoint.thread_pauser import ThreadPauser 4 | 5 | 6 | @mock.patch.object(threading.Condition, "wait") 7 | def test_sleep_until_resumed(mock_thread_wait): 8 | thread_pauser = ThreadPauser() 9 | thread_pauser.sleep_until_resumed() 10 | mock_thread_wait.assert_called_once 11 | 12 | 13 | @mock.patch.object(threading.Condition, "notify") 14 | def test_resume(mock_thread_notify): 15 | thread_pauser = ThreadPauser() 16 | thread_pauser.resume_with_result(mock.Mock) 17 | mock_thread_notify.assert_called_once 18 | -------------------------------------------------------------------------------- /src/ROS-TCP-Endpoint/test/test_unity_service.py: -------------------------------------------------------------------------------- 1 | from unittest import mock 2 | from ros_tcp_endpoint.unity_service import UnityService 3 | import rospy 4 | 5 | 6 | @mock.patch.object(rospy, "Service") 7 | def test_unity_service_send(mock_ros_service): 8 | mock_tcp_server = mock.Mock() 9 | unity_service = UnityService("color", mock.Mock(), mock_tcp_server) 10 | assert unity_service.node_name == "color_service" 11 | unity_service.send("test data") 12 | mock_tcp_server.send_unity_service.assert_called_once() 13 | 14 | 15 | @mock.patch.object(rospy, "Service") 16 | def test_unity_service_unregister(mock_ros_service): 17 | mock_tcp_server = mock.Mock() 18 | unity_service = UnityService("color", mock.Mock(), mock_tcp_server) 19 | assert unity_service.node_name == "color_service" 20 | unity_service.unregister() 21 | unity_service.service.shutdown.assert_called_once() 22 | -------------------------------------------------------------------------------- /src/bbox_calculation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(bbox_calculation) 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 | find_package(OpenCV 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 | ${PCL_INCLUDE_DIRS} 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 | ## Declare executables 53 | add_executable(bboxCalculation src/bboxCalculation.cpp) 54 | add_executable(bboxBroadcast src/bboxBroadcast.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(bboxCalculation ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 58 | target_link_libraries(bboxBroadcast ${catkin_LIBRARIES} ${PCL_LIBRARIES} ${OpenCV_LIBRARIES}) 59 | 60 | install(TARGETS bboxCalculation bboxBroadcast 61 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 63 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 64 | ) 65 | 66 | install(DIRECTORY launch/ 67 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 68 | ) 69 | install(DIRECTORY data/ 70 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/data 71 | ) 72 | -------------------------------------------------------------------------------- /src/bbox_calculation/launch/bbox_broadcast.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /src/bbox_calculation/launch/bbox_calculation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/bbox_calculation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | bbox_calculation 3 | 0.0.1 4 | Bounding Box Calculation 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/bbox_calculation/src/bboxBroadcast.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 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 object_list_file_dir; 32 | int maxObjectNum = 100; 33 | double broadcastDisThre = 2.0; 34 | double broadcastRate = 5.0; 35 | 36 | float vehicleX = 0, vehicleY = 0; 37 | double curTime = 0, sendTime = 0; 38 | 39 | void poseHandler(const nav_msgs::Odometry::ConstPtr& pose) 40 | { 41 | curTime = pose->header.stamp.toSec(); 42 | 43 | vehicleX = pose->pose.pose.position.x; 44 | vehicleY = pose->pose.pose.position.y; 45 | } 46 | 47 | int main(int argc, char** argv) 48 | { 49 | ros::init(argc, argv, "bboxBroadcast"); 50 | ros::NodeHandle nh; 51 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 52 | 53 | nhPrivate.getParam("object_list_file_dir", object_list_file_dir); 54 | nhPrivate.getParam("maxObjectNum", maxObjectNum); 55 | nhPrivate.getParam("broadcastDisThre", broadcastDisThre); 56 | nhPrivate.getParam("broadcastRate", broadcastRate); 57 | 58 | ros::Subscriber subPose = nh.subscribe ("/state_estimation", 5, poseHandler); 59 | 60 | ros::Publisher pubObjectMarker = nh.advertise("object_markers", 5); 61 | visualization_msgs::MarkerArray objectMarkerArray; 62 | 63 | const int objNumConst = maxObjectNum; 64 | float objMidX[objNumConst] = {0}; 65 | float objMidY[objNumConst] = {0}; 66 | float objMidZ[objNumConst] = {0}; 67 | float objL[objNumConst] = {0}; 68 | float objH[objNumConst] = {0}; 69 | float objW[objNumConst] = {0}; 70 | float objHeading[objNumConst] = {0}; 71 | string objLabel[objNumConst]; 72 | int objExist[objNumConst] = {0}; 73 | int objValid[objNumConst] = {0}; 74 | 75 | FILE *object_list_file = fopen(object_list_file_dir.c_str(), "r"); 76 | if (object_list_file == NULL) { 77 | printf ("\nCannot read object_list_with_labels.txt file, exit.\n\n"); 78 | exit(1); 79 | } 80 | 81 | int objID; 82 | char s[100], s2[100]; 83 | int val1, val2, val3, val4, val5, val6, val7, val8, val9; 84 | float x, y, z, l, w, h, heading; 85 | while (1) { 86 | val1 = fscanf(object_list_file, "%d", &objID); 87 | val2 = fscanf(object_list_file, "%f", &x); 88 | val3 = fscanf(object_list_file, "%f", &y); 89 | val4 = fscanf(object_list_file, "%f", &z); 90 | val5 = fscanf(object_list_file, "%f", &l); 91 | val6 = fscanf(object_list_file, "%f", &w); 92 | val7 = fscanf(object_list_file, "%f", &h); 93 | val8 = fscanf(object_list_file, "%f", &heading); 94 | val9 = fscanf(object_list_file, "%s", s); 95 | 96 | if (val1 != 1 || val2 != 1 || val3 != 1 || val4 != 1 || val5 != 1 || val6 != 1 || val7 != 1 || val8 != 1 || val9 != 1) { 97 | break; 98 | } 99 | 100 | while (s[strlen(s) - 1] != '"') { 101 | val9 = fscanf(object_list_file, "%s", s2); 102 | 103 | if (val9 != 1) break; 104 | 105 | strcat(s, " "); 106 | strcat(s, s2); 107 | } 108 | 109 | if (objID < 0 || objID >= maxObjectNum) continue; 110 | 111 | objMidX[objID] = x; 112 | objMidY[objID] = y; 113 | objMidZ[objID] = z; 114 | objL[objID] = l; 115 | objW[objID] = w; 116 | objH[objID] = h; 117 | objHeading[objID] = heading; 118 | objExist[objID] = 1; 119 | 120 | for (int i = 1; s[i] != '"' && i < 100; i++) objLabel[objID] += s[i]; 121 | } 122 | 123 | fclose(object_list_file); 124 | 125 | ros::Rate rate(100); 126 | bool status = ros::ok(); 127 | while (status) { 128 | ros::spinOnce(); 129 | 130 | if (curTime - sendTime > 1.0 / broadcastRate) { 131 | int objValidNum = 0; 132 | for (int j = 0; j < maxObjectNum; j++) { 133 | if (objExist[j] == 1) { 134 | float disX = objMidX[j] - vehicleX; 135 | float disY = objMidY[j] - vehicleY; 136 | float dis = sqrt(disX * disX + disY * disY); 137 | 138 | if (dis < broadcastDisThre) { 139 | objValid[j] = 1; 140 | objValidNum++; 141 | } else { 142 | objValid[j] = 0; 143 | } 144 | } 145 | } 146 | 147 | if (objValidNum > 0) { 148 | objectMarkerArray.markers.resize(objValidNum); 149 | 150 | int objValidCount = 0; 151 | for (int j = 0; j < maxObjectNum; j++) { 152 | if (objValid[j] == 1) { 153 | objectMarkerArray.markers[objValidCount].header.frame_id = "map"; 154 | objectMarkerArray.markers[objValidCount].header.stamp = ros::Time().fromSec(curTime); 155 | objectMarkerArray.markers[objValidCount].ns = objLabel[j]; 156 | objectMarkerArray.markers[objValidCount].id = j; 157 | objectMarkerArray.markers[objValidCount].action = visualization_msgs::Marker::ADD; 158 | objectMarkerArray.markers[objValidCount].type = visualization_msgs::Marker::CUBE; 159 | objectMarkerArray.markers[objValidCount].pose.position.x = objMidX[j]; 160 | objectMarkerArray.markers[objValidCount].pose.position.y = objMidY[j]; 161 | objectMarkerArray.markers[objValidCount].pose.position.z = objMidZ[j]; 162 | objectMarkerArray.markers[objValidCount].pose.orientation = tf::createQuaternionMsgFromRollPitchYaw(0, 0, objHeading[j]); 163 | objectMarkerArray.markers[objValidCount].scale.x = objL[j]; 164 | objectMarkerArray.markers[objValidCount].scale.y = objW[j]; 165 | objectMarkerArray.markers[objValidCount].scale.z = objH[j]; 166 | objectMarkerArray.markers[objValidCount].color.a = 0.5; 167 | objectMarkerArray.markers[objValidCount].color.r = 1.0; 168 | objectMarkerArray.markers[objValidCount].color.g = 0; 169 | objectMarkerArray.markers[objValidCount].color.b = 0; 170 | objValidCount++; 171 | } 172 | } 173 | 174 | pubObjectMarker.publish(objectMarkerArray); 175 | } 176 | 177 | sendTime = curTime; 178 | } 179 | 180 | status = ros::ok(); 181 | rate.sleep(); 182 | } 183 | 184 | return 0; 185 | } 186 | -------------------------------------------------------------------------------- /src/bbox_calculation/src/bboxCalculation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | 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 map_file_dir; 32 | string object_list_file_dir; 33 | int maxObjectNum = 100; 34 | 35 | pcl::PointCloud::Ptr mapCloud(new pcl::PointCloud()); 36 | 37 | void readMapFile() 38 | { 39 | FILE* map_file = fopen(map_file_dir.c_str(), "r"); 40 | if (map_file == NULL) { 41 | printf ("\nCannot read input files, exit.\n\n"); 42 | exit(1); 43 | } 44 | 45 | char str[50]; 46 | int val, pointNum; 47 | string strCur, strLast; 48 | while (strCur != "end_header") { 49 | val = fscanf(map_file, "%s", str); 50 | if (val != 1) { 51 | printf ("\nError reading input files, exit.\n\n"); 52 | exit(1); 53 | } 54 | 55 | strLast = strCur; 56 | strCur = string(str); 57 | 58 | if (strCur == "vertex" && strLast == "element") { 59 | val = fscanf(map_file, "%d", &pointNum); 60 | if (val != 1) { 61 | printf ("\nError reading input files, exit.\n\n"); 62 | exit(1); 63 | } 64 | } 65 | } 66 | 67 | mapCloud->clear(); 68 | pcl::PointXYZI point; 69 | int val1, val2, val3, val4; 70 | for (int i = 0; i < pointNum; i++) { 71 | val1 = fscanf(map_file, "%f", &point.x); 72 | val2 = fscanf(map_file, "%f", &point.y); 73 | val3 = fscanf(map_file, "%f", &point.z); 74 | val4 = fscanf(map_file, "%f", &point.intensity); 75 | 76 | if (val1 != 1 || val2 != 1 || val3 != 1 || val4 != 1) { 77 | printf ("\nError reading input files, exit.\n\n"); 78 | exit(1); 79 | } 80 | 81 | mapCloud->push_back(point); 82 | } 83 | 84 | fclose(map_file); 85 | } 86 | 87 | int main(int argc, char** argv) 88 | { 89 | ros::init(argc, argv, "bboxCalculation"); 90 | ros::NodeHandle nh; 91 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 92 | 93 | nhPrivate.getParam("map_file_dir", map_file_dir); 94 | nhPrivate.getParam("object_list_file_dir", object_list_file_dir); 95 | nhPrivate.getParam("maxObjectNum", maxObjectNum); 96 | 97 | readMapFile(); 98 | int mapCloudSize = mapCloud->points.size(); 99 | 100 | printf ("\nRead %d points from map file.\n", mapCloudSize); 101 | 102 | const int objNumConst = maxObjectNum; 103 | float objCX[objNumConst] = {0}; 104 | float objCY[objNumConst] = {0}; 105 | int objSize[objNumConst] = {0}; 106 | 107 | for (int i = 0; i < mapCloudSize; i++) { 108 | int j = int(mapCloud->points[i].intensity); 109 | if (j < 0 || j >= maxObjectNum) continue; 110 | 111 | objCX[j] += mapCloud->points[i].x; 112 | objCY[j] += mapCloud->points[i].y; 113 | objSize[j]++; 114 | } 115 | 116 | for (int j = 0; j < maxObjectNum; j++) { 117 | if (objSize[j] > 0) { 118 | objCX[j] /= objSize[j]; 119 | objCY[j] /= objSize[j]; 120 | } 121 | } 122 | 123 | float objC11[objNumConst] = {0}; 124 | float objC12[objNumConst] = {0}; 125 | float objC22[objNumConst] = {0}; 126 | 127 | for (int i = 0; i < mapCloudSize; i++) { 128 | int j = int(mapCloud->points[i].intensity); 129 | if (j < 0 || j >= maxObjectNum) continue; 130 | 131 | float dx = mapCloud->points[i].x - objCX[j]; 132 | float dy = mapCloud->points[i].y - objCY[j]; 133 | 134 | objC11[j] += dx * dx; 135 | objC12[j] += dx * dy; 136 | objC22[j] += dy * dy; 137 | } 138 | 139 | for (int j = 0; j < maxObjectNum; j++) { 140 | if (objSize[j] > 0) { 141 | objC11[j] /= objSize[j]; 142 | objC12[j] /= objSize[j]; 143 | objC22[j] /= objSize[j]; 144 | } 145 | } 146 | 147 | cv::Mat matCov(2, 2, CV_32F, cv::Scalar::all(0)); 148 | cv::Mat matE(1, 2, CV_32F, cv::Scalar::all(0)); 149 | cv::Mat matV(2, 2, CV_32F, cv::Scalar::all(0)); 150 | 151 | float objHeading[objNumConst] = {0}; 152 | 153 | for (int j = 0; j < maxObjectNum; j++) { 154 | if (objSize[j] > 0) { 155 | matCov.at(0, 0) = objC11[j]; 156 | matCov.at(0, 1) = objC12[j]; 157 | matCov.at(1, 0) = objC12[j]; 158 | matCov.at(1, 1) = objC22[j]; 159 | 160 | cv::eigen(matCov, matE, matV); 161 | objHeading[j] = atan2(matV.at(0, 1), matV.at(0, 0)); 162 | } 163 | } 164 | 165 | float objMinL[objNumConst] = {0}; 166 | float objMaxL[objNumConst] = {0}; 167 | float objMinW[objNumConst] = {0}; 168 | float objMaxW[objNumConst] = {0}; 169 | float objMinH[objNumConst] = {0}; 170 | float objMaxH[objNumConst] = {0}; 171 | 172 | for (int j = 0; j < maxObjectNum; j++) { 173 | if (objSize[j] > 0) { 174 | objMinL[j] = 1000000.0; 175 | objMaxL[j] = -1000000.0; 176 | objMinW[j] = 1000000.0; 177 | objMaxW[j] = -1000000.0; 178 | objMinH[j] = 1000000.0; 179 | objMaxH[j] = -1000000.0; 180 | } 181 | } 182 | 183 | for (int i = 0; i < mapCloudSize; i++) { 184 | int j = int(mapCloud->points[i].intensity); 185 | if (j < 0 || j >= maxObjectNum) continue; 186 | 187 | float l = cos(objHeading[j]) * mapCloud->points[i].x + sin(objHeading[j]) * mapCloud->points[i].y; 188 | float w = -sin(objHeading[j]) * mapCloud->points[i].x + cos(objHeading[j]) * mapCloud->points[i].y; 189 | float h = mapCloud->points[i].z; 190 | 191 | if (objMinL[j] > l) objMinL[j] = l; 192 | if (objMaxL[j] < l) objMaxL[j] = l; 193 | if (objMinW[j] > w) objMinW[j] = w; 194 | if (objMaxW[j] < w) objMaxW[j] = w; 195 | if (objMinH[j] > h) objMinH[j] = h; 196 | if (objMaxH[j] < h) objMaxH[j] = h; 197 | } 198 | 199 | FILE *object_list_file = fopen(object_list_file_dir.c_str(), "w"); 200 | if (object_list_file == NULL) { 201 | printf ("\nCannot write output file, exit.\n\n"); 202 | exit(1); 203 | } 204 | 205 | int objectCount = 0; 206 | for (int j = 0; j < maxObjectNum; j++) { 207 | if (objSize[j] > 0) { 208 | float midL = (objMinL[j] + objMaxL[j]) / 2.0; 209 | float midW = (objMinW[j] + objMaxW[j]) / 2.0; 210 | float midH = (objMinH[j] + objMaxH[j]) / 2.0; 211 | float midX = cos(objHeading[j]) * midL - sin(objHeading[j]) * midW; 212 | float midY = sin(objHeading[j]) * midL + cos(objHeading[j]) * midW; 213 | 214 | fprintf(object_list_file, "%d %f %f %f %f %f %f %f\n", j, midX, midY, midH, 215 | objMaxL[j] - objMinL[j], objMaxW[j] - objMinW[j], objMaxH[j] - objMinH[j], objHeading[j]); 216 | 217 | objectCount++; 218 | } 219 | } 220 | 221 | fclose(object_list_file); 222 | 223 | printf ("\nBounding boxes of %d objects saved to file. Exit.\n", objectCount); 224 | 225 | return 0; 226 | } 227 | -------------------------------------------------------------------------------- /src/joystick_drivers/README.md: -------------------------------------------------------------------------------- 1 | # ROS Joystick Drivers Stack # 2 | 3 | [![](https://github.com/ros-drivers/joystick_drivers/workflows/Basic%20Build%20Workflow/badge.svg?branch=master)](https://github.com/ros-drivers/joystick_drivers/actions) 4 | 5 | A simple set of nodes for supporting various types of joystick inputs and producing ROS messages from the underlying OS messages. 6 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package joy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.15.1 (2021-06-07) 6 | ------------------- 7 | 8 | 1.15.0 (2020-10-12) 9 | ------------------- 10 | * Added autodetection for force-feedback devices. (`#169 `_) 11 | * Added autodetection for force-feedback devices. 12 | * RAII for closedir 13 | * joy: Little fixes for force feedback. (`#167 `_) 14 | This commit increases the maximum magnitude of the FF effects to double the previous maximum. 15 | * Print out joystick name on initialization. (`#168 `_) 16 | This helps figuring out what string to give to the `dev_name` parameter. 17 | * Contributors: Martin Pecka 18 | 19 | 1.14.0 (2020-07-07) 20 | ------------------- 21 | * frame_id in the header of the joystick msg (`#166 `_) 22 | * roslint and Generic Clean-Up (`#161 `_) 23 | * Merge pull request `#158 `_ from clalancette/ros1-cleanups 24 | ROS1 joy cleanups 25 | * Greatly simplify the sticky_buttons support. 26 | * Small fixes to rumble support. 27 | * Use C++ style casts. 28 | * Use empty instead of length. 29 | * joy_def_ff -> joy_dev_ff 30 | * Cleanup header includes. 31 | * Use size_t appropriately. 32 | * NULL -> nullptr everywhere. 33 | * Style cleanup in joy_node.cpp. 34 | * Merge pull request `#154 `_ from zchen24/master 35 | Minor: moved default to right indent level 36 | * Contributors: Chris Lalancette, Joshua Whitley, Mamoun Gharbi, Zihan Chen 37 | 38 | 1.13.0 (2019-06-24) 39 | ------------------- 40 | * Merge pull request `#120 `_ from furushchev/remap 41 | add joy_remap and its sample 42 | * Merge pull request `#128 `_ from ros-drivers/fix/tab_errors 43 | Cleaning up Python indentation. 44 | * Merge pull request `#111 `_ from matt-attack/indigo-devel 45 | Add Basic Force Feedback Support 46 | * Merge pull request `#126 `_ from clalancette/minor-formatting 47 | * Put brackets around ROS\_* macros. 48 | In some circumstances they may be defined to empty, so we need 49 | to have brackets to ensure that they are syntactically valid. 50 | Signed-off-by: Chris Lalancette 51 | * Merge pull request `#122 `_ from lbucklandAS/fix-publish-timestamp 52 | Add timestamp to all joy messages 53 | * Change error messages and set ps3 as default controller 54 | * Better handle device loss 55 | Allow for loss and redetection of device with force feedback 56 | * Add basic force feedback over usb 57 | Addresses `#89 `_ 58 | * Contributors: Chris Lalancette, Furushchev, Joshua Whitley, Lucas Buckland, Matthew, Matthew Bries 59 | 60 | 1.12.0 (2018-06-11) 61 | ------------------- 62 | * Update timestamp when using autorepeat_rate 63 | * Added dev_name parameter to select joystick by name 64 | * Added Readme for joy package with description of new device name parameter 65 | * Fixed numerous outstanding PRs. 66 | * Added sticky buttons 67 | * Changed package xml to format 2 68 | * Fixed issue when the joystick data did not got send until changed. 69 | * Changed messaging to better reflect what the script is doing 70 | * Contributors: Dino Hüllmann, Jonathan Bohren, Joshua Whitley, Miklos Marton, Naoki Mizuno, jprod123, psimona 71 | 72 | 1.11.0 (2017-02-10) 73 | ------------------- 74 | * fixed joy/Cmakelists for osx 75 | * Update dependencies to remove warnings 76 | * Contributors: Marynel Vazquez, Mark D Horn 77 | 78 | 1.10.1 (2015-05-24) 79 | ------------------- 80 | * Remove stray architechture_independent flags 81 | * Contributors: Jonathan Bohren, Scott K Logan 82 | 83 | 1.10.0 (2014-06-26) 84 | ------------------- 85 | * First indigo release 86 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(joy) 3 | 4 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 5 | 6 | # Load catkin and all dependencies required for this package 7 | set(CATKIN_DEPS roscpp diagnostic_updater sensor_msgs roslint) 8 | find_package(catkin REQUIRED ${CATKIN_DEPS}) 9 | 10 | roslint_cpp() 11 | 12 | catkin_package(DEPENDS sensor_msgs) 13 | 14 | # Look for 15 | include(CheckIncludeFiles) 16 | check_include_files(linux/joystick.h HAVE_LINUX_JOYSTICK_H) 17 | 18 | if(CATKIN_ENABLE_TESTING) 19 | roslint_add_test() 20 | endif() 21 | 22 | if(HAVE_LINUX_JOYSTICK_H) 23 | include_directories(msg/cpp ${catkin_INCLUDE_DIRS}) 24 | add_executable(joy_node src/joy_node.cpp) 25 | target_link_libraries(joy_node ${catkin_LIBRARIES}) 26 | 27 | # Install targets 28 | install(TARGETS joy_node 29 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 30 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 31 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}) 32 | else() 33 | message("Warning: no ; won't build joy node") 34 | endif() 35 | 36 | install(DIRECTORY migration_rules scripts config launch 37 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 38 | USE_SOURCE_PERMISSIONS) 39 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/README.md: -------------------------------------------------------------------------------- 1 | # ROS Driver for Generic Linux Joysticks 2 | 3 | The joy package contains joy_node, a node that interfaces a generic Linux joystick to ROS. This node publishes a "Joy" message, which contains the current state of each one of the joystick's buttons and axes. 4 | 5 | ## Supported Hardware 6 | 7 | This node should work with any joystick that is supported by Linux. 8 | 9 | ## Published Topics 10 | 11 | * joy ([sensor_msgs/Joy](http://docs.ros.org/api/sensor_msgs/html/msg/Joy.html)): outputs the joystick state. 12 | 13 | ## Device Selection 14 | 15 | There are two parameters controlling which device should be used: 16 | 17 | * ~dev (string, default: "/dev/input/js0") 18 | * ~dev_name (string, default: "" (empty = disabled)) 19 | 20 | If ~dev_name is empty, ~dev defines the Linux joystick device from which to read joystick events. 21 | 22 | If ~dev_name is defined, the node enumerates all available joysticks, i.e. /dev/input/js*. The first joystick matching ~dev_name is opened. If no joystick matches the desired device name, the device specified by ~dev is used as a fallback. 23 | 24 | To get a list of the names of all connected joysticks, an invalid ~dev_name can be specified. For example: 25 | 26 | `rosrun joy joy_node _dev_name:="*"` 27 | 28 | The output might look like this: 29 | 30 | ``` 31 | [ INFO]: Found joystick: ST LIS3LV02DL Accelerometer (/dev/input/js1). 32 | [ INFO]: Found joystick: Microsoft X-Box 360 pad (/dev/input/js0). 33 | [ERROR]: Couldn't find a joystick with name *. Falling back to default device. 34 | ``` 35 | 36 | Then the node can be started with the device name given in the list. For example: 37 | 38 | `rosrun joy joy_node _dev_name:="Microsoft X-Box 360 pad"` 39 | 40 | ## Advanced Parameters 41 | 42 | * ~deadzone (double, default: 0.05) 43 | * Amount by which the joystick has to move before it is considered to be off-center. This parameter is specified relative to an axis normalized between -1 and 1. Thus, 0.1 means that the joystick has to move 10% of the way to the edge of an axis's range before that axis will output a non-zero value. Linux does its own deadzone processing, so in many cases this value can be set to zero. 44 | 45 | * ~autorepeat_rate (double, default: 0.0 (disabled)) 46 | * Rate in Hz at which a joystick that has a non-changing state will resend the previously sent message. 47 | 48 | * ~coalesce_interval (double, default: 0.001) 49 | * Axis events that are received within coalesce_interval (seconds) of each other are sent out in a single ROS message. Since the kernel sends each axis motion as a separate event, coalescing greatly reduces the rate at which messages are sent. This option can also be used to limit the rate of outgoing messages. Button events are always sent out immediately to avoid missing button presses. 50 | 51 | ## Further Information 52 | 53 | For further information have a look at the [Wiki page](http://wiki.ros.org/joy). -------------------------------------------------------------------------------- /src/joystick_drivers/joy/config/ps4joy.yaml: -------------------------------------------------------------------------------- 1 | mappings: 2 | axes: 3 | - axes[0] 4 | - axes[1] 5 | - axes[2] 6 | - axes[5] 7 | - axes[6] * 2.0 8 | - axes[7] * 2.0 9 | - axes[8] * -2.0 10 | - 0.0 11 | - max(axes[10], 0.0) * -1.0 12 | - min(axes[9], 0.0) 13 | - min(axes[10], 0.0) 14 | - max(axes[9], 0.0) * -1.0 15 | - (axes[3] - 1.0) * 0.5 16 | - (axes[4] - 1.0) * 0.5 17 | - buttons[4] * -1.0 18 | - buttons[5] * -1.0 19 | - buttons[3] * -1.0 20 | - buttons[2] * -1.0 21 | - buttons[1] * -1.0 22 | - buttons[0] * -1.0 23 | - 0.0 24 | - 0.0 25 | - 0.0 26 | - 0.0 27 | - 0.0 28 | - 0.0 29 | - 0.0 30 | buttons: 31 | - buttons[8] 32 | - buttons[10] 33 | - buttons[11] 34 | - buttons[9] 35 | - max(axes[10], 0.0) 36 | - min(axes[9], 0.0) * -1.0 37 | - min(axes[10], 0.0) * -1.0 38 | - max(axes[9], 0.0) 39 | - buttons[6] 40 | - buttons[7] 41 | - buttons[4] 42 | - buttons[5] 43 | - buttons[3] 44 | - buttons[2] 45 | - buttons[1] 46 | - buttons[0] 47 | - buttons[12] 48 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/launch/ps4joy.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/mainpage.dox: -------------------------------------------------------------------------------- 1 | /** 2 | 3 | \mainpage 4 | \htmlinclude manifest.html 5 | 6 | \b joy ROS joystick drivers for Linux. This package works with Logitech joysticks and PS3 SIXAXIS/DUALSHOCK devices. This package will only build is linux/joystick.h 7 | 8 | \section rosapi ROS API 9 | 10 | This package contains the message "Joy", which carries data from the joystick's axes and buttons. It also has the joy_node, which reads from a given joystick port and publishes "Joy" messages. 11 | 12 | List of nodes: 13 | - \b joy_node 14 | 15 | Deprecated nodes: 16 | - \b txjoy 17 | - \b joy 18 | The deprecated nodes are the same as the joy_node, but they will warn users when used. They are actually just bash scripts that call "joy_node" and print a warning. 19 | 20 |
21 | 22 | \subsection joy joy 23 | 24 | \b joy ROS joystick driver for all linux joysticks. The driver will poll a given port until it can read from it, the publish Joy messages of the joystick state. If the port closes, or it reads an error, it will reopen the port. All axes are in the range [-1, 1], and all buttons are 0 (off) or 1 (on). 25 | 26 | Since the driver will poll for the joystick port, and automatically reopen the port if it's closed, the joy_node should be "on" whenever possible. It is typically part of the robot's launch file. 27 | 28 | \subsubsection autorepeat Auto-Repeat/Signal Loss 29 | 30 | The joy_node takes an "~autorepeat_rate" parameter. If the linux kernal receives no events during the autorepeat interval, it will automatically repeat the last value of the joystick. This is an important safety feature, and allows users to recover from a joystick that has timed out. 31 | 32 | \subsubsection usage Usage 33 | \verbatim 34 | $ joy [standard ROS args] 35 | \endverbatim 36 | 37 | \subsubsection topic ROS topics 38 | 39 | Subscribes to (name / type): 40 | - None 41 | 42 | Publishes to (name / type): 43 | - \b "joy/Joy" : Joystick output. Axes are [-1, 1], buttons are 0 or 1 (depressed). 44 | 45 | \subsubsection parameters ROS parameters 46 | - \b "~dev" : Input device for joystick. Default: /dev/input/js0 47 | - \b "~deadzone" : Output is zero for axis in deadzone. Range: [-0.9, 0.9]. Default 0.05 48 | - \b "~autorepeat_rate" : If no events, repeats last known state at this rate. Defualt: 0 (disabled) 49 | - \b "~coalesce_interval" : Waits for this interval (seconds) after receiving an event. If multiple events happen in this interval, only one message will be sent. Reduces number of messages. Default: 0.001. 50 | 51 | 52 |
53 | 54 | 55 | 56 | */ 57 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/migration_rules/Joy.bmr: -------------------------------------------------------------------------------- 1 | class update_joy_Joy_e3ef016fcdf22397038b36036c66f7c8(MessageUpdateRule): 2 | old_type = "joy/Joy" 3 | old_full_text = """ 4 | float32[] axes 5 | int32[] buttons 6 | """ 7 | 8 | new_type = "sensor_msgs/Joy" 9 | new_full_text = """ 10 | # Reports the state of a joysticks axes and buttons. 11 | Header header # timestamp in the header is the time the data is received from the joystick 12 | float32[] axes # the axes measurements from a joystick 13 | int32[] buttons # the buttons measurements from a joystick 14 | 15 | ================================================================================ 16 | MSG: std_msgs/Header 17 | # Standard metadata for higher-level stamped data types. 18 | # This is generally used to communicate timestamped data 19 | # in a particular coordinate frame. 20 | # 21 | # sequence ID: consecutively increasing ID 22 | uint32 seq 23 | #Two-integer timestamp that is expressed as: 24 | # * stamp.secs: seconds (stamp_secs) since epoch 25 | # * stamp.nsecs: nanoseconds since stamp_secs 26 | # time-handling sugar is provided by the client library 27 | time stamp 28 | #Frame this data is associated with 29 | # 0: no frame 30 | # 1: global frame 31 | string frame_id 32 | """ 33 | 34 | order = 0 35 | migrated_types = [] 36 | 37 | valid = True 38 | 39 | def update(self, old_msg, new_msg): 40 | #No matching field name in old message 41 | new_msg.header = self.get_new_class('Header')() 42 | new_msg.axes = old_msg.axes 43 | new_msg.buttons = old_msg.buttons 44 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | joy 3 | 1.15.1 4 | BSD 5 | 6 | 7 | ROS driver for a generic Linux joystick. 8 | The joy package contains joy_node, a node that interfaces a 9 | generic Linux joystick to ROS. This node publishes a "Joy" 10 | message, which contains the current state of each one of the 11 | joystick's buttons and axes. 12 | 13 | 14 | Jonathan Bohren 15 | Morgan Quigley 16 | Brian Gerkey 17 | Kevin Watts 18 | Blaise Gassend 19 | 20 | http://www.ros.org/wiki/joy 21 | https://github.com/ros-drivers/joystick_drivers 22 | https://github.com/ros-drivers/joystick_drivers/issues 23 | 24 | catkin 25 | roslint 26 | 27 | diagnostic_updater 28 | joystick 29 | roscpp 30 | sensor_msgs 31 | 32 | rosbag 33 | 34 | 35 | 36 | 37 | 38 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/scripts/joy_remap.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # -*- coding: utf-8 -*- 3 | # Author: furushchev 4 | 5 | import ast 6 | import operator as op 7 | import rospy 8 | import traceback 9 | from sensor_msgs.msg import Joy 10 | 11 | 12 | class RestrictedEvaluator(object): 13 | def __init__(self): 14 | self.operators = { 15 | ast.Add: op.add, 16 | ast.Sub: op.sub, 17 | ast.Mult: op.mul, 18 | ast.Div: op.truediv, 19 | ast.BitXor: op.xor, 20 | ast.USub: op.neg, 21 | } 22 | self.functions = { 23 | 'abs': lambda x: abs(x), 24 | 'max': lambda *x: max(*x), 25 | 'min': lambda *x: min(*x), 26 | } 27 | 28 | def _reval_impl(self, node, variables): 29 | if isinstance(node, ast.Num): 30 | return node.n 31 | elif isinstance(node, ast.BinOp): 32 | op = self.operators[type(node.op)] 33 | return op(self._reval_impl(node.left, variables), 34 | self._reval_impl(node.right, variables)) 35 | elif isinstance(node, ast.UnaryOp): 36 | op = self.operators[type(node.op)] 37 | return op(self._reval_impl(node.operand, variables)) 38 | elif isinstance(node, ast.Call) and node.func.id in self.functions: 39 | func = self.functions[node.func.id] 40 | args = [self._reval_impl(n, variables) for n in node.args] 41 | return func(*args) 42 | elif isinstance(node, ast.Name) and node.id in variables: 43 | return variables[node.id] 44 | elif isinstance(node, ast.Subscript) and node.value.id in variables: 45 | var = variables[node.value.id] 46 | idx = node.slice.value.n 47 | try: 48 | return var[idx] 49 | except IndexError: 50 | raise IndexError("Variable '%s' out of range: %d >= %d" % (node.value.id, idx, len(var))) 51 | else: 52 | raise TypeError("Unsupported operation: %s" % node) 53 | 54 | def reval(self, expr, variables): 55 | expr = str(expr) 56 | if len(expr) > 1000: 57 | raise ValueError("The length of an expression must not be more than 1000 characters") 58 | try: 59 | return self._reval_impl(ast.parse(expr, mode='eval').body, variables) 60 | except Exception as e: 61 | rospy.logerr(traceback.format_exc()) 62 | raise e 63 | 64 | 65 | class JoyRemap(object): 66 | def __init__(self): 67 | self.evaluator = RestrictedEvaluator() 68 | self.mappings = self.load_mappings("~mappings") 69 | self.warn_remap("joy_out") 70 | self.pub = rospy.Publisher( 71 | "joy_out", Joy, queue_size=1) 72 | self.warn_remap("joy_in") 73 | self.sub = rospy.Subscriber( 74 | "joy_in", Joy, self.callback, 75 | queue_size=rospy.get_param("~queue_size", None)) 76 | 77 | def load_mappings(self, ns): 78 | btn_remap = rospy.get_param(ns + "/buttons", []) 79 | axes_remap = rospy.get_param(ns + "/axes", []) 80 | rospy.loginfo("loaded remapping: %d buttons, %d axes" % (len(btn_remap), len(axes_remap))) 81 | return {"buttons": btn_remap, "axes": axes_remap} 82 | 83 | def warn_remap(self, name): 84 | if name == rospy.remap_name(name): 85 | rospy.logwarn("topic '%s' is not remapped" % name) 86 | 87 | def callback(self, in_msg): 88 | out_msg = Joy(header=in_msg.header) 89 | map_axes = self.mappings["axes"] 90 | map_btns = self.mappings["buttons"] 91 | out_msg.axes = [0.0] * len(map_axes) 92 | out_msg.buttons = [0] * len(map_btns) 93 | in_dic = {"axes": in_msg.axes, "buttons": in_msg.buttons} 94 | for i, exp in enumerate(map_axes): 95 | try: 96 | out_msg.axes[i] = self.evaluator.reval(exp, in_dic) 97 | except NameError as e: 98 | rospy.logerr("You are using vars other than 'buttons' or 'axes': %s" % e) 99 | except UnboundLocalError as e: 100 | rospy.logerr("Wrong form: %s" % e) 101 | except Exception as e: 102 | raise e 103 | 104 | for i, exp in enumerate(map_btns): 105 | try: 106 | if self.evaluator.reval(exp, in_dic) > 0: 107 | out_msg.buttons[i] = 1 108 | except NameError as e: 109 | rospy.logerr("You are using vars other than 'buttons' or 'axes': %s" % e) 110 | except UnboundLocalError as e: 111 | rospy.logerr("Wrong form: %s" % e) 112 | except Exception as e: 113 | raise e 114 | 115 | self.pub.publish(out_msg) 116 | 117 | 118 | if __name__ == '__main__': 119 | rospy.init_node("joy_remap") 120 | n = JoyRemap() 121 | rospy.spin() 122 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/test/saved/Joy.saved: -------------------------------------------------------------------------------- 1 | [joy/Joy]: 2 | float32[] axes 3 | int32[] buttons 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/joystick_drivers/joy/test/test_joy_msg_migration.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2008, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of Willow Garage, Inc. nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | # 34 | 35 | import roslib 36 | roslib.load_manifest('joy') 37 | 38 | import sys 39 | import struct 40 | 41 | import unittest 42 | 43 | import rostest 44 | import rosbag 45 | import rosbagmigration 46 | 47 | import re 48 | from cStringIO import StringIO 49 | import os 50 | 51 | import rospy 52 | 53 | 54 | 55 | migrator = rosbagmigration.MessageMigrator() 56 | 57 | 58 | def repack(x): 59 | return struct.unpack('`_) 14 | * roslint and Generic Clean-Up (`#161 `_) 15 | * Contributors: Joshua Whitley 16 | 17 | 1.13.0 (2019-06-24) 18 | ------------------- 19 | 20 | 1.12.0 (2018-06-11) 21 | ------------------- 22 | * Addressed numerous outstanding PRs. 23 | * Changed package xml to format 2 24 | * Contributors: Jonathan Bohren, jprod123 25 | 26 | 1.11.0 (2017-02-10) 27 | ------------------- 28 | 29 | 1.10.1 (2015-05-24) 30 | ------------------- 31 | 32 | 1.10.0 (2014-06-26) 33 | ------------------- 34 | * First indigo release 35 | -------------------------------------------------------------------------------- /src/joystick_drivers/joystick_drivers/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(joystick_drivers) 3 | find_package(catkin REQUIRED) 4 | catkin_metapackage() 5 | -------------------------------------------------------------------------------- /src/joystick_drivers/joystick_drivers/package.xml: -------------------------------------------------------------------------------- 1 | 2 | joystick_drivers 3 | 1.15.1 4 | BSD 5 | 6 | 7 | This metapackage depends on packages for interfacing common 8 | joysticks and human input devices with ROS. 9 | 10 | 11 | http://www.ros.org/wiki/joystick_drivers 12 | https://github.com/ros-drivers/joystick_drivers 13 | https://github.com/ros-drivers/joystick_drivers/issues 14 | 15 | Jonathan Bohren 16 | 17 | catkin 18 | 19 | joy 20 | ps3joy 21 | spacenav_node 22 | wiimote 23 | 24 | 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package ps3joy 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.15.0 (2020-10-12) 6 | ------------------- 7 | 8 | 1.14.0 (2020-07-07) 9 | ------------------- 10 | * Fixing linter errors for Noetic. (`#174 `_) 11 | * Make sure to import struct where it is used. (`#162 `_) 12 | * roslint and Generic Clean-Up (`#161 `_) 13 | * Contributors: Chris Lalancette, Joshua Whitley 14 | 15 | 1.13.0 (2019-06-24) 16 | ------------------- 17 | * Merge pull request `#128 `_ from ros-drivers/fix/tab_errors 18 | * Cleaning up Python indentation. 19 | * Merge pull request `#123 `_ from cclauss/modernize-python2-code 20 | * Modernize Python 2 code to get ready for Python 3 21 | * Merge branch 'master' into indigo-devel 22 | * Contributors: Joshua Whitley, Matthew, cclauss 23 | 24 | 1.12.0 (2018-06-11) 25 | ------------------- 26 | * Addressed numerous outstanding PRs. 27 | * Created bluetooth_devices.md 28 | * Created testing guide for ps3joy. 29 | * Create procedure_test.md 30 | * Let ps3joy_node not quit on inactivity-timeout. 31 | * Refine diagnostics message usage in ps3joy_node 32 | * Improve ps3joy_node with rospy.init_node and .is_shutdown 33 | * Remove quit on failed root level check, part one of issue `#53 `_ 34 | * Create README 35 | * Changed package xml to format 2 36 | * Contributors: Alenso Labady, Felix Kolbe, Jonathan Bohren, alab288, jprod123 37 | 38 | 1.11.0 (2017-02-10) 39 | ------------------- 40 | * Update dependencies to remove warnings 41 | * Contributors: Mark D Horn 42 | 43 | 1.10.1 (2015-05-24) 44 | ------------------- 45 | * Remove stray architechture_independent flags 46 | * Contributors: Jonathan Bohren, Scott K Logan 47 | 48 | 1.10.0 (2014-06-26) 49 | ------------------- 50 | * First indigo reelase 51 | * Update ps3joy/package.xml URLs with github user ros to ros-drivers 52 | * Prompt for sudo password when required 53 | * Contributors: Felix Kolbe, Jonathan Bohren, dawonn 54 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(ps3joy) 3 | 4 | find_package(PkgConfig REQUIRED) 5 | pkg_search_module(LIBUSB REQUIRED libusb) 6 | 7 | if(LIBUSB_FOUND) 8 | include_directories(SYSTEM ${LIBUSB_INCLUDE_DIRS}) 9 | link_directories(${LIBUSB_LIBRARY_DIRS}) 10 | else() 11 | message( FATAL_ERROR "Failed to find libusb" ) 12 | endif() 13 | 14 | # Load catkin and all dependencies required for this package 15 | set(CATKIN_DEPS diagnostic_msgs sensor_msgs rospy rosgraph roslint) 16 | find_package(catkin REQUIRED COMPONENTS ${CATKIN_DEPS}) 17 | catkin_package(CATKIN_DEPENDS diagnostic_msgs sensor_msgs) 18 | 19 | roslint_python( 20 | scripts/ps3joy.py 21 | scripts/ps3joy_node.py 22 | scripts/ps3joysim.py 23 | ) 24 | 25 | include_directories(${catkin_INCLUDE_DIRS}) 26 | add_executable(sixpair src/sixpair.c) 27 | target_link_libraries(sixpair -lusb ${catkin_LIBRARIES}) 28 | 29 | if(CATKIN_ENABLE_TESTING) 30 | roslint_add_test() 31 | endif() 32 | 33 | # Install targets 34 | install(TARGETS sixpair 35 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 36 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 37 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 38 | ) 39 | 40 | install(DIRECTORY launch 41 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 42 | ) 43 | 44 | install(FILES diagnostics.yaml 45 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 46 | ) 47 | 48 | catkin_install_python(PROGRAMS 49 | scripts/ps3joy.py 50 | scripts/ps3joy_node.py 51 | scripts/ps3joysim.py 52 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 53 | ) 54 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/README.md: -------------------------------------------------------------------------------- 1 | # PlayStation 3 Joystick Driver for ROS 2 | 3 | This package provides a driver for the PS3 (SIXAXIS or DUALSHOCK3) bluetooth joystick. 4 | 5 | This driver provides a more reliable connection, and provides access to the joystick's accelerometers and gyroscope. Linux's native support for the PS3 joystick does lacks this functionality. 6 | 7 | Additional documentation: 8 | 9 | * [Troubleshooting](doc/troubleshooting.md) 10 | * [Testing Instructions](doc/testing.md) 11 | * [Bluetooth Device Compatibility](doc/bluetooth_devices.md) 12 | 13 | ## Dependencies 14 | 15 | * joystick 16 | * libusb-dev 17 | * bluez-5.37 18 | 19 | ## Pairing instructions 20 | 21 | 1. If you can connect the joystick and the bluetooth dongle into the same 22 | computer connect the joystick to the computer using a USB cable. 23 | 24 | 2. Load the bluetooth dongle's MAC address into the ps3 joystick using: 25 | ``` 26 | sudo bash 27 | rosrun ps3joy sixpair 28 | ``` 29 | If you cannot connect the joystick to the same computer as the dongle, 30 | find out the bluetooth dongle's MAC address by running (on the computer 31 | that has the bluetooth dongle): 32 | ``` 33 | hciconfig 34 | ``` 35 | If this does not work, you may need to do 36 | ``` 37 | sudo hciconfig hci0 up 38 | ``` 39 | and retry 40 | ``` 41 | hciconfig 42 | ``` 43 | 3. Plug the PS3 joystick into some other computer using a USB cable. 44 | 45 | 4. Replace the joystick's mac address in the following command: 46 | ``` 47 | sudo rosrun ps3joy sixpair 01:23:45:67:89:ab 48 | ``` 49 | 50 | ## Starting the PS3 joystick 51 | 52 | 5. Run the following command 53 | ``` 54 | rosrun ps3joy ps3joy.py 55 | ``` 56 | 6. Open a new terminal and reboot bluez and run joy with: 57 | ``` 58 | sudo systemctl restart bluetooth 59 | rosrun joy joy_node 60 | ``` 61 | 7. Open a new terminal and echo the joy topic 62 | ``` 63 | rostopic echo joy 64 | ``` 65 | 8. This should make a joystick appear at /dev/input/js? 66 | 67 | 9. You can check that it is working with 68 | jstest /dev/input/js? 69 | (replace ? with the name of your joystick) 70 | 71 | ## Command-line Options 72 | 73 | ### ps3joy.py 74 | 75 | ``` 76 | usage: ps3joy.py [--inactivity-timeout=] [--no-disable-bluetoothd] [--redirect-output] [--continuous-output]= 77 | : inactivity timeout in seconds (saves battery life). 78 | : file name to redirect output to. 79 | ``` 80 | 81 | `--inactivity-timeout` 82 | This may be useful for saving battery life and reducing contention on the 2.4 GHz network.Your PS3 controller 83 | will shutdown after a given amount of time of inactivity. 84 | 85 | `--no-disable-bluetoothd` 86 | ps3joy.py will not take down bluetoothd. Bluetoothd must be configured to not handle input device, otherwise 87 | you will receive an error saying "Error binding to socket". 88 | 89 | `--redirect-output` 90 | This can be helpful when ps3joy.py is running in the backgound. This will allow the standard output 91 | and error messages to redirected into a file. 92 | 93 | `--continuous-output` 94 | This will output continuous motion streams and as a result this will no longer leave extended periods of 95 | no messages on the /joy topic. ( This only works for ps3joy.py. Entering this parameter in ps3joy_node.py will 96 | result in the parameter being ignored.) 97 | 98 | ## Limitations 99 | 100 | This driver will not coexist with any other bluetooth device. In future releases, we plan to allow first non-HID and later any bluetooth device to coexist with this driver. The following devices do coexist: 101 | 102 | * Non-HID devices using a userland driver, such as one written using pybluez. 103 | * Keyboards or mice running in HID proxy mode, which appear to the kernel as USB devices. 104 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/diagnostics.yaml: -------------------------------------------------------------------------------- 1 | type: AnalyzerGroup 2 | pub_rate: 1.0 # Optional 3 | base_path: '' # Optional, prepended to all diagnostic output 4 | analyzers: 5 | PS3State: 6 | type: diagnostic_aggregator/GenericAnalyzer 7 | path: 'PS3 State' 8 | timeout: 5.0 9 | startswith: ['Battery', 'Charging State', 'Connection', 'ps3_joy'] 10 | remove_prefix: 'ps3_joy' 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/doc/bluetooth_devices.md: -------------------------------------------------------------------------------- 1 | 2 | ## Bluetooth Device Compatibility 3 | 4 | This driver works with most 2.x Bluetooth adapters.This driver should also work with 3.0 and 4.0 Bluetooth adapters. 5 | Please report other compatible and incompatible Bluetooth adapters through a pull request to this page. 6 | 7 | ### Adapters that are known to work 8 | 9 | * Cambridge Silicon Radio, Ltd Bluetooth Dongle (Bluetooth 4.0) 10 | 11 | ### Adapters that are known not to work 12 | 13 | * Linksys USBBT100 version 2 (Bluetooth 1.1) 14 | * USB device 0a12:0x0001 15 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/doc/testing.md: -------------------------------------------------------------------------------- 1 | # Testing procedures for the ps3joy package # 2 | 3 | ## Pairing the ps3 controller via bluetooth ## 4 | If the controller is not paired to the bluetooth dongle connect 5 | the controller via usb 6 | 7 | Enter the following commands: 8 | 9 | ``` 10 | sudo bash 11 | rosrun ps3joy sixpair 12 | ``` 13 | The current bluetooth master address and setting master address should the same. 14 | 15 | ``` 16 | Current Bluetooth master: 00:15:83:ed:3f:21 17 | Setting master bd_addr to 00:15:83:ed:3f:21 18 | ``` 19 | 20 | ## Running the ps3joy nodes ## 21 | The package should consists of the following nodes: 22 | * ps3joy.py 23 | * ps3joy_node.py 24 | 25 | 26 | Running ps3joy_node.py will **require** being root to run if the user does not have 27 | permissions to the hardware. 28 | 29 | Enter the following commands in the terminal: 30 | 31 | ``` 32 | sudo bash 33 | rosrun ps3joy ps3joy_node.py 34 | ``` 35 | The following message should be diplayed aftwards: 36 | 37 | ``` 38 | Waiting Connection. Disconnect your PS3 joystick from USB and press the pairing button. 39 | ``` 40 | 41 | If your joystick does not pair, open a new terminal and restart bluez by 42 | entering the following command: 43 | 44 | ``` 45 | sudo systemctl restart bluetooth 46 | ``` 47 | The terminal where ps3joy_node.py was launched should display the following message: 48 | ``` 49 | Connection is Activated. 50 | ``` 51 | ## ps3joy Diagnostics test ## 52 | Open a new terminal and enter the following following command: 53 | ``` 54 | rostopic list 55 | ``` 56 | You should see the following: 57 | ``` 58 | /diagnostics 59 | /joy/set_feedback 60 | /rosout 61 | /rosout_agg 62 | ``` 63 | Enter the following command to diagnose the current state of the joystick 64 | ``` 65 | rostopic echo /diagnostics 66 | ``` 67 | You will see the charging state, battery percentage and, connection type in your terminal: 68 | ``` 69 | header: 70 | seq: 1 71 | stamp: 72 | secs: 1498667204 73 | nsecs: 603754043 74 | frame_id: '' 75 | status: 76 | - 77 | level: 0 78 | name: Battery 79 | message: 100% Charge 80 | hardware_id: '' 81 | values: [] 82 | - 83 | level: 0 84 | name: Connection 85 | message: Bluetooth Connection 86 | hardware_id: '' 87 | values: [] 88 | - 89 | level: 0 90 | name: Charging State 91 | message: Not Charging 92 | hardware_id: '' 93 | values: [] 94 | ``` 95 | 96 | If you plug in the usb cable both the connection type and charging state will change: 97 | ``` 98 | header: 99 | seq: 8 100 | stamp: 101 | secs: 1498667507 102 | nsecs: 798973083 103 | frame_id: '' 104 | status: 105 | - 106 | level: 0 107 | name: Battery 108 | message: Charging 109 | hardware_id: '' 110 | values: [] 111 | - 112 | level: 0 113 | name: Connection 114 | message: USB Connection 115 | hardware_id: '' 116 | values: [] 117 | - 118 | level: 0 119 | name: Charging State 120 | message: Charging 121 | hardware_id: '' 122 | values: [] 123 | ``` 124 | 125 | ## Confirming the ps3 joystick input ## 126 | Check to see if your joystick is recgonized by your computer. 127 | 128 | ``` 129 | ls /dev/input/ 130 | ``` 131 | Tell the ps3joy node which device is the ps3 joystick 132 | ``` 133 | rosparam set joy_node/dev "dev/input/jsX" 134 | ``` 135 | X would be the number your device was given. 136 | you can now start the joy node: 137 | 138 | ``` 139 | rosrun joy joy_node 140 | ``` 141 | You should see something that looks like this: 142 | ``` 143 | [ INFO] [1498673536.447541090]: Opened joystick: /dev/input/js0. deadzone_: 0.050000. 144 | ``` 145 | Open a new terminal and use rostopic to observe the data from the controller. 146 | ``` 147 | rostopic echo joy 148 | ``` 149 | You should see the input data dipslayed on your terminal. 150 | 151 | ## Recharging the PS3 joystick 152 | 1. Have an available USB port on a computer, and the computer must be on while the joystick is 153 | charging. 154 | 155 | 2. Connect the PS3 joystick to a computer using an A to mini-B USB cable. 156 | 157 | 3. The LEDs on the joystick should blink at about 1Hz to indicate the the joystick is charging. 158 | 159 | ## Shutting down the ps3 joystick 160 | There are two ways to turn of the ps3 joystick 161 | 1. Press and hold the pairing button on the joystick for approximately 10 seconds 162 | 163 | 2. You can also also shut the controller down by killing the process of ps3joy_node.py 164 | press Ctrl-c on your keyboard to kill the processes and the joystick will turn off 165 | as well. 166 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/doc/troubleshooting.md: -------------------------------------------------------------------------------- 1 | ## Troubleshooting ## 2 | ------------------------- 3 | #### Issues pairing the PS3 joystick #### 4 | When pairing your joystick via bluetooth, you may recieve the following message on 5 | your terminal: 6 | ``` 7 | Current Bluetooth master: 00:15:83:ed:3f:21 8 | Unable to retrieve local bd_addr from `hcitool dev`. 9 | Please enable Bluetooth or specify an address manually. 10 | ``` 11 | This would indicate that your bluetooth is disabled. To enable your bluetooth, try the 12 | following: 13 | 14 | 1. Check the status of your bluetooth by entering the following: 15 | ``` 16 | sudo systemctl status bluetooth 17 | ``` 18 | You may see something like this: 19 | 20 | ``` 21 | ● bluetooth.service - Bluetooth service 22 | Loaded: loaded (/lib/systemd/system/bluetooth.service; disabled; vendor preset: enabled) 23 | Active: inactive (dead) 24 | Docs: man:bluetoothd(8) 25 | ``` 26 | 27 | If you do, that means your bluetooth service is disabled. Turn enable it enter 28 | ``` 29 | sudo systemctl start bluetooth 30 | sudo systemctl status bluetooth 31 | ``` 32 | After running these commands your bluetooth service should be up and running: 33 | 34 | ``` 35 | ● bluetooth.service - Bluetooth service 36 | Loaded: loaded (/lib/systemd/system/bluetooth.service; disabled; vendor preset: enabled) 37 | Active: active (running) since Thu 2017-06-29 16:21:43 EDT; 16s ago 38 | Docs: man:bluetoothd(8) 39 | Main PID: 27362 (bluetoothd) 40 | Status: "Running" 41 | CGroup: /system.slice/bluetooth.service 42 | └─27362 /usr/local/libexec/bluetooth/bluetoothd 43 | ``` 44 | Retry the commands that were mentioned in step 2 for pairing the PS3 joystick. 45 | 46 | 2. Run the following command: 47 | ``` 48 | hciconfig hci0 reset 49 | ``` 50 | followed by: 51 | ``` 52 | sudo bash 53 | rosrun ps3joy sixpair 54 | ``` 55 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/launch/ps3.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/package.xml: -------------------------------------------------------------------------------- 1 | 2 | ps3joy 3 | 1.15.0 4 | BSD 5 | 6 | 7 | Playstation 3 SIXAXIS or DUAL SHOCK 3 joystick driver. 8 | Driver for the Sony PlayStation 3 SIXAXIS or DUAL SHOCK 3 9 | joysticks. In its current state, this driver is not compatible 10 | with the use of other Bluetooth HID devices. The driver listens 11 | for a connection on the HID ports, starts the joystick 12 | streaming data, and passes the data to the Linux uinput device 13 | so that it shows up as a normal joystick. 14 | 15 | 16 | Jonathan Bohren 17 | Blaise Gassend 18 | pascal@pabr.org 19 | Melonee Wise 20 | 21 | http://www.ros.org/wiki/ps3joy 22 | https://github.com/ros-drivers/joystick_drivers 23 | https://github.com/ros-drivers/joystick_drivers/issues 24 | 25 | catkin 26 | 27 | roslint 28 | 29 | bluez 30 | diagnostic_msgs 31 | joystick 32 | libusb-dev 33 | python3-bluez 34 | rosgraph 35 | rospy 36 | sensor_msgs 37 | 38 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/scripts/ps3joysim.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | # Software License Agreement (BSD License) 3 | # 4 | # Copyright (c) 2009, Willow Garage, Inc. 5 | # All rights reserved. 6 | # 7 | # Redistribution and use in source and binary forms, with or without 8 | # modification, are permitted provided that the following conditions 9 | # are met: 10 | # 11 | # * Redistributions of source code must retain the above copyright 12 | # notice, this list of conditions and the following disclaimer. 13 | # * Redistributions in binary form must reproduce the above 14 | # copyright notice, this list of conditions and the following 15 | # disclaimer in the documentation and/or other materials provided 16 | # with the distribution. 17 | # * Neither the name of the Willow Garage nor the names of its 18 | # contributors may be used to endorse or promote products derived 19 | # from this software without specific prior written permission. 20 | # 21 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 22 | # "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 23 | # LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 24 | # FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 25 | # COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 26 | # INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 27 | # BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 28 | # LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 29 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 30 | # LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 31 | # ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 32 | # POSSIBILITY OF SUCH DAMAGE. 33 | 34 | from __future__ import print_function 35 | from bluetooth import * 36 | import select 37 | import fcntl 38 | import os 39 | import struct 40 | import time 41 | import sys 42 | import traceback 43 | import threading 44 | import ps3joy 45 | import socket 46 | import signal 47 | 48 | 49 | def mk_in_socket(): 50 | sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) 51 | sock.bind(("127.0.0.1", 0)) 52 | sock.listen(1) 53 | return sock, sock.getsockname()[1] 54 | 55 | 56 | # Class to spawn the ps3joy.py infrastructure in its own thread 57 | class driversim(threading.Thread): 58 | def __init__(self, intr, ctrl): 59 | threading.Thread.__init__(self) 60 | self.intr = intr 61 | self.ctrl = ctrl 62 | self.start() 63 | 64 | def run(self): 65 | self.cm = ps3joy.connection_manager(ps3joy.decoder()) 66 | self.cm.listen(self.intr, self.ctrl) 67 | print("driversim exiting") 68 | 69 | def shutdown(self): 70 | self.cm.shutdown = True 71 | 72 | 73 | class joysim(threading.Thread): 74 | def __init__(self, intr, ctrl): 75 | threading.Thread.__init__(self) 76 | print("Starting joystick simulator on ports", intr, "and", ctrl) 77 | self.intr = socket.socket() 78 | self.intr.connect(("127.0.0.1", intr)) 79 | if self.intr == -1: 80 | raise "Error creating interrput socket." 81 | self.ctrl = socket.socket() 82 | self.ctrl.connect(("127.0.0.1", ctrl)) 83 | if self.ctrl == -1: 84 | raise "Error creating control socket." 85 | self.active = False 86 | self.shutdown = False 87 | self.start() 88 | 89 | def run(self): 90 | while not self.active and not self.shutdown: 91 | (rd, wr, err) = select.select([self.ctrl], [], [], 1) 92 | if len(rd) == 1: 93 | cmd = self.ctrl.recv(128) 94 | if cmd == "\x53\xf4\x42\x03\x00\x00": 95 | self.active = True 96 | print("Got activate command") 97 | else: 98 | print("Got unknown command (len=%i)" % len(cmd), end=' ') 99 | time.sleep(1) 100 | for c in cmd: 101 | print("%x" % ord(c), end=' ') 102 | print() 103 | print("joyactivate exiting") 104 | 105 | def publishstate(self, ax, butt): 106 | if self.active: 107 | ranges = [255] * 17 + [8191] * 20 108 | axval = [int((v + 1) * s / 2) for (v, s) in zip(ax, ranges)] 109 | buttout = [] 110 | for i in range(0, 2): 111 | newval = 0 112 | for j in range(0, 8): 113 | newval = (newval << 1) 114 | if butt[i * 8 + j]: 115 | newval = newval + 1 116 | buttout.append(newval) 117 | joy_coding = "!1B2x3B1x4B4x12B15x4H" 118 | self.intr.send(struct.pack(joy_coding, 161, *(buttout + [0] + axval))) 119 | else: 120 | print("Tried to publish while inactive") 121 | 122 | 123 | if __name__ == "__main__": 124 | def stop_all_threads(a, b): 125 | exit(0) 126 | 127 | signal.signal(signal.SIGINT, stop_all_threads) 128 | 129 | # Create sockets for the driver side and pass them to the driver 130 | (intr_in, intr_port) = mk_in_socket() 131 | (ctrl_in, ctrl_port) = mk_in_socket() 132 | 133 | ds = driversim(intr_in, ctrl_in) 134 | 135 | # Give the simulator a chance to get going 136 | time.sleep(2) 137 | 138 | # Call up the simulator telling it which ports to connect to. 139 | js = joysim(intr_port, ctrl_port) 140 | buttons1 = [True] * 16 141 | axes1 = [1, 0, -1, .5] * 5 142 | buttons2 = [False] * 16 143 | axes2 = [-1] * 20 144 | buttons3 = [False] * 16 145 | axes3 = [0] * 20 146 | shutdown = False 147 | while not js.active and not shutdown: 148 | time.sleep(0.2) 149 | time.sleep(0.01) 150 | time.sleep(0.01) 151 | while not shutdown: 152 | js.publishstate(axes1, buttons2) 153 | time.sleep(0.01) 154 | axes1[0] = -axes1[0] 155 | js.publishstate(axes2, buttons2) 156 | time.sleep(0.01) 157 | 158 | print("main exiting") 159 | -------------------------------------------------------------------------------- /src/joystick_drivers/ps3joy/src/sixpair.c: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2007, 2008 pascal@pabr.org 3 | * All rights reserved. 4 | * 5 | * Redistribution and use in source and binary forms, with or without 6 | * modification, are permitted provided that the following conditions are met: 7 | * 8 | * * Redistributions of source code must retain the above copyright 9 | * notice, this list of conditions and the following disclaimer. 10 | * * Redistributions in binary form must reproduce the above copyright 11 | * notice, this list of conditions and the following disclaimer in the 12 | * documentation and/or other materials provided with the distribution. 13 | * * Neither the name of the Willow Garage, Inc. nor the names of its 14 | * contributors may be used to endorse or promote products derived from 15 | * this software without specific prior written permission. 16 | * 17 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 18 | * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 19 | * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 20 | * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 21 | * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 22 | * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 23 | * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 24 | * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 25 | * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 26 | * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 27 | * POSSIBILITY OF SUCH DAMAGE. 28 | */ 29 | 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | #define VENDOR 0x054c 36 | #define PRODUCT 0x0268 37 | 38 | #define USB_DIR_IN 0x80 39 | #define USB_DIR_OUT 0 40 | 41 | void fatal(char *msg) { perror(msg); exit(1); } 42 | 43 | void show_master(usb_dev_handle *devh, int itfnum) { 44 | printf("Current Bluetooth master: "); 45 | unsigned char msg[8]; 46 | int res = usb_control_msg 47 | (devh, USB_DIR_IN | USB_TYPE_CLASS | USB_RECIP_INTERFACE, 48 | 0x01, 0x03f5, itfnum, (void*)msg, sizeof(msg), 5000); 49 | if ( res < 0 ) { perror("USB_REQ_GET_CONFIGURATION"); return; } 50 | printf("%02x:%02x:%02x:%02x:%02x:%02x\n", 51 | msg[2], msg[3], msg[4], msg[5], msg[6], msg[7]); 52 | } 53 | 54 | void set_master(usb_dev_handle *devh, int itfnum, int mac[6]) { 55 | printf("Setting master bd_addr to %02x:%02x:%02x:%02x:%02x:%02x\n", 56 | mac[0], mac[1], mac[2], mac[3], mac[4], mac[5]); 57 | char msg[8]= { 0x01, 0x00, mac[0],mac[1],mac[2],mac[3],mac[4],mac[5] }; 58 | int res = usb_control_msg 59 | (devh, 60 | USB_DIR_OUT | USB_TYPE_CLASS | USB_RECIP_INTERFACE, 61 | 0x09, 62 | 0x03f5, itfnum, msg, sizeof(msg), 63 | 5000); 64 | if ( res < 0 ) fatal("USB_REQ_SET_CONFIGURATION"); 65 | } 66 | 67 | void process_device(int argc, char **argv, struct usb_device *dev, 68 | struct usb_config_descriptor *cfg, int itfnum) { 69 | int mac[6]; 70 | 71 | usb_dev_handle *devh = usb_open(dev); 72 | if ( ! devh ) fatal("usb_open"); 73 | 74 | usb_detach_kernel_driver_np(devh, itfnum); 75 | 76 | int res = usb_claim_interface(devh, itfnum); 77 | if ( res < 0 ) fatal("usb_claim_interface"); 78 | 79 | show_master(devh, itfnum); 80 | 81 | if ( argc >= 2 ) { 82 | if ( sscanf(argv[1], "%x:%x:%x:%x:%x:%x", 83 | &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) { 84 | 85 | printf("usage: %s []\n", argv[0]); 86 | exit(1); 87 | } 88 | } else { 89 | FILE *f = popen("hcitool dev", "r"); 90 | if ( !f || 91 | fscanf(f, "%*s\n%*s %x:%x:%x:%x:%x:%x", 92 | &mac[0],&mac[1],&mac[2],&mac[3],&mac[4],&mac[5]) != 6 ) { 93 | printf("Unable to retrieve local bd_addr from `hcitool dev`.\n"); 94 | printf("Please enable Bluetooth or specify an address manually.\n"); 95 | exit(1); 96 | } 97 | pclose(f); 98 | } 99 | 100 | set_master(devh, itfnum, mac); 101 | 102 | usb_close(devh); 103 | } 104 | 105 | int main(int argc, char *argv[]) { 106 | 107 | usb_init(); 108 | if ( usb_find_busses() < 0 ) fatal("usb_find_busses"); 109 | if ( usb_find_devices() < 0 ) fatal("usb_find_devices"); 110 | struct usb_bus *busses = usb_get_busses(); 111 | if ( ! busses ) fatal("usb_get_busses"); 112 | 113 | int found = 0; 114 | 115 | struct usb_bus *bus; 116 | for ( bus=busses; bus; bus=bus->next ) { 117 | struct usb_device *dev; 118 | for ( dev=bus->devices; dev; dev=dev->next) { 119 | struct usb_config_descriptor *cfg; 120 | for ( cfg = dev->config; 121 | cfg < dev->config + dev->descriptor.bNumConfigurations; 122 | ++cfg ) { 123 | int itfnum; 124 | for ( itfnum=0; itfnumbNumInterfaces; ++itfnum ) { 125 | struct usb_interface *itf = &cfg->interface[itfnum]; 126 | struct usb_interface_descriptor *alt; 127 | for ( alt = itf->altsetting; 128 | alt < itf->altsetting + itf->num_altsetting; 129 | ++alt ) { 130 | if ( dev->descriptor.idVendor == VENDOR && 131 | dev->descriptor.idProduct == PRODUCT && 132 | alt->bInterfaceClass == 3 ) { 133 | process_device(argc, argv, dev, cfg, itfnum); 134 | ++found; 135 | } 136 | } 137 | } 138 | } 139 | } 140 | } 141 | 142 | if ( ! found ) { 143 | printf("No controller found on USB busses. Please connect your joystick via USB.\n"); 144 | return 1; 145 | } 146 | 147 | return 0; 148 | 149 | } 150 | 151 | -------------------------------------------------------------------------------- /src/local_planner/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(local_planner) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(PCL REQUIRED) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | std_msgs 27 | sensor_msgs 28 | pcl_ros 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | ## Specify additional locations of header files 36 | ## Your package locations should be listed before other locations 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${PCL_INCLUDE_DIRS} 40 | "${PROJECT_SOURCE_DIR}/include" 41 | /usr/local/include # Location when using 'make system_install' 42 | /usr/include # More usual location (e.g. when installing using a package) 43 | ) 44 | 45 | ## Specify additional locations for library files 46 | link_directories( 47 | /usr/local/lib # Location when using 'make system_install' 48 | /usr/lib # More usual location (e.g. when installing using a package) 49 | ) 50 | 51 | ## Declare executables 52 | add_executable(localPlanner src/localPlanner.cpp) 53 | add_executable(pathFollower src/pathFollower.cpp) 54 | 55 | ## Specify libraries to link a library or executable target against 56 | target_link_libraries(localPlanner ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 57 | target_link_libraries(pathFollower ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS localPlanner pathFollower 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | install(DIRECTORY paths/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/paths 70 | ) 71 | -------------------------------------------------------------------------------- /src/local_planner/launch/local_planner.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | 43 | 44 | 45 | 46 | 47 | 48 | 49 | 50 | 51 | 52 | 53 | 54 | 55 | 56 | 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | 69 | 70 | 71 | 72 | 73 | 74 | 75 | 76 | 77 | 78 | 79 | 80 | 81 | 82 | 83 | 84 | 85 | 86 | 87 | 88 | 89 | 90 | 91 | 92 | -------------------------------------------------------------------------------- /src/local_planner/package.xml: -------------------------------------------------------------------------------- 1 | 2 | local_planner 3 | 0.0.1 4 | Local Planner 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/local_planner/paths/path_generator.m: -------------------------------------------------------------------------------- 1 | clc; 2 | clear all; 3 | close all; 4 | 5 | %% generate path 6 | %{. 7 | dis = 1.0; 8 | angle = 27; 9 | deltaAngle = angle / 3; 10 | scale = 0.65; 11 | 12 | pathStartAll = zeros(4, 0); 13 | pathAll = zeros(5, 0); 14 | pathList = zeros(5, 0); 15 | pathID = 0; 16 | groupID = 0; 17 | 18 | figure; 19 | hold on; 20 | box on; 21 | axis equal; 22 | xlabel('X (m)'); 23 | ylabel('Y (m)'); 24 | 25 | fprintf('\nGenerating paths\n'); 26 | 27 | for shift1 = -angle : deltaAngle : angle 28 | wayptsStart = [0, 0, 0; 29 | dis, shift1, 0]; 30 | 31 | pathStartR = 0 : 0.01 : dis; 32 | pathStartShift = spline(wayptsStart(:, 1), wayptsStart(:, 2), pathStartR); 33 | 34 | pathStartX = pathStartR .* cos(pathStartShift * pi / 180); 35 | pathStartY = pathStartR .* sin(pathStartShift * pi / 180); 36 | pathStartZ = zeros(size(pathStartX)); 37 | 38 | pathStart = [pathStartX; pathStartY; pathStartZ; ones(size(pathStartX)) * groupID]; 39 | pathStartAll = [pathStartAll, pathStart]; 40 | 41 | for shift2 = -angle * scale + shift1 : deltaAngle * scale : angle * scale + shift1 42 | for shift3 = -angle * scale^2 + shift2 : deltaAngle * scale^2 : angle * scale^2 + shift2 43 | waypts = [pathStartR', pathStartShift', pathStartZ'; 44 | 2 * dis, shift2, 0; 45 | 3 * dis - 0.001, shift3, 0; 46 | 3 * dis, shift3, 0]; 47 | 48 | pathR = 0 : 0.01 : waypts(end, 1); 49 | pathShift = spline(waypts(:, 1), waypts(:, 2), pathR); 50 | 51 | pathX = pathR .* cos(pathShift * pi / 180); 52 | pathY = pathR .* sin(pathShift * pi / 180); 53 | pathZ = zeros(size(pathX)); 54 | 55 | path = [pathX; pathY; pathZ; ones(size(pathX)) * pathID; ones(size(pathX)) * groupID]; 56 | pathAll = [pathAll, path]; 57 | pathList = [pathList, [pathX(end); pathY(end); pathZ(end); pathID; groupID]]; 58 | 59 | pathID = pathID + 1; 60 | 61 | plot3(pathX, pathY, pathZ); 62 | end 63 | end 64 | 65 | groupID = groupID + 1 66 | end 67 | 68 | pathID 69 | 70 | fileID = fopen('startPaths.ply', 'w'); 71 | fprintf(fileID, 'ply\n'); 72 | fprintf(fileID, 'format ascii 1.0\n'); 73 | fprintf(fileID, 'element vertex %d\n', size(pathStartAll, 2)); 74 | fprintf(fileID, 'property float x\n'); 75 | fprintf(fileID, 'property float y\n'); 76 | fprintf(fileID, 'property float z\n'); 77 | fprintf(fileID, 'property int group_id\n'); 78 | fprintf(fileID, 'end_header\n'); 79 | fprintf(fileID, '%f %f %f %d\n', pathStartAll); 80 | fclose(fileID); 81 | 82 | fileID = fopen('paths.ply', 'w'); 83 | fprintf(fileID, 'ply\n'); 84 | fprintf(fileID, 'format ascii 1.0\n'); 85 | fprintf(fileID, 'element vertex %d\n', size(pathAll, 2)); 86 | fprintf(fileID, 'property float x\n'); 87 | fprintf(fileID, 'property float y\n'); 88 | fprintf(fileID, 'property float z\n'); 89 | fprintf(fileID, 'property int path_id\n'); 90 | fprintf(fileID, 'property int group_id\n'); 91 | fprintf(fileID, 'end_header\n'); 92 | fprintf(fileID, '%f %f %f %d %d\n', pathAll); 93 | fclose(fileID); 94 | 95 | fileID = fopen('pathList.ply', 'w'); 96 | fprintf(fileID, 'ply\n'); 97 | fprintf(fileID, 'format ascii 1.0\n'); 98 | fprintf(fileID, 'element vertex %d\n', size(pathList, 2)); 99 | fprintf(fileID, 'property float end_x\n'); 100 | fprintf(fileID, 'property float end_y\n'); 101 | fprintf(fileID, 'property float end_z\n'); 102 | fprintf(fileID, 'property int path_id\n'); 103 | fprintf(fileID, 'property int group_id\n'); 104 | fprintf(fileID, 'end_header\n'); 105 | fprintf(fileID, '%f %f %f %d %d\n', pathList); 106 | fclose(fileID); 107 | 108 | pause(1.0); 109 | %} 110 | 111 | %% find correspondence 112 | %{. 113 | voxelSize = 0.02; 114 | searchRadius = 0.45; 115 | offsetX = 3.2; 116 | offsetY = 4.5; 117 | voxelNumX = 161; 118 | voxelNumY = 451; 119 | 120 | fprintf('\nPreparing voxels\n'); 121 | 122 | indPoint = 1; 123 | voxelPointNum = voxelNumX * voxelNumY; 124 | voxelPoints = zeros(voxelPointNum, 2); 125 | for indX = 0 : voxelNumX - 1 126 | x = offsetX - voxelSize * indX; 127 | scaleY = x / offsetX + searchRadius / offsetY * (offsetX - x) / offsetX; 128 | for indY = 0 : voxelNumY - 1 129 | y = scaleY * (offsetY - voxelSize * indY); 130 | 131 | voxelPoints(indPoint, 1) = x; 132 | voxelPoints(indPoint, 2) = y; 133 | 134 | indPoint = indPoint + 1; 135 | end 136 | end 137 | 138 | plot3(voxelPoints(:, 1), voxelPoints(:, 2), zeros(voxelPointNum, 1), 'k.'); 139 | pause(1.0); 140 | 141 | fprintf('\nCollision checking\n'); 142 | 143 | [ind, dis] = rangesearch(pathAll(1 : 2, :)', voxelPoints, searchRadius); 144 | 145 | fprintf('\nSaving correspondences\n'); 146 | 147 | fileID = fopen('correspondences.txt', 'w'); 148 | 149 | for i = 1 : voxelPointNum 150 | fprintf(fileID, '%d ', i - 1); 151 | 152 | indVoxel = sort(ind{i}); 153 | indVoxelNum = size(indVoxel, 2); 154 | 155 | pathIndRec = -1; 156 | for j = 1 : indVoxelNum 157 | pathInd = pathAll(4, indVoxel(j)); 158 | if pathInd == pathIndRec 159 | continue; 160 | end 161 | 162 | fprintf(fileID, '%d ', pathInd); 163 | pathIndRec = pathInd; 164 | end 165 | fprintf(fileID, '-1\n'); 166 | 167 | if mod(i, 1000) == 0 168 | i 169 | end 170 | end 171 | 172 | fclose(fileID); 173 | 174 | fprintf('\nProcessing complete\n'); 175 | %} 176 | -------------------------------------------------------------------------------- /src/score_calculation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(score_calculation) 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(scoreCalculation src/scoreCalculation.cpp) 53 | 54 | ## Specify libraries to link a library or executable target against 55 | target_link_libraries(scoreCalculation ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 56 | 57 | install(TARGETS scoreCalculation 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/score_calculation/launch/score_calculation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/score_calculation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | score_calculation 3 | 0.0.1 4 | Score Calculation 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/score_calculation/src/scoreCalculation.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | const double PI = 3.1415926; 17 | 18 | string ref_traj_dir; 19 | string actual_traj_dir; 20 | double penaltyScale = 0.01; 21 | 22 | pcl::PointCloud::Ptr refTraj(new pcl::PointCloud()); 23 | 24 | int main(int argc, char** argv) 25 | { 26 | ros::init(argc, argv, "scoreCalculation"); 27 | ros::NodeHandle nh; 28 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 29 | 30 | nhPrivate.getParam("ref_traj_dir", ref_traj_dir); 31 | nhPrivate.getParam("actual_traj_dir", actual_traj_dir); 32 | nhPrivate.getParam("penaltyScale", penaltyScale); 33 | 34 | pcl::PLYReader ply_reader; 35 | if (ply_reader.read(ref_traj_dir, *refTraj) == -1) { 36 | printf("\nCannot read reference trajectory file, exit.\n\n"); 37 | exit(1); 38 | } 39 | 40 | FILE *actual_traj_file = fopen(actual_traj_dir.c_str(), "r"); 41 | if (actual_traj_file == NULL) { 42 | printf ("\nCannot read actual trajectory file, exit.\n\n"); 43 | exit(1); 44 | } 45 | 46 | 47 | float penalty = 0; 48 | int actualTrajSize = 0; 49 | int refTrajSize = refTraj->size(); 50 | int val1, val2, val3, val4, val5, val6, val7; 51 | float x, y, z, roll, pitch, yaw, time, timeLast = 0; 52 | while (1) { 53 | val1 = fscanf(actual_traj_file, "%f", &x); 54 | val2 = fscanf(actual_traj_file, "%f", &y); 55 | val3 = fscanf(actual_traj_file, "%f", &z); 56 | val4 = fscanf(actual_traj_file, "%f", &roll); 57 | val5 = fscanf(actual_traj_file, "%f", &pitch); 58 | val6 = fscanf(actual_traj_file, "%f", &yaw); 59 | val7 = fscanf(actual_traj_file, "%f", &time); 60 | 61 | if (val1 != 1 || val2 != 1 || val3 != 1 || val4 != 1 || val5 != 1 || val6 != 1 || val7 != 1) { 62 | break; 63 | } 64 | 65 | float minDelta = 1000000.0; 66 | for (int i = 0; i < refTrajSize; i++) { 67 | float deltaX = refTraj->points[i].x - x; 68 | float deltaY = refTraj->points[i].y - y; 69 | float delta = sqrt(deltaX * deltaX + deltaY * deltaY); 70 | 71 | if (minDelta > delta) minDelta = delta; 72 | } 73 | 74 | penalty += penaltyScale * minDelta * (time - timeLast); 75 | timeLast = time; 76 | actualTrajSize++; 77 | } 78 | 79 | printf ("\nRef traj points: %d, Actual traj points: %d, Penalty: %f.\n", refTrajSize, actualTrajSize, penalty); 80 | printf ("Pleae type in points.\n"); 81 | 82 | int points = 0; 83 | scanf ("%d", &points); 84 | 85 | printf ("Score: %f.\n\n", points - penalty); 86 | 87 | return 0; 88 | } 89 | -------------------------------------------------------------------------------- /src/semantic_scan_generation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(semantic_scan_generation) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | 8 | ## Find catkin macros and libraries 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | cv_bridge 14 | pcl_ros 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | find_package(Eigen3 REQUIRED) 20 | 21 | ################################### 22 | ## catkin specific configuration ## 23 | ################################### 24 | 25 | catkin_package( 26 | CATKIN_DEPENDS 27 | roscpp 28 | std_msgs 29 | sensor_msgs 30 | cv_bridge 31 | pcl_ros 32 | ) 33 | 34 | ########### 35 | ## Build ## 36 | ########### 37 | 38 | ## Specify additional locations of header files 39 | ## Your package locations should be listed before other locations 40 | include_directories( 41 | ${catkin_INCLUDE_DIRS} 42 | ${OpenCV_INCLUDE_DIRS} 43 | ${PCL_INCLUDE_DIRS} 44 | ${EIGEN3_INCLUDE_DIR} 45 | "${PROJECT_SOURCE_DIR}/include" 46 | /usr/local/include # Location when using 'make system_install' 47 | /usr/include # More usual location (e.g. when installing using a package) 48 | ) 49 | 50 | ## Specify additional locations for library files 51 | link_directories( 52 | /usr/local/lib # Location when using 'make system_install' 53 | /usr/lib # More usual location (e.g. when installing using a package) 54 | ) 55 | 56 | ## Declare executables 57 | add_executable(semanticScanGeneration src/semanticScanGeneration.cpp) 58 | 59 | ## Specify libraries to link a library or executable target against 60 | target_link_libraries(semanticScanGeneration ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 61 | 62 | install(TARGETS semanticScanGeneration 63 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 64 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 65 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 66 | ) 67 | 68 | install(DIRECTORY launch/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 70 | ) 71 | -------------------------------------------------------------------------------- /src/semantic_scan_generation/launch/semantic_scan_generation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/semantic_scan_generation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | semantic_scan_generation 3 | 0.0.1 4 | Semantic Scan Generation 5 | Ji Zhang 6 | BSD 7 | 8 | Ji Zhang 9 | 10 | catkin 11 | 12 | roscpp 13 | std_msgs 14 | sensor_msgs 15 | cv_bridge 16 | pcl_ros 17 | 18 | roscpp 19 | std_msgs 20 | sensor_msgs 21 | cv_bridge 22 | pcl_ros 23 | 24 | -------------------------------------------------------------------------------- /src/semantic_scan_generation/src/semanticScanGeneration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | #include 12 | #include 13 | 14 | #include 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | #include 23 | #include 24 | #include 25 | 26 | using namespace std; 27 | using namespace cv; 28 | 29 | const double PI = 3.1415926; 30 | 31 | double cameraOffsetZ = 0; 32 | 33 | pcl::PointCloud::Ptr laserCloud(new pcl::PointCloud()); 34 | pcl::PointCloud::Ptr laserCloudSeg(new pcl::PointCloud()); 35 | 36 | const int stackNum = 400; 37 | float lidarXStack[stackNum]; 38 | float lidarYStack[stackNum]; 39 | float lidarZStack[stackNum]; 40 | float lidarRollStack[stackNum]; 41 | float lidarPitchStack[stackNum]; 42 | float lidarYawStack[stackNum]; 43 | double odomTimeStack[stackNum]; 44 | int odomIDPointer = -1; 45 | int imageIDPointer = 0; 46 | 47 | bool imageInit = false; 48 | double imageTime = 0; 49 | 50 | bool newLaserCloud = false; 51 | double laserCloudTime = 0; 52 | 53 | ros::Publisher *pubLaserCloudPointer = NULL; 54 | cv_bridge::CvImageConstPtr segImageCv; 55 | 56 | void odomHandler(const nav_msgs::Odometry::ConstPtr& odom) 57 | { 58 | double roll, pitch, yaw; 59 | geometry_msgs::Quaternion geoQuat = odom->pose.pose.orientation; 60 | tf::Matrix3x3(tf::Quaternion(geoQuat.x, geoQuat.y, geoQuat.z, geoQuat.w)).getRPY(roll, pitch, yaw); 61 | 62 | odomIDPointer = (odomIDPointer + 1) % stackNum; 63 | odomTimeStack[odomIDPointer] = odom->header.stamp.toSec(); 64 | lidarXStack[odomIDPointer] = odom->pose.pose.position.x; 65 | lidarYStack[odomIDPointer] = odom->pose.pose.position.y; 66 | lidarZStack[odomIDPointer] = odom->pose.pose.position.z; 67 | lidarRollStack[odomIDPointer] = roll; 68 | lidarPitchStack[odomIDPointer] = pitch; 69 | lidarYawStack[odomIDPointer] = yaw; 70 | } 71 | 72 | void semImageHandler(const sensor_msgs::ImageConstPtr& image) 73 | { 74 | imageTime = image->header.stamp.toSec(); 75 | segImageCv = cv_bridge::toCvShare(image, "bgr8"); 76 | 77 | imageInit = true; 78 | } 79 | 80 | void laserCloudHandler(const sensor_msgs::PointCloud2ConstPtr& laserCloudIn) 81 | { 82 | laserCloudTime = laserCloudIn->header.stamp.toSec(); 83 | 84 | laserCloud->clear(); 85 | pcl::fromROSMsg(*laserCloudIn, *laserCloud); 86 | 87 | newLaserCloud = true; 88 | } 89 | 90 | int main(int argc, char** argv) 91 | { 92 | ros::init(argc, argv, "semanticScanGeneration"); 93 | ros::NodeHandle nh; 94 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 95 | 96 | nhPrivate.getParam("cameraOffsetZ", cameraOffsetZ); 97 | 98 | ros::Subscriber subOdom = nh.subscribe ("/state_estimation", 50, odomHandler); 99 | 100 | ros::Subscriber subSegImage = nh.subscribe ("/camera/semantic_image", 2, semImageHandler); 101 | 102 | ros::Subscriber subLaserCloud = nh.subscribe ("/registered_scan", 2, laserCloudHandler); 103 | 104 | ros::Publisher pubLaserCloud = nh.advertise ("/semantic_scan", 2); 105 | pubLaserCloudPointer = &pubLaserCloud; 106 | 107 | ros::Rate rate(200); 108 | bool status = ros::ok(); 109 | while (status) 110 | { 111 | ros::spinOnce(); 112 | 113 | if (imageInit && newLaserCloud) { 114 | newLaserCloud = false; 115 | 116 | int laserCloudSize = laserCloud->points.size(); 117 | if (laserCloudSize <= 0) continue; 118 | 119 | if (odomIDPointer < 0) continue; 120 | while (odomTimeStack[imageIDPointer] < imageTime - 0.001 && 121 | imageIDPointer != (odomIDPointer + 1) % stackNum) { 122 | imageIDPointer = (imageIDPointer + 1) % stackNum; 123 | } 124 | if (fabs(odomTimeStack[imageIDPointer] - imageTime) > 0.001) continue; 125 | 126 | float lidarX = lidarXStack[imageIDPointer]; 127 | float lidarY = lidarYStack[imageIDPointer]; 128 | float lidarZ = lidarZStack[imageIDPointer]; 129 | float lidarRoll = lidarRollStack[imageIDPointer]; 130 | float lidarPitch = lidarPitchStack[imageIDPointer]; 131 | float lidarYaw = lidarYawStack[imageIDPointer]; 132 | 133 | int imageWidth = segImageCv->image.size().width; 134 | int imageHeight = segImageCv->image.size().height; 135 | 136 | float sinLidarRoll = sin(lidarRoll); 137 | float cosLidarRoll = cos(lidarRoll); 138 | float sinLidarPitch = sin(lidarPitch); 139 | float cosLidarPitch = cos(lidarPitch); 140 | float sinLidarYaw = sin(lidarYaw); 141 | float cosLidarYaw = cos(lidarYaw); 142 | 143 | pcl::PointXYZRGB point; 144 | laserCloudSeg->clear(); 145 | for (int i = 0; i < laserCloudSize; i++) { 146 | float x1 = laserCloud->points[i].x - lidarX; 147 | float y1 = laserCloud->points[i].y - lidarY; 148 | float z1 = laserCloud->points[i].z - lidarZ; 149 | 150 | float x2 = x1 * cosLidarYaw + y1 * sinLidarYaw; 151 | float y2 = -x1 * sinLidarYaw + y1 * cosLidarYaw; 152 | float z2 = z1; 153 | 154 | float x3 = x2 * cosLidarPitch - z2 * sinLidarPitch; 155 | float y3 = y2; 156 | float z3 = x2 * sinLidarPitch + z2 * cosLidarPitch; 157 | 158 | float x4 = x3; 159 | float y4 = y3 * cosLidarRoll + z3 * sinLidarRoll; 160 | float z4 = -y3 * sinLidarRoll + z3 * cosLidarRoll - cameraOffsetZ; 161 | 162 | float horiDis = sqrt(x4 * x4 + y4 * y4); 163 | int horiPixelID = -imageWidth / (2 * PI) * atan2(y4, x4) + imageWidth / 2 + 1; 164 | int vertPixelID = -imageWidth / (2 * PI) * atan(z4 / horiDis) + imageHeight / 2 + 1; 165 | int pixelID = imageWidth * vertPixelID + horiPixelID; 166 | 167 | if (horiPixelID >= 0 && horiPixelID < imageWidth && vertPixelID >= 0 && vertPixelID < imageHeight) { 168 | point.x = laserCloud->points[i].x; 169 | point.y = laserCloud->points[i].y; 170 | point.z = laserCloud->points[i].z; 171 | point.b = segImageCv->image.data[3 * pixelID]; 172 | point.g = segImageCv->image.data[3 * pixelID + 1]; 173 | point.r = segImageCv->image.data[3 * pixelID + 2]; 174 | 175 | laserCloudSeg->push_back(point); 176 | } 177 | } 178 | 179 | sensor_msgs::PointCloud2 laserCloudOut; 180 | pcl::toROSMsg(*laserCloudSeg, laserCloudOut); 181 | laserCloudOut.header.stamp = ros::Time().fromSec(laserCloudTime); 182 | laserCloudOut.header.frame_id = "map"; 183 | pubLaserCloudPointer->publish(laserCloudOut); 184 | } 185 | 186 | status = ros::ok(); 187 | rate.sleep(); 188 | } 189 | 190 | return 0; 191 | } 192 | -------------------------------------------------------------------------------- /src/sensor_scan_generation/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sensor_scan_generation) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | 8 | ## Find catkin macros and libraries 9 | find_package(catkin REQUIRED COMPONENTS 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | pcl_ros 14 | ) 15 | 16 | find_package(PCL REQUIRED) 17 | find_package(Eigen3 REQUIRED) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | std_msgs 27 | sensor_msgs 28 | pcl_ros 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | ## Specify additional locations of header files 36 | ## Your package locations should be listed before other locations 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${PCL_INCLUDE_DIRS} 40 | ${EIGEN3_INCLUDE_DIR} 41 | "${PROJECT_SOURCE_DIR}/include" 42 | /usr/local/include # Location when using 'make system_install' 43 | /usr/include # More usual location (e.g. when installing using a package) 44 | ) 45 | 46 | ## Specify additional locations for library files 47 | link_directories( 48 | /usr/local/lib # Location when using 'make system_install' 49 | /usr/lib # More usual location (e.g. when installing using a package) 50 | ) 51 | 52 | 53 | ## Declare executables 54 | add_executable(sensorScanGeneration src/sensorScanGeneration.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(sensorScanGeneration ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS sensorScanGeneration 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | -------------------------------------------------------------------------------- /src/sensor_scan_generation/launch/sensor_scan_generation.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | -------------------------------------------------------------------------------- /src/sensor_scan_generation/package.xml: -------------------------------------------------------------------------------- 1 | 2 | sensor_scan_generation 3 | 0.0.1 4 | sensor_scan_generation is a package to generate scan data in sensor frame with registered_scan in map frame. 5 | Hongbiao Zhu 6 | BSD 7 | 8 | Hongbiao Zhu 9 | 10 | catkin 11 | 12 | roscpp 13 | std_msgs 14 | sensor_msgs 15 | pcl_ros 16 | 17 | roscpp 18 | std_msgs 19 | sensor_msgs 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/sensor_scan_generation/src/sensorScanGeneration.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | #include 12 | 13 | #include 14 | #include 15 | #include 16 | 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | 23 | pcl::PointCloud::Ptr laserCloudIn(new pcl::PointCloud()); 24 | pcl::PointCloud::Ptr laserCLoudInSensorFrame(new pcl::PointCloud()); 25 | 26 | double robotX = 0; 27 | double robotY = 0; 28 | double robotZ = 0; 29 | double roll = 0; 30 | double pitch = 0; 31 | double yaw = 0; 32 | 33 | bool newTransformToMap = false; 34 | 35 | nav_msgs::Odometry odometryIn; 36 | ros::Publisher *pubOdometryPointer = NULL; 37 | tf::StampedTransform transformToMap; 38 | tf::TransformBroadcaster *tfBroadcasterPointer = NULL; 39 | 40 | ros::Publisher pubLaserCloud; 41 | 42 | void laserCloudAndOdometryHandler(const nav_msgs::Odometry::ConstPtr& odometry, 43 | const sensor_msgs::PointCloud2ConstPtr& laserCloud2) 44 | { 45 | laserCloudIn->clear(); 46 | laserCLoudInSensorFrame->clear(); 47 | 48 | pcl::fromROSMsg(*laserCloud2, *laserCloudIn); 49 | 50 | odometryIn = *odometry; 51 | 52 | transformToMap.setOrigin( 53 | tf::Vector3(odometryIn.pose.pose.position.x, odometryIn.pose.pose.position.y, odometryIn.pose.pose.position.z)); 54 | transformToMap.setRotation(tf::Quaternion(odometryIn.pose.pose.orientation.x, odometryIn.pose.pose.orientation.y, 55 | odometryIn.pose.pose.orientation.z, odometryIn.pose.pose.orientation.w)); 56 | 57 | int laserCloudInNum = laserCloudIn->points.size(); 58 | 59 | pcl::PointXYZ p1; 60 | tf::Vector3 vec; 61 | 62 | for (int i = 0; i < laserCloudInNum; i++) 63 | { 64 | p1 = laserCloudIn->points[i]; 65 | vec.setX(p1.x); 66 | vec.setY(p1.y); 67 | vec.setZ(p1.z); 68 | 69 | vec = transformToMap.inverse() * vec; 70 | 71 | p1.x = vec.x(); 72 | p1.y = vec.y(); 73 | p1.z = vec.z(); 74 | 75 | laserCLoudInSensorFrame->points.push_back(p1); 76 | } 77 | 78 | odometryIn.header.stamp = laserCloud2->header.stamp; 79 | odometryIn.header.frame_id = "map"; 80 | odometryIn.child_frame_id = "sensor_at_scan"; 81 | pubOdometryPointer->publish(odometryIn); 82 | 83 | transformToMap.stamp_ = laserCloud2->header.stamp; 84 | transformToMap.frame_id_ = "map"; 85 | transformToMap.child_frame_id_ = "sensor_at_scan"; 86 | tfBroadcasterPointer->sendTransform(transformToMap); 87 | 88 | sensor_msgs::PointCloud2 scan_data; 89 | pcl::toROSMsg(*laserCLoudInSensorFrame, scan_data); 90 | scan_data.header.stamp = laserCloud2->header.stamp; 91 | scan_data.header.frame_id = "sensor_at_scan"; 92 | pubLaserCloud.publish(scan_data); 93 | } 94 | 95 | int main(int argc, char** argv) 96 | { 97 | ros::init(argc, argv, "sensor_scan"); 98 | ros::NodeHandle nh; 99 | ros::NodeHandle nhPrivate = ros::NodeHandle("~"); 100 | 101 | // ROS message filters 102 | message_filters::Subscriber subOdometry; 103 | message_filters::Subscriber subLaserCloud; 104 | typedef message_filters::sync_policies::ApproximateTime syncPolicy; 105 | typedef message_filters::Synchronizer Sync; 106 | boost::shared_ptr sync_; 107 | subOdometry.subscribe(nh, "/state_estimation", 1); 108 | subLaserCloud.subscribe(nh, "/registered_scan", 1); 109 | sync_.reset(new Sync(syncPolicy(100), subOdometry, subLaserCloud)); 110 | sync_->registerCallback(boost::bind(laserCloudAndOdometryHandler, _1, _2)); 111 | 112 | ros::Publisher pubOdometry = nh.advertise ("/state_estimation_at_scan", 5); 113 | pubOdometryPointer = &pubOdometry; 114 | 115 | tf::TransformBroadcaster tfBroadcaster; 116 | tfBroadcasterPointer = &tfBroadcaster; 117 | 118 | pubLaserCloud = nh.advertise("/sensor_scan", 2); 119 | 120 | ros::spin(); 121 | 122 | return 0; 123 | } 124 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.0.2) 2 | project(teleop_rviz_plugin) 3 | find_package(catkin REQUIRED COMPONENTS rviz) 4 | catkin_package() 5 | include_directories(${catkin_INCLUDE_DIRS}) 6 | link_directories(${catkin_LIBRARY_DIRS}) 7 | 8 | set(CMAKE_AUTOMOC ON) 9 | 10 | if(rviz_QT_VERSION VERSION_LESS "5") 11 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 12 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 13 | include(${QT_USE_FILE}) 14 | else() 15 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 16 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 17 | set(QT_LIBRARIES Qt5::Widgets) 18 | endif() 19 | 20 | add_definitions(-DQT_NO_KEYWORDS) 21 | 22 | set(SRC_FILES 23 | src/drive_widget.cpp 24 | src/teleop_panel.cpp 25 | ) 26 | 27 | add_library(${PROJECT_NAME} ${SRC_FILES}) 28 | 29 | target_link_libraries(${PROJECT_NAME} ${QT_LIBRARIES} ${catkin_LIBRARIES}) 30 | 31 | install(TARGETS 32 | ${PROJECT_NAME} 33 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 34 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 35 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 36 | ) 37 | 38 | install(FILES 39 | plugin_description.xml 40 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) 41 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | teleop_rviz_plugin 3 | 0.0.1 4 | 5 | Teleop RVIZ Plugin 6 | 7 | Ji Zhang 8 | BSD 9 | 10 | catkin 11 | 12 | qtbase5-dev 13 | rviz 14 | 15 | libqt5-core 16 | libqt5-gui 17 | libqt5-widgets 18 | rviz 19 | 20 | 21 | 22 | 23 | 24 | 25 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A panel for driving diff-drive style robot base control. 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/drive_widget.cpp: -------------------------------------------------------------------------------- 1 | #include "drive_widget.h" 2 | 3 | namespace teleop_rviz_plugin 4 | { 5 | 6 | DriveWidget::DriveWidget( QWidget* parent ) 7 | : QWidget( parent ) 8 | , linear_velocity_( 0 ) 9 | , angular_velocity_( 0 ) 10 | , linear_scale_( 1 ) 11 | , angular_scale_( 1 ) 12 | , x_mouse_( 0 ) 13 | , y_mouse_( 0 ) 14 | , mouse_pressed_(false) 15 | { 16 | } 17 | 18 | void DriveWidget::paintEvent( QPaintEvent* event ) 19 | { 20 | QColor background; 21 | QColor crosshair; 22 | if( isEnabled() ) 23 | { 24 | background = Qt::white; 25 | crosshair = Qt::black; 26 | } 27 | else 28 | { 29 | background = Qt::lightGray; 30 | crosshair = Qt::darkGray; 31 | } 32 | 33 | int w = width(); 34 | int h = height(); 35 | int size = (( w > h ) ? h : w) - 1; 36 | int hpad = ( w - size ) / 2; 37 | int vpad = ( h - size ) / 2; 38 | 39 | QPainter painter( this ); 40 | painter.setBrush( background ); 41 | painter.setPen( crosshair ); 42 | 43 | painter.drawRect( QRect( hpad, vpad, size, size )); 44 | 45 | painter.drawLine( hpad, height() / 2, hpad + size, height() / 2 ); 46 | painter.drawLine( width() / 2, vpad, width() / 2, vpad + size ); 47 | 48 | if( isEnabled() && (angular_velocity_ != 0 || linear_velocity_ != 0 )) 49 | { 50 | QPen pen; 51 | pen.setWidth( size/150 ); 52 | pen.setColor( Qt::darkRed ); 53 | pen.setCapStyle( Qt::RoundCap ); 54 | pen.setJoinStyle( Qt::RoundJoin ); 55 | painter.setPen( pen ); 56 | 57 | QPointF joystick[ 2 ]; 58 | joystick[ 0 ].setX( w/2 ); 59 | joystick[ 0 ].setY( h/2 ); 60 | joystick[ 1 ].setX( x_mouse_ ); 61 | joystick[ 1 ].setY( y_mouse_ ); 62 | 63 | painter.drawPolyline( joystick, 2 ); 64 | painter.drawEllipse( x_mouse_ - 10, y_mouse_ - 10, 20, 20 ); 65 | } 66 | } 67 | 68 | void DriveWidget::mouseMoveEvent( QMouseEvent* event ) 69 | { 70 | sendVelocitiesFromMouse( event->x(), event->y(), width(), height() ); 71 | } 72 | 73 | void DriveWidget::mousePressEvent( QMouseEvent* event ) 74 | { 75 | sendVelocitiesFromMouse( event->x(), event->y(), width(), height() ); 76 | } 77 | 78 | void DriveWidget::leaveEvent( QEvent* event ) 79 | { 80 | stop(); 81 | } 82 | 83 | void DriveWidget::mouseReleaseEvent( QMouseEvent* event ) 84 | { 85 | stop(); 86 | } 87 | 88 | void DriveWidget::sendVelocitiesFromMouse( int x, int y, int width, int height ) 89 | { 90 | int size = (( width > height ) ? height : width ); 91 | int hpad = ( width - size ) / 2; 92 | int vpad = ( height - size ) / 2; 93 | 94 | x_mouse_ = x; 95 | if ( x_mouse_ < width / 2 - size / 2 ) x_mouse_ = width / 2 - size / 2; 96 | else if ( x_mouse_ > width / 2 + size / 2 ) x_mouse_ = width / 2 + size / 2; 97 | y_mouse_ = y; 98 | if ( y_mouse_ < height / 2 - size / 2 ) y_mouse_ = height / 2 - size / 2; 99 | else if ( y_mouse_ > height / 2 + size / 2 ) y_mouse_ = height / 2 + size / 2; 100 | 101 | linear_velocity_ = (1.0 - float( y - vpad ) / float( size / 2 )) * linear_scale_; 102 | if ( linear_velocity_ < -1.0 ) linear_velocity_ = -1.0; 103 | else if ( linear_velocity_ > 1.0 ) linear_velocity_ = 1.0; 104 | if ( fabs( linear_velocity_ ) < 0.1 ) linear_velocity_ = 0; 105 | angular_velocity_ = ( 1.0 - float( x - hpad ) / float( size / 2 )) * angular_scale_; 106 | if ( angular_velocity_ < -1.0 ) angular_velocity_ = -1.0; 107 | else if ( angular_velocity_ > 1.0 ) angular_velocity_ = 1.0; 108 | 109 | mouse_pressed_ = true; 110 | 111 | Q_EMIT outputVelocity( linear_velocity_, angular_velocity_, mouse_pressed_ ); 112 | update(); 113 | } 114 | 115 | void DriveWidget::stop() 116 | { 117 | linear_velocity_ = 0; 118 | angular_velocity_ = 0; 119 | mouse_pressed_ = false; 120 | 121 | Q_EMIT outputVelocity( linear_velocity_, angular_velocity_, mouse_pressed_ ); 122 | update(); 123 | } 124 | 125 | } 126 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/drive_widget.h: -------------------------------------------------------------------------------- 1 | #ifndef DRIVE_WIDGET_H 2 | #define DRIVE_WIDGET_H 3 | 4 | #include 5 | #include 6 | 7 | #include 8 | #include 9 | 10 | #include 11 | 12 | namespace teleop_rviz_plugin 13 | { 14 | 15 | class DriveWidget: public QWidget 16 | { 17 | Q_OBJECT 18 | public: 19 | DriveWidget( QWidget* parent = 0 ); 20 | virtual void paintEvent( QPaintEvent* event ); 21 | virtual void mouseMoveEvent( QMouseEvent* event ); 22 | virtual void mousePressEvent( QMouseEvent* event ); 23 | virtual void mouseReleaseEvent( QMouseEvent* event ); 24 | virtual void leaveEvent( QEvent* event ); 25 | virtual QSize sizeHint() const { return QSize( 150, 150 ); } 26 | 27 | Q_SIGNALS: 28 | void outputVelocity( float linear, float angular, bool pressed ); 29 | 30 | protected: 31 | void sendVelocitiesFromMouse( int x, int y, int width, int height ); 32 | void stop(); 33 | 34 | float linear_velocity_; 35 | float angular_velocity_; 36 | float linear_scale_; 37 | float angular_scale_; 38 | float x_mouse_, y_mouse_; 39 | bool mouse_pressed_; 40 | }; 41 | 42 | } 43 | 44 | #endif // DRIVE_WIDGET_H 45 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/teleop_panel.cpp: -------------------------------------------------------------------------------- 1 | #include "drive_widget.h" 2 | #include "teleop_panel.h" 3 | 4 | namespace teleop_rviz_plugin 5 | { 6 | 7 | TeleopPanel::TeleopPanel( QWidget* parent ) 8 | : rviz::Panel( parent ) 9 | , linear_velocity_( 0 ) 10 | , angular_velocity_( 0 ) 11 | , mouse_pressed_( false ) 12 | , mouse_pressed_sent_( false ) 13 | { 14 | QVBoxLayout* layout = new QVBoxLayout; 15 | push_button_1_ = new QPushButton( "Resume Navigation to Goal", this ); 16 | layout->addWidget( push_button_1_ ); 17 | drive_widget_ = new DriveWidget; 18 | layout->addWidget( drive_widget_ ); 19 | setLayout( layout ); 20 | 21 | QTimer* output_timer = new QTimer( this ); 22 | 23 | connect( push_button_1_, SIGNAL( pressed() ), this, SLOT( pressButton1() )); 24 | connect( drive_widget_, SIGNAL( outputVelocity( float, float, bool )), this, SLOT( setVel( float, float, bool ))); 25 | connect( output_timer, SIGNAL( timeout() ), this, SLOT( sendVel() )); 26 | 27 | output_timer->start( 100 ); 28 | 29 | velocity_publisher_ = nh_.advertise( "/joy", 5 ); 30 | drive_widget_->setEnabled( true ); 31 | } 32 | 33 | void TeleopPanel::pressButton1() 34 | { 35 | if ( ros::ok() && velocity_publisher_ ) 36 | { 37 | sensor_msgs::Joy joy; 38 | 39 | joy.axes.push_back(0); 40 | joy.axes.push_back(0); 41 | joy.axes.push_back(-1.0); 42 | joy.axes.push_back(0); 43 | joy.axes.push_back(0); 44 | joy.axes.push_back(1.0); 45 | joy.axes.push_back(0); 46 | joy.axes.push_back(0); 47 | 48 | joy.buttons.push_back(0); 49 | joy.buttons.push_back(0); 50 | joy.buttons.push_back(0); 51 | joy.buttons.push_back(0); 52 | joy.buttons.push_back(0); 53 | joy.buttons.push_back(0); 54 | joy.buttons.push_back(0); 55 | joy.buttons.push_back(1); 56 | joy.buttons.push_back(0); 57 | joy.buttons.push_back(0); 58 | joy.buttons.push_back(0); 59 | 60 | joy.header.stamp = ros::Time::now(); 61 | joy.header.frame_id = "teleop_panel"; 62 | velocity_publisher_.publish( joy ); 63 | } 64 | } 65 | 66 | void TeleopPanel::setVel( float lin, float ang, bool pre ) 67 | { 68 | linear_velocity_ = lin; 69 | angular_velocity_ = ang; 70 | mouse_pressed_ = pre; 71 | } 72 | 73 | void TeleopPanel::sendVel() 74 | { 75 | if( ros::ok() && velocity_publisher_ && ( mouse_pressed_ || mouse_pressed_sent_ )) 76 | { 77 | sensor_msgs::Joy joy; 78 | 79 | joy.axes.push_back( 0 ); 80 | joy.axes.push_back( 0 ); 81 | joy.axes.push_back( 1.0 ); 82 | joy.axes.push_back( angular_velocity_ ); 83 | joy.axes.push_back( linear_velocity_ ); 84 | joy.axes.push_back( 1.0 ); 85 | joy.axes.push_back( 0 ); 86 | joy.axes.push_back( 0 ); 87 | 88 | joy.buttons.push_back( 0 ); 89 | joy.buttons.push_back( 0 ); 90 | joy.buttons.push_back( 0 ); 91 | joy.buttons.push_back( 0 ); 92 | joy.buttons.push_back( 0 ); 93 | joy.buttons.push_back( 0 ); 94 | joy.buttons.push_back( 0 ); 95 | joy.buttons.push_back( 1 ); 96 | joy.buttons.push_back( 0 ); 97 | joy.buttons.push_back( 0 ); 98 | joy.buttons.push_back( 0 ); 99 | 100 | joy.header.stamp = ros::Time::now(); 101 | joy.header.frame_id = "teleop_panel"; 102 | velocity_publisher_.publish( joy ); 103 | 104 | mouse_pressed_sent_ = mouse_pressed_; 105 | } 106 | } 107 | 108 | } 109 | #include 110 | PLUGINLIB_EXPORT_CLASS( teleop_rviz_plugin::TeleopPanel,rviz::Panel ) 111 | -------------------------------------------------------------------------------- /src/teleop_rviz_plugin/src/teleop_panel.h: -------------------------------------------------------------------------------- 1 | #ifndef TELEOP_PANEL_H 2 | #define TELEOP_PANEL_H 3 | 4 | #ifndef Q_MOC_RUN 5 | # include 6 | 7 | # include 8 | #endif 9 | 10 | #include 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | 20 | #include 21 | #include 22 | #include 23 | #include 24 | 25 | class QLineEdit; 26 | 27 | namespace teleop_rviz_plugin 28 | { 29 | 30 | class DriveWidget; 31 | 32 | class TeleopPanel: public rviz::Panel 33 | { 34 | Q_OBJECT 35 | public: 36 | TeleopPanel( QWidget* parent = 0 ); 37 | 38 | public Q_SLOTS: 39 | void setVel( float linear_velocity_, float angular_velocity_, bool mouse_pressed_ ); 40 | 41 | protected Q_SLOTS: 42 | void pressButton1(); 43 | void sendVel(); 44 | 45 | protected: 46 | DriveWidget* drive_widget_; 47 | ros::Publisher velocity_publisher_; 48 | ros::NodeHandle nh_; 49 | 50 | QPushButton *push_button_1_; 51 | 52 | float linear_velocity_; 53 | float angular_velocity_; 54 | bool mouse_pressed_; 55 | bool mouse_pressed_sent_; 56 | }; 57 | 58 | } 59 | 60 | #endif // TELEOP_PANEL_H 61 | -------------------------------------------------------------------------------- /src/terrain_analysis/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(terrain_analysis) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | roscpp 27 | std_msgs 28 | sensor_msgs 29 | pcl_ros 30 | ) 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | 36 | ## Specify additional locations of header files 37 | ## Your package locations should be listed before other locations 38 | include_directories( 39 | ${catkin_INCLUDE_DIRS} 40 | ${OpenCV_INCLUDE_DIRS} 41 | ${PCL_INCLUDE_DIRS} 42 | "${PROJECT_SOURCE_DIR}/include" 43 | /usr/local/include # Location when using 'make system_install' 44 | /usr/include # More usual location (e.g. when installing using a package) 45 | ) 46 | 47 | ## Specify additional locations for library files 48 | link_directories( 49 | /usr/local/lib # Location when using 'make system_install' 50 | /usr/lib # More usual location (e.g. when installing using a package) 51 | ) 52 | 53 | ## Declare executables 54 | add_executable(terrainAnalysis src/terrainAnalysis.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(terrainAnalysis ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS terrainAnalysis 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | -------------------------------------------------------------------------------- /src/terrain_analysis/launch/terrain_analysis.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /src/terrain_analysis/package.xml: -------------------------------------------------------------------------------- 1 | 2 | terrain_analysis 3 | 0.0.1 4 | Terrain Analysis 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/terrain_analysis_ext/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(terrain_analysis_ext) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | roscpp 27 | std_msgs 28 | sensor_msgs 29 | pcl_ros 30 | ) 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | 36 | ## Specify additional locations of header files 37 | ## Your package locations should be listed before other locations 38 | include_directories( 39 | ${catkin_INCLUDE_DIRS} 40 | ${OpenCV_INCLUDE_DIRS} 41 | ${PCL_INCLUDE_DIRS} 42 | "${PROJECT_SOURCE_DIR}/include" 43 | /usr/local/include # Location when using 'make system_install' 44 | /usr/include # More usual location (e.g. when installing using a package) 45 | ) 46 | 47 | ## Specify additional locations for library files 48 | link_directories( 49 | /usr/local/lib # Location when using 'make system_install' 50 | /usr/lib # More usual location (e.g. when installing using a package) 51 | ) 52 | 53 | ## Declare executables 54 | add_executable(terrainAnalysisExt src/terrainAnalysisExt.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(terrainAnalysisExt ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS terrainAnalysisExt 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | -------------------------------------------------------------------------------- /src/terrain_analysis_ext/launch/terrain_analysis_ext.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | -------------------------------------------------------------------------------- /src/terrain_analysis_ext/package.xml: -------------------------------------------------------------------------------- 1 | 2 | terrain_analysis_ext 3 | 0.0.1 4 | Terrain Analysis in Extended Scale 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/vehicle_simulator/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(vehicle_simulator) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(OpenCV REQUIRED) 18 | find_package(PCL REQUIRED) 19 | 20 | ################################### 21 | ## catkin specific configuration ## 22 | ################################### 23 | 24 | catkin_package( 25 | CATKIN_DEPENDS 26 | roscpp 27 | std_msgs 28 | sensor_msgs 29 | pcl_ros 30 | ) 31 | 32 | ########### 33 | ## Build ## 34 | ########### 35 | 36 | ## Specify additional locations of header files 37 | ## Your package locations should be listed before other locations 38 | include_directories( 39 | ${catkin_INCLUDE_DIRS} 40 | ${OpenCV_INCLUDE_DIRS} 41 | ${PCL_INCLUDE_DIRS} 42 | "${PROJECT_SOURCE_DIR}/include" 43 | /usr/local/include # Location when using 'make system_install' 44 | /usr/include # More usual location (e.g. when installing using a package) 45 | ) 46 | 47 | ## Specify additional locations for library files 48 | link_directories( 49 | /usr/local/lib # Location when using 'make system_install' 50 | /usr/lib # More usual location (e.g. when installing using a package) 51 | ) 52 | 53 | ## Declare executables 54 | add_executable(vehicleSimulator src/vehicleSimulator.cpp) 55 | 56 | ## Specify libraries to link a library or executable target against 57 | target_link_libraries(vehicleSimulator ${catkin_LIBRARIES} ${OpenCV_LIBRARIES} ${PCL_LIBRARIES}) 58 | 59 | install(TARGETS vehicleSimulator 60 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 61 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 62 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 63 | ) 64 | 65 | install(DIRECTORY launch/ 66 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 67 | ) 68 | install(DIRECTORY log/ 69 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/log 70 | ) 71 | install(DIRECTORY mesh/ 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/mesh 73 | ) 74 | install(DIRECTORY rviz/ 75 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/rviz 76 | ) 77 | -------------------------------------------------------------------------------- /src/vehicle_simulator/launch/system_unity.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 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/vehicle_simulator/log/readme.txt: -------------------------------------------------------------------------------- 1 | Exploration metrics, vehicle trajectory, waypoints, and visualization markers 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 | 7 | 'waypoint_xxx.txt' contains four columns, respectively x (m), y (m), z (m), and time duration from start of the run (second). 8 | 9 | 'marker_xxx.txt' contains eight columns, respectively x (m), y (m), z (m), length (m), width (m), height (m), orientatoin_of_length_edge (rad), and time duration from start of the run (second). 10 | -------------------------------------------------------------------------------- /src/vehicle_simulator/mesh/unity/readme.txt: -------------------------------------------------------------------------------- 1 | Environment files are in this folder. 2 | 3 | 'environment' contains the Unity environment model. In the folder, 'AssetList.csv' is generated at runtime containing all assets in the environment model and their poses and semantic rendering colors, 'Dimensions.csv' and 'Categories.csv' contain the dimension information and category mapping. 4 | 5 | 'map.ply' is the point cloud of the environment in binary format. 6 | 7 | 'object_list.txt' is the object list with bounding boxes and labels of each object. Each row has 'object_id, x (m), y (m), z (m), length (m), width (m), height (m), orientatoin_of_length_edge (rad), label (string)'. 8 | 9 | 'traversable_area.ply' is the traversable area for the robot. 10 | 11 | 'map.jpg' is a screenshot of the segmented point cloud. 12 | 13 | 'render.jpg' is an image render of the environment. 14 | -------------------------------------------------------------------------------- /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 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /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 | 22 | 23 | -------------------------------------------------------------------------------- /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/waypoint_converter/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_converter) 3 | 4 | set(CMAKE_BUILD_TYPE Release) 5 | set(BUILD_STATIC_LIBS ON) 6 | set(BUILD_SHARED_LIBS OFF) 7 | set(CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) 8 | 9 | ## Find catkin macros and libraries 10 | find_package(catkin REQUIRED COMPONENTS 11 | roscpp 12 | std_msgs 13 | sensor_msgs 14 | pcl_ros 15 | ) 16 | 17 | find_package(PCL REQUIRED) 18 | 19 | ################################### 20 | ## catkin specific configuration ## 21 | ################################### 22 | 23 | catkin_package( 24 | CATKIN_DEPENDS 25 | roscpp 26 | std_msgs 27 | sensor_msgs 28 | pcl_ros 29 | ) 30 | 31 | ########### 32 | ## Build ## 33 | ########### 34 | 35 | ## Specify additional locations of header files 36 | ## Your package locations should be listed before other locations 37 | include_directories( 38 | ${catkin_INCLUDE_DIRS} 39 | ${PCL_INCLUDE_DIRS} 40 | "${PROJECT_SOURCE_DIR}/include" 41 | /usr/local/include # Location when using 'make system_install' 42 | /usr/include # More usual location (e.g. when installing using a package) 43 | ) 44 | 45 | ## Specify additional locations for library files 46 | link_directories( 47 | /usr/local/lib # Location when using 'make system_install' 48 | /usr/lib # More usual location (e.g. when installing using a package) 49 | ) 50 | 51 | ## Declare executables 52 | add_executable(waypointConverter src/waypointConverter.cpp) 53 | 54 | ## Specify libraries to link a library or executable target against 55 | target_link_libraries(waypointConverter ${catkin_LIBRARIES} ${PCL_LIBRARIES}) 56 | 57 | install(TARGETS waypointConverter 58 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 59 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 60 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 61 | ) 62 | 63 | install(DIRECTORY launch/ 64 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/launch 65 | ) 66 | install(DIRECTORY data/ 67 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}/data 68 | ) 69 | -------------------------------------------------------------------------------- /src/waypoint_converter/launch/waypoint_converter.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 9 | 10 | 11 | 12 | 13 | 14 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | -------------------------------------------------------------------------------- /src/waypoint_converter/package.xml: -------------------------------------------------------------------------------- 1 | 2 | waypoint_converter 3 | 0.0.1 4 | Waypoint Converter from Waypoint with Heading 5 | Ji Zhang 6 | BSD 7 | 8 | catkin 9 | 10 | roscpp 11 | std_msgs 12 | sensor_msgs 13 | message_filters 14 | pcl_ros 15 | 16 | roscpp 17 | std_msgs 18 | sensor_msgs 19 | message_filters 20 | pcl_ros 21 | 22 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(waypoint_rviz_plugin) 3 | 4 | find_package(catkin REQUIRED COMPONENTS 5 | roscpp 6 | rospy 7 | rviz 8 | ) 9 | 10 | set(CMAKE_AUTOMOC ON) 11 | if(rviz_QT_VERSION VERSION_LESS "5") 12 | message(STATUS "Using Qt4 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 13 | find_package(Qt4 ${rviz_QT_VERSION} EXACT REQUIRED QtCore QtGui) 14 | include(${QT_USE_FILE}) 15 | else() 16 | message(STATUS "Using Qt5 based on the rviz_QT_VERSION: ${rviz_QT_VERSION}") 17 | find_package(Qt5 ${rviz_QT_VERSION} EXACT REQUIRED Core Widgets) 18 | set(QT_LIBRARIES Qt5::Widgets) 19 | endif() 20 | 21 | add_definitions(-DQT_NO_KEYWORDS) 22 | 23 | catkin_package( 24 | LIBRARIES ${PROJECT_NAME} 25 | CATKIN_DEPENDS roscpp rviz 26 | ) 27 | 28 | include_directories( 29 | include 30 | ${QT_INCLUDE_DIRS} 31 | ${catkin_INCLUDE_DIRS} 32 | ) 33 | 34 | set(HEADER_FILES 35 | include/waypoint_tool.h 36 | ) 37 | set(SRC_FILES 38 | src/waypoint_tool.cpp 39 | ) 40 | 41 | add_library(${PROJECT_NAME} 42 | ${SRC_FILES} ${HEADER_FILES} 43 | ) 44 | 45 | add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 46 | target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} ${QT_LIBRARIES} ${Boost_LIBRARIES} ${OGRE_OV_LIBRARIES_ABS}) 47 | 48 | install(TARGETS ${PROJECT_NAME} 49 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 50 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 51 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 52 | ) 53 | 54 | install(FILES plugin_description.xml 55 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 56 | ) 57 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/include/waypoint_tool.h: -------------------------------------------------------------------------------- 1 | #ifndef WAYPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H 2 | #define WAYPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | #include 9 | #include 10 | 11 | #include "rviz/display_context.h" 12 | #include "rviz/properties/string_property.h" 13 | #include "rviz/default_plugin/tools/pose_tool.h" 14 | 15 | namespace rviz 16 | { 17 | class StringProperty; 18 | 19 | class WaypointTool : public PoseTool 20 | { 21 | Q_OBJECT 22 | public: 23 | WaypointTool(); 24 | virtual ~WaypointTool() 25 | { 26 | } 27 | virtual void onInitialize(); 28 | 29 | protected: 30 | virtual void onPoseSet(double x, double y, double theta); 31 | 32 | private Q_SLOTS: 33 | void updateTopic(); 34 | 35 | private: 36 | ros::NodeHandle nh_; 37 | ros::Subscriber sub_; 38 | ros::Publisher pub_; 39 | ros::Publisher pub_joy_; 40 | 41 | StringProperty* topic_property_; 42 | }; 43 | } 44 | 45 | #endif // WAYPOINT_RVIZ_PLUGIN_WAYPOINT_TOOL_H 46 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | waypoint_rviz_plugin 4 | 0.0.1 5 | Waypoint RVIZ Plugin 6 | Chao Cao 7 | BSD 8 | 9 | catkin 10 | 11 | roscpp 12 | rospy 13 | rviz 14 | qtbase5-dev 15 | 16 | roscpp 17 | rospy 18 | rviz 19 | 20 | roscpp 21 | rospy 22 | rviz 23 | libqt5-core 24 | libqt5-gui 25 | libqt5-widgets 26 | 27 | 28 | 29 | 30 | 31 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/plugin_description.xml: -------------------------------------------------------------------------------- 1 | 2 | 5 | 6 | A tool for sending waypoints 7 | 8 | 9 | 10 | -------------------------------------------------------------------------------- /src/waypoint_rviz_plugin/src/waypoint_tool.cpp: -------------------------------------------------------------------------------- 1 | #include "waypoint_tool.h" 2 | 3 | namespace rviz 4 | { 5 | WaypointTool::WaypointTool() 6 | { 7 | shortcut_key_ = 'w'; 8 | 9 | topic_property_ = new StringProperty("Topic", "waypoint", "The topic on which to publish navigation waypionts.", 10 | getPropertyContainer(), SLOT(updateTopic()), this); 11 | } 12 | 13 | void WaypointTool::onInitialize() 14 | { 15 | PoseTool::onInitialize(); 16 | setName("Waypoint with Heading"); 17 | updateTopic(); 18 | } 19 | 20 | void WaypointTool::updateTopic() 21 | { 22 | pub_ = nh_.advertise("/way_point_with_heading", 5); 23 | pub_joy_ = nh_.advertise("/joy", 5); 24 | } 25 | 26 | void WaypointTool::onPoseSet(double x, double y, double theta) 27 | { 28 | sensor_msgs::Joy joy; 29 | 30 | joy.axes.push_back(0); 31 | joy.axes.push_back(0); 32 | joy.axes.push_back(-1.0); 33 | joy.axes.push_back(0); 34 | joy.axes.push_back(0); 35 | joy.axes.push_back(1.0); 36 | joy.axes.push_back(0); 37 | joy.axes.push_back(0); 38 | 39 | joy.buttons.push_back(0); 40 | joy.buttons.push_back(0); 41 | joy.buttons.push_back(0); 42 | joy.buttons.push_back(0); 43 | joy.buttons.push_back(0); 44 | joy.buttons.push_back(0); 45 | joy.buttons.push_back(0); 46 | joy.buttons.push_back(1); 47 | joy.buttons.push_back(0); 48 | joy.buttons.push_back(0); 49 | joy.buttons.push_back(0); 50 | 51 | joy.header.stamp = ros::Time::now(); 52 | joy.header.frame_id = "waypoint_tool"; 53 | pub_joy_.publish(joy); 54 | 55 | geometry_msgs::Pose2D waypoint; 56 | waypoint.x = x; 57 | waypoint.y = y; 58 | waypoint.theta = theta; 59 | 60 | pub_.publish(waypoint); 61 | usleep(10000); 62 | pub_.publish(waypoint); 63 | } 64 | } 65 | 66 | #include 67 | PLUGINLIB_EXPORT_CLASS(rviz::WaypointTool, rviz::Tool) 68 | -------------------------------------------------------------------------------- /system_bring_up.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | SCRIPT_DIR="$( cd "$( dirname "${BASH_SOURCE[0]}" )" &> /dev/null && pwd )" 4 | 5 | cd $SCRIPT_DIR 6 | source ./devel/setup.bash 7 | ./src/vehicle_simulator/mesh/unity/environment/Model.x86_64 & 8 | roslaunch vehicle_simulator system_unity.launch 9 | --------------------------------------------------------------------------------